Revision 4:0b4c320bc948, committed 2012-03-02

Comitter:

Lauszus

Date:

Fri Mar 02 09:06:47 2012 +0000

Parent:

3:c3963f37d597

Child:

5:fc6c7a059759

Commit message:

Changed in this revision

--- a/BalancingRobot.cpp	Sun Feb 26 13:11:10 2012 +0000
+++ b/BalancingRobot.cpp	Fri Mar 02 09:06:47 2012 +0000
@@ -1,371 +1,398 @@
-/*
- * 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 "Encoder.h"
-
-/* Serial communication */
-Serial xbee(p13,p14); // For wireless debugging
-Serial ps3(p9,p10); // For remote control
-Serial debug(USBTX, USBRX); // USB
-
-/* Setup encoders */
-Encoder leftEncoder(p29,p30);
-Encoder rightEncoder(p28,p27);
-
-/* Timer instance */
-Timer t;
-
-int main() {
-    xbee.baud(115200);
-    ps3.baud(115200);
-    debug.baud(115200);
-
-    leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
-    rightPWM.period(0.00005);
-
-    calibrateSensors(); // Calibrate the gyro and accelerometer relative to ground
-
-    xbee.printf("Initialized\n");
-    processing(); // Send output to processing application
-
-    // 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();
-
-        //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
-
-        if (ps3.readable()) // Check if there's any incoming data
-            receivePS3();
-        if (xbee.readable()) // For setting the PID values
-            receiveXbee();
-
-        if (pitch < 70 || pitch > 110) // Stop if falling or laying down
-            stopAndReset();
-        else
-            PID(targetAngle,targetOffset);
-
-
-        loopCounter++;
-        if (loopCounter == 10) { // Update wheel velocity every 100ms
-            loopCounter = 0;
-            wheelPosition = leftEncoder.read() + rightEncoder.read();
-            wheelVelocity = wheelPosition - lastWheelPosition;
-            lastWheelPosition = wheelPosition;
-            //xbee.printf("WheelVelocity: %i\n",wheelVelocity);
-        }
-
-        /* 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 PID(double restAngle, double offset) {
-    if (steerForward) {
-        offset += (double)wheelVelocity/velocityScale; // 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/velocityScale; // Scale down offset at high speed and scale up when reversing
-        restAngle += offset;
-        //xbee.printf("Backward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
-    } else if (steerStop) {
-        long positionError = wheelPosition - targetPosition;
-        if(abs(positionError) > 500) {
-            restAngle -= (double)positionError/positionScale;
-            restAngle -= (double)wheelVelocity/velocityScale;
-        } else
-            restAngle -= (double)wheelVelocity/(velocityScale*2);
-        
-        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);
-    }
-    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);
-}
-void receivePS3() {
-    char input[16]; // The serial buffer is only 16 characters
-    int i = 0;
-    while (1) {
-        input[i] = ps3.getc(); // keep reading until it reads a semicolon or it's larger than the serial buffer
-        if (input[i] == ';' || i == 15)
-            break;
-        i++;
-    }
-    //debug.printf("Input: %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
-        //xbee.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;
-        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) { // keep reading until it reads a semicolon or it's larger than the serial buffer
-        input[i] = xbee.getc();
-        if (input[i] == ';' || i == 15)
-            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;
-}
-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://academic.csuohio.edu/simond/courses/eec644/kalman.pdfhttp://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 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()*4095;
-        adc[1] += accX.read()*4095;
-        adc[2] += accY.read()*4095;
-        adc[3] += accZ.read()*4095;
-        */
-        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 == 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;
-    }
+/*
+ * 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 "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
+
+/* Setup encoders */
+Encoder leftEncoder(p29,p30);
+Encoder rightEncoder(p28,p27);
+
+/* Setup timer instance */
+Timer t;
+
+int main() {
+    /* Set baudrate */
+    xbee.baud(115200);
+    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 */
+    calibrateSensors();
+
+    xbee.printf("Initialized\n");
+    processing(); // Send output to processing application
+
+    /* Setup timing */
+    t.start();
+    loopStartTime = t.read_us();
+    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
+        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
+            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();
+            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 < 7.92 && 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;
+
+        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);
+    }
+    /* Update PID values */
+    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);
+
+    /* 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: %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
+        //xbee.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++;
+    }
+    //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 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;
+        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

--- a/BalancingRobot.h	Sun Feb 26 13:11:10 2012 +0000
+++ b/BalancingRobot.h	Fri Mar 02 09:06:47 2012 +0000
@@ -9,13 +9,11 @@
 DigitalOut leftA(p21);
 DigitalOut leftB(p22);
 PwmOut leftPWM(p23);
-//AnalogIn leftCurrentSense(p15); // Not used
 
 /* Right motor */
 DigitalOut rightA(p24);
 DigitalOut rightB(p25);
 PwmOut rightPWM(p26);
-//AnalogIn rightCurrentSense(p16); // Not used
 
 /* IMU */
 AnalogIn gyroY(p17);
@@ -23,13 +21,17 @@
 AnalogIn accY(p19);
 AnalogIn accZ(p20);
 
+/* Battery voltage */
+AnalogIn batteryVoltage(p15); // The analog pin is connected to a 56k-10k voltage divider
+DigitalOut buzzer(p5); // Connected to a BC547 transistor - there is a protection diode at the buzzer 
+
 // Zero voltage values for the sensors - [0] = gyroY, [1] = accX, [2] = accY, [3] = accZ
 double zeroValues[4];
 
 /* Kalman filter variables and constants */
 const float Q_angle = 0.001; // Process noise covariance for the accelerometer - Sw
 const float Q_gyro = 0.003; // Process noise covariance for the gyro - Sw
-const float R_angle = 0.03; // Measurent noise covariance - Sv
+const float R_angle = 0.03; // Measurement noise covariance - Sv
 
 double angle = 180; // It starts at 180 degrees
 double bias = 0;
@@ -42,8 +44,7 @@
 double gyroYrate;
 double pitch;
 
-/* PID values */
-// Motors
+/* PID variables */
 double Kp = 11;
 double Ki = 2;
 double Kd = 12;
@@ -78,15 +79,25 @@
 bool steerRight;
 bool steerRotateRight;
 
+bool stopped;
+
+const double turnSpeed = 0.1;
+const double rotateSpeed = 0.2;
+
 double targetOffset = 0;
 
 uint8_t loopCounter = 0; // Used for wheel velocity
 long wheelPosition;
 long lastWheelPosition;
+long wheelVelocity;
 long targetPosition;
-long wheelVelocity;
-double positionScale = 1000; // one resolution is 464 pulses
-double velocityScale = 40;
+int zoneA = 2000;
+int zoneB = 1000;
+double positionScaleA = 250; // one resolution is 464 pulses
+double positionScaleB = 500; 
+double positionScaleC = 1000;
+double velocityScaleMove = 40;
+double velocityScaleStop = 40;//30
 
 void calibrateSensors();
 void PID(double restAngle, double offset);
@@ -99,6 +110,5 @@
 void receivePS3();
 void receiveXbee();
 void stopAndReset();
-void processing();
 
 #endif
\ No newline at end of file

--- a/Encoder.h	Sun Feb 26 13:11:10 2012 +0000
+++ b/Encoder.h	Fri Mar 02 09:06:47 2012 +0000
@@ -1,26 +1,26 @@
-#ifndef _encoder_h_
-#define _encoder_h_
-
-class Encoder {
-public:
-    Encoder(PinName pinA, PinName pinB) : _HallSensorA(pinA), _HallSensorB(pinB) {
-        _counter = 0;
-        _HallSensorA.rise(this, &Encoder::EncodeA);
-    }
-    long read() {
-        return _counter;
-    }
-private:
-    volatile long _counter;
-    InterruptIn _HallSensorA;
-    DigitalIn _HallSensorB;
-
-    void EncodeA() {
-        if (_HallSensorB.read())
-            _counter++;
-        else
-            _counter--;
-    }
-};
-
+#ifndef _encoder_h_
+#define _encoder_h_
+
+class Encoder {
+public:
+    Encoder(PinName pinA, PinName pinB) : _HallSensorA(pinA), _HallSensorB(pinB) {
+        _counter = 0;
+        _HallSensorA.rise(this, &Encoder::EncodeA);
+    }
+    long read() {
+        return _counter;
+    }
+private:
+    volatile long _counter;
+    InterruptIn _HallSensorA;
+    DigitalIn _HallSensorB;
+
+    void EncodeA() {
+        if (_HallSensorB.read())
+            _counter++;
+        else
+            _counter--;
+    }
+};
+
 #endif
\ No newline at end of file