Diff: BalancingRobot.cpp

Revision:

0:f5c02b620688

Child:

1:01295228342f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BalancingRobot.cpp	Tue Feb 14 10:48:01 2012 +0000
@@ -0,0 +1,306 @@
+#include "BalancingRobot.h"
+
+/* Serial communication */
+Serial xbee(p13,p14); // For wireless debugging
+Serial ps3(p9,p10); // For remote control
+Serial debug(USBTX, USBRX); // USB
+
+/* Timer instance */
+Timer t;
+
+int main() {
+    xbee.baud(115200);
+    ps3.baud(115200);
+    debug.baud(115200);
+
+    leftPWM.period(0.00005); //The motor driver can handle pwm values up to 20kHz (1/20000=0.00005)
+    rightPWM.period(0.00005);
+
+    calibrateSensors(); // Calibrate the gyro and accelerometer relative to ground
+
+    xbee.printf("Initialized\n");
+
+    // Start timing
+    t.start();
+    loopStartTime = t.read_us();
+    timer = loopStartTime;
+
+    while (1) {
+        // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html
+        accYangle = getAccY();
+        gyroYrate = getGyroYrate();
+
+        pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
+        timer = t.read_us();
+
+        if (ps3.readable()) // Check if there's any incoming data
+            receiveSerial();
+
+        //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
+
+        if (pitch < 75 || pitch > 105) //Stop if falling or laying down
+            stopAndReset();
+        else
+            PID(targetAngle);
+
+        /* Used 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 stopAndReset() {
+    stop(both);
+    lastError = 0;
+    iTerm = 0;
+}
+void receiveSerial() {
+    char input[16]; // The serial buffer is only 16 characters, so the data has to be split up
+    int i = 0;
+    while (ps3.readable()) {
+        input[i] = ps3.getc();
+        i++;
+    }
+    LPC_UART0->FCR |= 0x06; // Flush Serial
+    //debug.printf("Input: %s\n",input);
+
+    /* For remote control */
+    if (input[0] == 'F') { // Forward
+        steerForward = true;
+        steerBackward = false;
+        steerLeft = false;
+        steerRotateLeft = false;
+        steerRight = false;
+        steerRotateRight = false;
+    } else if (input[0] == 'B') { // Backward
+        steerForward = false;
+        steerBackward = true;
+        steerLeft = false;
+        steerRotateLeft = false;
+        steerRight = false;
+        steerRotateRight = false;
+    } else if (input[0] == 'L') { // Left
+        if (input[1] == 'R') { // Left Rotate
+            steerLeft = false;
+            steerRotateLeft = true;
+        } else {
+            steerLeft = true;
+            steerRotateLeft = false;
+        }
+        steerForward = false;
+        steerBackward = false;
+        steerRight = false;
+        steerRotateRight = false;
+    } else if (input[0] == 'R') { // Right
+        if (input[1] == 'R') { // Right Rotate
+            steerRight = false;
+            steerRotateRight = true;
+        } else {
+            steerRight = true;
+            steerRotateRight = false;
+        }
+        steerForward = false;
+        steerBackward = false;
+        steerLeft = false;
+        steerRotateLeft = false;
+    } else if (input[0] == 'S') { // Stop
+        steerForward = false;
+        steerBackward = false;
+        steerLeft = false;
+        steerRotateLeft = false;
+        steerRight = false;
+        steerRotateRight = false;
+    }
+
+    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
+        if (targetAngle < 75 || targetAngle > 105) // The serial communication sometimes behaves weird
+            targetAngle = 90;
+        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 PID(double restAngle) {
+    if (steerForward)
+        restAngle -= 1;
+    else if (steerBackward)
+        restAngle += 1;
+
+    double error = (restAngle - pitch)/100;
+    double pTerm = Kp * error;
+    iTerm += Ki * error;
+    double dTerm = Kd * (error - lastError);
+    lastError = error;
+
+    double 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);
+
+    double PIDLeft;
+    double PIDRight;
+    if (steerLeft) {
+        PIDLeft = PIDValue-(0.1);
+        PIDRight = PIDValue+(0.1);
+    } else if (steerRotateLeft) {
+        PIDLeft = PIDValue-(0.2);
+        PIDRight = PIDValue+(0.2);
+    } else if (steerRight) {
+        PIDLeft = PIDValue-(-0.1);
+        PIDRight = PIDValue+(-0.1);
+    } else if (steerRotateRight) {
+        PIDLeft = PIDValue-(-0.2);
+        PIDRight = PIDValue+(-0.2);
+    } else {
+        PIDLeft = PIDValue;
+        PIDRight = PIDValue;
+    }
+    PIDLeft *= 0.9; // 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);
+}
+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
+    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*4095=4.132227273
+    double gyroRate = -(((gyroY.read()*4095) - zeroValues[0]) / 4.132227273);
+    return gyroRate;
+}
+double getAccY() {
+    // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3*4095=409.5
+    double accXval = ((accX.read()*4095) - zeroValues[1]) / 409.5;
+    double accYval = ((accY.read()*4095) - zeroValues[2]) / 409.5;
+    accYval--;//-1g when lying down
+    double accZval = ((accZ.read()*4095) - zeroValues[3]) / 409.5;
+
+    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() {
+    onboardLED1 = 1;
+    onboardLED2 = 1;
+    onboardLED3 = 1;
+    onboardLED4 = 1;
+
+    double adc[4];
+    for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
+        adc[0] += gyroY.read()*4095;
+        adc[1] += accX.read()*4095;
+        adc[2] += accY.read()*4095;
+        adc[3] += accZ.read()*4095;
+        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
+
+    onboardLED1 = 0;
+    onboardLED2 = 0;
+    onboardLED3 = 0;
+    onboardLED4 = 0;
+}
+void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
+    if (motor == right) {
+        leftPWM = speed;
+        if (direction == forward) {
+            leftA = 0;
+            leftB = 1;
+        } else if (direction == backward) {
+            leftA = 1;
+            leftB = 0;
+        }
+    } else if (motor == left) {
+        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