Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@2:3d0be48abcf2, 2014-03-15 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Mar 15 22:37:06 2014 +0000
Revision:
2:3d0be48abcf2
Parent:
1:801f0b9a862a
Child:
3:58726d2e11f0
3/15;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fairy_Paolina 0:ff94cc47fef7 1 #include "rtos.h"
Fairy_Paolina 0:ff94cc47fef7 2 #include "PID.h"
Fairy_Paolina 0:ff94cc47fef7 3 #include "PololuQik2.h"
Fairy_Paolina 0:ff94cc47fef7 4 #include "QEI.h"
Fairy_Paolina 0:ff94cc47fef7 5 #include "mbed.h"
Fairy_Paolina 0:ff94cc47fef7 6 #include "HCSR04.h"
Fairy_Paolina 0:ff94cc47fef7 7 #include "stdio.h"
Fairy_Paolina 0:ff94cc47fef7 8 #include "LPC17xx.h"
Fairy_Paolina 0:ff94cc47fef7 9
Fairy_Paolina 2:3d0be48abcf2 10 #define PIN_TRIGGERL (p12)
Fairy_Paolina 2:3d0be48abcf2 11 #define PIN_ECHOL (p11)
Fairy_Paolina 2:3d0be48abcf2 12 #define PIN_TRIGGERR (p29)
Fairy_Paolina 2:3d0be48abcf2 13 #define PIN_ECHOR (p30)
Fairy_Paolina 0:ff94cc47fef7 14 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 15 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 16 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 17 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 1:801f0b9a862a 18 #define MAX_SPEED (0.3*127)
Fairy_Paolina 2:3d0be48abcf2 19 #define PPRL (965)
Fairy_Paolina 2:3d0be48abcf2 20 #define PPRR (1075)
Fairy_Paolina 2:3d0be48abcf2 21 #define LEFT (1)
Fairy_Paolina 2:3d0be48abcf2 22 #define RIGHT (0)
Fairy_Paolina 0:ff94cc47fef7 23
Fairy_Paolina 0:ff94cc47fef7 24 float range, pid_return;
Fairy_Paolina 0:ff94cc47fef7 25 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 26 bool cRc;
Fairy_Paolina 0:ff94cc47fef7 27
Fairy_Paolina 0:ff94cc47fef7 28 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 29 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 30 Serial pc(USBTX,USBRX);
Fairy_Paolina 2:3d0be48abcf2 31 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
Fairy_Paolina 2:3d0be48abcf2 32 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina 2:3d0be48abcf2 33 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 34 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 2:3d0be48abcf2 35 QEI rightEncoder(p17,p18,NC,PPRR,QEI::X4_ENCODING);
Fairy_Paolina 2:3d0be48abcf2 36 QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING);
Fairy_Paolina 1:801f0b9a862a 37 //InterruptIn encoder(p29);
Fairy_Paolina 1:801f0b9a862a 38
Fairy_Paolina 0:ff94cc47fef7 39
Fairy_Paolina 0:ff94cc47fef7 40 //Functions
Fairy_Paolina 0:ff94cc47fef7 41
Fairy_Paolina 2:3d0be48abcf2 42 void wall_follow(int side);
Fairy_Paolina 2:3d0be48abcf2 43 void wall_follow2(int side);
Fairy_Paolina 0:ff94cc47fef7 44 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 45 void leftTurn(void);
Fairy_Paolina 1:801f0b9a862a 46 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 47 void us_distance(void);
Fairy_Paolina 0:ff94cc47fef7 48
Fairy_Paolina 1:801f0b9a862a 49 //Variables
Fairy_Paolina 0:ff94cc47fef7 50
Fairy_Paolina 2:3d0be48abcf2 51 int main(void)
Fairy_Paolina 2:3d0be48abcf2 52 {
Fairy_Paolina 2:3d0be48abcf2 53 float location=0;
Fairy_Paolina 0:ff94cc47fef7 54
Fairy_Paolina 0:ff94cc47fef7 55 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 56 bt.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 57 motors.begin();
Fairy_Paolina 2:3d0be48abcf2 58
Fairy_Paolina 2:3d0be48abcf2 59 //motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 2:3d0be48abcf2 60 //motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 0:ff94cc47fef7 61
Fairy_Paolina 2:3d0be48abcf2 62 //motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 63
Fairy_Paolina 2:3d0be48abcf2 64
Fairy_Paolina 2:3d0be48abcf2 65 //wall_follow(RIGHT);
Fairy_Paolina 2:3d0be48abcf2 66 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 67 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 68 wall_follow2(LEFT);
Fairy_Paolina 2:3d0be48abcf2 69 location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
Fairy_Paolina 2:3d0be48abcf2 70
Fairy_Paolina 2:3d0be48abcf2 71 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 72 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 73 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 2:3d0be48abcf2 74 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 2:3d0be48abcf2 75 while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300);
Fairy_Paolina 0:ff94cc47fef7 76 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 77
Fairy_Paolina 1:801f0b9a862a 78 leftTurn();
Fairy_Paolina 2:3d0be48abcf2 79
Fairy_Paolina 2:3d0be48abcf2 80 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 81 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 82 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 2:3d0be48abcf2 83 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 2:3d0be48abcf2 84 while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30);
Fairy_Paolina 2:3d0be48abcf2 85 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 86
Fairy_Paolina 2:3d0be48abcf2 87 leftTurn();
Fairy_Paolina 2:3d0be48abcf2 88 wall_follow2(RIGHT);
Fairy_Paolina 1:801f0b9a862a 89 rightTurn();
Fairy_Paolina 0:ff94cc47fef7 90
Fairy_Paolina 2:3d0be48abcf2 91
Fairy_Paolina 2:3d0be48abcf2 92
Fairy_Paolina 2:3d0be48abcf2 93 bt.printf("LOCATION %f\n\r",location);
Fairy_Paolina 2:3d0be48abcf2 94
Fairy_Paolina 2:3d0be48abcf2 95 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 96 // leftTurn();
Fairy_Paolina 2:3d0be48abcf2 97 // wait(1);
Fairy_Paolina 2:3d0be48abcf2 98 // rightTurn();
Fairy_Paolina 2:3d0be48abcf2 99
Fairy_Paolina 2:3d0be48abcf2 100
Fairy_Paolina 0:ff94cc47fef7 101 }
Fairy_Paolina 0:ff94cc47fef7 102
Fairy_Paolina 2:3d0be48abcf2 103 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 104 {
Fairy_Paolina 2:3d0be48abcf2 105 //Nothing
Fairy_Paolina 0:ff94cc47fef7 106 }
Fairy_Paolina 0:ff94cc47fef7 107
Fairy_Paolina 0:ff94cc47fef7 108 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 109 {
Fairy_Paolina 2:3d0be48abcf2 110 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 111 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 112 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 113 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 114 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 115 }
Fairy_Paolina 0:ff94cc47fef7 116 }
Fairy_Paolina 0:ff94cc47fef7 117
Fairy_Paolina 2:3d0be48abcf2 118 void wall_follow(int side)
Fairy_Paolina 0:ff94cc47fef7 119 {
Fairy_Paolina 2:3d0be48abcf2 120 while(1) {
Fairy_Paolina 2:3d0be48abcf2 121
Fairy_Paolina 2:3d0be48abcf2 122 pid1.setInputLimits(0, 5.0);
Fairy_Paolina 1:801f0b9a862a 123 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 124 pid1.setSetPoint(5.0);
Fairy_Paolina 2:3d0be48abcf2 125 if(side){
Fairy_Paolina 2:3d0be48abcf2 126 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 127 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 128 rangeFinderLeft.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 129 }
Fairy_Paolina 2:3d0be48abcf2 130 else{
Fairy_Paolina 2:3d0be48abcf2 131 rangeFinderRight.startMeas();
Fairy_Paolina 2:3d0be48abcf2 132 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 133 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 134 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 135 }
Fairy_Paolina 0:ff94cc47fef7 136
Fairy_Paolina 0:ff94cc47fef7 137 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 138 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 139
Fairy_Paolina 2:3d0be48abcf2 140 if(pid_return > 0) {
Fairy_Paolina 2:3d0be48abcf2 141 if(side){
Fairy_Paolina 2:3d0be48abcf2 142 motors.setMotor0Speed(MAX_SPEED - pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 143 motors.setMotor1Speed(MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 144 }
Fairy_Paolina 2:3d0be48abcf2 145 else{
Fairy_Paolina 2:3d0be48abcf2 146 motors.setMotor1Speed(MAX_SPEED - pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 147 motors.setMotor0Speed(MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 148 }
Fairy_Paolina 2:3d0be48abcf2 149 }else if(pid_return < 0) {
Fairy_Paolina 2:3d0be48abcf2 150 if(side){
Fairy_Paolina 2:3d0be48abcf2 151 motors.setMotor0Speed(MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 152 motors.setMotor1Speed(MAX_SPEED + pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 153 }
Fairy_Paolina 2:3d0be48abcf2 154 else{
Fairy_Paolina 2:3d0be48abcf2 155 motors.setMotor1Speed(MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 156 motors.setMotor0Speed(MAX_SPEED + pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 157 }
Fairy_Paolina 2:3d0be48abcf2 158 }else {
Fairy_Paolina 2:3d0be48abcf2 159 motors.setMotor0Speed(MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 160 motors.setMotor1Speed(MAX_SPEED);//left
Fairy_Paolina 0:ff94cc47fef7 161 }
Fairy_Paolina 2:3d0be48abcf2 162 }
Fairy_Paolina 0:ff94cc47fef7 163 }
Fairy_Paolina 0:ff94cc47fef7 164
Fairy_Paolina 0:ff94cc47fef7 165 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 0:ff94cc47fef7 166
Fairy_Paolina 2:3d0be48abcf2 167 void wall_follow2(int side)
Fairy_Paolina 0:ff94cc47fef7 168 {
Fairy_Paolina 0:ff94cc47fef7 169 int SeeWaveGap = false;
Fairy_Paolina 1:801f0b9a862a 170 int count=0;
Fairy_Paolina 2:3d0be48abcf2 171
Fairy_Paolina 2:3d0be48abcf2 172 while(1) {
Fairy_Paolina 2:3d0be48abcf2 173
Fairy_Paolina 2:3d0be48abcf2 174 pid1.setInputLimits(0.0, 5.0);
Fairy_Paolina 1:801f0b9a862a 175 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 176 pid1.setSetPoint(5.0);
Fairy_Paolina 2:3d0be48abcf2 177
Fairy_Paolina 2:3d0be48abcf2 178 if(side){
Fairy_Paolina 2:3d0be48abcf2 179 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 180 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 181 rangeFinderLeft.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 182 }
Fairy_Paolina 2:3d0be48abcf2 183 else{
Fairy_Paolina 2:3d0be48abcf2 184 rangeFinderRight.startMeas();
Fairy_Paolina 2:3d0be48abcf2 185 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 186 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 187 }
Fairy_Paolina 2:3d0be48abcf2 188
Fairy_Paolina 2:3d0be48abcf2 189
Fairy_Paolina 0:ff94cc47fef7 190 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 1:801f0b9a862a 191 /* If after 60 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 192 /* then robot is at wave opening */
Fairy_Paolina 2:3d0be48abcf2 193
Fairy_Paolina 1:801f0b9a862a 194 pc.printf("range %f\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 195 if(range > 20) {
Fairy_Paolina 2:3d0be48abcf2 196 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 197 bt.printf("wavegap\r\n");
Fairy_Paolina 2:3d0be48abcf2 198 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 199 break;
Fairy_Paolina 0:ff94cc47fef7 200 }
Fairy_Paolina 2:3d0be48abcf2 201
Fairy_Paolina 0:ff94cc47fef7 202 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 203 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 204 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 2:3d0be48abcf2 205
Fairy_Paolina 2:3d0be48abcf2 206 if(pid_return > 0) {
Fairy_Paolina 2:3d0be48abcf2 207 if(side){
Fairy_Paolina 2:3d0be48abcf2 208 motors.setMotor0Speed(MAX_SPEED - pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 209 motors.setMotor1Speed(MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 210 }
Fairy_Paolina 2:3d0be48abcf2 211 else{
Fairy_Paolina 2:3d0be48abcf2 212 motors.setMotor1Speed(MAX_SPEED - pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 213 motors.setMotor0Speed(MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 214 }
Fairy_Paolina 2:3d0be48abcf2 215 }else if(pid_return < 0) {
Fairy_Paolina 2:3d0be48abcf2 216 if(side){
Fairy_Paolina 2:3d0be48abcf2 217 motors.setMotor0Speed(MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 218 motors.setMotor1Speed(MAX_SPEED + pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 219 }
Fairy_Paolina 2:3d0be48abcf2 220 else{
Fairy_Paolina 2:3d0be48abcf2 221 motors.setMotor1Speed(MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 222 motors.setMotor0Speed(MAX_SPEED + pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 223 }
Fairy_Paolina 2:3d0be48abcf2 224 } else {
Fairy_Paolina 0:ff94cc47fef7 225 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 226 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 227 }
Fairy_Paolina 2:3d0be48abcf2 228 }
Fairy_Paolina 0:ff94cc47fef7 229 }
Fairy_Paolina 0:ff94cc47fef7 230
Fairy_Paolina 0:ff94cc47fef7 231
Fairy_Paolina 0:ff94cc47fef7 232 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
Fairy_Paolina 0:ff94cc47fef7 233 /* MEANT FOR RETURNING FROM OIL RIGS */
Fairy_Paolina 0:ff94cc47fef7 234
Fairy_Paolina 0:ff94cc47fef7 235 void wall_follow3(int &currentLocation, int &WaveOpening)
Fairy_Paolina 0:ff94cc47fef7 236 {
Fairy_Paolina 2:3d0be48abcf2 237 while(1) {
Fairy_Paolina 2:3d0be48abcf2 238
Fairy_Paolina 2:3d0be48abcf2 239
Fairy_Paolina 2:3d0be48abcf2 240 pid1.setInputLimits(0, 5);
Fairy_Paolina 1:801f0b9a862a 241 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 242 pid1.setSetPoint(5.0);
Fairy_Paolina 2:3d0be48abcf2 243
Fairy_Paolina 2:3d0be48abcf2 244 rangeFinderLeft.startMeas();
Fairy_Paolina 0:ff94cc47fef7 245 wait_ms(100);
Fairy_Paolina 2:3d0be48abcf2 246 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
Fairy_Paolina 0:ff94cc47fef7 247 //bt.printf("Range = %f\n", range);
Fairy_Paolina 2:3d0be48abcf2 248 }
Fairy_Paolina 2:3d0be48abcf2 249
Fairy_Paolina 0:ff94cc47fef7 250 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 0:ff94cc47fef7 251 /* If after 100 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 252 /* then robot is at wave opening */
Fairy_Paolina 2:3d0be48abcf2 253
Fairy_Paolina 2:3d0be48abcf2 254
Fairy_Paolina 2:3d0be48abcf2 255 if(range > 20 ) {
Fairy_Paolina 0:ff94cc47fef7 256 currentLocation--;
Fairy_Paolina 0:ff94cc47fef7 257 }
Fairy_Paolina 2:3d0be48abcf2 258
Fairy_Paolina 2:3d0be48abcf2 259 if( currentLocation == WaveOpening) {
Fairy_Paolina 0:ff94cc47fef7 260 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 261
Fairy_Paolina 0:ff94cc47fef7 262 break;
Fairy_Paolina 0:ff94cc47fef7 263 }
Fairy_Paolina 2:3d0be48abcf2 264
Fairy_Paolina 2:3d0be48abcf2 265
Fairy_Paolina 0:ff94cc47fef7 266 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 267 pid_return = pid1.compute();
Fairy_Paolina 0:ff94cc47fef7 268 bt.printf("Range: %f\n PID: %f", range, pid_return);
Fairy_Paolina 2:3d0be48abcf2 269
Fairy_Paolina 2:3d0be48abcf2 270 if(pid_return > 0) {
Fairy_Paolina 0:ff94cc47fef7 271 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 272 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 273 } else if(pid_return < 0) {
Fairy_Paolina 0:ff94cc47fef7 274 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 275 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 2:3d0be48abcf2 276 } else {
Fairy_Paolina 0:ff94cc47fef7 277 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 278 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 279 }
Fairy_Paolina 2:3d0be48abcf2 280 }
Fairy_Paolina 0:ff94cc47fef7 281 }
Fairy_Paolina 1:801f0b9a862a 282
Fairy_Paolina 1:801f0b9a862a 283 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 284 {
Fairy_Paolina 2:3d0be48abcf2 285 float speedL, speedR;
Fairy_Paolina 2:3d0be48abcf2 286
Fairy_Paolina 2:3d0be48abcf2 287 speedL=speedR= 0.4;
Fairy_Paolina 2:3d0be48abcf2 288
Fairy_Paolina 1:801f0b9a862a 289 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 290 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 291 motors.setMotor0Speed(-speedR*127);//right
Fairy_Paolina 2:3d0be48abcf2 292 motors.setMotor1Speed(speedL*127);//left
Fairy_Paolina 2:3d0be48abcf2 293 while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
Fairy_Paolina 1:801f0b9a862a 294 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 295 }
Fairy_Paolina 1:801f0b9a862a 296
Fairy_Paolina 1:801f0b9a862a 297 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 298 {
Fairy_Paolina 1:801f0b9a862a 299 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 300 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 301 motors.setMotor0Speed(0.4*127);// right
Fairy_Paolina 2:3d0be48abcf2 302 motors.setMotor1Speed(-0.4*127);// left
Fairy_Paolina 2:3d0be48abcf2 303 while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
Fairy_Paolina 1:801f0b9a862a 304 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 305 }