Diff: BalancingRobot.cpp

Revision:

1:01295228342f

Parent:

0:f5c02b620688

Child:

2:caec5534774d

--- a/BalancingRobot.cpp	Tue Feb 14 10:48:01 2012 +0000
+++ b/BalancingRobot.cpp	Tue Feb 14 18:31:05 2012 +0000
@@ -1,3 +1,12 @@
+/* 
+ * 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
+ */
+
 #include "BalancingRobot.h"
 
 /* Serial communication */
@@ -38,7 +47,7 @@
 
         //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
 
-        if (pitch < 75 || pitch > 105) //Stop if falling or laying down
+        if (pitch < 75 || pitch > 105) // Stop if falling or laying down
             stopAndReset();
         else
             PID(targetAngle);
@@ -60,12 +69,13 @@
 }
 void receiveSerial() {
     char input[16]; // The serial buffer is only 16 characters, so the data has to be split up
-    int i = 0;
+    int i = 0;    
     while (ps3.readable()) {
-        input[i] = ps3.getc();
-        i++;
-    }
-    LPC_UART0->FCR |= 0x06; // Flush Serial
+        input[i] = ps3.getc();   
+        if(input[i] == ';')
+            break;
+        i++;            
+    }    
     //debug.printf("Input: %s\n",input);
 
     /* For remote control */
@@ -118,9 +128,10 @@
 
     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
+        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;
+            targetAngle = lastTargetAngle;
+        lastTargetAngle = targetAngle;
         xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging
     } else if (input[0] == 'A') { // Abort
         stopAndReset();
@@ -129,9 +140,9 @@
 }
 void PID(double restAngle) {
     if (steerForward)
-        restAngle -= 1;
+        restAngle -= 1.5;
     else if (steerBackward)
-        restAngle += 1;
+        restAngle += 1.5;
 
     double error = (restAngle - pitch)/100;
     double pTerm = Kp * error;