phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-05-20 11:31:48 +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" #include "Dezibot.h"
#define interval 30
Dezibot dezibot = Dezibot(); Dezibot dezibot = Dezibot();
#define maxDuty 160 #define maxDuty 6000
#define minDuty 90 #define minDuty 3500
#define baseDuty 110 #define baseDuty 3900
void setup() { void setup() {
// put your setup code here, to run once: // put your setup code here, to run once:
dezibot.begin(); dezibot.begin();
@ -12,14 +11,18 @@ void setup() {
//analogWrite(11, 2000); //analogWrite(11, 2000);
//analogWrite(12, 2000); //analogWrite(12, 2000);
} }
uint8_t leftDuty= baseDuty; uint16_t leftDuty= baseDuty;
uint8_t rightDuty = baseDuty; uint16_t rightDuty = baseDuty;
unsigned long previous = millis(); unsigned long previous = millis();
unsigned long current = millis(); unsigned long current = millis();
long averageZRotation = 0; long averageZRotation = 0;
long averageXAccel = 0; long averageXAccel = 0;
int left=0; int left=0;
int right=0; int right=0;
int difference = 0;
int changerate = 0;
int interval = 40;
int threshold = 150;
int resultcounter = 0; int resultcounter = 0;
void loop() { void loop() {
// put your main code here, to run repeatedly: // put your main code here, to run repeatedly:
@ -28,76 +31,46 @@ void loop() {
averageXAccel += dezibot.motionDetection.getAcceleration().x; averageXAccel += dezibot.motionDetection.getAcceleration().x;
averageZRotation += result.z; averageZRotation += result.z;
//Serial.println(result.z); //Serial.println(result.z);
if (result.z > 100){ if (result.z > threshold){
right++; right++;
} else if(result.z < -100) { } else if(result.z < -threshold) {
left++; left++;
} }
resultcounter++; resultcounter++;
if ((current - previous) > interval){ if ((current - previous) > interval){
Serial.println("============================================="); //averageZRotation = averageZRotation / resultcounter;
previous = current; difference = abs(left-right);
averageZRotation = averageZRotation / resultcounter; if (difference>25){
averageXAccel = averageXAccel / resultcounter; changerate = 100;
Serial.print("Z:"); } else if(difference>20){
Serial.println(averageZRotation); 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.clearDisplay();
dezibot.display.setCursor(0, 0); dezibot.display.setCursor(0, 0);
dezibot.display.setTextColor(WHITE); dezibot.display.setTextColor(WHITE);
dezibot.display.setTextSize(2); // Draw 2X-scale text 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(left);
dezibot.display.print(" "); dezibot.display.print(" ");
dezibot.display.println(right); 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 if(left>right){ //rotates anticlock
leftDuty++; leftDuty+=changerate;
rightDuty--; rightDuty-=changerate;
/*if(abs(leftDuty-baseDuty)<abs(rightDuty-baseDuty)){ } else if(left<right){
rightDuty--; leftDuty-=changerate;
} else{ rightDuty+=changerate;
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--;
}*/
} }
analogWrite(12,leftDuty); dezibot.motion.left.setSpeed(leftDuty);
analogWrite(11, rightDuty); dezibot.motion.right.setSpeed(rightDuty);
dezibot.display.println(leftDuty); dezibot.display.println(leftDuty);
dezibot.display.println(rightDuty); dezibot.display.println(rightDuty);
dezibot.display.display(); dezibot.display.display();
@ -108,5 +81,6 @@ void loop() {
resultcounter = 0; resultcounter = 0;
left = 0; left = 0;
right = 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(0x1F);
handler->transfer(0x0F); handler->transfer(0x0F);
//busy Wait for startup //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); digitalWrite(34,HIGH);
handler->endTransaction(); handler->endTransaction();
}; };