Berk & Ay / Mbed 2 deprecated Self-Balancing-Robot-BerkYasarYavuz_AdilBerkayTemiz

Committed for sharing.

Dependencies:   mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library

BalancingRobot.cpp@11:0a2944e7be23, 2020-01-01 (annotated)

Committer:
berkyavuz1997
Date:
Wed Jan 01 20:31:22 2020 +0000
Revision:
11:0a2944e7be23
Parent:
9:67f2110fce8e 
Committed for sharing.

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"
berkyavuz1997 11:0a2944e7be23 11 #include "ultrasonic.h"
berkyavuz1997 11:0a2944e7be23 12
berkyavuz1997 11:0a2944e7be23 13 //#include "HALLFX_ENCODER.h"
Lauszus 4:0b4c320bc948 14
Lauszus 4:0b4c320bc948 15 /* Setup serial communication */
berkyavuz1997 11:0a2944e7be23 16 Serial blue(PTE22, PTE23); // For remote control ps3
lakshmananag 9:67f2110fce8e 17 Serial pc(USBTX, USBRX); // USB
Lauszus 4:0b4c320bc948 18
lakshmananag 9:67f2110fce8e 19 /* IMU */
berkyavuz1997 11:0a2944e7be23 20 MPU6050 mpu6050;
berkyavuz1997 11:0a2944e7be23 21
berkyavuz1997 11:0a2944e7be23 22
lakshmananag 9:67f2110fce8e 23
berkyavuz1997 11:0a2944e7be23 24 /* HC-SR04 */
berkyavuz1997 11:0a2944e7be23 25 void dist(int distance)
berkyavuz1997 11:0a2944e7be23 26 {
berkyavuz1997 11:0a2944e7be23 27 //put code here to happen when the distance is changed
berkyavuz1997 11:0a2944e7be23 28
berkyavuz1997 11:0a2944e7be23 29 }
berkyavuz1997 11:0a2944e7be23 30
berkyavuz1997 11:0a2944e7be23 31 ultrasonic frontUltra(PTA13, PTD5, .1, 1, &dist);
berkyavuz1997 11:0a2944e7be23 32 ultrasonic backUltra(PTD0, PTD2, .1, 1, &dist);
Lauszus 4:0b4c320bc948 33
Lauszus 4:0b4c320bc948 34 /* Setup timer instance */
Lauszus 4:0b4c320bc948 35 Timer t;
Lauszus 4:0b4c320bc948 36
lakshmananag 9:67f2110fce8e 37 /* Ticker for led toggling */
berkyavuz1997 11:0a2944e7be23 38 Ticker toggler1;
berkyavuz1997 11:0a2944e7be23 39
lakshmananag 9:67f2110fce8e 40
berkyavuz1997 11:0a2944e7be23 41
Lauszus 4:0b4c320bc948 42 int main() {
berkyavuz1997 11:0a2944e7be23 43
berkyavuz1997 11:0a2944e7be23 44
berkyavuz1997 11:0a2944e7be23 45
berkyavuz1997 11:0a2944e7be23 46
Lauszus 4:0b4c320bc948 47 /* Set baudrate */
berkyavuz1997 11:0a2944e7be23 48 blue.baud(115200);
berkyavuz1997 11:0a2944e7be23 49 pc.baud(115200);
berkyavuz1997 11:0a2944e7be23 50
Lauszus 7:f1d24c6119ac 51
Lauszus 4:0b4c320bc948 52 /* Set PWM frequency */
Lauszus 4:0b4c320bc948 53 leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
Lauszus 4:0b4c320bc948 54 rightPWM.period(0.00005);
Lauszus 7:f1d24c6119ac 55
berkyavuz1997 11:0a2944e7be23 56
berkyavuz1997 11:0a2944e7be23 57
Lauszus 4:0b4c320bc948 58 /* Calibrate the gyro and accelerometer relative to ground */
lakshmananag 9:67f2110fce8e 59 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
lakshmananag 9:67f2110fce8e 60 wait(0.5);
lakshmananag 9:67f2110fce8e 61 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
berkyavuz1997 11:0a2944e7be23 62 //pc.printf("Calibration is completed. \r\n");
lakshmananag 9:67f2110fce8e 63 mpu6050.init(); // Initialize the sensor
berkyavuz1997 11:0a2944e7be23 64 //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
berkyavuz1997 11:0a2944e7be23 65
berkyavuz1997 11:0a2944e7be23 66
Lauszus 7:f1d24c6119ac 67
Lauszus 4:0b4c320bc948 68 /* Setup timing */
Lauszus 4:0b4c320bc948 69 t.start();
berkyavuz1997 11:0a2944e7be23 70
Lauszus 4:0b4c320bc948 71 loopStartTime = t.read_us();
Lauszus 4:0b4c320bc948 72 timer = loopStartTime;
berkyavuz1997 11:0a2944e7be23 73 DesiredAngle = -2.5;
berkyavuz1997 11:0a2944e7be23 74 ConstantAngle = DesiredAngle;
berkyavuz1997 11:0a2944e7be23 75
Lauszus 4:0b4c320bc948 76 while (1) {
berkyavuz1997 11:0a2944e7be23 77
lakshmananag 9:67f2110fce8e 78 /* Calculate pitch angle using a Complementary filter */
lakshmananag 9:67f2110fce8e 79 mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
lakshmananag 9:67f2110fce8e 80 wait_us(5);
Lauszus 4:0b4c320bc948 81 timer = t.read_us();
Lauszus 7:f1d24c6119ac 82
Lauszus 4:0b4c320bc948 83 /* Drive motors */
lakshmananag 9:67f2110fce8e 84 if (abs(pitchAngle)> 20) // Stop if falling or laying down
Lauszus 4:0b4c320bc948 85 stopAndReset();
Lauszus 4:0b4c320bc948 86 else
berkyavuz1997 11:0a2944e7be23 87 PID(DesiredAngle, targetOffset);
berkyavuz1997 11:0a2944e7be23 88
berkyavuz1997 11:0a2944e7be23 89 /* Update wheel velocity every 100ms
Lauszus 4:0b4c320bc948 90 loopCounter++;
lakshmananag 9:67f2110fce8e 91 if (loopCounter%10 == 0)
lakshmananag 9:67f2110fce8e 92 { // If remainder is equal 0, it must be 10,20,30 etc.
lakshmananag 9:67f2110fce8e 93 LeftWheelPosition = leftEncoder.read();
lakshmananag 9:67f2110fce8e 94 RightWheelPosition = rightEncoder.read();
lakshmananag 9:67f2110fce8e 95 wheelPosition = LeftWheelPosition + RightWheelPosition;
Lauszus 4:0b4c320bc948 96 wheelVelocity = wheelPosition - lastWheelPosition;
Lauszus 4:0b4c320bc948 97 lastWheelPosition = wheelPosition;
Lauszus 7:f1d24c6119ac 98
lakshmananag 9:67f2110fce8e 99 if (abs(wheelVelocity) <= 20 && !stopped)
lakshmananag 9:67f2110fce8e 100 { // Set new targetPosition if braking
Lauszus 4:0b4c320bc948 101 targetPosition = wheelPosition;
Lauszus 4:0b4c320bc948 102 stopped = true;
Lauszus 4:0b4c320bc948 103 }
Lauszus 4:0b4c320bc948 104 }
berkyavuz1997 11:0a2944e7be23 105 */
Lauszus 7:f1d24c6119ac 106
berkyavuz1997 11:0a2944e7be23 107 //pc.printf("Pitch: %f, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue);
lakshmananag 9:67f2110fce8e 108
berkyavuz1997 11:0a2944e7be23 109 //Check for incoming serial data
lakshmananag 9:67f2110fce8e 110 if (blue.readable()) // Check if there's any incoming data
lakshmananag 9:67f2110fce8e 111 receiveBluetooth();
berkyavuz1997 11:0a2944e7be23 112
berkyavuz1997 11:0a2944e7be23 113
berkyavuz1997 11:0a2944e7be23 114
berkyavuz1997 11:0a2944e7be23 115 frontUltra.startUpdates();
berkyavuz1997 11:0a2944e7be23 116
berkyavuz1997 11:0a2944e7be23 117 distF = frontUltra.getCurrentDistance();
berkyavuz1997 11:0a2944e7be23 118
berkyavuz1997 11:0a2944e7be23 119 backUltra.startUpdates();
berkyavuz1997 11:0a2944e7be23 120
berkyavuz1997 11:0a2944e7be23 121 distB = backUltra.getCurrentDistance();
berkyavuz1997 11:0a2944e7be23 122
berkyavuz1997 11:0a2944e7be23 123
berkyavuz1997 11:0a2944e7be23 124 pc.printf("Front distance: %d cm \t Back Distance %d cm \r\n", distF, distB);
berkyavuz1997 11:0a2944e7be23 125
Lauszus 4:0b4c320bc948 126 /* Use a time fixed loop */
Lauszus 4:0b4c320bc948 127 lastLoopUsefulTime = t.read_us() - loopStartTime;
Lauszus 4:0b4c320bc948 128 if (lastLoopUsefulTime < STD_LOOP_TIME)
Lauszus 4:0b4c320bc948 129 wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
berkyavuz1997 11:0a2944e7be23 130
Lauszus 4:0b4c320bc948 131 lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
berkyavuz1997 11:0a2944e7be23 132 //pc.printf("STD_LOOP_TIME: %d\n\r",STD_LOOP_TIME);
berkyavuz1997 11:0a2944e7be23 133 //pc.printf("anlik: %d\n\r",t.read_us());
berkyavuz1997 11:0a2944e7be23 134 //pc.printf("lastLoopTime: %d\n\r",lastLoopTime);
berkyavuz1997 11:0a2944e7be23 135 //pc.printf("lastLoopUsefulTime: %d\n\r",lastLoopTime);
Lauszus 4:0b4c320bc948 136 loopStartTime = t.read_us();
Lauszus 4:0b4c320bc948 137 }
Lauszus 4:0b4c320bc948 138 }
lakshmananag 9:67f2110fce8e 139
lakshmananag 9:67f2110fce8e 140 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
lakshmananag 9:67f2110fce8e 141
lakshmananag 9:67f2110fce8e 142 void toggle_led1() {ledToggle(1);}
lakshmananag 9:67f2110fce8e 143 void toggle_led2() {ledToggle(2);}
lakshmananag 9:67f2110fce8e 144
lakshmananag 9:67f2110fce8e 145 void PID(double restAngle, double offset)
lakshmananag 9:67f2110fce8e 146 {
berkyavuz1997 11:0a2944e7be23 147
Lauszus 4:0b4c320bc948 148 /* Steer robot */
berkyavuz1997 11:0a2944e7be23 149 /*if (steerForward)
lakshmananag 9:67f2110fce8e 150 {
berkyavuz1997 11:0a2944e7be23 151 /* offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
berkyavuz1997 11:0a2944e7be23 152 restAngle -= offset;
berkyavuz1997 11:0a2944e7be23 153 restAngle -= -1.0;
berkyavuz1997 11:0a2944e7be23 154
berkyavuz1997 11:0a2944e7be23 155
berkyavuz1997 11:0a2944e7be23 156 }
lakshmananag 9:67f2110fce8e 157 else if (steerBackward)
lakshmananag 9:67f2110fce8e 158 {
berkyavuz1997 11:0a2944e7be23 159 //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
berkyavuz1997 11:0a2944e7be23 160 //restAngle += offset;
berkyavuz1997 11:0a2944e7be23 161 restAngle += 1.0;
berkyavuz1997 11:0a2944e7be23 162 }*/
lakshmananag 9:67f2110fce8e 163 /* Brake */
berkyavuz1997 11:0a2944e7be23 164 /*else if (steerStop)
berkyavuz1997 11:0a2944e7be23 165
berkyavuz1997 11:0a2944e7be23 166 restAngle = 0.0;
lakshmananag 9:67f2110fce8e 167 {
lakshmananag 9:67f2110fce8e 168 //long positionError = wheelPosition - targetPosition;
lakshmananag 9:67f2110fce8e 169
lakshmananag 9:67f2110fce8e 170 /* if (abs(positionError) > zoneA) // Inside zone A
Lauszus 4:0b4c320bc948 171 restAngle -= (double)positionError/positionScaleA;
Lauszus 4:0b4c320bc948 172 else if (abs(positionError) > zoneB) // Inside zone B
Lauszus 4:0b4c320bc948 173 restAngle -= (double)positionError/positionScaleB;
Lauszus 4:0b4c320bc948 174 else // Inside zone C
lakshmananag 9:67f2110fce8e 175 restAngle -= (double)positionError/positionScaleC; */
Lauszus 7:f1d24c6119ac 176
berkyavuz1997 11:0a2944e7be23 177 //restAngle -= (double)wheelVelocity/velocityScaleStop;
Lauszus 7:f1d24c6119ac 178
berkyavuz1997 11:0a2944e7be23 179 /*if (restAngle < -5.00) // Limit rest Angle
lakshmananag 9:67f2110fce8e 180 restAngle = -5.00;
lakshmananag 9:67f2110fce8e 181 else if (restAngle > 5.00)
lakshmananag 9:67f2110fce8e 182 restAngle = 5.00;
Lauszus 4:0b4c320bc948 183 }
lakshmananag 9:67f2110fce8e 184
Lauszus 4:0b4c320bc948 185 /* Update PID values */
berkyavuz1997 11:0a2944e7be23 186 error1 = restAngle - pitchAngle;
berkyavuz1997 11:0a2944e7be23 187 double pTerm = Kp * error1;
berkyavuz1997 11:0a2944e7be23 188 iTerm += Ki * error1;
berkyavuz1997 11:0a2944e7be23 189 double dTerm = Kd * (error1 - lastError);
berkyavuz1997 11:0a2944e7be23 190 lastError = error1;
Lauszus 7:f1d24c6119ac 191
lakshmananag 9:67f2110fce8e 192 PIDValue = pTerm + iTerm + dTerm;
berkyavuz1997 11:0a2944e7be23 193 //pc.printf("DesiredAngle = %lf", DesiredAngle);
lakshmananag 9:67f2110fce8e 194 //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 195
Lauszus 4:0b4c320bc948 196 /* Steer robot sideways */
Lauszus 4:0b4c320bc948 197 double PIDLeft;
berkyavuz1997 11:0a2944e7be23 198 double PIDRight;
berkyavuz1997 11:0a2944e7be23 199
Lauszus 4:0b4c320bc948 200 if (steerLeft) {
Lauszus 4:0b4c320bc948 201 PIDLeft = PIDValue-turnSpeed;
Lauszus 4:0b4c320bc948 202 PIDRight = PIDValue+turnSpeed;
Lauszus 4:0b4c320bc948 203 } else if (steerRotateLeft) {
Lauszus 4:0b4c320bc948 204 PIDLeft = PIDValue-rotateSpeed;
Lauszus 4:0b4c320bc948 205 PIDRight = PIDValue+rotateSpeed;
Lauszus 4:0b4c320bc948 206 } else if (steerRight) {
Lauszus 4:0b4c320bc948 207 PIDLeft = PIDValue+turnSpeed;
Lauszus 4:0b4c320bc948 208 PIDRight = PIDValue-turnSpeed;
Lauszus 4:0b4c320bc948 209 } else if (steerRotateRight) {
Lauszus 4:0b4c320bc948 210 PIDLeft = PIDValue+rotateSpeed;
Lauszus 4:0b4c320bc948 211 PIDRight = PIDValue-rotateSpeed;
Lauszus 4:0b4c320bc948 212 } else {
berkyavuz1997 11:0a2944e7be23 213
berkyavuz1997 11:0a2944e7be23 214 if (steerForward && (DesiredAngle > ( ConstantAngle - 1.8 ) ))
berkyavuz1997 11:0a2944e7be23 215 DesiredAngle -= rotateSpeed;
berkyavuz1997 11:0a2944e7be23 216
berkyavuz1997 11:0a2944e7be23 217 else if (steerBackward && (DesiredAngle < (ConstantAngle + 1.8) ))
berkyavuz1997 11:0a2944e7be23 218 DesiredAngle += rotateSpeed;
berkyavuz1997 11:0a2944e7be23 219
berkyavuz1997 11:0a2944e7be23 220 else {
berkyavuz1997 11:0a2944e7be23 221 if(!steerForward && DesiredAngle < ConstantAngle)
berkyavuz1997 11:0a2944e7be23 222 DesiredAngle += rotateSpeed;
berkyavuz1997 11:0a2944e7be23 223
berkyavuz1997 11:0a2944e7be23 224 else if(!steerBackward && DesiredAngle > ConstantAngle)
berkyavuz1997 11:0a2944e7be23 225 DesiredAngle -= rotateSpeed;
berkyavuz1997 11:0a2944e7be23 226 }
berkyavuz1997 11:0a2944e7be23 227
Lauszus 4:0b4c320bc948 228 PIDLeft = PIDValue;
Lauszus 4:0b4c320bc948 229 PIDRight = PIDValue;
berkyavuz1997 11:0a2944e7be23 230
Lauszus 4:0b4c320bc948 231 }
lakshmananag 9:67f2110fce8e 232 //PIDLeft *= 0.95; // compensate for difference in the motors
Lauszus 7:f1d24c6119ac 233
Lauszus 4:0b4c320bc948 234 /* Set PWM Values */
berkyavuz1997 11:0a2944e7be23 235 if (PIDLeft >= 0.0)
berkyavuz1997 11:0a2944e7be23 236 move(left, backward, PIDLeft + 0.55);
Lauszus 4:0b4c320bc948 237 else
berkyavuz1997 11:0a2944e7be23 238 move(left, forward, (PIDLeft - 0.55) * -1);
berkyavuz1997 11:0a2944e7be23 239 if (PIDRight >= 0.0)
berkyavuz1997 11:0a2944e7be23 240 move(right, backward, PIDRight + 0.55);
Lauszus 4:0b4c320bc948 241 else
berkyavuz1997 11:0a2944e7be23 242 move(right, forward, (PIDRight - 0.55) * -1);
berkyavuz1997 11:0a2944e7be23 243 //pc.printf("RestAngle3 %lf" ,restAngle);
Lauszus 4:0b4c320bc948 244 }
berkyavuz1997 11:0a2944e7be23 245
lakshmananag 9:67f2110fce8e 246 void receiveBluetooth() {
lakshmananag 9:67f2110fce8e 247 //char input[16]; // The serial buffer is only 16 characters
lakshmananag 9:67f2110fce8e 248 char phone_char;
lakshmananag 9:67f2110fce8e 249 //int i = 0;
lakshmananag 9:67f2110fce8e 250 //while (1) {
lakshmananag 9:67f2110fce8e 251 //input[i] = blue.getc();
lakshmananag 9:67f2110fce8e 252 //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
lakshmananag 9:67f2110fce8e 253 //break;
lakshmananag 9:67f2110fce8e 254 //i++;
lakshmananag 9:67f2110fce8e 255
lakshmananag 9:67f2110fce8e 256 //pc.printf("Im here..");
lakshmananag 9:67f2110fce8e 257 //}
lakshmananag 9:67f2110fce8e 258 phone_char = blue.getc();
berkyavuz1997 11:0a2944e7be23 259
berkyavuz1997 11:0a2944e7be23 260 /* if(distB < 50 && distB > 4) {
berkyavuz1997 11:0a2944e7be23 261 phone_char = 'S';
berkyavuz1997 11:0a2944e7be23 262 if (distB < 20)
berkyavuz1997 11:0a2944e7be23 263 phone_char = 'F';
berkyavuz1997 11:0a2944e7be23 264 } */
berkyavuz1997 11:0a2944e7be23 265
berkyavuz1997 11:0a2944e7be23 266
berkyavuz1997 11:0a2944e7be23 267 if(distB < 20 && distB > 4)
berkyavuz1997 11:0a2944e7be23 268 phone_char = 'F';
berkyavuz1997 11:0a2944e7be23 269 else if ( distB < 30)
berkyavuz1997 11:0a2944e7be23 270 phone_char = 'S';
berkyavuz1997 11:0a2944e7be23 271 else if (distB < 50)
berkyavuz1997 11:0a2944e7be23 272 phone_char = 'B';
berkyavuz1997 11:0a2944e7be23 273
berkyavuz1997 11:0a2944e7be23 274
lakshmananag 9:67f2110fce8e 275 pc.putc(phone_char);
berkyavuz1997 11:0a2944e7be23 276 pc.printf("Input: %c\n",phone_char);
Lauszus 7:f1d24c6119ac 277
Lauszus 4:0b4c320bc948 278 // Set all false
Lauszus 4:0b4c320bc948 279 steerForward = false;
Lauszus 4:0b4c320bc948 280 steerBackward = false;
Lauszus 4:0b4c320bc948 281 steerStop = false;
Lauszus 4:0b4c320bc948 282 steerLeft = false;
Lauszus 4:0b4c320bc948 283 steerRotateLeft = false;
Lauszus 4:0b4c320bc948 284 steerRight = false;
Lauszus 4:0b4c320bc948 285 steerRotateRight = false;
Lauszus 7:f1d24c6119ac 286
Lauszus 4:0b4c320bc948 287 /* For remote control */
lakshmananag 9:67f2110fce8e 288 if (/*input[0]*/phone_char == 'F') { // Forward
lakshmananag 9:67f2110fce8e 289 //strtok(/*input*/phone_char, ","); // Ignore 'F'
lakshmananag 9:67f2110fce8e 290 //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double
lakshmananag 9:67f2110fce8e 291 //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
berkyavuz1997 11:0a2944e7be23 292 //DesiredAnglee = 0.8;
berkyavuz1997 11:0a2944e7be23 293 pc.printf("F");
Lauszus 4:0b4c320bc948 294 steerForward = true;
lakshmananag 9:67f2110fce8e 295 } else if (/*input[0]*/phone_char == 'B') { // Backward
lakshmananag 9:67f2110fce8e 296 //strtok(/*input*/phone_char, ","); // Ignore 'B'
berkyavuz1997 11:0a2944e7be23 297 //targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
lakshmananag 9:67f2110fce8e 298 //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
berkyavuz1997 11:0a2944e7be23 299 //DesiredAnglee = -5.2;
berkyavuz1997 11:0a2944e7be23 300 pc.printf("B");
Lauszus 4:0b4c320bc948 301 steerBackward = true;
lakshmananag 9:67f2110fce8e 302 } else if (/*input[0]*/phone_char == 'L') { // Left
lakshmananag 9:67f2110fce8e 303 if (/*input[0]*/phone_char == 'L') // Left Rotate
Lauszus 4:0b4c320bc948 304 steerRotateLeft = true;
Lauszus 4:0b4c320bc948 305 else
Lauszus 4:0b4c320bc948 306 steerLeft = true;
lakshmananag 9:67f2110fce8e 307 } else if (/*input[0]*/phone_char == 'R') { // Right
lakshmananag 9:67f2110fce8e 308 if (/*input[0]*/phone_char == 'R') // Right Rotate
Lauszus 4:0b4c320bc948 309 steerRotateRight = true;
Lauszus 4:0b4c320bc948 310 else
Lauszus 4:0b4c320bc948 311 steerRight = true;
lakshmananag 9:67f2110fce8e 312 } else if (/*input[0]*/phone_char == 'S') { // Stop
Lauszus 4:0b4c320bc948 313 steerStop = true;
Lauszus 4:0b4c320bc948 314 stopped = false;
berkyavuz1997 11:0a2944e7be23 315 //targetPosition = wheelPosition;
lakshmananag 9:67f2110fce8e 316 } else if (/*input[0]*/phone_char == 'A') { // Abort
Lauszus 4:0b4c320bc948 317 stopAndReset();
lakshmananag 9:67f2110fce8e 318 while (blue.getc() != 'C'); // Wait until continue is send
berkyavuz1997 11:0a2944e7be23 319
Lauszus 4:0b4c320bc948 320 }
Lauszus 4:0b4c320bc948 321 }
lakshmananag 9:67f2110fce8e 322
Lauszus 4:0b4c320bc948 323 void stopAndReset() {
Lauszus 4:0b4c320bc948 324 stop(both);
Lauszus 4:0b4c320bc948 325 lastError = 0;
Lauszus 4:0b4c320bc948 326 iTerm = 0;
Lauszus 4:0b4c320bc948 327 targetPosition = wheelPosition;
Lauszus 4:0b4c320bc948 328 }
lakshmananag 9:67f2110fce8e 329
Lauszus 4:0b4c320bc948 330 void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
Lauszus 4:0b4c320bc948 331 if (motor == left) {
Lauszus 4:0b4c320bc948 332 leftPWM = speed;
Lauszus 4:0b4c320bc948 333 if (direction == forward) {
Lauszus 4:0b4c320bc948 334 leftA = 0;
Lauszus 4:0b4c320bc948 335 leftB = 1;
Lauszus 4:0b4c320bc948 336 } else if (direction == backward) {
Lauszus 4:0b4c320bc948 337 leftA = 1;
Lauszus 4:0b4c320bc948 338 leftB = 0;
Lauszus 4:0b4c320bc948 339 }
Lauszus 4:0b4c320bc948 340 } else if (motor == right) {
Lauszus 4:0b4c320bc948 341 rightPWM = speed;
Lauszus 4:0b4c320bc948 342 if (direction == forward) {
Lauszus 4:0b4c320bc948 343 rightA = 0;
Lauszus 4:0b4c320bc948 344 rightB = 1;
Lauszus 4:0b4c320bc948 345 } else if (direction == backward) {
Lauszus 4:0b4c320bc948 346 rightA = 1;
Lauszus 4:0b4c320bc948 347 rightB = 0;
Lauszus 4:0b4c320bc948 348 }
Lauszus 4:0b4c320bc948 349 } else if (motor == both) {
Lauszus 4:0b4c320bc948 350 leftPWM = speed;
Lauszus 4:0b4c320bc948 351 rightPWM = speed;
Lauszus 4:0b4c320bc948 352 if (direction == forward) {
Lauszus 4:0b4c320bc948 353 leftA = 0;
Lauszus 4:0b4c320bc948 354 leftB = 1;
Lauszus 7:f1d24c6119ac 355
Lauszus 4:0b4c320bc948 356 rightA = 0;
Lauszus 4:0b4c320bc948 357 rightB = 1;
Lauszus 4:0b4c320bc948 358 } else if (direction == backward) {
Lauszus 4:0b4c320bc948 359 leftA = 1;
Lauszus 4:0b4c320bc948 360 leftB = 0;
Lauszus 7:f1d24c6119ac 361
Lauszus 4:0b4c320bc948 362 rightA = 1;
Lauszus 4:0b4c320bc948 363 rightB = 0;
Lauszus 4:0b4c320bc948 364 }
Lauszus 4:0b4c320bc948 365 }
Lauszus 4:0b4c320bc948 366 }
Lauszus 4:0b4c320bc948 367 void stop(Motor motor) {
Lauszus 4:0b4c320bc948 368 if (motor == left) {
Lauszus 4:0b4c320bc948 369 leftPWM = 1;
Lauszus 4:0b4c320bc948 370 leftA = 1;
Lauszus 4:0b4c320bc948 371 leftB = 1;
Lauszus 4:0b4c320bc948 372 } else if (motor == right) {
Lauszus 4:0b4c320bc948 373 rightPWM = 1;
Lauszus 4:0b4c320bc948 374 rightA = 1;
Lauszus 4:0b4c320bc948 375 rightB = 1;
Lauszus 4:0b4c320bc948 376 } else if (motor == both) {
Lauszus 4:0b4c320bc948 377 leftPWM = 1;
Lauszus 4:0b4c320bc948 378 leftA = 1;
Lauszus 4:0b4c320bc948 379 leftB = 1;
Lauszus 7:f1d24c6119ac 380
Lauszus 4:0b4c320bc948 381 rightPWM = 1;
Lauszus 4:0b4c320bc948 382 rightA = 1;
Lauszus 4:0b4c320bc948 383 rightB = 1;
Lauszus 4:0b4c320bc948 384 }
Lauszus 0:f5c02b620688 385 }