phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-05-20 03:21:46 +02:00
Code Issues Packages Projects Releases Wiki Activity

improved the calibrated movement example

Browse Source
This commit is contained in:
hhaupt 2024-05-12 23:25:08 +02:00
parent e03bde591d
commit 8fb1ab7651
2 changed files with 43 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
example/IMU/Motion_Correction/motion_correction/motion_correction.ino
View File

@ -1,9 +1,8 @@
#include "Dezibot.h"
#define interval 30
Dezibot dezibot = Dezibot();
#define maxDuty 160
#define minDuty 90
#define baseDuty 110
#define maxDuty 6000
#define minDuty 3500
#define baseDuty 3900
void setup() {
// put your setup code here, to run once:
dezibot.begin();
@ -12,14 +11,18 @@ void setup() {
//analogWrite(11, 2000);
//analogWrite(12, 2000);
}
uint8_t leftDuty= baseDuty;
uint8_t rightDuty = baseDuty;
uint16_t leftDuty= baseDuty;
uint16_t rightDuty = baseDuty;
unsigned long previous = millis();
unsigned long current = millis();
long averageZRotation = 0;
long averageXAccel = 0;
int left=0;
int right=0;
int difference = 0;
int changerate = 0;
int interval = 40;
int threshold = 150;
int resultcounter = 0;
void loop() {
// put your main code here, to run repeatedly:
@ -28,76 +31,46 @@ void loop() {
averageXAccel += dezibot.motionDetection.getAcceleration().x;
averageZRotation += result.z;
//Serial.println(result.z);
if (result.z > 100){
if (result.z > threshold){
right++;
} else if(result.z < -100) {
} else if(result.z < -threshold) {
left++;
}
resultcounter++;
if ((current - previous) > interval){
Serial.println("=============================================");
previous = current;
averageZRotation = averageZRotation / resultcounter;
averageXAccel = averageXAccel / resultcounter;
Serial.print("Z:");
Serial.println(averageZRotation);
//averageZRotation = averageZRotation / resultcounter;
difference = abs(left-right);
if (difference>25){
changerate = 100;
} else if(difference>20){
changerate = 50;
} else if(difference >15){
changerate = 30;
} else if(difference > 10){
changerate = 10;
} else{
changerate = 5;
}
//Serial.print("Z:");
//Serial.println(averageZRotation);
dezibot.display.clearDisplay();
dezibot.display.setCursor(0, 0);
dezibot.display.setTextColor(WHITE);
dezibot.display.setTextSize(2); // Draw 2X-scale text
dezibot.display.println(averageZRotation);
dezibot.display.println(averageZRotation);
//dezibot.display.println(resultcounter);
dezibot.display.print(left);
dezibot.display.print(" ");
dezibot.display.println(right);
Serial.println("=============================================");
/*if(averageZRotation < -20){
//turns right
if((rightDuty < maxDuty || leftDuty <= minDuty) && rightDuty<255){
rightDuty++;
} else{
leftDuty--;
}
} else if(averageZRotation > 20){
//turns left
if(leftDuty < maxDuty || rightDuty <= minDuty && leftDuty<255){
leftDuty++;
} else {
rightDuty--;
}
}*/
if(left>right){ //rotates anticlock
leftDuty++;
rightDuty--;
/*if(abs(leftDuty-baseDuty)<abs(rightDuty-baseDuty)){
rightDuty--;
} else{
leftDuty++;
}
if (leftDuty == maxDuty){
rightDuty -= 5;
leftDuty -= 1;
}else {
leftDuty++;
}*/
} else if((left-right)<0){
leftDuty--;
rightDuty++;
/*if(abs(leftDuty-baseDuty)>abs(rightDuty-baseDuty)){
rightDuty++;
} else{
leftDuty--;
}
if( leftDuty == minDuty){
rightDuty += 5;
leftDuty += 1;
} else{
leftDuty--;
}*/
leftDuty+=changerate;
rightDuty-=changerate;
} else if(left<right){
leftDuty-=changerate;
rightDuty+=changerate;
}
analogWrite(12,leftDuty);
analogWrite(11, rightDuty);
dezibot.motion.left.setSpeed(leftDuty);
dezibot.motion.right.setSpeed(rightDuty);
dezibot.display.println(leftDuty);
dezibot.display.println(rightDuty);
dezibot.display.display();
@ -108,5 +81,6 @@ void loop() {
resultcounter = 0;
left = 0;
right = 0;
previous = current;
}
}

8
src/motionDetection/MotionDetection.cpp
View File

@ -15,7 +15,13 @@ void MotionDetection::begin(void){
handler->transfer(0x1F);
handler->transfer(0x0F);
//busy Wait for startup
delayMicroseconds(200);
delayMicroseconds(250);
//set Gyroconfig
handler->transfer(0x20);
handler->transfer(0x25);
//set Gyro Filter
handler->transfer(0x23);
handler->transfer(0x37);
digitalWrite(34,HIGH);
handler->endTransaction();
};