Kristian Lauszus
/
BalancingRobotPS3
Code for my balancing robot, controlled with a PS3 controller via bluetooth
Diff: BalancingRobot.cpp
- Revision:
- 3:c3963f37d597
- Parent:
- 2:caec5534774d
- Child:
- 4:0b4c320bc948
diff -r caec5534774d -r c3963f37d597 BalancingRobot.cpp --- a/BalancingRobot.cpp Thu Feb 16 20:25:52 2012 +0000 +++ b/BalancingRobot.cpp Sun Feb 26 13:11:10 2012 +0000 @@ -4,16 +4,22 @@ * 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 + * 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; @@ -22,7 +28,7 @@ ps3.baud(115200); debug.baud(115200); - leftPWM.period(0.00005); //The motor driver can handle pwm values up to 20kHz (1/20000=0.00005) + 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 @@ -43,18 +49,28 @@ 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(); - //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle); - - if (pitch < 75 || pitch > 105) // Stop if falling or laying down + 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) @@ -65,98 +81,29 @@ //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime); } } -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++; - } - //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; - } 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 - // Everything is allready false +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); } - - 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 - 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 -= offset; - else if (steerBackward) - restAngle += offset; - double error = (restAngle - pitch)/100; double pTerm = Kp * error; iTerm += Ki * error; @@ -197,6 +144,94 @@ 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; @@ -205,6 +240,7 @@ 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") @@ -237,16 +273,16 @@ 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); + // (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*4095=409.5 - double accXval = ((accX.read()*4095) - zeroValues[1]) / 409.5; - double accYval = ((accY.read()*4095) - zeroValues[2]) / 409.5; + // (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()*4095) - zeroValues[3]) / 409.5; + 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; @@ -254,17 +290,20 @@ return angleY; } void calibrateSensors() { - onboardLED1 = 1; - onboardLED2 = 1; - onboardLED3 = 1; - onboardLED4 = 1; + LEDs = 0xF; // Turn all onboard LEDs on - double adc[4]; + 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 @@ -272,10 +311,7 @@ zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis - onboardLED1 = 0; - onboardLED2 = 0; - onboardLED3 = 0; - onboardLED4 = 0; + 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) {