Justin Carr / NavigationTest

Tools Section Navigation Calibrated slightly

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@8:32ba0fad1689, 2014-03-22 (annotated)

Committer:
jjcarr2
Date:
Sat Mar 22 16:24:59 2014 +0000
Revision:
8:32ba0fad1689
Parent:
7:0b7897232e93 
Modular function blah;

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"
jjcarr2 8:32ba0fad1689 9
jjcarr2 2:0351277ee5dd 10 #define PIN_TRIGGERL (p12)
jjcarr2 2:0351277ee5dd 11 #define PIN_ECHOL (p11)
jjcarr2 2:0351277ee5dd 12 #define PIN_TRIGGERR (p29)
jjcarr2 2:0351277ee5dd 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)
jjcarr2 2:0351277ee5dd 19 #define PPR (4331/4)
jjcarr2 2:0351277ee5dd 20 #define LEFT (1)
jjcarr2 2:0351277ee5dd 21 #define RIGHT (0)
jjcarr2 2:0351277ee5dd 22 #define FORWARD (1)
jjcarr2 8:32ba0fad1689 23 #define BACKWARD (0)
jjcarr2 2:0351277ee5dd 24 #define TOOLS (0)
jjcarr2 2:0351277ee5dd 25 #define MID (1)
jjcarr2 8:32ba0fad1689 26 #define FIRST_WAVE (0)
jjcarr2 8:32ba0fad1689 27 #define FAR (1)
jjcarr2 8:32ba0fad1689 28
jjcarr2 8:32ba0fad1689 29
jjcarr2 8:32ba0fad1689 30 float range, pid_return, usValue;
Fairy_Paolina 0:ff94cc47fef7 31 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 32 bool cRc;
jjcarr2 8:32ba0fad1689 33
Fairy_Paolina 0:ff94cc47fef7 34 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 35 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 36 Serial pc(USBTX,USBRX);
jjcarr2 2:0351277ee5dd 37 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
jjcarr2 2:0351277ee5dd 38 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
jjcarr2 8:32ba0fad1689 39 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 40 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
jjcarr2 2:0351277ee5dd 41 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
jjcarr2 2:0351277ee5dd 42 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 1:801f0b9a862a 43 //InterruptIn encoder(p29);
jjcarr2 8:32ba0fad1689 44
jjcarr2 8:32ba0fad1689 45
Fairy_Paolina 0:ff94cc47fef7 46 //Functions
jjcarr2 8:32ba0fad1689 47
jjcarr2 2:0351277ee5dd 48 float wall_follow(int side, int direction, int section);
jjcarr2 8:32ba0fad1689 49 void wall_follow2(int side, int direction, int section, float location);
Fairy_Paolina 0:ff94cc47fef7 50 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 51 void leftTurn(void);
Fairy_Paolina 1:801f0b9a862a 52 void rightTurn(void);
jjcarr2 8:32ba0fad1689 53 void us_distance(int side);
jjcarr2 8:32ba0fad1689 54 void tools_section(float* location, float &current);
jjcarr2 8:32ba0fad1689 55 void mid_section(float* location, float &current, int* direction);
jjcarr2 8:32ba0fad1689 56 void mid_section2(float* location, float &current, int* direction);
jjcarr2 8:32ba0fad1689 57 void rig_section(float* location, float &current, int* direction, int rig);
jjcarr2 8:32ba0fad1689 58 void overBump(int wave);
jjcarr2 8:32ba0fad1689 59 void alignWithWall(int section);
jjcarr2 8:32ba0fad1689 60 void alignParallel(int side, int turn, float limit);
jjcarr2 8:32ba0fad1689 61
Fairy_Paolina 1:801f0b9a862a 62 //Variables
jjcarr2 8:32ba0fad1689 63
jjcarr2 2:0351277ee5dd 64 int main(void)
jjcarr2 2:0351277ee5dd 65 {
jjcarr2 2:0351277ee5dd 66 float location[3], current=0;
jjcarr2 2:0351277ee5dd 67 int direction[3];
jjcarr2 8:32ba0fad1689 68
Fairy_Paolina 0:ff94cc47fef7 69 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 70 bt.baud(115200);
jjcarr2 2:0351277ee5dd 71 motors.begin();
jjcarr2 2:0351277ee5dd 72
jjcarr2 8:32ba0fad1689 73 bt.printf("START\r\n");
jjcarr2 2:0351277ee5dd 74 //Go to tools
jjcarr2 2:0351277ee5dd 75 tools_section(location, current);
jjcarr2 8:32ba0fad1689 76 mid_section(location, current, direction);
jjcarr2 8:32ba0fad1689 77 mid_section2(location, current, direction);
jjcarr2 8:32ba0fad1689 78
jjcarr2 8:32ba0fad1689 79
jjcarr2 8:32ba0fad1689 80
Fairy_Paolina 0:ff94cc47fef7 81 }
jjcarr2 8:32ba0fad1689 82
jjcarr2 2:0351277ee5dd 83 void errFunction(void)
jjcarr2 2:0351277ee5dd 84 {
jjcarr2 2:0351277ee5dd 85 //Nothing
Fairy_Paolina 0:ff94cc47fef7 86 }
jjcarr2 8:32ba0fad1689 87
jjcarr2 8:32ba0fad1689 88 void us_distance(int side)
jjcarr2 8:32ba0fad1689 89 {
jjcarr2 8:32ba0fad1689 90 if(side == LEFT) {
jjcarr2 8:32ba0fad1689 91 rangeFinderLeft.startMeas();
jjcarr2 8:32ba0fad1689 92 wait_us(20);
jjcarr2 8:32ba0fad1689 93 rangeFinderLeft.getMeas(range);
jjcarr2 8:32ba0fad1689 94 } else {
jjcarr2 8:32ba0fad1689 95
jjcarr2 8:32ba0fad1689 96 rangeFinderRight.startMeas();
jjcarr2 8:32ba0fad1689 97 wait_us(20);
jjcarr2 8:32ba0fad1689 98 rangeFinderRight.getMeas(range);
jjcarr2 8:32ba0fad1689 99 }
jjcarr2 8:32ba0fad1689 100
jjcarr2 8:32ba0fad1689 101 }
jjcarr2 8:32ba0fad1689 102
jjcarr2 8:32ba0fad1689 103 void alignParallel(int side, int turn, float limit)
Fairy_Paolina 0:ff94cc47fef7 104 {
jjcarr2 8:32ba0fad1689 105 leftEncoder.reset();
jjcarr2 8:32ba0fad1689 106 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 107
jjcarr2 8:32ba0fad1689 108 if(turn == LEFT) {
jjcarr2 8:32ba0fad1689 109 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 110 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 8:32ba0fad1689 111 while(rightEncoder.getPulses() < 100);
jjcarr2 8:32ba0fad1689 112
jjcarr2 8:32ba0fad1689 113 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 114
jjcarr2 8:32ba0fad1689 115 motors.setMotor0Speed(0.7*MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 116 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
jjcarr2 8:32ba0fad1689 117 } else {
jjcarr2 8:32ba0fad1689 118 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 119 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 8:32ba0fad1689 120 while(leftEncoder.getPulses() < 100);
jjcarr2 8:32ba0fad1689 121
jjcarr2 8:32ba0fad1689 122 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 123
jjcarr2 8:32ba0fad1689 124 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 125 motors.setMotor1Speed(0.7*MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 126 }
jjcarr2 8:32ba0fad1689 127
jjcarr2 8:32ba0fad1689 128 usValue = 0;
jjcarr2 8:32ba0fad1689 129
jjcarr2 8:32ba0fad1689 130 if(side == LEFT) {
jjcarr2 8:32ba0fad1689 131 while(1) {
jjcarr2 8:32ba0fad1689 132 us_distance(LEFT);
jjcarr2 8:32ba0fad1689 133 //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
jjcarr2 8:32ba0fad1689 134 if(range > usValue && usValue != 0 && range < limit) {
jjcarr2 8:32ba0fad1689 135 break;
jjcarr2 8:32ba0fad1689 136 } else {
jjcarr2 8:32ba0fad1689 137 usValue = range;
jjcarr2 8:32ba0fad1689 138 }
jjcarr2 8:32ba0fad1689 139 }
jjcarr2 8:32ba0fad1689 140 } else {
jjcarr2 8:32ba0fad1689 141 while(1) {
jjcarr2 8:32ba0fad1689 142 us_distance(RIGHT);
jjcarr2 8:32ba0fad1689 143 //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
jjcarr2 8:32ba0fad1689 144 if(range > usValue && usValue != 0 && range < limit) {
jjcarr2 8:32ba0fad1689 145 break;
jjcarr2 8:32ba0fad1689 146 } else {
jjcarr2 8:32ba0fad1689 147 usValue = range;
jjcarr2 8:32ba0fad1689 148 }
jjcarr2 8:32ba0fad1689 149 }
jjcarr2 8:32ba0fad1689 150 }
jjcarr2 8:32ba0fad1689 151 motors.stopBothMotors();
Fairy_Paolina 0:ff94cc47fef7 152 }
jjcarr2 8:32ba0fad1689 153
jjcarr2 2:0351277ee5dd 154 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 155 {
jjcarr2 8:32ba0fad1689 156 float location, wavegap=0, set=4;
jjcarr2 8:32ba0fad1689 157 int dir=1;
jjcarr2 8:32ba0fad1689 158
jjcarr2 2:0351277ee5dd 159 pid1.reset();
jjcarr2 8:32ba0fad1689 160
jjcarr2 2:0351277ee5dd 161 if(direction == BACKWARD) dir=-1;
jjcarr2 7:0b7897232e93 162 if(section == TOOLS)set= 10;
jjcarr2 8:32ba0fad1689 163
jjcarr2 2:0351277ee5dd 164 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 165 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 166
jjcarr2 2:0351277ee5dd 167 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 8:32ba0fad1689 168
jjcarr2 8:32ba0fad1689 169 while(location< 78) {
jjcarr2 2:0351277ee5dd 170 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 8:32ba0fad1689 171
jjcarr2 2:0351277ee5dd 172 pid1.setInputLimits(0, set);
jjcarr2 2:0351277ee5dd 173 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 174 pid1.setSetPoint(set);
jjcarr2 8:32ba0fad1689 175 if(side) {
jjcarr2 8:32ba0fad1689 176 us_distance(LEFT);
jjcarr2 8:32ba0fad1689 177 } else {
jjcarr2 8:32ba0fad1689 178 us_distance(RIGHT);
jjcarr2 2:0351277ee5dd 179 }
jjcarr2 8:32ba0fad1689 180
jjcarr2 2:0351277ee5dd 181 if(range > 20) {
jjcarr2 2:0351277ee5dd 182 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 8:32ba0fad1689 183 //bt.printf("wavegap %f\r\n",wavegap);
jjcarr2 2:0351277ee5dd 184 // AT WAVE OPENING!!!!
jjcarr2 8:32ba0fad1689 185 motors.setMotor1Speed(dir*0.3*127);//left
jjcarr2 8:32ba0fad1689 186 motors.setMotor0Speed(dir*0.3*127);//right
jjcarr2 2:0351277ee5dd 187 } else {
jjcarr2 8:32ba0fad1689 188
jjcarr2 8:32ba0fad1689 189 pid1.setProcessValue(range);
jjcarr2 8:32ba0fad1689 190 pid_return = pid1.compute();
jjcarr2 8:32ba0fad1689 191
jjcarr2 8:32ba0fad1689 192 if(pid_return > 0) {
jjcarr2 8:32ba0fad1689 193 if(side) {
jjcarr2 8:32ba0fad1689 194 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 8:32ba0fad1689 195 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 8:32ba0fad1689 196 } else {
jjcarr2 8:32ba0fad1689 197 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 8:32ba0fad1689 198 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 8:32ba0fad1689 199 }
jjcarr2 8:32ba0fad1689 200 } else if(pid_return < 0) {
jjcarr2 8:32ba0fad1689 201 if(side) {
jjcarr2 8:32ba0fad1689 202 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 8:32ba0fad1689 203 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 8:32ba0fad1689 204 } else {
jjcarr2 8:32ba0fad1689 205 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 8:32ba0fad1689 206 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 8:32ba0fad1689 207 }
jjcarr2 8:32ba0fad1689 208 } else {
jjcarr2 8:32ba0fad1689 209 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 210 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 211 }
jjcarr2 2:0351277ee5dd 212 }
jjcarr2 8:32ba0fad1689 213 }
jjcarr2 2:0351277ee5dd 214 return wavegap;
jjcarr2 2:0351277ee5dd 215 }
jjcarr2 8:32ba0fad1689 216
jjcarr2 2:0351277ee5dd 217 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
jjcarr2 8:32ba0fad1689 218
jjcarr2 8:32ba0fad1689 219 void wall_follow2(int side, int direction, int section, float location)
jjcarr2 2:0351277ee5dd 220 {
jjcarr2 8:32ba0fad1689 221 int SeeWaveGap = false, dir=1;
jjcarr2 8:32ba0fad1689 222 float set=4, loc=0;
jjcarr2 8:32ba0fad1689 223
jjcarr2 2:0351277ee5dd 224 pid1.reset();
jjcarr2 8:32ba0fad1689 225
jjcarr2 2:0351277ee5dd 226 if(direction == BACKWARD) dir=-1;
jjcarr2 2:0351277ee5dd 227 if(section == TOOLS)set= 5;
jjcarr2 8:32ba0fad1689 228
jjcarr2 2:0351277ee5dd 229 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 230 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 231
jjcarr2 8:32ba0fad1689 232 while(loc + location < 80) {
jjcarr2 8:32ba0fad1689 233 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 8:32ba0fad1689 234
jjcarr2 2:0351277ee5dd 235 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 236 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 237 pid1.setSetPoint(set);
jjcarr2 8:32ba0fad1689 238
jjcarr2 8:32ba0fad1689 239 if(side) {
jjcarr2 8:32ba0fad1689 240 us_distance(LEFT);
jjcarr2 8:32ba0fad1689 241 } else {
jjcarr2 8:32ba0fad1689 242 us_distance(RIGHT);
jjcarr2 2:0351277ee5dd 243 }
jjcarr2 8:32ba0fad1689 244
jjcarr2 8:32ba0fad1689 245
jjcarr2 2:0351277ee5dd 246 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 247 /* If after 20 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 248 /* then robot is at wave opening */
jjcarr2 8:32ba0fad1689 249
jjcarr2 2:0351277ee5dd 250 pc.printf("range %f\r\n",range);
jjcarr2 2:0351277ee5dd 251 if(range > 20) {
jjcarr2 2:0351277ee5dd 252 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 253 bt.printf("wavegap\r\n");
jjcarr2 2:0351277ee5dd 254 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 255 break;
jjcarr2 2:0351277ee5dd 256 }
jjcarr2 8:32ba0fad1689 257
Fairy_Paolina 0:ff94cc47fef7 258 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 259 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 260 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
jjcarr2 8:32ba0fad1689 261
jjcarr2 8:32ba0fad1689 262 if(pid_return > 0) {
jjcarr2 8:32ba0fad1689 263 if(side) {
jjcarr2 2:0351277ee5dd 264 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 265 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 8:32ba0fad1689 266 } else {
jjcarr2 2:0351277ee5dd 267 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 268 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 269 }
jjcarr2 8:32ba0fad1689 270 } else if(pid_return < 0) {
jjcarr2 8:32ba0fad1689 271 if(side) {
jjcarr2 2:0351277ee5dd 272 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 273 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 8:32ba0fad1689 274 } else {
jjcarr2 2:0351277ee5dd 275 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 276 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 277 }
jjcarr2 2:0351277ee5dd 278 } else {
jjcarr2 2:0351277ee5dd 279 motors.setMotor0Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 280 motors.setMotor1Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 281 }
jjcarr2 2:0351277ee5dd 282 }
jjcarr2 8:32ba0fad1689 283 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 284 }
jjcarr2 8:32ba0fad1689 285
jjcarr2 8:32ba0fad1689 286
jjcarr2 8:32ba0fad1689 287 void alignWithWall(int section)
jjcarr2 2:0351277ee5dd 288 {
jjcarr2 8:32ba0fad1689 289 usValue = 0;
jjcarr2 8:32ba0fad1689 290
jjcarr2 8:32ba0fad1689 291 if(section == TOOLS) {
jjcarr2 8:32ba0fad1689 292 // turn at an angle
jjcarr2 8:32ba0fad1689 293 leftEncoder.reset();
jjcarr2 8:32ba0fad1689 294 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 295 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 296 motors.setMotor1Speed(0.4*MAX_SPEED); //left
jjcarr2 8:32ba0fad1689 297 while(rightEncoder.getPulses()>-1000);
jjcarr2 8:32ba0fad1689 298 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 299
jjcarr2 8:32ba0fad1689 300 //go backwards toward wall
jjcarr2 8:32ba0fad1689 301 leftEncoder.reset();
jjcarr2 8:32ba0fad1689 302 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 303 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 304 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 8:32ba0fad1689 305 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
jjcarr2 8:32ba0fad1689 306
jjcarr2 8:32ba0fad1689 307 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 308
jjcarr2 8:32ba0fad1689 309 // turn left towards wall
jjcarr2 8:32ba0fad1689 310 alignParallel(LEFT,LEFT,100);
jjcarr2 8:32ba0fad1689 311 } else {
jjcarr2 8:32ba0fad1689 312 // turn right towards wall
jjcarr2 8:32ba0fad1689 313 alignParallel(LEFT,RIGHT, 60);
jjcarr2 2:0351277ee5dd 314 }
Fairy_Paolina 0:ff94cc47fef7 315 }
jjcarr2 8:32ba0fad1689 316
Fairy_Paolina 1:801f0b9a862a 317 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 318 {
Fairy_Paolina 1:801f0b9a862a 319 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 320 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 321 motors.setMotor0Speed(-0.4*127);//right
jjcarr2 8:32ba0fad1689 322 motors.setMotor1Speed(0.4*127);//left
jjcarr2 8:32ba0fad1689 323 while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000);
Fairy_Paolina 1:801f0b9a862a 324 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 325 }
jjcarr2 8:32ba0fad1689 326
Fairy_Paolina 1:801f0b9a862a 327 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 328 {
Fairy_Paolina 1:801f0b9a862a 329 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 330 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 331 motors.setMotor0Speed(0.4*127);// right
jjcarr2 2:0351277ee5dd 332 motors.setMotor1Speed(-0.4*127);// left
jjcarr2 8:32ba0fad1689 333 while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
Fairy_Paolina 1:801f0b9a862a 334 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 335 }
jjcarr2 8:32ba0fad1689 336
jjcarr2 8:32ba0fad1689 337
jjcarr2 8:32ba0fad1689 338 void overBump(int wave)
jjcarr2 8:32ba0fad1689 339 {
jjcarr2 8:32ba0fad1689 340 int preLeft=5000, preRight=5000 ;
jjcarr2 8:32ba0fad1689 341
jjcarr2 7:0b7897232e93 342 leftEncoder.reset();
jjcarr2 7:0b7897232e93 343 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 344 motors.setMotor0Speed(0.15*127); //right
jjcarr2 8:32ba0fad1689 345 motors.setMotor1Speed(0.15*127); //left
jjcarr2 8:32ba0fad1689 346 while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0) {
jjcarr2 8:32ba0fad1689 347 preLeft=leftEncoder.getPulses();
jjcarr2 8:32ba0fad1689 348 preRight=rightEncoder.getPulses();
jjcarr2 8:32ba0fad1689 349 wait_ms(20);
jjcarr2 8:32ba0fad1689 350 if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0;
jjcarr2 8:32ba0fad1689 351 }
jjcarr2 8:32ba0fad1689 352
jjcarr2 8:32ba0fad1689 353 if(wave == FAR) {
jjcarr2 8:32ba0fad1689 354 while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight) {
jjcarr2 8:32ba0fad1689 355 preLeft=leftEncoder.getPulses();
jjcarr2 8:32ba0fad1689 356 preRight=rightEncoder.getPulses();
jjcarr2 8:32ba0fad1689 357 wait_ms(20);
jjcarr2 8:32ba0fad1689 358 }
jjcarr2 8:32ba0fad1689 359
jjcarr2 8:32ba0fad1689 360 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 361 }
jjcarr2 8:32ba0fad1689 362
jjcarr2 7:0b7897232e93 363 leftEncoder.reset();
jjcarr2 7:0b7897232e93 364 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 365 motors.setMotor0Speed(0.4*127); //right
jjcarr2 8:32ba0fad1689 366 motors.setMotor1Speed(0.4*127); //left
jjcarr2 8:32ba0fad1689 367 while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
jjcarr2 8:32ba0fad1689 368
jjcarr2 8:32ba0fad1689 369 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 370 }
jjcarr2 8:32ba0fad1689 371
jjcarr2 8:32ba0fad1689 372 void tools_section(float* location, float &current)
jjcarr2 8:32ba0fad1689 373 {
jjcarr2 8:32ba0fad1689 374
jjcarr2 8:32ba0fad1689 375 wall_follow(LEFT,FORWARD, TOOLS);
jjcarr2 8:32ba0fad1689 376 // current position in reference to the starting position
jjcarr2 8:32ba0fad1689 377 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 8:32ba0fad1689 378 bt.printf("current %f \r\n",current);
jjcarr2 8:32ba0fad1689 379
jjcarr2 8:32ba0fad1689 380 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 381
jjcarr2 8:32ba0fad1689 382 //Tool aquiring
jjcarr2 8:32ba0fad1689 383 wait(2);
jjcarr2 8:32ba0fad1689 384 // After tool is aquired
jjcarr2 8:32ba0fad1689 385
jjcarr2 8:32ba0fad1689 386 alignWithWall(TOOLS);
jjcarr2 8:32ba0fad1689 387 current-=8;
jjcarr2 8:32ba0fad1689 388 wait_ms(100);
jjcarr2 8:32ba0fad1689 389
jjcarr2 8:32ba0fad1689 390 us_distance(LEFT);
jjcarr2 7:0b7897232e93 391
jjcarr2 8:32ba0fad1689 392 if(range < 20) {
jjcarr2 8:32ba0fad1689 393 wall_follow2(LEFT,BACKWARD,TOOLS, current);
jjcarr2 8:32ba0fad1689 394 location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 8:32ba0fad1689 395 current= location[0];
jjcarr2 8:32ba0fad1689 396
jjcarr2 8:32ba0fad1689 397 /* // go backwards
jjcarr2 8:32ba0fad1689 398 leftEncoder.reset();
jjcarr2 8:32ba0fad1689 399 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 400 motors.setMotor0Speed(-0.3*127); //right
jjcarr2 8:32ba0fad1689 401 motors.setMotor1Speed(-0.3*127); //left
jjcarr2 8:32ba0fad1689 402 while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
jjcarr2 8:32ba0fad1689 403
jjcarr2 8:32ba0fad1689 404 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 405 */
jjcarr2 8:32ba0fad1689 406
jjcarr2 8:32ba0fad1689 407 leftTurn();
jjcarr2 8:32ba0fad1689 408 overBump(FAR);
jjcarr2 8:32ba0fad1689 409 } else {
jjcarr2 8:32ba0fad1689 410 location[0]= 77;
jjcarr2 8:32ba0fad1689 411 leftTurn();
jjcarr2 8:32ba0fad1689 412 wait_ms(20);
jjcarr2 8:32ba0fad1689 413 overBump(FIRST_WAVE);
jjcarr2 7:0b7897232e93 414 }
jjcarr2 8:32ba0fad1689 415
jjcarr2 8:32ba0fad1689 416 bt.printf("wavegap = %f\r\n",location[0]);
jjcarr2 7:0b7897232e93 417 }
jjcarr2 7:0b7897232e93 418
jjcarr2 8:32ba0fad1689 419 void mid_section(float* location, float &current, int* direction)
jjcarr2 8:32ba0fad1689 420 {
jjcarr2 8:32ba0fad1689 421
jjcarr2 8:32ba0fad1689 422 motors.begin();
jjcarr2 8:32ba0fad1689 423 alignWithWall(MID);
jjcarr2 8:32ba0fad1689 424 /*
jjcarr2 8:32ba0fad1689 425 leftEncoder.reset();
jjcarr2 8:32ba0fad1689 426 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 427 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 8:32ba0fad1689 428 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 8:32ba0fad1689 429 while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
jjcarr2 8:32ba0fad1689 430 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 431 */
jjcarr2 8:32ba0fad1689 432 bt.printf("mid section current = %f\r\n",current);
jjcarr2 8:32ba0fad1689 433 wall_follow2(LEFT,FORWARD,MID, current);
jjcarr2 8:32ba0fad1689 434 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 8:32ba0fad1689 435 bt.printf("after wf2 current = %f\r\n",current);
jjcarr2 8:32ba0fad1689 436
jjcarr2 8:32ba0fad1689 437 if(current != 0) {
jjcarr2 8:32ba0fad1689 438 direction[0]= RIGHT;
jjcarr2 8:32ba0fad1689 439 current+= location[0];
jjcarr2 8:32ba0fad1689 440 location[1]= current;
jjcarr2 8:32ba0fad1689 441 } else {
jjcarr2 8:32ba0fad1689 442 current=location[0];
jjcarr2 8:32ba0fad1689 443 direction[0]= LEFT;
jjcarr2 8:32ba0fad1689 444 wall_follow2(LEFT,BACKWARD,MID,current);
jjcarr2 8:32ba0fad1689 445 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 8:32ba0fad1689 446 }
jjcarr2 8:32ba0fad1689 447
jjcarr2 8:32ba0fad1689 448 bt.printf("wavegap2 = %f\r\n",location[1]);
jjcarr2 2:0351277ee5dd 449 leftTurn();
jjcarr2 8:32ba0fad1689 450 overBump(FAR);
jjcarr2 8:32ba0fad1689 451 // go forward
jjcarr2 8:32ba0fad1689 452 leftEncoder.reset();
jjcarr2 8:32ba0fad1689 453 rightEncoder.reset();
jjcarr2 8:32ba0fad1689 454 motors.setMotor0Speed(0.2*127); //right
jjcarr2 8:32ba0fad1689 455 motors.setMotor1Speed(0.2*127); //left
jjcarr2 8:32ba0fad1689 456 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 8:32ba0fad1689 457 motors.stopBothMotors();
jjcarr2 8:32ba0fad1689 458
jjcarr2 8:32ba0fad1689 459 }
jjcarr2 8:32ba0fad1689 460
jjcarr2 8:32ba0fad1689 461 void mid_section2(float* location, float &current, int* direction)
jjcarr2 8:32ba0fad1689 462 {
jjcarr2 8:32ba0fad1689 463
jjcarr2 8:32ba0fad1689 464 motors.begin();
jjcarr2 8:32ba0fad1689 465 rightTurn();
jjcarr2 8:32ba0fad1689 466 alignWithWall(MID);
jjcarr2 8:32ba0fad1689 467 wall_follow2(LEFT,FORWARD,MID, current);
jjcarr2 8:32ba0fad1689 468 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 8:32ba0fad1689 469
jjcarr2 8:32ba0fad1689 470 if(current != 0) {
jjcarr2 8:32ba0fad1689 471 direction[1]= RIGHT;
jjcarr2 8:32ba0fad1689 472 current+= location[1];
jjcarr2 8:32ba0fad1689 473 location[2]= current;
jjcarr2 8:32ba0fad1689 474 } else {
jjcarr2 8:32ba0fad1689 475 current=location[1];
jjcarr2 8:32ba0fad1689 476 direction[1]= LEFT;
jjcarr2 8:32ba0fad1689 477 wall_follow2(LEFT,BACKWARD,MID,current);
jjcarr2 8:32ba0fad1689 478 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 8:32ba0fad1689 479 }
jjcarr2 8:32ba0fad1689 480
jjcarr2 7:0b7897232e93 481 leftTurn();
jjcarr2 8:32ba0fad1689 482 overBump(FAR);
jjcarr2 2:0351277ee5dd 483 }
jjcarr2 8:32ba0fad1689 484
jjcarr2 8:32ba0fad1689 485 void rig_section(float* location, float &current, int* direction, int rig)
jjcarr2 8:32ba0fad1689 486 {
jjcarr2 8:32ba0fad1689 487
jjcarr2 8:32ba0fad1689 488
jjcarr2 8:32ba0fad1689 489 }