Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@12:081b8fc654af, 2014-03-27 (annotated)

Committer:
jjcarr2
Date:
Thu Mar 27 22:48:00 2014 +0000
Revision:
12:081b8fc654af
Parent:
11:12ce7600f2f9
Child:
13:b6480275c445
Almost!

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 8:11ef93eebe07 9 #include "Sharp.h"
Fairy_Paolina 9:f34700716f1d 10
Fairy_Paolina 2:3d0be48abcf2 11 #define PIN_TRIGGERL (p12)
Fairy_Paolina 2:3d0be48abcf2 12 #define PIN_ECHOL (p11)
Fairy_Paolina 2:3d0be48abcf2 13 #define PIN_TRIGGERR (p29)
Fairy_Paolina 2:3d0be48abcf2 14 #define PIN_ECHOR (p30)
Fairy_Paolina 0:ff94cc47fef7 15 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 16 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 17 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 18 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 1:801f0b9a862a 19 #define MAX_SPEED (0.3*127)
Fairy_Paolina 4:f2333e66ec2c 20 #define PPR (4331/4)
Fairy_Paolina 2:3d0be48abcf2 21 #define LEFT (1)
Fairy_Paolina 2:3d0be48abcf2 22 #define RIGHT (0)
jjcarr2 12:081b8fc654af 23 #define STRAIGHT (2)
Fairy_Paolina 3:58726d2e11f0 24 #define FORWARD (1)
Fairy_Paolina 9:f34700716f1d 25 #define BACKWARD (0)
Fairy_Paolina 3:58726d2e11f0 26 #define TOOLS (0)
Fairy_Paolina 3:58726d2e11f0 27 #define MID (1)
Fairy_Paolina 9:f34700716f1d 28 #define RIGS (2)
jjcarr2 12:081b8fc654af 29 #define RETURN (3)
Fairy_Paolina 6:f5c26372b2d0 30 #define FIRST_WAVE (0)
Fairy_Paolina 6:f5c26372b2d0 31 #define FAR (1)
Fairy_Paolina 9:f34700716f1d 32
Fairy_Paolina 9:f34700716f1d 33
Fairy_Paolina 9:f34700716f1d 34 float range, range2, pid_return;
Fairy_Paolina 0:ff94cc47fef7 35 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 36 bool cRc;
Fairy_Paolina 9:f34700716f1d 37
Fairy_Paolina 0:ff94cc47fef7 38 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 39 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 40 Serial pc(USBTX,USBRX);
Fairy_Paolina 2:3d0be48abcf2 41 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
Fairy_Paolina 2:3d0be48abcf2 42 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina 2:3d0be48abcf2 43 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 44 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 4:f2333e66ec2c 45 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 4:f2333e66ec2c 46 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 8:11ef93eebe07 47 Sharp IR(p20);
Fairy_Paolina 1:801f0b9a862a 48 //InterruptIn encoder(p29);
Fairy_Paolina 9:f34700716f1d 49
Fairy_Paolina 9:f34700716f1d 50
Fairy_Paolina 0:ff94cc47fef7 51 //Functions
Fairy_Paolina 9:f34700716f1d 52
Fairy_Paolina 3:58726d2e11f0 53 float wall_follow(int side, int direction, int section);
Fairy_Paolina 6:f5c26372b2d0 54 void wall_follow2(int side, int direction, int section, float location);
Fairy_Paolina 0:ff94cc47fef7 55 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 56 void leftTurn(void);
Fairy_Paolina 9:f34700716f1d 57 void slightleft(void);
Fairy_Paolina 1:801f0b9a862a 58 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 59 void us_distance(void);
Fairy_Paolina 3:58726d2e11f0 60 void tools_section(float* location, float &current);
Fairy_Paolina 6:f5c26372b2d0 61 void mid_section(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 62 void mid_section2(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 63 void rig_section(float* location, float &current, int* direction, int rig);
jjcarr2 12:081b8fc654af 64 void tools_section_return(float* location, float &current);
jjcarr2 12:081b8fc654af 65 void mid_section_return(float* location, float &current, int* direction);
jjcarr2 12:081b8fc654af 66 void mid_section2_return(float* location, float &current, int* direction);
jjcarr2 12:081b8fc654af 67 void rig_section_return(float* location, float &current, int* direction, int rig);
Fairy_Paolina 8:11ef93eebe07 68 void overBump(int section);
Fairy_Paolina 6:f5c26372b2d0 69 void alignWithWall(int section);
jjcarr2 12:081b8fc654af 70 void UntilWall(int dir);
jjcarr2 12:081b8fc654af 71
Fairy_Paolina 9:f34700716f1d 72
Fairy_Paolina 1:801f0b9a862a 73 //Variables
Fairy_Paolina 9:f34700716f1d 74
Fairy_Paolina 2:3d0be48abcf2 75 int main(void)
Fairy_Paolina 2:3d0be48abcf2 76 {
Fairy_Paolina 3:58726d2e11f0 77 float location[3], current=0;
Fairy_Paolina 3:58726d2e11f0 78 int direction[3];
Fairy_Paolina 8:11ef93eebe07 79 double distance;
Fairy_Paolina 9:f34700716f1d 80
Fairy_Paolina 0:ff94cc47fef7 81 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 82 bt.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 83 motors.begin();
Fairy_Paolina 9:f34700716f1d 84
Fairy_Paolina 7:78745a518957 85 bt.printf("START\r\n");
Fairy_Paolina 3:58726d2e11f0 86 //Go to tools
Fairy_Paolina 4:f2333e66ec2c 87 tools_section(location, current);
Fairy_Paolina 6:f5c26372b2d0 88 mid_section(location, current, direction);
Fairy_Paolina 9:f34700716f1d 89 //mid_section2(location, current, direction);
jjcarr2 12:081b8fc654af 90 /* while(1) {
jjcarr2 12:081b8fc654af 91 //bt.printf("IR %f\r\n", US.getDistance());
jjcarr2 12:081b8fc654af 92 rangeFinderLeft.startMeas();
jjcarr2 12:081b8fc654af 93 wait_ms(20);
jjcarr2 12:081b8fc654af 94 rangeFinderLeft.getMeas(range);
jjcarr2 12:081b8fc654af 95 bt.printf("Range = %f\n\r", range);
Fairy_Paolina 9:f34700716f1d 96
jjcarr2 12:081b8fc654af 97 wait_ms(200);
jjcarr2 12:081b8fc654af 98 }*/
Fairy_Paolina 9:f34700716f1d 99
Fairy_Paolina 9:f34700716f1d 100
Fairy_Paolina 9:f34700716f1d 101 //leftTurn();
Fairy_Paolina 9:f34700716f1d 102 //rightTurn();
Fairy_Paolina 9:f34700716f1d 103
Fairy_Paolina 9:f34700716f1d 104
Fairy_Paolina 0:ff94cc47fef7 105 }
Fairy_Paolina 9:f34700716f1d 106
Fairy_Paolina 2:3d0be48abcf2 107 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 108 {
Fairy_Paolina 2:3d0be48abcf2 109 //Nothing
Fairy_Paolina 0:ff94cc47fef7 110 }
Fairy_Paolina 9:f34700716f1d 111
Fairy_Paolina 0:ff94cc47fef7 112 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 113 {
Fairy_Paolina 2:3d0be48abcf2 114 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 115 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 116 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 117 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 118 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 119 }
Fairy_Paolina 0:ff94cc47fef7 120 }
Fairy_Paolina 9:f34700716f1d 121
Fairy_Paolina 3:58726d2e11f0 122 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 123 {
Fairy_Paolina 8:11ef93eebe07 124 float location, wavegap=0, set=5;
Fairy_Paolina 5:70ccef3734ae 125 int dir=1;
Fairy_Paolina 9:f34700716f1d 126
Fairy_Paolina 4:f2333e66ec2c 127 pid1.reset();
Fairy_Paolina 9:f34700716f1d 128
Fairy_Paolina 3:58726d2e11f0 129 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 5:70ccef3734ae 130 if(section == TOOLS)set= 10;
Fairy_Paolina 9:f34700716f1d 131
Fairy_Paolina 3:58726d2e11f0 132 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 133 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 134
Fairy_Paolina 4:f2333e66ec2c 135 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 136
Fairy_Paolina 9:f34700716f1d 137 while(location< 73) {
jjcarr2 12:081b8fc654af 138
jjcarr2 12:081b8fc654af 139 if(section == RETURN) {
jjcarr2 12:081b8fc654af 140 if(location <10) break;
jjcarr2 12:081b8fc654af 141 }
jjcarr2 12:081b8fc654af 142
Fairy_Paolina 4:f2333e66ec2c 143 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 144
Fairy_Paolina 3:58726d2e11f0 145 pid1.setInputLimits(0, set);
Fairy_Paolina 1:801f0b9a862a 146 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 147 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 148 if(side) {
Fairy_Paolina 2:3d0be48abcf2 149 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 150 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 151 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 152 } else {
Fairy_Paolina 2:3d0be48abcf2 153 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 154 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 155 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 156 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 157 }
Fairy_Paolina 9:f34700716f1d 158
Fairy_Paolina 3:58726d2e11f0 159 if(range > 20) {
Fairy_Paolina 4:f2333e66ec2c 160 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 7:78745a518957 161 //bt.printf("wavegap %f\r\n",wavegap);
Fairy_Paolina 3:58726d2e11f0 162 // AT WAVE OPENING!!!!
Fairy_Paolina 9:f34700716f1d 163 motors.setMotor1Speed(dir*0.4*127);//left
Fairy_Paolina 9:f34700716f1d 164 motors.setMotor0Speed(dir*0.4*127);//right
Fairy_Paolina 9:f34700716f1d 165 } else {
Fairy_Paolina 9:f34700716f1d 166
Fairy_Paolina 6:f5c26372b2d0 167 pid1.setProcessValue(range);
Fairy_Paolina 6:f5c26372b2d0 168 pid_return = pid1.compute();
Fairy_Paolina 9:f34700716f1d 169
Fairy_Paolina 6:f5c26372b2d0 170 if(pid_return > 0) {
Fairy_Paolina 9:f34700716f1d 171 if(side) {
Fairy_Paolina 6:f5c26372b2d0 172 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 173 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 9:f34700716f1d 174 } else {
Fairy_Paolina 6:f5c26372b2d0 175 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 6:f5c26372b2d0 176 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 177 }
Fairy_Paolina 9:f34700716f1d 178 } else if(pid_return < 0) {
Fairy_Paolina 9:f34700716f1d 179 if(side) {
Fairy_Paolina 6:f5c26372b2d0 180 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 181 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 9:f34700716f1d 182 } else {
Fairy_Paolina 6:f5c26372b2d0 183 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 6:f5c26372b2d0 184 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 185 }
Fairy_Paolina 9:f34700716f1d 186 } else {
Fairy_Paolina 6:f5c26372b2d0 187 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 188 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 189 }
Fairy_Paolina 0:ff94cc47fef7 190 }
Fairy_Paolina 2:3d0be48abcf2 191 }
Fairy_Paolina 3:58726d2e11f0 192 return wavegap;
Fairy_Paolina 0:ff94cc47fef7 193 }
Fairy_Paolina 9:f34700716f1d 194
Fairy_Paolina 0:ff94cc47fef7 195 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 9:f34700716f1d 196
Fairy_Paolina 6:f5c26372b2d0 197 void wall_follow2(int side, int direction, int section, float location)
Fairy_Paolina 0:ff94cc47fef7 198 {
jjcarr2 12:081b8fc654af 199 bool SeeWaveGap = false, dir=1, limit=83;
Fairy_Paolina 8:11ef93eebe07 200 float set=5, loc=0;
Fairy_Paolina 9:f34700716f1d 201
Fairy_Paolina 4:f2333e66ec2c 202 pid1.reset();
Fairy_Paolina 9:f34700716f1d 203
Fairy_Paolina 3:58726d2e11f0 204 if(direction == BACKWARD) dir=-1;
jjcarr2 12:081b8fc654af 205 if(section == TOOLS) {
jjcarr2 11:12ce7600f2f9 206 set= 5;
jjcarr2 11:12ce7600f2f9 207 limit = 78;
jjcarr2 11:12ce7600f2f9 208 }
jjcarr2 12:081b8fc654af 209
jjcarr2 12:081b8fc654af 210
Fairy_Paolina 3:58726d2e11f0 211 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 212 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 213
jjcarr2 11:12ce7600f2f9 214 while(dir*loc + location <= limit) {
Fairy_Paolina 7:78745a518957 215 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 216
Fairy_Paolina 5:70ccef3734ae 217 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 218 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 5:70ccef3734ae 219 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 220
Fairy_Paolina 9:f34700716f1d 221 if(side) {
Fairy_Paolina 2:3d0be48abcf2 222 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 223 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 224 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 225 } else {
Fairy_Paolina 2:3d0be48abcf2 226 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 227 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 228 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 229 }
Fairy_Paolina 9:f34700716f1d 230
jjcarr2 12:081b8fc654af 231 if(section == RIGS) {
jjcarr2 12:081b8fc654af 232 rangeFinderRight.startMeas();
jjcarr2 12:081b8fc654af 233 wait_ms(38);
jjcarr2 12:081b8fc654af 234 rangeFinderRight.getMeas(range);
jjcarr2 12:081b8fc654af 235 if(range< 20) {
jjcarr2 12:081b8fc654af 236 motors.stopBothMotors();
jjcarr2 12:081b8fc654af 237 break;
jjcarr2 12:081b8fc654af 238 }
jjcarr2 12:081b8fc654af 239 }
jjcarr2 12:081b8fc654af 240
Fairy_Paolina 9:f34700716f1d 241
jjcarr2 10:2aa70a504c18 242 //bt.printf("wall follow 2 range %f\r\n",range);
jjcarr2 10:2aa70a504c18 243 //bt.printf("loc+location = %f\r\n", loc+location);
jjcarr2 11:12ce7600f2f9 244 if(range > 20 && !SeeWaveGap) {
jjcarr2 12:081b8fc654af 245 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 12:081b8fc654af 246 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 12:081b8fc654af 247 wait_ms(30);
jjcarr2 12:081b8fc654af 248 SeeWaveGap = true;
jjcarr2 12:081b8fc654af 249 } else if(range > 20 && SeeWaveGap) {
jjcarr2 12:081b8fc654af 250 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 251 bt.printf("wavegap\r\n");
Fairy_Paolina 2:3d0be48abcf2 252 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 253 break;
jjcarr2 11:12ce7600f2f9 254 } else {
Fairy_Paolina 9:f34700716f1d 255
jjcarr2 12:081b8fc654af 256 pid1.setProcessValue(range);
jjcarr2 12:081b8fc654af 257 pid_return = pid1.compute();
jjcarr2 12:081b8fc654af 258 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 9:f34700716f1d 259
jjcarr2 12:081b8fc654af 260 if(pid_return > 0) {
jjcarr2 12:081b8fc654af 261 if(side) {
jjcarr2 12:081b8fc654af 262 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 12:081b8fc654af 263 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 12:081b8fc654af 264 } else {
jjcarr2 12:081b8fc654af 265 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 12:081b8fc654af 266 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 12:081b8fc654af 267 }
jjcarr2 12:081b8fc654af 268 } else if(pid_return < 0) {
jjcarr2 12:081b8fc654af 269 if(side) {
jjcarr2 12:081b8fc654af 270 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 12:081b8fc654af 271 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 12:081b8fc654af 272 } else {
jjcarr2 12:081b8fc654af 273 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 12:081b8fc654af 274 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 12:081b8fc654af 275 }
Fairy_Paolina 9:f34700716f1d 276 } else {
jjcarr2 12:081b8fc654af 277 motors.setMotor0Speed(dir*MAX_SPEED);
jjcarr2 12:081b8fc654af 278 motors.setMotor1Speed(dir*MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 279 }
Fairy_Paolina 0:ff94cc47fef7 280 }
jjcarr2 12:081b8fc654af 281 }
Fairy_Paolina 6:f5c26372b2d0 282 motors.stopBothMotors();
Fairy_Paolina 0:ff94cc47fef7 283 }
Fairy_Paolina 9:f34700716f1d 284
Fairy_Paolina 9:f34700716f1d 285
Fairy_Paolina 9:f34700716f1d 286 void alignWithWall(int section)
Fairy_Paolina 9:f34700716f1d 287 {
Fairy_Paolina 6:f5c26372b2d0 288 float usValue = 0;
Fairy_Paolina 9:f34700716f1d 289
Fairy_Paolina 9:f34700716f1d 290 if(section == TOOLS) {
Fairy_Paolina 6:f5c26372b2d0 291 // turn at an angle
Fairy_Paolina 6:f5c26372b2d0 292 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 293 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 294 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 295 motors.setMotor1Speed(0.4*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 296 while(rightEncoder.getPulses()>-1000);
Fairy_Paolina 6:f5c26372b2d0 297 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 298
Fairy_Paolina 6:f5c26372b2d0 299 //go backwards toward wall
Fairy_Paolina 6:f5c26372b2d0 300 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 301 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 302 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 303 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 304 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
Fairy_Paolina 9:f34700716f1d 305
Fairy_Paolina 6:f5c26372b2d0 306 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 307
Fairy_Paolina 9:f34700716f1d 308 // turn left towards wall
Fairy_Paolina 6:f5c26372b2d0 309 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 310 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 311 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 312 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 313 while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
Fairy_Paolina 9:f34700716f1d 314
Fairy_Paolina 9:f34700716f1d 315 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 316
Fairy_Paolina 7:78745a518957 317 motors.setMotor0Speed(0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 318 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 319 } else {
Fairy_Paolina 8:11ef93eebe07 320 rightTurn();
Fairy_Paolina 7:78745a518957 321 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 322 motors.setMotor1Speed(0.7*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 323 }
Fairy_Paolina 9:f34700716f1d 324
Fairy_Paolina 6:f5c26372b2d0 325 usValue = 0;
Fairy_Paolina 9:f34700716f1d 326 while(1) {
Fairy_Paolina 2:3d0be48abcf2 327 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 328 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 329 rangeFinderLeft.getMeas(range);
Fairy_Paolina 7:78745a518957 330 //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
Fairy_Paolina 9:f34700716f1d 331 if(range > usValue && usValue != 0 && range < 25) {
Fairy_Paolina 0:ff94cc47fef7 332 break;
Fairy_Paolina 2:3d0be48abcf2 333 } else {
Fairy_Paolina 9:f34700716f1d 334 usValue = range;
Fairy_Paolina 0:ff94cc47fef7 335 }
Fairy_Paolina 2:3d0be48abcf2 336 }
Fairy_Paolina 6:f5c26372b2d0 337 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 338 }
Fairy_Paolina 9:f34700716f1d 339
Fairy_Paolina 1:801f0b9a862a 340 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 341 {
Fairy_Paolina 9:f34700716f1d 342 motors.begin();
Fairy_Paolina 1:801f0b9a862a 343 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 344 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 345 motors.setMotor0Speed(-0.5*127);//right
Fairy_Paolina 9:f34700716f1d 346 motors.setMotor1Speed(0.5*127);//left
Fairy_Paolina 9:f34700716f1d 347 while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
Fairy_Paolina 1:801f0b9a862a 348 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 349 }
Fairy_Paolina 9:f34700716f1d 350
Fairy_Paolina 1:801f0b9a862a 351 void leftTurn(void)
Fairy_Paolina 9:f34700716f1d 352 {
Fairy_Paolina 9:f34700716f1d 353 /*
Fairy_Paolina 1:801f0b9a862a 354 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 355 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 356 motors.setMotor0Speed(0.4*MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 357 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 358 while(abs(leftEncoder.getPulses())<2500);
Fairy_Paolina 9:f34700716f1d 359 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 360 */
Fairy_Paolina 9:f34700716f1d 361 motors.begin();
Fairy_Paolina 9:f34700716f1d 362 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 363 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 364 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 365 motors.setMotor1Speed(-0.5*127);// left
jjcarr2 11:12ce7600f2f9 366 while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
Fairy_Paolina 5:70ccef3734ae 367 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 368 }
jjcarr2 11:12ce7600f2f9 369
jjcarr2 12:081b8fc654af 370 void slightleft(void)
jjcarr2 12:081b8fc654af 371 {
jjcarr2 12:081b8fc654af 372
Fairy_Paolina 9:f34700716f1d 373 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 374 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 375 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 376 motors.setMotor1Speed(-0.5*127);// left
jjcarr2 11:12ce7600f2f9 377 while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
Fairy_Paolina 9:f34700716f1d 378 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 379 }
Fairy_Paolina 9:f34700716f1d 380
jjcarr2 12:081b8fc654af 381 void UntilWall(int dir)
jjcarr2 12:081b8fc654af 382 {
jjcarr2 12:081b8fc654af 383
jjcarr2 12:081b8fc654af 384 if(dir == BACKWARD) dir=-1;
jjcarr2 12:081b8fc654af 385
jjcarr2 12:081b8fc654af 386 leftEncoder.reset();
jjcarr2 12:081b8fc654af 387 rightEncoder.reset();
jjcarr2 12:081b8fc654af 388 motors.setMotor0Speed(dir*0.2*127); //right
jjcarr2 12:081b8fc654af 389 motors.setMotor1Speed(dir*0.2*127); //left
jjcarr2 12:081b8fc654af 390
jjcarr2 12:081b8fc654af 391 range = 30;
jjcarr2 12:081b8fc654af 392
jjcarr2 12:081b8fc654af 393 while(range > 20) {
jjcarr2 12:081b8fc654af 394 rangeFinderRight.startMeas();
jjcarr2 12:081b8fc654af 395 wait_ms(20);
jjcarr2 12:081b8fc654af 396 rangeFinderRight.getMeas(range);
jjcarr2 12:081b8fc654af 397 }
jjcarr2 12:081b8fc654af 398
jjcarr2 12:081b8fc654af 399 motors.setMotor0Speed(dir*-0.2*127); //right
jjcarr2 12:081b8fc654af 400 motors.setMotor1Speed(dir*-0.2*127); //left
jjcarr2 12:081b8fc654af 401 wait_ms(5);
jjcarr2 12:081b8fc654af 402 motors.stopBothMotors();
jjcarr2 12:081b8fc654af 403 }
Fairy_Paolina 9:f34700716f1d 404
Fairy_Paolina 9:f34700716f1d 405 void overBump(int section)
Fairy_Paolina 9:f34700716f1d 406 {
Fairy_Paolina 9:f34700716f1d 407 int preLeft=5000, preRight=5000, out=0;
jjcarr2 12:081b8fc654af 408
Fairy_Paolina 9:f34700716f1d 409 motors.begin();
jjcarr2 12:081b8fc654af 410
Fairy_Paolina 9:f34700716f1d 411 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 412 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 413 motors.setMotor0Speed(-0.2*127); //right
Fairy_Paolina 9:f34700716f1d 414 motors.setMotor1Speed(-0.2*127); //left
Fairy_Paolina 9:f34700716f1d 415 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
Fairy_Paolina 9:f34700716f1d 416 motors.stopBothMotors();
jjcarr2 12:081b8fc654af 417
Fairy_Paolina 3:58726d2e11f0 418 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 419 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 420 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 9:f34700716f1d 421 motors.setMotor1Speed(0.2*127); //left
jjcarr2 12:081b8fc654af 422 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0) {
jjcarr2 12:081b8fc654af 423 preLeft=leftEncoder.getPulses();
jjcarr2 12:081b8fc654af 424 preRight=rightEncoder.getPulses();
jjcarr2 12:081b8fc654af 425 wait_ms(100);
jjcarr2 12:081b8fc654af 426 //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
jjcarr2 12:081b8fc654af 427 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 8:11ef93eebe07 428 }
jjcarr2 12:081b8fc654af 429
Fairy_Paolina 8:11ef93eebe07 430 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 431 motors.begin();
Fairy_Paolina 9:f34700716f1d 432 wait(2);
Fairy_Paolina 9:f34700716f1d 433 /*
Fairy_Paolina 9:f34700716f1d 434 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 435 motors.setMotor0Speed(0.15*127); //right
Fairy_Paolina 9:f34700716f1d 436 motors.setMotor1Speed(0.15*127); //left
Fairy_Paolina 9:f34700716f1d 437 preLeft=preRight=5000 ;
Fairy_Paolina 9:f34700716f1d 438 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 439 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 440 */
Fairy_Paolina 9:f34700716f1d 441 // while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 9:f34700716f1d 442 /* preLeft=leftEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 443 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 444 bt.printf("second while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 445 wait_ms(200);
Fairy_Paolina 9:f34700716f1d 446 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 9:f34700716f1d 447 }*/
jjcarr2 12:081b8fc654af 448
Fairy_Paolina 8:11ef93eebe07 449 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 450 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 451 motors.setMotor0Speed(0.3*127); //right
Fairy_Paolina 9:f34700716f1d 452 motors.setMotor1Speed(0.3*127); //left
Fairy_Paolina 9:f34700716f1d 453
Fairy_Paolina 9:f34700716f1d 454 while(!out) {
Fairy_Paolina 6:f5c26372b2d0 455 preLeft=leftEncoder.getPulses();
Fairy_Paolina 6:f5c26372b2d0 456 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 457
Fairy_Paolina 9:f34700716f1d 458 rangeFinderLeft.startMeas();
Fairy_Paolina 9:f34700716f1d 459 rangeFinderRight.startMeas();
Fairy_Paolina 9:f34700716f1d 460 wait_ms(20);
Fairy_Paolina 9:f34700716f1d 461 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 462 rangeFinderRight.getMeas(range2);
Fairy_Paolina 9:f34700716f1d 463 if(range < 10 || range2 < 10) out=1;
Fairy_Paolina 9:f34700716f1d 464
Fairy_Paolina 9:f34700716f1d 465 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
Fairy_Paolina 9:f34700716f1d 466 motors.setMotor0Speed(0.4*127); //right
Fairy_Paolina 9:f34700716f1d 467 motors.setMotor1Speed(0.4*127); //left
Fairy_Paolina 9:f34700716f1d 468 }
jjcarr2 12:081b8fc654af 469 if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
Fairy_Paolina 8:11ef93eebe07 470 }
Fairy_Paolina 9:f34700716f1d 471
Fairy_Paolina 8:11ef93eebe07 472 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 473 wait(2);
Fairy_Paolina 9:f34700716f1d 474 motors.begin();
jjcarr2 12:081b8fc654af 475
Fairy_Paolina 8:11ef93eebe07 476 preLeft=preRight=5000 ;
Fairy_Paolina 8:11ef93eebe07 477 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 478 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 479 motors.setMotor0Speed(.25*127); //right
Fairy_Paolina 9:f34700716f1d 480 motors.setMotor1Speed(.25*127); //left
Fairy_Paolina 9:f34700716f1d 481
Fairy_Paolina 9:f34700716f1d 482 if(section == TOOLS || section == MID) {
Fairy_Paolina 8:11ef93eebe07 483 while(IR.getDistance() > 20 ) {
jjcarr2 10:2aa70a504c18 484 //bt.printf("IR %f\r\n", IR.getDistance());
jjcarr2 10:2aa70a504c18 485 //bt.printf("third while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 486 }
Fairy_Paolina 9:f34700716f1d 487 } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
Fairy_Paolina 8:11ef93eebe07 488
Fairy_Paolina 9:f34700716f1d 489 motors.setMotor0Speed(-.25*127); //right
Fairy_Paolina 9:f34700716f1d 490 motors.setMotor1Speed(-.25*127); //left
Fairy_Paolina 9:f34700716f1d 491 wait_ms(10);
Fairy_Paolina 3:58726d2e11f0 492 motors.stopBothMotors();
Fairy_Paolina 8:11ef93eebe07 493 wait(2);
Fairy_Paolina 9:f34700716f1d 494 motors.begin();
Fairy_Paolina 9:f34700716f1d 495
Fairy_Paolina 3:58726d2e11f0 496 }
Fairy_Paolina 9:f34700716f1d 497
Fairy_Paolina 9:f34700716f1d 498 void tools_section(float* location, float &current)
Fairy_Paolina 9:f34700716f1d 499 {
Fairy_Paolina 9:f34700716f1d 500 wall_follow(LEFT,FORWARD, TOOLS);
Fairy_Paolina 6:f5c26372b2d0 501 // current position in reference to the starting position
Fairy_Paolina 4:f2333e66ec2c 502 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 6:f5c26372b2d0 503 bt.printf("current %f \r\n",current);
Fairy_Paolina 9:f34700716f1d 504
jjcarr2 12:081b8fc654af 505
jjcarr2 12:081b8fc654af 506 motors.setMotor0Speed(-.2*127); //right
jjcarr2 12:081b8fc654af 507 motors.setMotor1Speed(-.2*127); //left
jjcarr2 12:081b8fc654af 508 wait_ms(5);
Fairy_Paolina 5:70ccef3734ae 509 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 510
jjcarr2 12:081b8fc654af 511 motors.setMotor0Speed(.2*127); //right
jjcarr2 12:081b8fc654af 512 motors.setMotor1Speed(.2*127); //left
jjcarr2 12:081b8fc654af 513 while(IR.getDistance()>6);
jjcarr2 12:081b8fc654af 514
jjcarr2 12:081b8fc654af 515 motors.setMotor0Speed(-.2*127); //right
jjcarr2 12:081b8fc654af 516 motors.setMotor1Speed(-.2*127); //left
jjcarr2 12:081b8fc654af 517 wait_ms(5);
jjcarr2 12:081b8fc654af 518 motors.stopBothMotors();
jjcarr2 12:081b8fc654af 519
jjcarr2 12:081b8fc654af 520
Fairy_Paolina 6:f5c26372b2d0 521 //Tool aquiring
Fairy_Paolina 6:f5c26372b2d0 522 wait(2);
Fairy_Paolina 6:f5c26372b2d0 523 // After tool is aquired
Fairy_Paolina 9:f34700716f1d 524
Fairy_Paolina 6:f5c26372b2d0 525 alignWithWall(TOOLS);
jjcarr2 12:081b8fc654af 526
Fairy_Paolina 6:f5c26372b2d0 527 wait_ms(100);
jjcarr2 12:081b8fc654af 528
Fairy_Paolina 9:f34700716f1d 529 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 9:f34700716f1d 530 current= 78;
jjcarr2 12:081b8fc654af 531
Fairy_Paolina 6:f5c26372b2d0 532 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 533 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 534 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 535
Fairy_Paolina 9:f34700716f1d 536 if(range < 20) {
Fairy_Paolina 6:f5c26372b2d0 537 wall_follow2(LEFT,BACKWARD,TOOLS, current);
jjcarr2 12:081b8fc654af 538
jjcarr2 11:12ce7600f2f9 539 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 11:12ce7600f2f9 540 current= location[0];
jjcarr2 12:081b8fc654af 541
Fairy_Paolina 9:f34700716f1d 542 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 543 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 544 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 545 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 546 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
jjcarr2 12:081b8fc654af 547
Fairy_Paolina 9:f34700716f1d 548 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 549 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 550 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 551 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 552 while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
Fairy_Paolina 9:f34700716f1d 553 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 554
Fairy_Paolina 9:f34700716f1d 555 wait_ms(500);
Fairy_Paolina 6:f5c26372b2d0 556 leftTurn();
Fairy_Paolina 9:f34700716f1d 557 slightleft();
Fairy_Paolina 8:11ef93eebe07 558 overBump(TOOLS);
Fairy_Paolina 9:f34700716f1d 559 } else {
Fairy_Paolina 9:f34700716f1d 560 location[0]= 77;
Fairy_Paolina 6:f5c26372b2d0 561 leftTurn();
Fairy_Paolina 6:f5c26372b2d0 562 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 563 overBump(FIRST_WAVE);
Fairy_Paolina 9:f34700716f1d 564 }
Fairy_Paolina 9:f34700716f1d 565
jjcarr2 10:2aa70a504c18 566 bt.printf("First Wavegap = %f\r\n",location[0]);
Fairy_Paolina 6:f5c26372b2d0 567 }
Fairy_Paolina 9:f34700716f1d 568
Fairy_Paolina 9:f34700716f1d 569 void mid_section(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 570 {
Fairy_Paolina 9:f34700716f1d 571
Fairy_Paolina 9:f34700716f1d 572 motors.begin();
jjcarr2 12:081b8fc654af 573
jjcarr2 12:081b8fc654af 574 if(IR.getDistance() > 20) {
jjcarr2 12:081b8fc654af 575 direction[0]= STRAIGHT;
jjcarr2 12:081b8fc654af 576 return;
jjcarr2 12:081b8fc654af 577 }
jjcarr2 12:081b8fc654af 578
Fairy_Paolina 6:f5c26372b2d0 579 alignWithWall(MID);
Fairy_Paolina 8:11ef93eebe07 580
Fairy_Paolina 6:f5c26372b2d0 581 bt.printf("mid section current = %f\r\n",current);
Fairy_Paolina 6:f5c26372b2d0 582 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 583 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 584 bt.printf("after wf2 current = %f\r\n",current);
Fairy_Paolina 9:f34700716f1d 585
Fairy_Paolina 9:f34700716f1d 586 if(current != 0) {
Fairy_Paolina 6:f5c26372b2d0 587 direction[0]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 588 current+= location[0];
Fairy_Paolina 6:f5c26372b2d0 589 location[1]= current;
Fairy_Paolina 9:f34700716f1d 590 } else {
Fairy_Paolina 9:f34700716f1d 591 current=location[0];
Fairy_Paolina 6:f5c26372b2d0 592 direction[0]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 593 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 594 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 595 }
Fairy_Paolina 9:f34700716f1d 596
Fairy_Paolina 6:f5c26372b2d0 597 bt.printf("wavegap2 = %f\r\n",location[1]);
Fairy_Paolina 9:f34700716f1d 598 leftTurn();
Fairy_Paolina 8:11ef93eebe07 599 overBump(TOOLS);
Fairy_Paolina 6:f5c26372b2d0 600 // go forward
Fairy_Paolina 6:f5c26372b2d0 601 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 602 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 603 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 6:f5c26372b2d0 604 motors.setMotor1Speed(0.2*127); //left
Fairy_Paolina 6:f5c26372b2d0 605 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
Fairy_Paolina 5:70ccef3734ae 606 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 607
Fairy_Paolina 6:f5c26372b2d0 608 }
Fairy_Paolina 9:f34700716f1d 609
Fairy_Paolina 9:f34700716f1d 610 void mid_section2(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 611 {
Fairy_Paolina 9:f34700716f1d 612
Fairy_Paolina 9:f34700716f1d 613 motors.begin();
jjcarr2 12:081b8fc654af 614
jjcarr2 12:081b8fc654af 615 if(IR.getDistance() > 20) {
jjcarr2 12:081b8fc654af 616 direction[0]= STRAIGHT;
jjcarr2 12:081b8fc654af 617 return;
jjcarr2 12:081b8fc654af 618 }
jjcarr2 12:081b8fc654af 619
Fairy_Paolina 6:f5c26372b2d0 620 alignWithWall(MID);
Fairy_Paolina 6:f5c26372b2d0 621 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 622 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 9:f34700716f1d 623
Fairy_Paolina 9:f34700716f1d 624 if(current != 0) {
Fairy_Paolina 6:f5c26372b2d0 625 direction[1]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 626 current+= location[1];
Fairy_Paolina 6:f5c26372b2d0 627 location[2]= current;
Fairy_Paolina 9:f34700716f1d 628 } else {
Fairy_Paolina 9:f34700716f1d 629 current=location[1];
Fairy_Paolina 6:f5c26372b2d0 630 direction[1]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 631 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 632 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 633 }
Fairy_Paolina 9:f34700716f1d 634
Fairy_Paolina 9:f34700716f1d 635 leftTurn();
Fairy_Paolina 8:11ef93eebe07 636 overBump(RIGS);
Fairy_Paolina 9:f34700716f1d 637 }
Fairy_Paolina 9:f34700716f1d 638
Fairy_Paolina 9:f34700716f1d 639 void rig_section(float* location, float &current, int* direction, int rig)
Fairy_Paolina 9:f34700716f1d 640 {
jjcarr2 12:081b8fc654af 641 float loc;
jjcarr2 12:081b8fc654af 642
jjcarr2 12:081b8fc654af 643 if(rig == 1) loc= 16;
jjcarr2 12:081b8fc654af 644 else if(rig == 2) loc= 37;
jjcarr2 12:081b8fc654af 645 else loc = 58;
jjcarr2 12:081b8fc654af 646
jjcarr2 12:081b8fc654af 647 rightTurn();
jjcarr2 12:081b8fc654af 648
jjcarr2 12:081b8fc654af 649
jjcarr2 12:081b8fc654af 650 if(current > loc) {
jjcarr2 12:081b8fc654af 651 UntilWall(BACKWARD);
jjcarr2 12:081b8fc654af 652 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 653 wall_follow2(RIGHT, BACKWARD, RIGS, current);
jjcarr2 12:081b8fc654af 654 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 655
jjcarr2 12:081b8fc654af 656 if((current- loc)>10) {
jjcarr2 12:081b8fc654af 657 wall_follow2(RIGHT, BACKWARD, RIGS, current);
jjcarr2 12:081b8fc654af 658 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 659 }
jjcarr2 12:081b8fc654af 660 } else {
jjcarr2 12:081b8fc654af 661 UntilWall(FORWARD);
jjcarr2 12:081b8fc654af 662 wall_follow2(RIGHT, FORWARD, RIGS, current);
jjcarr2 12:081b8fc654af 663 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 664
jjcarr2 12:081b8fc654af 665 if((current- loc)<-10) {
jjcarr2 12:081b8fc654af 666 wall_follow2(RIGHT, FORWARD, RIGS, current);
jjcarr2 12:081b8fc654af 667 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 668 }
jjcarr2 12:081b8fc654af 669 }
Fairy_Paolina 9:f34700716f1d 670
Fairy_Paolina 9:f34700716f1d 671
jjcarr2 12:081b8fc654af 672 }
jjcarr2 12:081b8fc654af 673
jjcarr2 12:081b8fc654af 674 void tools_section_return(float* location, float &current)
jjcarr2 12:081b8fc654af 675 {
jjcarr2 12:081b8fc654af 676 if(location[0] > 16) {
jjcarr2 12:081b8fc654af 677 rightTurn();
jjcarr2 12:081b8fc654af 678 leftEncoder.reset();
jjcarr2 12:081b8fc654af 679 rightEncoder.reset();
jjcarr2 12:081b8fc654af 680 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 12:081b8fc654af 681 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 12:081b8fc654af 682 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 12:081b8fc654af 683 wall_follow(RIGHT, FORWARD, RETURN);
jjcarr2 12:081b8fc654af 684 }
jjcarr2 12:081b8fc654af 685 motors.stopBothMotors();
jjcarr2 12:081b8fc654af 686
jjcarr2 12:081b8fc654af 687 }
jjcarr2 12:081b8fc654af 688
jjcarr2 12:081b8fc654af 689 void mid_section_return(float* location, float &current, int* direction)
jjcarr2 12:081b8fc654af 690 {
jjcarr2 12:081b8fc654af 691 if(direction[1] == RIGHT) {
jjcarr2 12:081b8fc654af 692 rightTurn();
jjcarr2 12:081b8fc654af 693 alignWithWall(MID);
jjcarr2 12:081b8fc654af 694 wall_follow2(LEFT, FORWARD, MID, current);
jjcarr2 12:081b8fc654af 695 leftTurn();
jjcarr2 12:081b8fc654af 696 } else if(direction[1] == LEFT) {
jjcarr2 12:081b8fc654af 697 leftTurn();
jjcarr2 12:081b8fc654af 698 wall_follow2(RIGHT, FORWARD, MID, current);
jjcarr2 12:081b8fc654af 699 rightTurn();
jjcarr2 12:081b8fc654af 700 } else {
jjcarr2 12:081b8fc654af 701 //GO FORWARD
jjcarr2 12:081b8fc654af 702 }
jjcarr2 12:081b8fc654af 703 overBump(RIGS);
jjcarr2 12:081b8fc654af 704 }
jjcarr2 12:081b8fc654af 705
jjcarr2 12:081b8fc654af 706 void mid_section2_return(float* location, float &current, int* direction)
jjcarr2 12:081b8fc654af 707 {
jjcarr2 12:081b8fc654af 708 if(direction[2] == RIGHT) {
jjcarr2 12:081b8fc654af 709 rightTurn();
jjcarr2 12:081b8fc654af 710 wall_follow2(LEFT, FORWARD, MID, current);
jjcarr2 12:081b8fc654af 711 leftTurn();
jjcarr2 12:081b8fc654af 712 } else if(direction[2] == LEFT) {
jjcarr2 12:081b8fc654af 713 leftTurn();
jjcarr2 12:081b8fc654af 714 wall_follow2(RIGHT, FORWARD, MID, current);
jjcarr2 12:081b8fc654af 715 rightTurn();
jjcarr2 12:081b8fc654af 716 } else {
jjcarr2 12:081b8fc654af 717 //GO FORWARD
jjcarr2 12:081b8fc654af 718 }
jjcarr2 12:081b8fc654af 719 overBump(MID);
jjcarr2 12:081b8fc654af 720 }
jjcarr2 12:081b8fc654af 721
jjcarr2 12:081b8fc654af 722 void rig_section_return(float* location, float &current, int* direction, int rig)
jjcarr2 12:081b8fc654af 723 {
jjcarr2 12:081b8fc654af 724 if(location[2] > current) {
jjcarr2 12:081b8fc654af 725 wall_follow2(RIGHT, FORWARD, RIGS, current);
jjcarr2 12:081b8fc654af 726 } else {
jjcarr2 12:081b8fc654af 727 wall_follow2(RIGHT, BACKWARD, RIGS, current);
jjcarr2 12:081b8fc654af 728 }
jjcarr2 12:081b8fc654af 729 rightTurn();
jjcarr2 12:081b8fc654af 730 overBump(MID);
Fairy_Paolina 8:11ef93eebe07 731 }