Diff: BalancingRobot.cpp

Revision:

9:67f2110fce8e

Parent:

7:f1d24c6119ac

diff -r 87dd9d5f7001 -r 67f2110fce8e BalancingRobot.cpp
--- a/BalancingRobot.cpp	Fri Apr 13 19:45:25 2012 +0000
+++ b/BalancingRobot.cpp	Thu Aug 25 22:50:49 2016 +0000
@@ -1,43 +1,48 @@
 /*
  * The code is released under the GNU General Public License.
- * Developed by Kristian Lauszus
  * This is the algorithm for my balancing robot/segway.
- * It is controlled by a PS3 Controller via bluetooth.
- * The remote control code can be found at my other github repository: https://github.com/TKJElectronics/BalancingRobot
- * For details, see http://blog.tkjelectronics.dk/2012/02/the-balancing-robot/
+ * It is controlled by Bluetooth
  */
 
 #include "mbed.h"
 #include "BalancingRobot.h"
-#include "Encoder.h"
+#include "MPU6050.h"
+#include "ledControl.h"
+#include "HALLFX_ENCODER.h"
 
 /* Setup serial communication */
-Serial xbee(p13,p14); // For wireless debugging and setting PID constants
-Serial ps3(p9,p10); // For remote control
-Serial debug(USBTX, USBRX); // USB
+Serial blue(p28,p27); // For remote control ps3
+Serial pc(USBTX, USBRX); // USB
 
-/* Setup encoders */
-Encoder leftEncoder(p29,p30);
-Encoder rightEncoder(p28,p27);
+/* IMU */
+MPU6050 mpu6050;  
+
+/* Encoders*/
+HALLFX_ENCODER leftEncoder(p13);
+HALLFX_ENCODER rightEncoder(p14);
 
 /* Setup timer instance */
 Timer t;
 
+/* Ticker for led toggling */
+Ticker toggler1;               
+
 int main() {
     /* Set baudrate */
-    xbee.baud(115200);
-    ps3.baud(115200);
-    debug.baud(115200);
+    blue.baud(9600);
+    pc.baud(9600);
     
     /* Set PWM frequency */
     leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
     rightPWM.period(0.00005);
     
     /* Calibrate the gyro and accelerometer relative to ground */
-    calibrateSensors();
-    
-    xbee.printf("Initialized\n");
-    processing(); // Send output to processing application
+    mpu6050.whoAmI();                            // Communication test: WHO_AM_I register reading 
+    wait(0.5);
+    mpu6050.calibrate(accelBias,gyroBias);       // Calibrate MPU6050 and load biases into bias registers
+    pc.printf("Calibration is completed. \r\n"); 
+    mpu6050.init();                              // Initialize the sensor
+    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
     
     /* Setup timing */
     t.start();
@@ -45,103 +50,99 @@
     timer = loopStartTime;
     
     while (1) {
-        /* Calculate pitch */
-        accYangle = getAccY();
-        gyroYrate = getGyroYrate();
-        
-        // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html
-        pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
+        /* Calculate pitch angle using a Complementary filter */
+        mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
+        wait_us(5);
         timer = t.read_us();
         
-        //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
-        
         /* Drive motors */
-        if (pitch < 60 || pitch > 120) // Stop if falling or laying down
+        if (abs(pitchAngle)> 20) // Stop if falling or laying down
             stopAndReset();
         else
             PID(targetAngle,targetOffset);
         
         /* Update wheel velocity every 100ms */
         loopCounter++;
-        if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc.
-            //xbee.printf("Wheel - timer: %i\n",t.read_ms());
-            wheelPosition = leftEncoder.read() + rightEncoder.read();
+        if (loopCounter%10 == 0) 
+        { // If remainder is equal 0, it must be 10,20,30 etc.
+            LeftWheelPosition = leftEncoder.read();
+            RightWheelPosition = rightEncoder.read();
+            wheelPosition = LeftWheelPosition + RightWheelPosition;
             wheelVelocity = wheelPosition - lastWheelPosition;
             lastWheelPosition = wheelPosition;
-            //xbee.printf("WheelVelocity: %i\n",wheelVelocity);
             
-            if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking
+            if (abs(wheelVelocity) <= 20 && !stopped) 
+            { // Set new targetPosition if braking
                 targetPosition = wheelPosition;
                 stopped = true;
-                //xbee.printf("Stopped\n");
             }
         }
         
+        pc.printf("Pitch: %f, error: %f, PIDValue: %f \n",pitchAngle,error,PIDValue);
+        pc.printf("Left Wheel Encoder: %f, Right Wheel Encoder: %f \n", LeftWheelPosition, RightWheelPosition);
+        pc.printf("Wheel Position: %f, Last wheel position:%f, Velocity: %f \n\n", wheelPosition, lastWheelPosition, wheelVelocity);
+                
         /* Check for incoming serial data */
-        if (ps3.readable()) // Check if there's any incoming data
-            receivePS3();
-        if (xbee.readable()) // For setting the PID values
-            receiveXbee();
-        
-        /* Read battery voltage every 1s */
-        if (loopCounter == 100) {
-            loopCounter = 0; // Reset loop counter
-            double analogVoltage = batteryVoltage.read()/1*3.3; // Convert to voltage
-            analogVoltage *= 6.6; // The analog pin is connected to a 56k-10k voltage divider
-            xbee.printf("analogVoltage: %f - timer: %i\n",analogVoltage,t.read_ms());
-            if (analogVoltage < 9 && pitch > 60 && pitch < 120) // Set buzzer on, if voltage gets critical low
-                buzzer = 1; // The mbed resets at aproximatly 8.5V           
-        }
-        
+        if (blue.readable()) // Check if there's any incoming data
+            receiveBluetooth();
+          
         /* Use a time fixed loop */
         lastLoopUsefulTime = t.read_us() - loopStartTime;
         if (lastLoopUsefulTime < STD_LOOP_TIME)
             wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
         lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
         loopStartTime = t.read_us();
-        
-        //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime);
     }
 }
-void PID(double restAngle, double offset) {
+
+void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
+
+void toggle_led1() {ledToggle(1);}
+void toggle_led2() {ledToggle(2);}
+
+void PID(double restAngle, double offset) 
+{
     /* Steer robot */
-    if (steerForward) {
+    if (steerForward) 
+    {
         offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
         restAngle -= offset;
-        //xbee.printf("Forward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
-    } else if (steerBackward) {
+    } 
+    else if (steerBackward) 
+    {
         offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
         restAngle += offset;
-        //xbee.printf("Backward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
     }
-    /* Break */
-    else if (steerStop) {
-        long positionError = wheelPosition - targetPosition;
-        if (abs(positionError) > zoneA) // Inside zone A
+    /* Brake */
+    else if (steerStop) 
+    {
+        //long positionError = wheelPosition - targetPosition;
+        
+    /*  if (abs(positionError) > zoneA) // Inside zone A
             restAngle -= (double)positionError/positionScaleA;
         else if (abs(positionError) > zoneB) // Inside zone B
             restAngle -= (double)positionError/positionScaleB;
         else // Inside zone C
-            restAngle -= (double)positionError/positionScaleC;
+            restAngle -= (double)positionError/positionScaleC; */
         
         restAngle -= (double)wheelVelocity/velocityScaleStop;
         
-        if (restAngle < 80) // Limit rest Angle
-            restAngle = 80;
-        else if (restAngle > 100)
-            restAngle = 100;
-        //xbee.printf("restAngle: %f\t WheelVelocity: %i positionError: %i\n",restAngle,wheelVelocity,positionError);
+        if (restAngle < -5.00) // Limit rest Angle
+            restAngle = -5.00;
+        else if (restAngle > 5.00)
+            restAngle = 5.00;
     }
+    
     /* Update PID values */
-    double error = (restAngle - pitch)/100;
+    error = restAngle - pitchAngle;
     double pTerm = Kp * error;
     iTerm += Ki * error;
     double dTerm = Kd * (error - lastError);
     lastError = error;
     
-    double PIDValue = pTerm + iTerm + dTerm;
+    PIDValue = pTerm + iTerm + dTerm;
     
-    //debug.printf("Pitch: %5.2f\tPIDValue:  %5.2f\tpTerm: %5.2f\tiTerm: %5.2f\tdTerm: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,pTerm,iTerm,dTerm,Kp,Ki,Kd);
+    //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd);
     
     /* Steer robot sideways */
     double PIDLeft;
@@ -162,7 +163,7 @@
         PIDLeft = PIDValue;
         PIDRight = PIDValue;
     }
-    PIDLeft *= 0.95; // compensate for difference in the motors
+    //PIDLeft *= 0.95; // compensate for difference in the motors
     
     /* Set PWM Values */
     if (PIDLeft >= 0)
@@ -174,16 +175,21 @@
     else
         move(right, backward, PIDRight * -1);
 }
-void receivePS3() {
-    char input[16]; // The serial buffer is only 16 characters
-    int i = 0;
-    while (1) {
-        input[i] = ps3.getc();
-        if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
-            break;
-        i++;
-    }
-    //debug.printf("Input: %s\n",input);
+void receiveBluetooth() {
+    //char input[16]; // The serial buffer is only 16 characters
+    char phone_char;
+    //int i = 0;
+    //while (1) {
+        //input[i] = blue.getc();
+        //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
+            //break;
+        //i++;
+        
+        //pc.printf("Im here..");
+    //}
+    phone_char = blue.getc();
+    pc.putc(phone_char);
+    //pc.printf("Input: %c\n",i);
     
     // Set all false
     steerForward = false;
@@ -195,151 +201,43 @@
     steerRotateRight = false;
     
     /* For remote control */
-    if (input[0] == 'F') { // Forward
-        strtok(input, ","); // Ignore 'F'
-        targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-        //xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
+    if (/*input[0]*/phone_char == 'F') { // Forward
+        //strtok(/*input*/phone_char, ","); // Ignore 'F'
+        //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double
+        //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
         steerForward = true;
-    } else if (input[0] == 'B') { // Backward
-        strtok(input, ","); // Ignore 'B'
+    } else if (/*input[0]*/phone_char == 'B') { // Backward
+        //strtok(/*input*/phone_char, ","); // Ignore 'B'
         targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-        //xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
+        //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
         steerBackward = true;
-    } else if (input[0] == 'L') { // Left
-        if (input[1] == 'R') // Left Rotate
+    } else if (/*input[0]*/phone_char == 'L') { // Left
+        if (/*input[0]*/phone_char == 'L') // Left Rotate
             steerRotateLeft = true;
         else
             steerLeft = true;
-    } else if (input[0] == 'R') { // Right
-        if (input[1] == 'R') // Right Rotate
+    } else if (/*input[0]*/phone_char == 'R') { // Right
+        if (/*input[0]*/phone_char == 'R') // Right Rotate
             steerRotateRight = true;
         else
             steerRight = true;
-    } else if (input[0] == 'S') { // Stop
+    } else if (/*input[0]*/phone_char == 'S') { // Stop
         steerStop = true;
         stopped = false;
         targetPosition = wheelPosition;
-    }
-    
-    else if (input[0] == 'T') { // Set the target angle
-        strtok(input, ","); // Ignore 'T'
-        targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-        xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging
-    } else if (input[0] == 'A') { // Abort
+    } else if (/*input[0]*/phone_char == 'A') { // Abort
         stopAndReset();
-        while (ps3.getc() != 'C'); // Wait until continue is send
+        while (blue.getc() != 'C'); // Wait until continue is send
     }
 }
-void receiveXbee() {
-    char input[16]; // The serial buffer is only 16 characters
-    int i = 0;
-    while (1) {
-        input[i] = xbee.getc();
-        if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
-            break;
-        i++;
-    }
-    //debug.printf("Input: %s\n",input);
-    
-    if (input[0] == 'T') { // Set the target angle
-        strtok(input, ","); // Ignore 'T'
-        targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-    } else if (input[0] == 'P') {
-        strtok(input, ",");//Ignore 'P'
-        Kp = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-    } else if (input[0] == 'I') {
-        strtok(input, ",");//Ignore 'I'
-        Ki = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-    } else if (input[0] == 'D') {
-        strtok(input, ",");//Ignore 'D'
-        Kd = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
-    } else if (input[0] == 'A') { // Abort
-        stopAndReset();
-        while (xbee.getc() != 'C'); // Wait until continue is send
-    } else if (input[0] == 'G') // The processing application sends this at startup
-        processing(); // Send output to processing application
-}
-void processing() {
-    /* Send output to processing application */
-    xbee.printf("Processing,%5.2f,%5.2f,%5.2f,%5.2f\n",Kp,Ki,Kd,targetAngle);
-}
+
 void stopAndReset() {
     stop(both);
     lastError = 0;
     iTerm = 0;
     targetPosition = wheelPosition;
-    buzzer = 0;
 }
-double kalman(double newAngle, double newRate, double dtime) {
-    // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
-    // with slightly modifications by Kristian Lauszus
-    // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
-    dt = dtime / 1000000; // Convert from microseconds to seconds
-    
-    // Discrete Kalman filter time update equations - Time Update ("Predict")
-    // Update xhat - Project the state ahead
-    angle += dt * (newRate - bias);
-    
-    // Update estimation error covariance - Project the error covariance ahead
-    P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
-    P_01 += -dt * P_11;
-    P_10 += -dt * P_11;
-    P_11 += +Q_gyro * dt;
-    
-    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
-    // Calculate Kalman gain - Compute the Kalman gain
-    S = P_00 + R_angle;
-    K_0 = P_00 / S;
-    K_1 = P_10 / S;
-    
-    // Calculate angle and resting rate - Update estimate with measurement zk
-    y = newAngle - angle;
-    angle += K_0 * y;
-    bias += K_1 * y;
-    
-    // Calculate estimation error covariance - Update the error covariance
-    P_00 -= K_0 * P_00;
-    P_01 -= K_0 * P_01;
-    P_10 -= K_1 * P_00;
-    P_11 -= K_1 * P_01;
-    
-    return angle;
-}
-double getGyroYrate() {
-    // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3=0.001009091
-    double gyroRate = -((gyroY.read() - zeroValues[0]) / 0.001009091);
-    return gyroRate;
-}
-double getAccY() {
-    // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3=0.1
-    double accXval = (accX.read() - zeroValues[1]) / 0.1;
-    double accYval = (accY.read() - zeroValues[2]) / 0.1;
-    accYval--;//-1g when lying down
-    double accZval = (accZ.read() - zeroValues[3]) / 0.1;
-    
-    double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the length of force vector
-    double angleY = acos(accYval / R) * RAD_TO_DEG;
-    
-    return angleY;
-}
-void calibrateSensors() {
-    LEDs = 0xF; // Turn all onboard LEDs on
-    
-    double adc[4] = {0,0,0,0};
-    for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
-        adc[0] += gyroY.read();
-        adc[1] += accX.read();
-        adc[2] += accY.read();
-        adc[3] += accZ.read();
-        wait_ms(10);
-    }
-    zeroValues[0] = adc[0] / 100; // Gyro X-axis
-    zeroValues[1] = adc[1] / 100; // Accelerometer X-axis
-    zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis
-    zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis
-    
-    LEDs = 0x0; // Turn all onboard LEDs off
-}
+
 void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
     if (motor == left) {
         leftPWM = speed;