Balance bot

Calibration
Balance bot example

Author: kubascikmichal

RoboHeart.cpp

@@ -209,9 +209,8 @@ bool RoboHeart::isCalibrated(int timeout_ms) {
         }
     delay(1);
     }
-    
     Serial.println(counter);
-    return (counter < (timeout_ms / 4));
+    return (counter < (timeout_ms / 3));
 }
 
 float RoboHeart::getRotationX(){

RoboHeart.h

@@ -18,7 +18,7 @@
 #include "RoboHeartStepperMotor.h"
 #include "pins_RoboHeart.h"
 #include <math.h>
-#define TRESHOLD 0.2  //treshold in degrees/s
+#define TRESHOLD 0.3  //treshold in degrees/s
 
 #define MOTOR_A_CHANNEL1 0
 #define MOTOR_A_CHANNEL2 1
@@ -74,8 +74,8 @@ class RoboHeart {
     RoboHeartDRV8836* _turnMotor = NULL;
     RoboHeartDRV8836* _directionMotor = NULL;
 
-    void calculateDiff(int timeout_ms = 500);
-    bool isCalibrated(int timeout_ms = 500);
+    void calculateDiff(int timeout_ms = 250);
+    bool isCalibrated(int timeout_ms = 250);
 
 };
 

examples/RoboHeartBalanceBot/RoboHeartBalanceBot.ino

@@ -71,8 +71,7 @@ void tick() {
 // and later used to indicate stable vertical position
 // of the Balancing Bot.
 void processPinInterrupt() {
-    offsetAngleDeg = processAngle(heart.getRotationX());
-    targetAngleDeg = offsetAngleDeg;
+    heart.resetGyro();
     errorSum = 0;
 }
 

examples/RoboHeartBalanceBotContol/RoboHeartBalanceBotContol.ino

@@ -173,8 +173,9 @@ void bleConnected() {
 
 // handle -180 after crossing 180 Degrees
 float processAngle(float angle) {
-    if (angle < -90) {
-        return angle = 360 + angle;
+    // handle -180 after crossing 180
+    if (angle > 180) {
+        return angle - 360;
     }
     return angle;
 }
@@ -192,8 +193,8 @@ void tick() {
 // and later used to indicate stable vertical position
 // of the Balancing Bot.
 void processPinInterrupt() {
-    offsetAngleDeg = heart.getRotationX();
-    targetAngleDeg = offsetAngleDeg;
+    heart.resetGyro();
+    errorSum = 0;
 }
 
 InterfaceBLE ble = InterfaceBLE();