Diff: BalancingRobot.cpp

Revision:

2:caec5534774d

Parent:

1:01295228342f

Child:

3:c3963f37d597

--- a/BalancingRobot.cpp	Tue Feb 14 18:31:05 2012 +0000
+++ b/BalancingRobot.cpp	Thu Feb 16 20:25:52 2012 +0000
@@ -1,4 +1,4 @@
-/* 
+/*
  * The code is released under the GNU General Public License.
  * Developed by Kristian Lauszus
  * This is the algorithm for my balancing robot/segway.
@@ -28,6 +28,7 @@
     calibrateSensors(); // Calibrate the gyro and accelerometer relative to ground
 
     xbee.printf("Initialized\n");
+    processing(); // Send output to processing application
 
     // Start timing
     t.start();
@@ -43,14 +44,16 @@
         timer = t.read_us();
 
         if (ps3.readable()) // Check if there's any incoming data
-            receiveSerial();
+            receivePS3();
+        if (xbee.readable()) // For setting the PID values
+            receiveXbee();
 
         //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
 
         if (pitch < 75 || pitch > 105) // Stop if falling or laying down
             stopAndReset();
         else
-            PID(targetAngle);
+            PID(targetAngle,targetOffset);
 
         /* Used a time fixed loop */
         lastLoopUsefulTime = t.read_us() - loopStartTime;
@@ -62,87 +65,97 @@
         //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();   
-        if(input[i] == ';')
+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
+        if (input[i] == ';')
             break;
-        i++;            
-    }    
+        i++;
+    }
     //debug.printf("Input: %s\n",input);
 
+    // Set all false
+    steerForward = false;
+    steerBackward = 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;
-        steerBackward = false;
-        steerLeft = false;
-        steerRotateLeft = false;
-        steerRight = false;
-        steerRotateRight = false;
     } else if (input[0] == 'B') { // Backward
-        steerForward = false;
+        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;
-        steerLeft = false;
-        steerRotateLeft = false;
-        steerRight = false;
-        steerRotateRight = false;
     } else if (input[0] == 'L') { // Left
-        if (input[1] == 'R') { // Left Rotate
-            steerLeft = false;
+        if (input[1] == 'R') // Left Rotate
             steerRotateLeft = true;
-        } else {
+        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;
+        if (input[1] == 'R') // Right Rotate
             steerRotateRight = true;
-        } else {
+        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;
+        // Everything is allready 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 = lastTargetAngle;
-        lastTargetAngle = targetAngle;
         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) {
+void receiveXbee() {
+    char input[16]; // The serial buffer is only 16 characters
+    int i = 0;
+    while (1) { // keep reading until it reads a semicolon
+        input[i] = xbee.getc();
+        if (input[i] == ';')
+            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 PID(double restAngle, double offset) {
     if (steerForward)
-        restAngle -= 1.5;
+        restAngle -= offset;
     else if (steerBackward)
-        restAngle += 1.5;
+        restAngle += offset;
 
     double error = (restAngle - pitch)/100;
     double pTerm = Kp * error;
@@ -184,6 +197,11 @@
     else
         move(right, backward, PIDRight * -1);
 }
+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