Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@11:12ce7600f2f9, 2014-03-27 (annotated)

Committer:
jjcarr2
Date:
Thu Mar 27 21:09:55 2014 +0000
Revision:
11:12ce7600f2f9
Parent:
10:2aa70a504c18
Child:
12:081b8fc654af
yeah;

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