Lakshmanan Gopalakrishnan / Mbed 2 deprecated Balancing_Robot_2

First trial

Dependencies:   MPU6050 Motor ledControl2 mbed

Fork of BalancingRobotPS3 by Kristian Lauszus

BalancingRobot.cpp@10:8050817ae610, 2016-08-25 (annotated)

Committer:
lakshmananag
Date:
Thu Aug 25 22:55:36 2016 +0000
Revision:
10:8050817ae610
Parent:
9:67f2110fce8e 
First trial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lauszus 4:0b4c320bc948 1 /*
Lauszus 4:0b4c320bc948 2 * The code is released under the GNU General Public License.
Lauszus 4:0b4c320bc948 3 * This is the algorithm for my balancing robot/segway.
lakshmananag 9:67f2110fce8e 4 * It is controlled by Bluetooth
Lauszus 4:0b4c320bc948 5 */
Lauszus 4:0b4c320bc948 6
Lauszus 4:0b4c320bc948 7 #include "mbed.h"
Lauszus 4:0b4c320bc948 8 #include "BalancingRobot.h"
lakshmananag 9:67f2110fce8e 9 #include "MPU6050.h"
lakshmananag 9:67f2110fce8e 10 #include "ledControl.h"
lakshmananag 9:67f2110fce8e 11 #include "HALLFX_ENCODER.h"
Lauszus 4:0b4c320bc948 12
Lauszus 4:0b4c320bc948 13 /* Setup serial communication */
lakshmananag 9:67f2110fce8e 14 Serial blue(p28,p27); // For remote control ps3
lakshmananag 9:67f2110fce8e 15 Serial pc(USBTX, USBRX); // USB
Lauszus 4:0b4c320bc948 16
lakshmananag 9:67f2110fce8e 17 /* IMU */
lakshmananag 9:67f2110fce8e 18 MPU6050 mpu6050;
lakshmananag 9:67f2110fce8e 19
lakshmananag 9:67f2110fce8e 20 /* Encoders*/
lakshmananag 9:67f2110fce8e 21 HALLFX_ENCODER leftEncoder(p13);
lakshmananag 9:67f2110fce8e 22 HALLFX_ENCODER rightEncoder(p14);
Lauszus 4:0b4c320bc948 23
Lauszus 4:0b4c320bc948 24 /* Setup timer instance */
Lauszus 4:0b4c320bc948 25 Timer t;
Lauszus 4:0b4c320bc948 26
lakshmananag 9:67f2110fce8e 27 /* Ticker for led toggling */
lakshmananag 9:67f2110fce8e 28 Ticker toggler1;
lakshmananag 9:67f2110fce8e 29
Lauszus 4:0b4c320bc948 30 int main() {
Lauszus 4:0b4c320bc948 31 /* Set baudrate */
lakshmananag 9:67f2110fce8e 32 blue.baud(9600);
lakshmananag 9:67f2110fce8e 33 pc.baud(9600);
Lauszus 7:f1d24c6119ac 34
Lauszus 4:0b4c320bc948 35 /* Set PWM frequency */
Lauszus 4:0b4c320bc948 36 leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
Lauszus 4:0b4c320bc948 37 rightPWM.period(0.00005);
Lauszus 7:f1d24c6119ac 38
Lauszus 4:0b4c320bc948 39 /* Calibrate the gyro and accelerometer relative to ground */
lakshmananag 9:67f2110fce8e 40 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
lakshmananag 9:67f2110fce8e 41 wait(0.5);
lakshmananag 9:67f2110fce8e 42 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
lakshmananag 9:67f2110fce8e 43 pc.printf("Calibration is completed. \r\n");
lakshmananag 9:67f2110fce8e 44 mpu6050.init(); // Initialize the sensor
lakshmananag 9:67f2110fce8e 45 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
Lauszus 7:f1d24c6119ac 46
Lauszus 4:0b4c320bc948 47 /* Setup timing */
Lauszus 4:0b4c320bc948 48 t.start();
Lauszus 4:0b4c320bc948 49 loopStartTime = t.read_us();
Lauszus 4:0b4c320bc948 50 timer = loopStartTime;
Lauszus 7:f1d24c6119ac 51
Lauszus 4:0b4c320bc948 52 while (1) {
lakshmananag 9:67f2110fce8e 53 /* Calculate pitch angle using a Complementary filter */
lakshmananag 9:67f2110fce8e 54 mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
lakshmananag 9:67f2110fce8e 55 wait_us(5);
Lauszus 4:0b4c320bc948 56 timer = t.read_us();
Lauszus 7:f1d24c6119ac 57
Lauszus 4:0b4c320bc948 58 /* Drive motors */
lakshmananag 9:67f2110fce8e 59 if (abs(pitchAngle)> 20) // Stop if falling or laying down
Lauszus 4:0b4c320bc948 60 stopAndReset();
Lauszus 4:0b4c320bc948 61 else
Lauszus 4:0b4c320bc948 62 PID(targetAngle,targetOffset);
Lauszus 7:f1d24c6119ac 63
Lauszus 4:0b4c320bc948 64 /* Update wheel velocity every 100ms */
Lauszus 4:0b4c320bc948 65 loopCounter++;
lakshmananag 9:67f2110fce8e 66 if (loopCounter%10 == 0)
lakshmananag 9:67f2110fce8e 67 { // If remainder is equal 0, it must be 10,20,30 etc.
lakshmananag 9:67f2110fce8e 68 LeftWheelPosition = leftEncoder.read();
lakshmananag 9:67f2110fce8e 69 RightWheelPosition = rightEncoder.read();
lakshmananag 9:67f2110fce8e 70 wheelPosition = LeftWheelPosition + RightWheelPosition;
Lauszus 4:0b4c320bc948 71 wheelVelocity = wheelPosition - lastWheelPosition;
Lauszus 4:0b4c320bc948 72 lastWheelPosition = wheelPosition;
Lauszus 7:f1d24c6119ac 73
lakshmananag 9:67f2110fce8e 74 if (abs(wheelVelocity) <= 20 && !stopped)
lakshmananag 9:67f2110fce8e 75 { // Set new targetPosition if braking
Lauszus 4:0b4c320bc948 76 targetPosition = wheelPosition;
Lauszus 4:0b4c320bc948 77 stopped = true;
Lauszus 4:0b4c320bc948 78 }
Lauszus 4:0b4c320bc948 79 }
Lauszus 7:f1d24c6119ac 80
lakshmananag 9:67f2110fce8e 81 pc.printf("Pitch: %f, error: %f, PIDValue: %f \n",pitchAngle,error,PIDValue);
lakshmananag 9:67f2110fce8e 82 pc.printf("Left Wheel Encoder: %f, Right Wheel Encoder: %f \n", LeftWheelPosition, RightWheelPosition);
lakshmananag 9:67f2110fce8e 83 pc.printf("Wheel Position: %f, Last wheel position:%f, Velocity: %f \n\n", wheelPosition, lastWheelPosition, wheelVelocity);
lakshmananag 9:67f2110fce8e 84
Lauszus 4:0b4c320bc948 85 /* Check for incoming serial data */
lakshmananag 9:67f2110fce8e 86 if (blue.readable()) // Check if there's any incoming data
lakshmananag 9:67f2110fce8e 87 receiveBluetooth();
lakshmananag 9:67f2110fce8e 88
Lauszus 4:0b4c320bc948 89 /* Use a time fixed loop */
Lauszus 4:0b4c320bc948 90 lastLoopUsefulTime = t.read_us() - loopStartTime;
Lauszus 4:0b4c320bc948 91 if (lastLoopUsefulTime < STD_LOOP_TIME)
Lauszus 4:0b4c320bc948 92 wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
Lauszus 4:0b4c320bc948 93 lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
Lauszus 4:0b4c320bc948 94 loopStartTime = t.read_us();
Lauszus 4:0b4c320bc948 95 }
Lauszus 4:0b4c320bc948 96 }
lakshmananag 9:67f2110fce8e 97
lakshmananag 9:67f2110fce8e 98 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
lakshmananag 9:67f2110fce8e 99
lakshmananag 9:67f2110fce8e 100 void toggle_led1() {ledToggle(1);}
lakshmananag 9:67f2110fce8e 101 void toggle_led2() {ledToggle(2);}
lakshmananag 9:67f2110fce8e 102
lakshmananag 9:67f2110fce8e 103 void PID(double restAngle, double offset)
lakshmananag 9:67f2110fce8e 104 {
Lauszus 4:0b4c320bc948 105 /* Steer robot */
lakshmananag 9:67f2110fce8e 106 if (steerForward)
lakshmananag 9:67f2110fce8e 107 {
Lauszus 4:0b4c320bc948 108 offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
Lauszus 4:0b4c320bc948 109 restAngle -= offset;
lakshmananag 9:67f2110fce8e 110 }
lakshmananag 9:67f2110fce8e 111 else if (steerBackward)
lakshmananag 9:67f2110fce8e 112 {
Lauszus 4:0b4c320bc948 113 offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
Lauszus 4:0b4c320bc948 114 restAngle += offset;
Lauszus 4:0b4c320bc948 115 }
lakshmananag 9:67f2110fce8e 116 /* Brake */
lakshmananag 9:67f2110fce8e 117 else if (steerStop)
lakshmananag 9:67f2110fce8e 118 {
lakshmananag 9:67f2110fce8e 119 //long positionError = wheelPosition - targetPosition;
lakshmananag 9:67f2110fce8e 120
lakshmananag 9:67f2110fce8e 121 /* if (abs(positionError) > zoneA) // Inside zone A
Lauszus 4:0b4c320bc948 122 restAngle -= (double)positionError/positionScaleA;
Lauszus 4:0b4c320bc948 123 else if (abs(positionError) > zoneB) // Inside zone B
Lauszus 4:0b4c320bc948 124 restAngle -= (double)positionError/positionScaleB;
Lauszus 4:0b4c320bc948 125 else // Inside zone C
lakshmananag 9:67f2110fce8e 126 restAngle -= (double)positionError/positionScaleC; */
Lauszus 7:f1d24c6119ac 127
Lauszus 4:0b4c320bc948 128 restAngle -= (double)wheelVelocity/velocityScaleStop;
Lauszus 7:f1d24c6119ac 129
lakshmananag 9:67f2110fce8e 130 if (restAngle < -5.00) // Limit rest Angle
lakshmananag 9:67f2110fce8e 131 restAngle = -5.00;
lakshmananag 9:67f2110fce8e 132 else if (restAngle > 5.00)
lakshmananag 9:67f2110fce8e 133 restAngle = 5.00;
Lauszus 4:0b4c320bc948 134 }
lakshmananag 9:67f2110fce8e 135
Lauszus 4:0b4c320bc948 136 /* Update PID values */
lakshmananag 9:67f2110fce8e 137 error = restAngle - pitchAngle;
Lauszus 4:0b4c320bc948 138 double pTerm = Kp * error;
Lauszus 4:0b4c320bc948 139 iTerm += Ki * error;
Lauszus 4:0b4c320bc948 140 double dTerm = Kd * (error - lastError);
Lauszus 4:0b4c320bc948 141 lastError = error;
Lauszus 7:f1d24c6119ac 142
lakshmananag 9:67f2110fce8e 143 PIDValue = pTerm + iTerm + dTerm;
Lauszus 7:f1d24c6119ac 144
lakshmananag 9:67f2110fce8e 145 //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd);
Lauszus 7:f1d24c6119ac 146
Lauszus 4:0b4c320bc948 147 /* Steer robot sideways */
Lauszus 4:0b4c320bc948 148 double PIDLeft;
Lauszus 4:0b4c320bc948 149 double PIDRight;
Lauszus 4:0b4c320bc948 150 if (steerLeft) {
Lauszus 4:0b4c320bc948 151 PIDLeft = PIDValue-turnSpeed;
Lauszus 4:0b4c320bc948 152 PIDRight = PIDValue+turnSpeed;
Lauszus 4:0b4c320bc948 153 } else if (steerRotateLeft) {
Lauszus 4:0b4c320bc948 154 PIDLeft = PIDValue-rotateSpeed;
Lauszus 4:0b4c320bc948 155 PIDRight = PIDValue+rotateSpeed;
Lauszus 4:0b4c320bc948 156 } else if (steerRight) {
Lauszus 4:0b4c320bc948 157 PIDLeft = PIDValue+turnSpeed;
Lauszus 4:0b4c320bc948 158 PIDRight = PIDValue-turnSpeed;
Lauszus 4:0b4c320bc948 159 } else if (steerRotateRight) {
Lauszus 4:0b4c320bc948 160 PIDLeft = PIDValue+rotateSpeed;
Lauszus 4:0b4c320bc948 161 PIDRight = PIDValue-rotateSpeed;
Lauszus 4:0b4c320bc948 162 } else {
Lauszus 4:0b4c320bc948 163 PIDLeft = PIDValue;
Lauszus 4:0b4c320bc948 164 PIDRight = PIDValue;
Lauszus 4:0b4c320bc948 165 }
lakshmananag 9:67f2110fce8e 166 //PIDLeft *= 0.95; // compensate for difference in the motors
Lauszus 7:f1d24c6119ac 167
Lauszus 4:0b4c320bc948 168 /* Set PWM Values */
Lauszus 4:0b4c320bc948 169 if (PIDLeft >= 0)
Lauszus 4:0b4c320bc948 170 move(left, forward, PIDLeft);
Lauszus 4:0b4c320bc948 171 else
Lauszus 4:0b4c320bc948 172 move(left, backward, PIDLeft * -1);
Lauszus 4:0b4c320bc948 173 if (PIDRight >= 0)
Lauszus 4:0b4c320bc948 174 move(right, forward, PIDRight);
Lauszus 4:0b4c320bc948 175 else
Lauszus 4:0b4c320bc948 176 move(right, backward, PIDRight * -1);
Lauszus 4:0b4c320bc948 177 }
lakshmananag 9:67f2110fce8e 178 void receiveBluetooth() {
lakshmananag 9:67f2110fce8e 179 //char input[16]; // The serial buffer is only 16 characters
lakshmananag 9:67f2110fce8e 180 char phone_char;
lakshmananag 9:67f2110fce8e 181 //int i = 0;
lakshmananag 9:67f2110fce8e 182 //while (1) {
lakshmananag 9:67f2110fce8e 183 //input[i] = blue.getc();
lakshmananag 9:67f2110fce8e 184 //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
lakshmananag 9:67f2110fce8e 185 //break;
lakshmananag 9:67f2110fce8e 186 //i++;
lakshmananag 9:67f2110fce8e 187
lakshmananag 9:67f2110fce8e 188 //pc.printf("Im here..");
lakshmananag 9:67f2110fce8e 189 //}
lakshmananag 9:67f2110fce8e 190 phone_char = blue.getc();
lakshmananag 9:67f2110fce8e 191 pc.putc(phone_char);
lakshmananag 9:67f2110fce8e 192 //pc.printf("Input: %c\n",i);
Lauszus 7:f1d24c6119ac 193
Lauszus 4:0b4c320bc948 194 // Set all false
Lauszus 4:0b4c320bc948 195 steerForward = false;
Lauszus 4:0b4c320bc948 196 steerBackward = false;
Lauszus 4:0b4c320bc948 197 steerStop = false;
Lauszus 4:0b4c320bc948 198 steerLeft = false;
Lauszus 4:0b4c320bc948 199 steerRotateLeft = false;
Lauszus 4:0b4c320bc948 200 steerRight = false;
Lauszus 4:0b4c320bc948 201 steerRotateRight = false;
Lauszus 7:f1d24c6119ac 202
Lauszus 4:0b4c320bc948 203 /* For remote control */
lakshmananag 9:67f2110fce8e 204 if (/*input[0]*/phone_char == 'F') { // Forward
lakshmananag 9:67f2110fce8e 205 //strtok(/*input*/phone_char, ","); // Ignore 'F'
lakshmananag 9:67f2110fce8e 206 //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double
lakshmananag 9:67f2110fce8e 207 //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
Lauszus 4:0b4c320bc948 208 steerForward = true;
lakshmananag 9:67f2110fce8e 209 } else if (/*input[0]*/phone_char == 'B') { // Backward
lakshmananag 9:67f2110fce8e 210 //strtok(/*input*/phone_char, ","); // Ignore 'B'
Lauszus 4:0b4c320bc948 211 targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
lakshmananag 9:67f2110fce8e 212 //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
Lauszus 4:0b4c320bc948 213 steerBackward = true;
lakshmananag 9:67f2110fce8e 214 } else if (/*input[0]*/phone_char == 'L') { // Left
lakshmananag 9:67f2110fce8e 215 if (/*input[0]*/phone_char == 'L') // Left Rotate
Lauszus 4:0b4c320bc948 216 steerRotateLeft = true;
Lauszus 4:0b4c320bc948 217 else
Lauszus 4:0b4c320bc948 218 steerLeft = true;
lakshmananag 9:67f2110fce8e 219 } else if (/*input[0]*/phone_char == 'R') { // Right
lakshmananag 9:67f2110fce8e 220 if (/*input[0]*/phone_char == 'R') // Right Rotate
Lauszus 4:0b4c320bc948 221 steerRotateRight = true;
Lauszus 4:0b4c320bc948 222 else
Lauszus 4:0b4c320bc948 223 steerRight = true;
lakshmananag 9:67f2110fce8e 224 } else if (/*input[0]*/phone_char == 'S') { // Stop
Lauszus 4:0b4c320bc948 225 steerStop = true;
Lauszus 4:0b4c320bc948 226 stopped = false;
Lauszus 4:0b4c320bc948 227 targetPosition = wheelPosition;
lakshmananag 9:67f2110fce8e 228 } else if (/*input[0]*/phone_char == 'A') { // Abort
Lauszus 4:0b4c320bc948 229 stopAndReset();
lakshmananag 9:67f2110fce8e 230 while (blue.getc() != 'C'); // Wait until continue is send
Lauszus 4:0b4c320bc948 231 }
Lauszus 4:0b4c320bc948 232 }
lakshmananag 9:67f2110fce8e 233
Lauszus 4:0b4c320bc948 234 void stopAndReset() {
Lauszus 4:0b4c320bc948 235 stop(both);
Lauszus 4:0b4c320bc948 236 lastError = 0;
Lauszus 4:0b4c320bc948 237 iTerm = 0;
Lauszus 4:0b4c320bc948 238 targetPosition = wheelPosition;
Lauszus 4:0b4c320bc948 239 }
lakshmananag 9:67f2110fce8e 240
Lauszus 4:0b4c320bc948 241 void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
Lauszus 4:0b4c320bc948 242 if (motor == left) {
Lauszus 4:0b4c320bc948 243 leftPWM = speed;
Lauszus 4:0b4c320bc948 244 if (direction == forward) {
Lauszus 4:0b4c320bc948 245 leftA = 0;
Lauszus 4:0b4c320bc948 246 leftB = 1;
Lauszus 4:0b4c320bc948 247 } else if (direction == backward) {
Lauszus 4:0b4c320bc948 248 leftA = 1;
Lauszus 4:0b4c320bc948 249 leftB = 0;
Lauszus 4:0b4c320bc948 250 }
Lauszus 4:0b4c320bc948 251 } else if (motor == right) {
Lauszus 4:0b4c320bc948 252 rightPWM = speed;
Lauszus 4:0b4c320bc948 253 if (direction == forward) {
Lauszus 4:0b4c320bc948 254 rightA = 0;
Lauszus 4:0b4c320bc948 255 rightB = 1;
Lauszus 4:0b4c320bc948 256 } else if (direction == backward) {
Lauszus 4:0b4c320bc948 257 rightA = 1;
Lauszus 4:0b4c320bc948 258 rightB = 0;
Lauszus 4:0b4c320bc948 259 }
Lauszus 4:0b4c320bc948 260 } else if (motor == both) {
Lauszus 4:0b4c320bc948 261 leftPWM = speed;
Lauszus 4:0b4c320bc948 262 rightPWM = speed;
Lauszus 4:0b4c320bc948 263 if (direction == forward) {
Lauszus 4:0b4c320bc948 264 leftA = 0;
Lauszus 4:0b4c320bc948 265 leftB = 1;
Lauszus 7:f1d24c6119ac 266
Lauszus 4:0b4c320bc948 267 rightA = 0;
Lauszus 4:0b4c320bc948 268 rightB = 1;
Lauszus 4:0b4c320bc948 269 } else if (direction == backward) {
Lauszus 4:0b4c320bc948 270 leftA = 1;
Lauszus 4:0b4c320bc948 271 leftB = 0;
Lauszus 7:f1d24c6119ac 272
Lauszus 4:0b4c320bc948 273 rightA = 1;
Lauszus 4:0b4c320bc948 274 rightB = 0;
Lauszus 4:0b4c320bc948 275 }
Lauszus 4:0b4c320bc948 276 }
Lauszus 4:0b4c320bc948 277 }
Lauszus 4:0b4c320bc948 278 void stop(Motor motor) {
Lauszus 4:0b4c320bc948 279 if (motor == left) {
Lauszus 4:0b4c320bc948 280 leftPWM = 1;
Lauszus 4:0b4c320bc948 281 leftA = 1;
Lauszus 4:0b4c320bc948 282 leftB = 1;
Lauszus 4:0b4c320bc948 283 } else if (motor == right) {
Lauszus 4:0b4c320bc948 284 rightPWM = 1;
Lauszus 4:0b4c320bc948 285 rightA = 1;
Lauszus 4:0b4c320bc948 286 rightB = 1;
Lauszus 4:0b4c320bc948 287 } else if (motor == both) {
Lauszus 4:0b4c320bc948 288 leftPWM = 1;
Lauszus 4:0b4c320bc948 289 leftA = 1;
Lauszus 4:0b4c320bc948 290 leftB = 1;
Lauszus 7:f1d24c6119ac 291
Lauszus 4:0b4c320bc948 292 rightPWM = 1;
Lauszus 4:0b4c320bc948 293 rightA = 1;
Lauszus 4:0b4c320bc948 294 rightB = 1;
Lauszus 4:0b4c320bc948 295 }
Lauszus 0:f5c02b620688 296 }