Diff: BalancingRobot.cpp

Revision:

0:0150acbc6cf4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BalancingRobot.cpp	Wed Apr 18 06:00:26 2012 +0000
@@ -0,0 +1,455 @@
+/*
+ * 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/
+ */
+
+#include "mbed.h"
+#include "BalancingRobot.h"
+#include "dof9RazorImuAhrs.h"
+
+
+/* Setup encoders */
+//#include "Encoder.h"
+
+//Encoder leftEncoder(p29,p30);
+//Encoder rightEncoder(p28,p27);
+
+/* 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
+dof9RazorImuAhrs theRazor(p28, p27);
+
+
+/* Setup timer instance */
+Timer t;
+
+int main() {
+
+    /* Set baudrate */
+    xbee.baud(57600);
+    ps3.baud(115200);
+    debug.baud(115200);
+    
+    /* 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 - JAOL dos that on the IMU - thank you ATmega!
+    
+    */
+   //calibrateSensors();
+    //debug.printf("Initialized\n");
+    xbee.printf("Initialized\n");
+    processing(); // Send output to processing application
+    
+    /* Setup timing */
+    t.start();
+    loopStartTime = t.read_us();
+    timer = loopStartTime;
+    
+    while (1) {
+        theRazor.update();
+        /* 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
+       
+     // pitch = kalman(accYangle, theRazor.getGyroY(), t.read_us() - timer); // calculate the angle using a Kalman filter
+    
+    
+     
+     pitch = theRazor.getPitch();
+    
+    
+    // Trace Bay
+    //xbee.printf("gyroYrate: %f\n",gyroYrate);
+    //xbee.printf("accYangle: %f\n",accYangle);
+    //xbee.printf("accYangle: %f\n",gyroYrate*dt);
+    
+   // xbee.printf("IMUs Gyro Y: %f\n",theRazor.getGyroY());
+   // xbee.printf("IMUs Accel Y: %f\n",theRazor.getAccY());
+    //xbee.printf("theRazor.getPitch(): %f\n",theRazor.getPitch());
+  // xbee.printf("Pitch: %f\n",pitch);
+        
+    timer = t.read_us();
+ 
+        
+        /* Drive motors */
+        if (pitch < -75 || pitch > 75) // 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 = 0;
+           wheelVelocity = wheelPosition - lastWheelPosition;
+           lastWheelPosition = wheelPosition;
+           xbee.printf("WheelVelocity: %i\n",wheelVelocity);
+            
+           if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking
+               targetPosition = wheelPosition;
+               stopped = true;
+               xbee.printf("Stopped\n");
+           }
+        }
+        */
+      
+       
+        /* 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 1V           
+        }
+       
+         
+         
+        /* 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) {
+
+    /* Steer robot */
+    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) {
+        //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
+            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)wheelVelocity/velocityScaleStop;
+        */
+          //xbee.printf("restAngle: %f\n", restAngle);
+       /*
+        if (restAngle < 10) // Limit rest Angle
+            restAngle = 0;
+        else if (restAngle > 85)
+            restAngle = 60;
+      */
+        
+    }
+    /* Update PID values */
+    double error = (restAngle - pitch)/100; // was 100
+    double pTerm = Kp * error;
+    iTerm += Ki * error;
+    double dTerm = Kd * (error - lastError);
+    lastError = error;
+    
+    double PIDValue = pTerm + iTerm + dTerm;
+    
+    xbee.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);
+    
+    /* Steer robot sideways */
+    double PIDLeft;
+    double PIDRight;
+    if (steerLeft) {
+        PIDLeft = PIDValue-turnSpeed;
+        PIDRight = PIDValue+turnSpeed;
+    } else if (steerRotateLeft) {
+        PIDLeft = PIDValue-rotateSpeed;
+        PIDRight = PIDValue+rotateSpeed;
+    } else if (steerRight) {
+        PIDLeft = PIDValue+turnSpeed;
+        PIDRight = PIDValue-turnSpeed;
+    } else if (steerRotateRight) {
+        PIDLeft = PIDValue+rotateSpeed;
+        PIDRight = PIDValue-rotateSpeed;
+    } else {
+        PIDLeft = PIDValue;
+        PIDRight = PIDValue;
+    }
+    //PIDLeft *= 0.95; // compensate for difference in the motors
+    
+    /* Set PWM Values */
+    if (PIDLeft >= 0)
+        move(left, forward, PIDLeft);
+    else
+        move(left, backward, PIDLeft * -1);
+    if (PIDRight >= 0)
+        move(right, forward, PIDRight);
+    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 PS3 Remote: %s\n",input);
+    
+    // Set all false
+    steerForward = false;
+    steerBackward = false;
+    steerStop = false;
+    steerLeft = false;
+    steerRotateLeft = false;
+    steerRight = false;
+    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
+        debug.printf("%f\n",targetOffset); // Print targetOffset for debugging
+        steerForward = true;
+    } else if (input[0] == 'B') { // Backward
+        strtok(input, ","); // 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
+        steerBackward = true;
+    } else if (input[0] == 'L') { // Left
+        if (input[1] == 'R') // Left Rotate
+            steerRotateLeft = true;
+        else
+            steerLeft = true;
+    } else if (input[0] == 'R') { // Right
+        if (input[1] == 'R') // Right Rotate
+            steerRotateRight = true;
+        else
+            steerRight = true;
+    } else if (input[0] == '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
+        stopAndReset();
+        while (ps3.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++;
+    }
+   xbee.printf("xBee Received 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);
+    
+    //angle += dt * newRate;
+    
+    // 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 = -((theRazor.getGyroY() - zeroValues[0]) / 0.001009091);
+    return gyroRate;
+}
+double getAccY() {
+    // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3=0.1
+    double accXval = (theRazor.getAccX() - zeroValues[1]) / 0.1;
+    double accYval = (theRazor.getAccY() - zeroValues[2]) / 0.1;
+    //accYval--;//-1g when lying down
+   
+    accYval++;// stading up
+   
+    double accZval = (theRazor.getAccZ() - zeroValues[3]) / 0.1;
+    
+    double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the 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] += theRazor.getGyroY();
+        adc[1] += theRazor.getAccX();
+        adc[2] += theRazor.getAccY();
+        adc[3] += theRazor.getAccZ();
+        
+        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)
+
+//xbee.printf("Motor actions (Motors, Direction, Speed):%f,%f,%f\n",motor,direction,speed);
+
+    if (motor == left) {
+        leftPWM = speed;
+        if (direction == forward) {
+            leftA = 0;
+            leftB = 1;
+        } else if (direction == backward) {
+            leftA = 1;
+            leftB = 0;
+        }
+    } else if (motor == right) {
+        rightPWM = speed;
+        if (direction == forward) {
+            rightA = 0;
+            rightB = 1;
+        } else if (direction == backward) {
+            rightA = 1;
+            rightB = 0;
+        }
+    } else if (motor == both) {
+        leftPWM = speed;
+        rightPWM = speed;
+        if (direction == forward) {
+            leftA = 0;
+            leftB = 1;
+            
+            rightA = 0;
+            rightB = 1;
+        } else if (direction == backward) {
+            leftA = 1;
+            leftB = 0;
+            
+            rightA = 1;
+            rightB = 0;
+        }
+    }
+}
+void stop(Motor motor) {
+    if (motor == left) {
+        leftPWM = 1;
+        leftA = 1;
+        leftB = 1;
+    } else if (motor == right) {
+        rightPWM = 1;
+        rightA = 1;
+        rightB = 1;
+    } else if (motor == both) {
+        leftPWM = 1;
+        leftA = 1;
+        leftB = 1;
+        
+        rightPWM = 1;
+        rightA = 1;
+        rightB = 1;
+    }
+}
\ No newline at end of file