Kristian Lauszus
/
BalancingRobotPS3
Code for my balancing robot, controlled with a PS3 controller via bluetooth
Diff: BalancingRobot.cpp
- Revision:
- 1:01295228342f
- Parent:
- 0:f5c02b620688
- Child:
- 2:caec5534774d
diff -r f5c02b620688 -r 01295228342f BalancingRobot.cpp --- 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;