Kristian Lauszus / Mbed 2 deprecated BalancingRobotPS3

Code for my balancing robot, controlled with a PS3 controller via bluetooth

Dependencies:   mbed

BalancingRobot.cpp@0:f5c02b620688, 2012-02-14 (annotated)

Committer:
Lauszus
Date:
Tue Feb 14 10:48:01 2012 +0000
Revision:
0:f5c02b620688
Child:
1:01295228342f
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lauszus 0:f5c02b620688 1 #include "BalancingRobot.h"
Lauszus 0:f5c02b620688 2
Lauszus 0:f5c02b620688 3 /* Serial communication */
Lauszus 0:f5c02b620688 4 Serial xbee(p13,p14); // For wireless debugging
Lauszus 0:f5c02b620688 5 Serial ps3(p9,p10); // For remote control
Lauszus 0:f5c02b620688 6 Serial debug(USBTX, USBRX); // USB
Lauszus 0:f5c02b620688 7
Lauszus 0:f5c02b620688 8 /* Timer instance */
Lauszus 0:f5c02b620688 9 Timer t;
Lauszus 0:f5c02b620688 10
Lauszus 0:f5c02b620688 11 int main() {
Lauszus 0:f5c02b620688 12 xbee.baud(115200);
Lauszus 0:f5c02b620688 13 ps3.baud(115200);
Lauszus 0:f5c02b620688 14 debug.baud(115200);
Lauszus 0:f5c02b620688 15
Lauszus 0:f5c02b620688 16 leftPWM.period(0.00005); //The motor driver can handle pwm values up to 20kHz (1/20000=0.00005)
Lauszus 0:f5c02b620688 17 rightPWM.period(0.00005);
Lauszus 0:f5c02b620688 18
Lauszus 0:f5c02b620688 19 calibrateSensors(); // Calibrate the gyro and accelerometer relative to ground
Lauszus 0:f5c02b620688 20
Lauszus 0:f5c02b620688 21 xbee.printf("Initialized\n");
Lauszus 0:f5c02b620688 22
Lauszus 0:f5c02b620688 23 // Start timing
Lauszus 0:f5c02b620688 24 t.start();
Lauszus 0:f5c02b620688 25 loopStartTime = t.read_us();
Lauszus 0:f5c02b620688 26 timer = loopStartTime;
Lauszus 0:f5c02b620688 27
Lauszus 0:f5c02b620688 28 while (1) {
Lauszus 0:f5c02b620688 29 // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html
Lauszus 0:f5c02b620688 30 accYangle = getAccY();
Lauszus 0:f5c02b620688 31 gyroYrate = getGyroYrate();
Lauszus 0:f5c02b620688 32
Lauszus 0:f5c02b620688 33 pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
Lauszus 0:f5c02b620688 34 timer = t.read_us();
Lauszus 0:f5c02b620688 35
Lauszus 0:f5c02b620688 36 if (ps3.readable()) // Check if there's any incoming data
Lauszus 0:f5c02b620688 37 receiveSerial();
Lauszus 0:f5c02b620688 38
Lauszus 0:f5c02b620688 39 //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
Lauszus 0:f5c02b620688 40
Lauszus 0:f5c02b620688 41 if (pitch < 75 || pitch > 105) //Stop if falling or laying down
Lauszus 0:f5c02b620688 42 stopAndReset();
Lauszus 0:f5c02b620688 43 else
Lauszus 0:f5c02b620688 44 PID(targetAngle);
Lauszus 0:f5c02b620688 45
Lauszus 0:f5c02b620688 46 /* Used a time fixed loop */
Lauszus 0:f5c02b620688 47 lastLoopUsefulTime = t.read_us() - loopStartTime;
Lauszus 0:f5c02b620688 48 if (lastLoopUsefulTime < STD_LOOP_TIME)
Lauszus 0:f5c02b620688 49 wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
Lauszus 0:f5c02b620688 50 lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
Lauszus 0:f5c02b620688 51 loopStartTime = t.read_us();
Lauszus 0:f5c02b620688 52
Lauszus 0:f5c02b620688 53 //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime);
Lauszus 0:f5c02b620688 54 }
Lauszus 0:f5c02b620688 55 }
Lauszus 0:f5c02b620688 56 void stopAndReset() {
Lauszus 0:f5c02b620688 57 stop(both);
Lauszus 0:f5c02b620688 58 lastError = 0;
Lauszus 0:f5c02b620688 59 iTerm = 0;
Lauszus 0:f5c02b620688 60 }
Lauszus 0:f5c02b620688 61 void receiveSerial() {
Lauszus 0:f5c02b620688 62 char input[16]; // The serial buffer is only 16 characters, so the data has to be split up
Lauszus 0:f5c02b620688 63 int i = 0;
Lauszus 0:f5c02b620688 64 while (ps3.readable()) {
Lauszus 0:f5c02b620688 65 input[i] = ps3.getc();
Lauszus 0:f5c02b620688 66 i++;
Lauszus 0:f5c02b620688 67 }
Lauszus 0:f5c02b620688 68 LPC_UART0->FCR |= 0x06; // Flush Serial
Lauszus 0:f5c02b620688 69 //debug.printf("Input: %s\n",input);
Lauszus 0:f5c02b620688 70
Lauszus 0:f5c02b620688 71 /* For remote control */
Lauszus 0:f5c02b620688 72 if (input[0] == 'F') { // Forward
Lauszus 0:f5c02b620688 73 steerForward = true;
Lauszus 0:f5c02b620688 74 steerBackward = false;
Lauszus 0:f5c02b620688 75 steerLeft = false;
Lauszus 0:f5c02b620688 76 steerRotateLeft = false;
Lauszus 0:f5c02b620688 77 steerRight = false;
Lauszus 0:f5c02b620688 78 steerRotateRight = false;
Lauszus 0:f5c02b620688 79 } else if (input[0] == 'B') { // Backward
Lauszus 0:f5c02b620688 80 steerForward = false;
Lauszus 0:f5c02b620688 81 steerBackward = true;
Lauszus 0:f5c02b620688 82 steerLeft = false;
Lauszus 0:f5c02b620688 83 steerRotateLeft = false;
Lauszus 0:f5c02b620688 84 steerRight = false;
Lauszus 0:f5c02b620688 85 steerRotateRight = false;
Lauszus 0:f5c02b620688 86 } else if (input[0] == 'L') { // Left
Lauszus 0:f5c02b620688 87 if (input[1] == 'R') { // Left Rotate
Lauszus 0:f5c02b620688 88 steerLeft = false;
Lauszus 0:f5c02b620688 89 steerRotateLeft = true;
Lauszus 0:f5c02b620688 90 } else {
Lauszus 0:f5c02b620688 91 steerLeft = true;
Lauszus 0:f5c02b620688 92 steerRotateLeft = false;
Lauszus 0:f5c02b620688 93 }
Lauszus 0:f5c02b620688 94 steerForward = false;
Lauszus 0:f5c02b620688 95 steerBackward = false;
Lauszus 0:f5c02b620688 96 steerRight = false;
Lauszus 0:f5c02b620688 97 steerRotateRight = false;
Lauszus 0:f5c02b620688 98 } else if (input[0] == 'R') { // Right
Lauszus 0:f5c02b620688 99 if (input[1] == 'R') { // Right Rotate
Lauszus 0:f5c02b620688 100 steerRight = false;
Lauszus 0:f5c02b620688 101 steerRotateRight = true;
Lauszus 0:f5c02b620688 102 } else {
Lauszus 0:f5c02b620688 103 steerRight = true;
Lauszus 0:f5c02b620688 104 steerRotateRight = false;
Lauszus 0:f5c02b620688 105 }
Lauszus 0:f5c02b620688 106 steerForward = false;
Lauszus 0:f5c02b620688 107 steerBackward = false;
Lauszus 0:f5c02b620688 108 steerLeft = false;
Lauszus 0:f5c02b620688 109 steerRotateLeft = false;
Lauszus 0:f5c02b620688 110 } else if (input[0] == 'S') { // Stop
Lauszus 0:f5c02b620688 111 steerForward = false;
Lauszus 0:f5c02b620688 112 steerBackward = false;
Lauszus 0:f5c02b620688 113 steerLeft = false;
Lauszus 0:f5c02b620688 114 steerRotateLeft = false;
Lauszus 0:f5c02b620688 115 steerRight = false;
Lauszus 0:f5c02b620688 116 steerRotateRight = false;
Lauszus 0:f5c02b620688 117 }
Lauszus 0:f5c02b620688 118
Lauszus 0:f5c02b620688 119 else if (input[0] == 'T') { // Set the target angle
Lauszus 0:f5c02b620688 120 strtok(input, ","); // Ignore 'T'
Lauszus 0:f5c02b620688 121 targetAngle = atof(strtok(NULL, ",")); // read until the end and then convert from string to double
Lauszus 0:f5c02b620688 122 if (targetAngle < 75 || targetAngle > 105) // The serial communication sometimes behaves weird
Lauszus 0:f5c02b620688 123 targetAngle = 90;
Lauszus 0:f5c02b620688 124 xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging
Lauszus 0:f5c02b620688 125 } else if (input[0] == 'A') { // Abort
Lauszus 0:f5c02b620688 126 stopAndReset();
Lauszus 0:f5c02b620688 127 while (ps3.getc() != 'C'); // Wait until continue is send
Lauszus 0:f5c02b620688 128 }
Lauszus 0:f5c02b620688 129 }
Lauszus 0:f5c02b620688 130 void PID(double restAngle) {
Lauszus 0:f5c02b620688 131 if (steerForward)
Lauszus 0:f5c02b620688 132 restAngle -= 1;
Lauszus 0:f5c02b620688 133 else if (steerBackward)
Lauszus 0:f5c02b620688 134 restAngle += 1;
Lauszus 0:f5c02b620688 135
Lauszus 0:f5c02b620688 136 double error = (restAngle - pitch)/100;
Lauszus 0:f5c02b620688 137 double pTerm = Kp * error;
Lauszus 0:f5c02b620688 138 iTerm += Ki * error;
Lauszus 0:f5c02b620688 139 double dTerm = Kd * (error - lastError);
Lauszus 0:f5c02b620688 140 lastError = error;
Lauszus 0:f5c02b620688 141
Lauszus 0:f5c02b620688 142 double PIDValue = pTerm + iTerm + dTerm;
Lauszus 0:f5c02b620688 143
Lauszus 0:f5c02b620688 144 //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);
Lauszus 0:f5c02b620688 145
Lauszus 0:f5c02b620688 146 double PIDLeft;
Lauszus 0:f5c02b620688 147 double PIDRight;
Lauszus 0:f5c02b620688 148 if (steerLeft) {
Lauszus 0:f5c02b620688 149 PIDLeft = PIDValue-(0.1);
Lauszus 0:f5c02b620688 150 PIDRight = PIDValue+(0.1);
Lauszus 0:f5c02b620688 151 } else if (steerRotateLeft) {
Lauszus 0:f5c02b620688 152 PIDLeft = PIDValue-(0.2);
Lauszus 0:f5c02b620688 153 PIDRight = PIDValue+(0.2);
Lauszus 0:f5c02b620688 154 } else if (steerRight) {
Lauszus 0:f5c02b620688 155 PIDLeft = PIDValue-(-0.1);
Lauszus 0:f5c02b620688 156 PIDRight = PIDValue+(-0.1);
Lauszus 0:f5c02b620688 157 } else if (steerRotateRight) {
Lauszus 0:f5c02b620688 158 PIDLeft = PIDValue-(-0.2);
Lauszus 0:f5c02b620688 159 PIDRight = PIDValue+(-0.2);
Lauszus 0:f5c02b620688 160 } else {
Lauszus 0:f5c02b620688 161 PIDLeft = PIDValue;
Lauszus 0:f5c02b620688 162 PIDRight = PIDValue;
Lauszus 0:f5c02b620688 163 }
Lauszus 0:f5c02b620688 164 PIDLeft *= 0.9; // compensate for difference in the motors
Lauszus 0:f5c02b620688 165
Lauszus 0:f5c02b620688 166 //Set PWM Values
Lauszus 0:f5c02b620688 167 if (PIDLeft >= 0)
Lauszus 0:f5c02b620688 168 move(left, forward, PIDLeft);
Lauszus 0:f5c02b620688 169 else
Lauszus 0:f5c02b620688 170 move(left, backward, PIDLeft * -1);
Lauszus 0:f5c02b620688 171 if (PIDRight >= 0)
Lauszus 0:f5c02b620688 172 move(right, forward, PIDRight);
Lauszus 0:f5c02b620688 173 else
Lauszus 0:f5c02b620688 174 move(right, backward, PIDRight * -1);
Lauszus 0:f5c02b620688 175 }
Lauszus 0:f5c02b620688 176 double kalman(double newAngle, double newRate, double dtime) {
Lauszus 0:f5c02b620688 177 // KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
Lauszus 0:f5c02b620688 178 // with slightly modifications by Kristian Lauszus
Lauszus 0:f5c02b620688 179 dt = dtime / 1000000; // Convert from microseconds to seconds
Lauszus 0:f5c02b620688 180
Lauszus 0:f5c02b620688 181 // Discrete Kalman filter time update equations - Time Update ("Predict")
Lauszus 0:f5c02b620688 182 // Update xhat - Project the state ahead
Lauszus 0:f5c02b620688 183 angle += dt * (newRate - bias);
Lauszus 0:f5c02b620688 184
Lauszus 0:f5c02b620688 185 // Update estimation error covariance - Project the error covariance ahead
Lauszus 0:f5c02b620688 186 P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
Lauszus 0:f5c02b620688 187 P_01 += -dt * P_11;
Lauszus 0:f5c02b620688 188 P_10 += -dt * P_11;
Lauszus 0:f5c02b620688 189 P_11 += +Q_gyro * dt;
Lauszus 0:f5c02b620688 190
Lauszus 0:f5c02b620688 191 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
Lauszus 0:f5c02b620688 192 // Calculate Kalman gain - Compute the Kalman gain
Lauszus 0:f5c02b620688 193 S = P_00 + R_angle;
Lauszus 0:f5c02b620688 194 K_0 = P_00 / S;
Lauszus 0:f5c02b620688 195 K_1 = P_10 / S;
Lauszus 0:f5c02b620688 196
Lauszus 0:f5c02b620688 197 // Calculate angle and resting rate - Update estimate with measurement zk
Lauszus 0:f5c02b620688 198 y = newAngle - angle;
Lauszus 0:f5c02b620688 199 angle += K_0 * y;
Lauszus 0:f5c02b620688 200 bias += K_1 * y;
Lauszus 0:f5c02b620688 201
Lauszus 0:f5c02b620688 202 // Calculate estimation error covariance - Update the error covariance
Lauszus 0:f5c02b620688 203 P_00 -= K_0 * P_00;
Lauszus 0:f5c02b620688 204 P_01 -= K_0 * P_01;
Lauszus 0:f5c02b620688 205 P_10 -= K_1 * P_00;
Lauszus 0:f5c02b620688 206 P_11 -= K_1 * P_01;
Lauszus 0:f5c02b620688 207
Lauszus 0:f5c02b620688 208 return angle;
Lauszus 0:f5c02b620688 209 }
Lauszus 0:f5c02b620688 210 double getGyroYrate() {
Lauszus 0:f5c02b620688 211 // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3*4095=4.132227273
Lauszus 0:f5c02b620688 212 double gyroRate = -(((gyroY.read()*4095) - zeroValues[0]) / 4.132227273);
Lauszus 0:f5c02b620688 213 return gyroRate;
Lauszus 0:f5c02b620688 214 }
Lauszus 0:f5c02b620688 215 double getAccY() {
Lauszus 0:f5c02b620688 216 // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3*4095=409.5
Lauszus 0:f5c02b620688 217 double accXval = ((accX.read()*4095) - zeroValues[1]) / 409.5;
Lauszus 0:f5c02b620688 218 double accYval = ((accY.read()*4095) - zeroValues[2]) / 409.5;
Lauszus 0:f5c02b620688 219 accYval--;//-1g when lying down
Lauszus 0:f5c02b620688 220 double accZval = ((accZ.read()*4095) - zeroValues[3]) / 409.5;
Lauszus 0:f5c02b620688 221
Lauszus 0:f5c02b620688 222 double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the force vector
Lauszus 0:f5c02b620688 223 double angleY = acos(accYval / R) * RAD_TO_DEG;
Lauszus 0:f5c02b620688 224
Lauszus 0:f5c02b620688 225 return angleY;
Lauszus 0:f5c02b620688 226 }
Lauszus 0:f5c02b620688 227 void calibrateSensors() {
Lauszus 0:f5c02b620688 228 onboardLED1 = 1;
Lauszus 0:f5c02b620688 229 onboardLED2 = 1;
Lauszus 0:f5c02b620688 230 onboardLED3 = 1;
Lauszus 0:f5c02b620688 231 onboardLED4 = 1;
Lauszus 0:f5c02b620688 232
Lauszus 0:f5c02b620688 233 double adc[4];
Lauszus 0:f5c02b620688 234 for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
Lauszus 0:f5c02b620688 235 adc[0] += gyroY.read()*4095;
Lauszus 0:f5c02b620688 236 adc[1] += accX.read()*4095;
Lauszus 0:f5c02b620688 237 adc[2] += accY.read()*4095;
Lauszus 0:f5c02b620688 238 adc[3] += accZ.read()*4095;
Lauszus 0:f5c02b620688 239 wait_ms(10);
Lauszus 0:f5c02b620688 240 }
Lauszus 0:f5c02b620688 241 zeroValues[0] = adc[0] / 100; // Gyro X-axis
Lauszus 0:f5c02b620688 242 zeroValues[1] = adc[1] / 100; // Accelerometer X-axis
Lauszus 0:f5c02b620688 243 zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis
Lauszus 0:f5c02b620688 244 zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis
Lauszus 0:f5c02b620688 245
Lauszus 0:f5c02b620688 246 onboardLED1 = 0;
Lauszus 0:f5c02b620688 247 onboardLED2 = 0;
Lauszus 0:f5c02b620688 248 onboardLED3 = 0;
Lauszus 0:f5c02b620688 249 onboardLED4 = 0;
Lauszus 0:f5c02b620688 250 }
Lauszus 0:f5c02b620688 251 void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
Lauszus 0:f5c02b620688 252 if (motor == right) {
Lauszus 0:f5c02b620688 253 leftPWM = speed;
Lauszus 0:f5c02b620688 254 if (direction == forward) {
Lauszus 0:f5c02b620688 255 leftA = 0;
Lauszus 0:f5c02b620688 256 leftB = 1;
Lauszus 0:f5c02b620688 257 } else if (direction == backward) {
Lauszus 0:f5c02b620688 258 leftA = 1;
Lauszus 0:f5c02b620688 259 leftB = 0;
Lauszus 0:f5c02b620688 260 }
Lauszus 0:f5c02b620688 261 } else if (motor == left) {
Lauszus 0:f5c02b620688 262 rightPWM = speed;
Lauszus 0:f5c02b620688 263 if (direction == forward) {
Lauszus 0:f5c02b620688 264 rightA = 0;
Lauszus 0:f5c02b620688 265 rightB = 1;
Lauszus 0:f5c02b620688 266 } else if (direction == backward) {
Lauszus 0:f5c02b620688 267 rightA = 1;
Lauszus 0:f5c02b620688 268 rightB = 0;
Lauszus 0:f5c02b620688 269 }
Lauszus 0:f5c02b620688 270 } else if (motor == both) {
Lauszus 0:f5c02b620688 271 leftPWM = speed;
Lauszus 0:f5c02b620688 272 rightPWM = speed;
Lauszus 0:f5c02b620688 273 if (direction == forward) {
Lauszus 0:f5c02b620688 274 leftA = 0;
Lauszus 0:f5c02b620688 275 leftB = 1;
Lauszus 0:f5c02b620688 276
Lauszus 0:f5c02b620688 277 rightA = 0;
Lauszus 0:f5c02b620688 278 rightB = 1;
Lauszus 0:f5c02b620688 279 } else if (direction == backward) {
Lauszus 0:f5c02b620688 280 leftA = 1;
Lauszus 0:f5c02b620688 281 leftB = 0;
Lauszus 0:f5c02b620688 282
Lauszus 0:f5c02b620688 283 rightA = 1;
Lauszus 0:f5c02b620688 284 rightB = 0;
Lauszus 0:f5c02b620688 285 }
Lauszus 0:f5c02b620688 286 }
Lauszus 0:f5c02b620688 287 }
Lauszus 0:f5c02b620688 288 void stop(Motor motor) {
Lauszus 0:f5c02b620688 289 if (motor == left) {
Lauszus 0:f5c02b620688 290 leftPWM = 1;
Lauszus 0:f5c02b620688 291 leftA = 1;
Lauszus 0:f5c02b620688 292 leftB = 1;
Lauszus 0:f5c02b620688 293 } else if (motor == right) {
Lauszus 0:f5c02b620688 294 rightPWM = 1;
Lauszus 0:f5c02b620688 295 rightA = 1;
Lauszus 0:f5c02b620688 296 rightB = 1;
Lauszus 0:f5c02b620688 297 } else if (motor == both) {
Lauszus 0:f5c02b620688 298 leftPWM = 1;
Lauszus 0:f5c02b620688 299 leftA = 1;
Lauszus 0:f5c02b620688 300 leftB = 1;
Lauszus 0:f5c02b620688 301
Lauszus 0:f5c02b620688 302 rightPWM = 1;
Lauszus 0:f5c02b620688 303 rightA = 1;
Lauszus 0:f5c02b620688 304 rightB = 1;
Lauszus 0:f5c02b620688 305 }
Lauszus 0:f5c02b620688 306 }