Tools Section Navigation Calibrated slightly
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos
Fork of NavigationTest by
main.cpp@7:0b7897232e93, 2014-03-21 (annotated)
- Committer:
- jjcarr2
- Date:
- Fri Mar 21 00:02:51 2014 +0000
- Revision:
- 7:0b7897232e93
- Parent:
- 6:109f46d3cb96
- Child:
- 8:32ba0fad1689
Tools Section Calibrated further
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:0351277ee5dd | 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 | 2:0351277ee5dd | 23 | #define BACKWARD (0) |
jjcarr2 | 2:0351277ee5dd | 24 | #define TOOLS (0) |
jjcarr2 | 2:0351277ee5dd | 25 | #define MID (1) |
jjcarr2 | 2:0351277ee5dd | 26 | #define RIGS (2) |
jjcarr2 | 2:0351277ee5dd | 27 | |
Fairy_Paolina | 0:ff94cc47fef7 | 28 | float range, pid_return; |
Fairy_Paolina | 0:ff94cc47fef7 | 29 | void errFunction(void); |
Fairy_Paolina | 0:ff94cc47fef7 | 30 | bool cRc; |
jjcarr2 | 2:0351277ee5dd | 31 | |
Fairy_Paolina | 0:ff94cc47fef7 | 32 | //Hardware Initialization |
Fairy_Paolina | 0:ff94cc47fef7 | 33 | Serial bt(p13,p14); |
Fairy_Paolina | 0:ff94cc47fef7 | 34 | Serial pc(USBTX,USBRX); |
jjcarr2 | 2:0351277ee5dd | 35 | HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL ); |
jjcarr2 | 2:0351277ee5dd | 36 | HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); |
jjcarr2 | 7:0b7897232e93 | 37 | PID pid1(15.0,0.0,10.0,0.02); |
Fairy_Paolina | 0:ff94cc47fef7 | 38 | PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); |
jjcarr2 | 2:0351277ee5dd | 39 | QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING); |
jjcarr2 | 2:0351277ee5dd | 40 | QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); |
Fairy_Paolina | 1:801f0b9a862a | 41 | //InterruptIn encoder(p29); |
jjcarr2 | 2:0351277ee5dd | 42 | |
jjcarr2 | 2:0351277ee5dd | 43 | |
Fairy_Paolina | 0:ff94cc47fef7 | 44 | //Functions |
jjcarr2 | 2:0351277ee5dd | 45 | |
jjcarr2 | 2:0351277ee5dd | 46 | float wall_follow(int side, int direction, int section); |
jjcarr2 | 2:0351277ee5dd | 47 | void wall_follow2(int side, int direction, int section); |
Fairy_Paolina | 0:ff94cc47fef7 | 48 | void wall_follow3(int ¤tLocation, int &WaveOpening); |
Fairy_Paolina | 1:801f0b9a862a | 49 | void leftTurn(void); |
Fairy_Paolina | 1:801f0b9a862a | 50 | void rightTurn(void); |
Fairy_Paolina | 0:ff94cc47fef7 | 51 | void us_distance(void); |
jjcarr2 | 2:0351277ee5dd | 52 | void tools_section(float *location, float ¤t); |
jjcarr2 | 2:0351277ee5dd | 53 | void overBump(void); |
jjcarr2 | 7:0b7897232e93 | 54 | void alignWithWall(void); |
jjcarr2 | 2:0351277ee5dd | 55 | |
Fairy_Paolina | 1:801f0b9a862a | 56 | //Variables |
jjcarr2 | 2:0351277ee5dd | 57 | |
jjcarr2 | 2:0351277ee5dd | 58 | int main(void) |
jjcarr2 | 2:0351277ee5dd | 59 | { |
jjcarr2 | 2:0351277ee5dd | 60 | float location[3], current=0; |
jjcarr2 | 2:0351277ee5dd | 61 | int direction[3]; |
jjcarr2 | 2:0351277ee5dd | 62 | |
Fairy_Paolina | 0:ff94cc47fef7 | 63 | pc.baud(115200); |
Fairy_Paolina | 1:801f0b9a862a | 64 | bt.baud(115200); |
jjcarr2 | 2:0351277ee5dd | 65 | motors.begin(); |
jjcarr2 | 6:109f46d3cb96 | 66 | |
jjcarr2 | 7:0b7897232e93 | 67 | /*****************************OLD CODE FOR GOING THROUGH WAVE SECTION****************/ |
jjcarr2 | 7:0b7897232e93 | 68 | /* |
jjcarr2 | 7:0b7897232e93 | 69 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 70 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 71 | wall_follow2(LEFT,FORWARD,TOOLS); |
jjcarr2 | 7:0b7897232e93 | 72 | |
jjcarr2 | 7:0b7897232e93 | 73 | motors.stopBothMotors(); |
jjcarr2 | 7:0b7897232e93 | 74 | wait_ms(500); |
jjcarr2 | 7:0b7897232e93 | 75 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 76 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 77 | |
jjcarr2 | 7:0b7897232e93 | 78 | motors.setMotor0Speed(0.6*MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 79 | motors.setMotor1Speed(0.6*MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 80 | |
jjcarr2 | 7:0b7897232e93 | 81 | wait_ms(200); |
jjcarr2 | 7:0b7897232e93 | 82 | motors.stopBothMotors(); |
jjcarr2 | 7:0b7897232e93 | 83 | |
jjcarr2 | 7:0b7897232e93 | 84 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 85 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 86 | bt.printf("1st turn left\r\n"); |
jjcarr2 | 7:0b7897232e93 | 87 | |
jjcarr2 | 7:0b7897232e93 | 88 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 89 | motors.setMotor1Speed(-MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 90 | |
jjcarr2 | 7:0b7897232e93 | 91 | while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2) < 1); |
jjcarr2 | 7:0b7897232e93 | 92 | |
jjcarr2 | 7:0b7897232e93 | 93 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 94 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 95 | |
jjcarr2 | 7:0b7897232e93 | 96 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 97 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 98 | |
jjcarr2 | 7:0b7897232e93 | 99 | while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/PPR)/2) < 2); |
jjcarr2 | 7:0b7897232e93 | 100 | |
jjcarr2 | 7:0b7897232e93 | 101 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 102 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 103 | motors.setMotor0Speed(-MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 104 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 105 | float usValue = 0; |
jjcarr2 | 7:0b7897232e93 | 106 | while(1){ |
jjcarr2 | 7:0b7897232e93 | 107 | rangeFinderLeft.startMeas(); |
jjcarr2 | 7:0b7897232e93 | 108 | wait_ms(100); |
jjcarr2 | 7:0b7897232e93 | 109 | rangeFinderLeft.getMeas(range); |
jjcarr2 | 7:0b7897232e93 | 110 | bt.printf("Range %f \t OldValue %f\n\r",range, usValue); |
jjcarr2 | 7:0b7897232e93 | 111 | if(range > usValue && usValue != 0 && range < 10){ |
jjcarr2 | 7:0b7897232e93 | 112 | break; |
jjcarr2 | 7:0b7897232e93 | 113 | } else { |
jjcarr2 | 7:0b7897232e93 | 114 | usValue = range; |
jjcarr2 | 7:0b7897232e93 | 115 | } |
jjcarr2 | 7:0b7897232e93 | 116 | } |
jjcarr2 | 7:0b7897232e93 | 117 | motors.stopBothMotors(); |
jjcarr2 | 7:0b7897232e93 | 118 | |
jjcarr2 | 7:0b7897232e93 | 119 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 120 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 121 | |
jjcarr2 | 7:0b7897232e93 | 122 | wall_follow2(LEFT,FORWARD,TOOLS); |
jjcarr2 | 7:0b7897232e93 | 123 | |
jjcarr2 | 7:0b7897232e93 | 124 | while(1){} |
jjcarr2 | 7:0b7897232e93 | 125 | */ |
jjcarr2 | 7:0b7897232e93 | 126 | /* |
jjcarr2 | 2:0351277ee5dd | 127 | // Very Consistent Turn |
jjcarr2 | 2:0351277ee5dd | 128 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 129 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 130 | |
jjcarr2 | 2:0351277ee5dd | 131 | motors.setMotor0Speed(-MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 132 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 133 | float usValue = 0; |
jjcarr2 | 2:0351277ee5dd | 134 | while(1){ |
jjcarr2 | 2:0351277ee5dd | 135 | rangeFinderLeft.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 136 | wait_ms(100); |
jjcarr2 | 2:0351277ee5dd | 137 | rangeFinderLeft.getMeas(range); |
jjcarr2 | 2:0351277ee5dd | 138 | bt.printf("Range %f \t OldValue %f\n\r",range, usValue); |
jjcarr2 | 2:0351277ee5dd | 139 | if(range > usValue && usValue != 0 && range < 10){ |
jjcarr2 | 2:0351277ee5dd | 140 | break; |
jjcarr2 | 2:0351277ee5dd | 141 | } else { |
jjcarr2 | 2:0351277ee5dd | 142 | usValue = range; |
jjcarr2 | 2:0351277ee5dd | 143 | } |
jjcarr2 | 2:0351277ee5dd | 144 | } |
jjcarr2 | 7:0b7897232e93 | 145 | */ |
jjcarr2 | 2:0351277ee5dd | 146 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 147 | /* |
jjcarr2 | 2:0351277ee5dd | 148 | |
jjcarr2 | 2:0351277ee5dd | 149 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 150 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 151 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 152 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 153 | |
jjcarr2 | 2:0351277ee5dd | 154 | while(leftEncoder.getPulses()/(PPR) < 3); |
Fairy_Paolina | 1:801f0b9a862a | 155 | */ |
jjcarr2 | 2:0351277ee5dd | 156 | |
jjcarr2 | 2:0351277ee5dd | 157 | //leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 158 | //rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 159 | //motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 160 | //motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 161 | |
jjcarr2 | 2:0351277ee5dd | 162 | //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3) |
jjcarr2 | 2:0351277ee5dd | 163 | |
jjcarr2 | 2:0351277ee5dd | 164 | |
jjcarr2 | 2:0351277ee5dd | 165 | |
jjcarr2 | 2:0351277ee5dd | 166 | //Go to tools |
jjcarr2 | 2:0351277ee5dd | 167 | tools_section(location, current); |
jjcarr2 | 2:0351277ee5dd | 168 | bt.printf("Location 0 = %f", location[0]); |
Fairy_Paolina | 0:ff94cc47fef7 | 169 | |
jjcarr2 | 2:0351277ee5dd | 170 | /* |
jjcarr2 | 2:0351277ee5dd | 171 | //////////////////////////////// without predefined wavegaps////////////////////////////////////////////// |
jjcarr2 | 2:0351277ee5dd | 172 | current=0; |
jjcarr2 | 2:0351277ee5dd | 173 | if(location[0]< 75){ |
jjcarr2 | 2:0351277ee5dd | 174 | turnRight(); |
jjcarr2 | 2:0351277ee5dd | 175 | current=wall_follow(LEFT,FORWARD); |
jjcarr2 | 2:0351277ee5dd | 176 | if(current == 0)turnLeft(); |
jjcarr2 | 2:0351277ee5dd | 177 | else{ |
jjcarr2 | 2:0351277ee5dd | 178 | direction[0]= RIGHT; |
jjcarr2 | 2:0351277ee5dd | 179 | turnLeft(); |
jjcarr2 | 2:0351277ee5dd | 180 | overBump(); |
jjcarr2 | 2:0351277ee5dd | 181 | } |
jjcarr2 | 2:0351277ee5dd | 182 | } |
jjcarr2 | 2:0351277ee5dd | 183 | else if(location[0]>=75 || current == 0){ |
jjcarr2 | 2:0351277ee5dd | 184 | turnLeft(); |
jjcarr2 | 2:0351277ee5dd | 185 | wall_follow2(RIGHT,FORWARD); |
jjcarr2 | 2:0351277ee5dd | 186 | } |
jjcarr2 | 2:0351277ee5dd | 187 | |
jjcarr2 | 2:0351277ee5dd | 188 | |
jjcarr2 | 2:0351277ee5dd | 189 | |
jjcarr2 | 2:0351277ee5dd | 190 | |
jjcarr2 | 2:0351277ee5dd | 191 | |
jjcarr2 | 2:0351277ee5dd | 192 | |
jjcarr2 | 2:0351277ee5dd | 193 | // left or right |
jjcarr2 | 2:0351277ee5dd | 194 | |
jjcarr2 | 2:0351277ee5dd | 195 | |
jjcarr2 | 2:0351277ee5dd | 196 | location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; |
jjcarr2 | 2:0351277ee5dd | 197 | |
jjcarr2 | 2:0351277ee5dd | 198 | |
jjcarr2 | 2:0351277ee5dd | 199 | |
jjcarr2 | 2:0351277ee5dd | 200 | |
jjcarr2 | 2:0351277ee5dd | 201 | |
jjcarr2 | 2:0351277ee5dd | 202 | |
jjcarr2 | 2:0351277ee5dd | 203 | |
Fairy_Paolina | 1:801f0b9a862a | 204 | leftTurn(); |
jjcarr2 | 2:0351277ee5dd | 205 | //wall_follow2(RIGHT); |
Fairy_Paolina | 1:801f0b9a862a | 206 | rightTurn(); |
Fairy_Paolina | 0:ff94cc47fef7 | 207 | |
jjcarr2 | 2:0351277ee5dd | 208 | |
jjcarr2 | 2:0351277ee5dd | 209 | |
jjcarr2 | 2:0351277ee5dd | 210 | bt.printf("LOCATION %f\n\r",location); |
jjcarr2 | 2:0351277ee5dd | 211 | |
jjcarr2 | 2:0351277ee5dd | 212 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 213 | // leftTurn(); |
jjcarr2 | 2:0351277ee5dd | 214 | // wait(1); |
jjcarr2 | 2:0351277ee5dd | 215 | // rightTurn(); |
jjcarr2 | 2:0351277ee5dd | 216 | */ |
jjcarr2 | 2:0351277ee5dd | 217 | |
Fairy_Paolina | 0:ff94cc47fef7 | 218 | } |
jjcarr2 | 2:0351277ee5dd | 219 | |
jjcarr2 | 2:0351277ee5dd | 220 | void errFunction(void) |
jjcarr2 | 2:0351277ee5dd | 221 | { |
jjcarr2 | 2:0351277ee5dd | 222 | //Nothing |
Fairy_Paolina | 0:ff94cc47fef7 | 223 | } |
jjcarr2 | 2:0351277ee5dd | 224 | |
Fairy_Paolina | 0:ff94cc47fef7 | 225 | void us_distance(void) |
Fairy_Paolina | 0:ff94cc47fef7 | 226 | { |
jjcarr2 | 7:0b7897232e93 | 227 | |
jjcarr2 | 2:0351277ee5dd | 228 | rangeFinderLeft.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 229 | wait_us(20); |
jjcarr2 | 2:0351277ee5dd | 230 | if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) { |
jjcarr2 | 2:0351277ee5dd | 231 | pc.printf("Range = %f\n\r", range); |
jjcarr2 | 2:0351277ee5dd | 232 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 233 | } |
jjcarr2 | 2:0351277ee5dd | 234 | |
jjcarr2 | 2:0351277ee5dd | 235 | float wall_follow(int side, int direction, int section) |
Fairy_Paolina | 0:ff94cc47fef7 | 236 | { |
jjcarr2 | 2:0351277ee5dd | 237 | float location, wavegap; |
jjcarr2 | 2:0351277ee5dd | 238 | int dir=1, set=5; |
jjcarr2 | 2:0351277ee5dd | 239 | |
jjcarr2 | 2:0351277ee5dd | 240 | pid1.reset(); |
jjcarr2 | 2:0351277ee5dd | 241 | |
jjcarr2 | 2:0351277ee5dd | 242 | if(direction == BACKWARD) dir=-1; |
jjcarr2 | 7:0b7897232e93 | 243 | if(section == TOOLS)set= 10; |
jjcarr2 | 2:0351277ee5dd | 244 | |
jjcarr2 | 2:0351277ee5dd | 245 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 246 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 247 | |
jjcarr2 | 2:0351277ee5dd | 248 | location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; |
jjcarr2 | 2:0351277ee5dd | 249 | |
jjcarr2 | 2:0351277ee5dd | 250 | while(location< 75) { |
jjcarr2 | 2:0351277ee5dd | 251 | location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; |
jjcarr2 | 2:0351277ee5dd | 252 | |
jjcarr2 | 2:0351277ee5dd | 253 | pid1.setInputLimits(0, set); |
jjcarr2 | 2:0351277ee5dd | 254 | pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); |
jjcarr2 | 2:0351277ee5dd | 255 | pid1.setSetPoint(set); |
jjcarr2 | 2:0351277ee5dd | 256 | if(side){ //Side = 1 for Left |
jjcarr2 | 2:0351277ee5dd | 257 | rangeFinderLeft.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 258 | wait_ms(20); |
jjcarr2 | 2:0351277ee5dd | 259 | rangeFinderLeft.getMeas(range); |
jjcarr2 | 2:0351277ee5dd | 260 | } |
jjcarr2 | 2:0351277ee5dd | 261 | else{ //Use right Ultrasonic |
jjcarr2 | 2:0351277ee5dd | 262 | rangeFinderRight.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 263 | wait_ms(20); |
jjcarr2 | 2:0351277ee5dd | 264 | rangeFinderRight.getMeas(range); |
jjcarr2 | 2:0351277ee5dd | 265 | pc.printf("%d\r\n",range); |
jjcarr2 | 2:0351277ee5dd | 266 | } |
jjcarr2 | 2:0351277ee5dd | 267 | |
jjcarr2 | 2:0351277ee5dd | 268 | if(range > 20) { |
jjcarr2 | 2:0351277ee5dd | 269 | wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; |
jjcarr2 | 2:0351277ee5dd | 270 | bt.printf("wavegap %f\r\n",wavegap); |
jjcarr2 | 2:0351277ee5dd | 271 | // AT WAVE OPENING!!!! |
jjcarr2 | 7:0b7897232e93 | 272 | motors.setMotor1Speed(dir*MAX_SPEED);//left |
jjcarr2 | 2:0351277ee5dd | 273 | motors.setMotor0Speed(dir*MAX_SPEED);//right |
jjcarr2 | 7:0b7897232e93 | 274 | |
jjcarr2 | 2:0351277ee5dd | 275 | } else { |
Fairy_Paolina | 0:ff94cc47fef7 | 276 | |
jjcarr2 | 7:0b7897232e93 | 277 | |
jjcarr2 | 2:0351277ee5dd | 278 | pid1.setProcessValue(range); |
jjcarr2 | 2:0351277ee5dd | 279 | pid_return = pid1.compute(); |
jjcarr2 | 2:0351277ee5dd | 280 | |
jjcarr2 | 2:0351277ee5dd | 281 | if(pid_return > 0) { |
jjcarr2 | 2:0351277ee5dd | 282 | if(side){ |
jjcarr2 | 2:0351277ee5dd | 283 | motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right |
jjcarr2 | 2:0351277ee5dd | 284 | motors.setMotor1Speed(dir*MAX_SPEED);//left |
jjcarr2 | 2:0351277ee5dd | 285 | } |
jjcarr2 | 2:0351277ee5dd | 286 | else{ |
jjcarr2 | 2:0351277ee5dd | 287 | motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left |
jjcarr2 | 2:0351277ee5dd | 288 | motors.setMotor0Speed(dir*MAX_SPEED);//right |
jjcarr2 | 2:0351277ee5dd | 289 | } |
jjcarr2 | 2:0351277ee5dd | 290 | }else if(pid_return < 0) { |
jjcarr2 | 2:0351277ee5dd | 291 | if(side){ |
jjcarr2 | 2:0351277ee5dd | 292 | motors.setMotor0Speed(dir*MAX_SPEED);//right |
jjcarr2 | 2:0351277ee5dd | 293 | motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left |
jjcarr2 | 2:0351277ee5dd | 294 | } |
jjcarr2 | 2:0351277ee5dd | 295 | else{ |
jjcarr2 | 2:0351277ee5dd | 296 | motors.setMotor1Speed(dir*MAX_SPEED);//left |
jjcarr2 | 2:0351277ee5dd | 297 | motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right |
jjcarr2 | 2:0351277ee5dd | 298 | } |
jjcarr2 | 2:0351277ee5dd | 299 | }else { |
jjcarr2 | 2:0351277ee5dd | 300 | motors.setMotor0Speed(dir*MAX_SPEED);//right |
jjcarr2 | 2:0351277ee5dd | 301 | motors.setMotor1Speed(dir*MAX_SPEED);//left |
jjcarr2 | 2:0351277ee5dd | 302 | } |
jjcarr2 | 7:0b7897232e93 | 303 | } |
jjcarr2 | 7:0b7897232e93 | 304 | } |
jjcarr2 | 2:0351277ee5dd | 305 | return wavegap; |
jjcarr2 | 2:0351277ee5dd | 306 | } |
jjcarr2 | 2:0351277ee5dd | 307 | |
jjcarr2 | 2:0351277ee5dd | 308 | /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ |
jjcarr2 | 2:0351277ee5dd | 309 | |
jjcarr2 | 2:0351277ee5dd | 310 | void wall_follow2(int side, int direction, int section) |
jjcarr2 | 2:0351277ee5dd | 311 | { |
jjcarr2 | 7:0b7897232e93 | 312 | int SeeWaveGap = false, count=0, set=5; |
jjcarr2 | 7:0b7897232e93 | 313 | float dir=1.0; |
jjcarr2 | 2:0351277ee5dd | 314 | |
jjcarr2 | 2:0351277ee5dd | 315 | pid1.reset(); |
jjcarr2 | 2:0351277ee5dd | 316 | |
jjcarr2 | 2:0351277ee5dd | 317 | if(direction == BACKWARD) dir=-1; |
jjcarr2 | 2:0351277ee5dd | 318 | if(section == TOOLS)set= 5; |
jjcarr2 | 2:0351277ee5dd | 319 | |
jjcarr2 | 2:0351277ee5dd | 320 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 321 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 322 | |
jjcarr2 | 2:0351277ee5dd | 323 | while(1) { |
jjcarr2 | 2:0351277ee5dd | 324 | |
jjcarr2 | 2:0351277ee5dd | 325 | pid1.setInputLimits(0.0, set); |
Fairy_Paolina | 1:801f0b9a862a | 326 | pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); |
jjcarr2 | 2:0351277ee5dd | 327 | pid1.setSetPoint(set); |
jjcarr2 | 2:0351277ee5dd | 328 | |
jjcarr2 | 2:0351277ee5dd | 329 | if(side){ |
jjcarr2 | 2:0351277ee5dd | 330 | rangeFinderLeft.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 331 | wait_ms(20); |
jjcarr2 | 2:0351277ee5dd | 332 | rangeFinderLeft.getMeas(range); |
jjcarr2 | 2:0351277ee5dd | 333 | } |
jjcarr2 | 2:0351277ee5dd | 334 | else{ |
jjcarr2 | 2:0351277ee5dd | 335 | rangeFinderRight.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 336 | wait_ms(20); |
jjcarr2 | 2:0351277ee5dd | 337 | rangeFinderRight.getMeas(range); |
jjcarr2 | 2:0351277ee5dd | 338 | } |
jjcarr2 | 2:0351277ee5dd | 339 | |
jjcarr2 | 2:0351277ee5dd | 340 | |
jjcarr2 | 2:0351277ee5dd | 341 | /*************CHECK FOR WAVE OPENING*****************/ |
jjcarr2 | 2:0351277ee5dd | 342 | /* If after 20 ms the ultrasonic still sees 20+ cm */ |
jjcarr2 | 2:0351277ee5dd | 343 | /* then robot is at wave opening */ |
jjcarr2 | 2:0351277ee5dd | 344 | |
jjcarr2 | 2:0351277ee5dd | 345 | pc.printf("range %f\r\n",range); |
jjcarr2 | 2:0351277ee5dd | 346 | if(range > 20) { |
jjcarr2 | 2:0351277ee5dd | 347 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 348 | bt.printf("wavegap\r\n"); |
jjcarr2 | 2:0351277ee5dd | 349 | // AT WAVE OPENING!!!! |
jjcarr2 | 2:0351277ee5dd | 350 | break; |
jjcarr2 | 2:0351277ee5dd | 351 | } |
jjcarr2 | 2:0351277ee5dd | 352 | |
Fairy_Paolina | 0:ff94cc47fef7 | 353 | pid1.setProcessValue(range); |
jjcarr2 | 2:0351277ee5dd | 354 | pid_return = pid1.compute(); |
jjcarr2 | 2:0351277ee5dd | 355 | //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return); |
jjcarr2 | 2:0351277ee5dd | 356 | |
jjcarr2 | 2:0351277ee5dd | 357 | if(pid_return > 0) { |
jjcarr2 | 2:0351277ee5dd | 358 | if(side){ |
jjcarr2 | 2:0351277ee5dd | 359 | motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right |
jjcarr2 | 2:0351277ee5dd | 360 | motors.setMotor1Speed(dir*MAX_SPEED);//left |
jjcarr2 | 2:0351277ee5dd | 361 | } |
jjcarr2 | 2:0351277ee5dd | 362 | else{ |
jjcarr2 | 2:0351277ee5dd | 363 | motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left |
jjcarr2 | 2:0351277ee5dd | 364 | motors.setMotor0Speed(dir*MAX_SPEED);//right |
jjcarr2 | 2:0351277ee5dd | 365 | } |
jjcarr2 | 2:0351277ee5dd | 366 | }else if(pid_return < 0) { |
jjcarr2 | 2:0351277ee5dd | 367 | if(side){ |
jjcarr2 | 2:0351277ee5dd | 368 | motors.setMotor0Speed(dir*MAX_SPEED);//right |
jjcarr2 | 2:0351277ee5dd | 369 | motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left |
jjcarr2 | 2:0351277ee5dd | 370 | } |
jjcarr2 | 2:0351277ee5dd | 371 | else{ |
jjcarr2 | 2:0351277ee5dd | 372 | motors.setMotor1Speed(dir*MAX_SPEED);//left |
jjcarr2 | 2:0351277ee5dd | 373 | motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right |
jjcarr2 | 2:0351277ee5dd | 374 | } |
jjcarr2 | 2:0351277ee5dd | 375 | } else { |
jjcarr2 | 2:0351277ee5dd | 376 | motors.setMotor0Speed(dir*MAX_SPEED); |
jjcarr2 | 2:0351277ee5dd | 377 | motors.setMotor1Speed(dir*MAX_SPEED); |
jjcarr2 | 2:0351277ee5dd | 378 | } |
jjcarr2 | 2:0351277ee5dd | 379 | } |
jjcarr2 | 2:0351277ee5dd | 380 | } |
jjcarr2 | 2:0351277ee5dd | 381 | |
jjcarr2 | 2:0351277ee5dd | 382 | |
jjcarr2 | 2:0351277ee5dd | 383 | /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */ |
jjcarr2 | 2:0351277ee5dd | 384 | /* MEANT FOR RETURNING FROM OIL RIGS */ |
jjcarr2 | 2:0351277ee5dd | 385 | |
jjcarr2 | 2:0351277ee5dd | 386 | void wall_follow3(int ¤tLocation, int &WaveOpening) |
jjcarr2 | 2:0351277ee5dd | 387 | { |
jjcarr2 | 2:0351277ee5dd | 388 | while(1) { |
jjcarr2 | 2:0351277ee5dd | 389 | |
jjcarr2 | 2:0351277ee5dd | 390 | |
jjcarr2 | 2:0351277ee5dd | 391 | pid1.setInputLimits(0, 5); |
jjcarr2 | 2:0351277ee5dd | 392 | pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); |
jjcarr2 | 2:0351277ee5dd | 393 | pid1.setSetPoint(5.0); |
jjcarr2 | 2:0351277ee5dd | 394 | |
jjcarr2 | 2:0351277ee5dd | 395 | rangeFinderLeft.startMeas(); |
jjcarr2 | 2:0351277ee5dd | 396 | wait_ms(100); |
jjcarr2 | 2:0351277ee5dd | 397 | if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) { |
jjcarr2 | 2:0351277ee5dd | 398 | //bt.printf("Range = %f\n", range); |
jjcarr2 | 2:0351277ee5dd | 399 | } |
jjcarr2 | 2:0351277ee5dd | 400 | |
jjcarr2 | 2:0351277ee5dd | 401 | /*************CHECK FOR WAVE OPENING*****************/ |
jjcarr2 | 2:0351277ee5dd | 402 | /* If after 100 ms the ultrasonic still sees 20+ cm */ |
jjcarr2 | 2:0351277ee5dd | 403 | /* then robot is at wave opening */ |
jjcarr2 | 2:0351277ee5dd | 404 | |
jjcarr2 | 2:0351277ee5dd | 405 | |
jjcarr2 | 2:0351277ee5dd | 406 | if(range > 20 ) { |
jjcarr2 | 2:0351277ee5dd | 407 | currentLocation--; |
jjcarr2 | 2:0351277ee5dd | 408 | } |
jjcarr2 | 2:0351277ee5dd | 409 | |
jjcarr2 | 2:0351277ee5dd | 410 | if( currentLocation == WaveOpening) { |
jjcarr2 | 2:0351277ee5dd | 411 | // AT WAVE OPENING!!!! |
jjcarr2 | 2:0351277ee5dd | 412 | |
jjcarr2 | 2:0351277ee5dd | 413 | break; |
jjcarr2 | 2:0351277ee5dd | 414 | } |
jjcarr2 | 2:0351277ee5dd | 415 | |
jjcarr2 | 2:0351277ee5dd | 416 | |
jjcarr2 | 2:0351277ee5dd | 417 | pid1.setProcessValue(range); |
jjcarr2 | 2:0351277ee5dd | 418 | pid_return = pid1.compute(); |
jjcarr2 | 2:0351277ee5dd | 419 | bt.printf("Range: %f\n PID: %f", range, pid_return); |
jjcarr2 | 2:0351277ee5dd | 420 | |
jjcarr2 | 2:0351277ee5dd | 421 | if(pid_return > 0) { |
jjcarr2 | 2:0351277ee5dd | 422 | motors.setMotor0Speed(MAX_SPEED - pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 423 | motors.setMotor1Speed(MAX_SPEED); |
jjcarr2 | 2:0351277ee5dd | 424 | } else if(pid_return < 0) { |
Fairy_Paolina | 0:ff94cc47fef7 | 425 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 426 | motors.setMotor1Speed(MAX_SPEED + pid_return); |
jjcarr2 | 2:0351277ee5dd | 427 | } else { |
Fairy_Paolina | 0:ff94cc47fef7 | 428 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 429 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 430 | } |
jjcarr2 | 2:0351277ee5dd | 431 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 432 | } |
jjcarr2 | 2:0351277ee5dd | 433 | |
Fairy_Paolina | 1:801f0b9a862a | 434 | void rightTurn(void) |
Fairy_Paolina | 1:801f0b9a862a | 435 | { |
jjcarr2 | 2:0351277ee5dd | 436 | float speedL, speedR; |
jjcarr2 | 2:0351277ee5dd | 437 | |
jjcarr2 | 2:0351277ee5dd | 438 | speedL=speedR= 0.4; |
jjcarr2 | 2:0351277ee5dd | 439 | |
Fairy_Paolina | 1:801f0b9a862a | 440 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 441 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 442 | motors.setMotor0Speed(-speedR*127);//right |
jjcarr2 | 2:0351277ee5dd | 443 | motors.setMotor1Speed(speedL*127);//left |
jjcarr2 | 7:0b7897232e93 | 444 | while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); |
Fairy_Paolina | 1:801f0b9a862a | 445 | motors.stopBothMotors(); |
Fairy_Paolina | 1:801f0b9a862a | 446 | } |
jjcarr2 | 2:0351277ee5dd | 447 | |
Fairy_Paolina | 1:801f0b9a862a | 448 | void leftTurn(void) |
Fairy_Paolina | 1:801f0b9a862a | 449 | { |
Fairy_Paolina | 1:801f0b9a862a | 450 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 451 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 452 | motors.setMotor0Speed(0.4*127);// right |
jjcarr2 | 2:0351277ee5dd | 453 | motors.setMotor1Speed(-0.4*127);// left |
jjcarr2 | 7:0b7897232e93 | 454 | while(leftEncoder.getPulses()>-940 || rightEncoder.getPulses()<940); |
jjcarr2 | 2:0351277ee5dd | 455 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 456 | } |
jjcarr2 | 2:0351277ee5dd | 457 | |
jjcarr2 | 2:0351277ee5dd | 458 | void overBump(void){ |
jjcarr2 | 2:0351277ee5dd | 459 | |
jjcarr2 | 2:0351277ee5dd | 460 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 461 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 462 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 463 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 464 | while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1); |
jjcarr2 | 2:0351277ee5dd | 465 | /* |
jjcarr2 | 2:0351277ee5dd | 466 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 467 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 468 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 469 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 470 | while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3); |
jjcarr2 | 2:0351277ee5dd | 471 | */ |
Fairy_Paolina | 1:801f0b9a862a | 472 | motors.stopBothMotors(); |
Fairy_Paolina | 1:801f0b9a862a | 473 | } |
jjcarr2 | 7:0b7897232e93 | 474 | void alignWithWall(void){ |
jjcarr2 | 7:0b7897232e93 | 475 | |
jjcarr2 | 7:0b7897232e93 | 476 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 477 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 478 | |
jjcarr2 | 7:0b7897232e93 | 479 | motors.setMotor0Speed(-MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 480 | motors.setMotor1Speed(0.4*MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 481 | while(rightEncoder.getPulses()>-1000); |
jjcarr2 | 7:0b7897232e93 | 482 | motors.stopBothMotors(); |
jjcarr2 | 7:0b7897232e93 | 483 | |
jjcarr2 | 7:0b7897232e93 | 484 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 485 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 486 | |
jjcarr2 | 7:0b7897232e93 | 487 | motors.setMotor0Speed(-MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 488 | motors.setMotor1Speed(-MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 489 | while(abs(leftEncoder.getPulses()*11.12/PPR) < 3); |
jjcarr2 | 7:0b7897232e93 | 490 | |
jjcarr2 | 7:0b7897232e93 | 491 | motors.setMotor0Speed(0.1*MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 492 | motors.setMotor1Speed(0.1*MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 493 | |
jjcarr2 | 7:0b7897232e93 | 494 | wait_ms(200); |
jjcarr2 | 7:0b7897232e93 | 495 | |
jjcarr2 | 7:0b7897232e93 | 496 | motors.stopBothMotors(); |
jjcarr2 | 7:0b7897232e93 | 497 | |
jjcarr2 | 7:0b7897232e93 | 498 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 499 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 500 | |
jjcarr2 | 7:0b7897232e93 | 501 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 502 | motors.setMotor1Speed(-MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 503 | while(rightEncoder.getPulses() < 260); |
jjcarr2 | 7:0b7897232e93 | 504 | |
jjcarr2 | 7:0b7897232e93 | 505 | motors.setMotor0Speed(0.7*MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 506 | motors.setMotor1Speed(-0.7*MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 507 | float usValue = 0; |
jjcarr2 | 7:0b7897232e93 | 508 | while(1){ |
jjcarr2 | 7:0b7897232e93 | 509 | rangeFinderLeft.startMeas(); |
jjcarr2 | 7:0b7897232e93 | 510 | wait_ms(100); |
jjcarr2 | 7:0b7897232e93 | 511 | rangeFinderLeft.getMeas(range); |
jjcarr2 | 7:0b7897232e93 | 512 | bt.printf("Range %f \t OldValue %f\n\r",range, usValue); |
jjcarr2 | 7:0b7897232e93 | 513 | if(range > usValue && usValue != 0 && range < 25){ |
jjcarr2 | 7:0b7897232e93 | 514 | break; |
jjcarr2 | 7:0b7897232e93 | 515 | } else { |
jjcarr2 | 7:0b7897232e93 | 516 | usValue = range; |
jjcarr2 | 7:0b7897232e93 | 517 | } |
jjcarr2 | 7:0b7897232e93 | 518 | } |
jjcarr2 | 7:0b7897232e93 | 519 | |
jjcarr2 | 7:0b7897232e93 | 520 | |
jjcarr2 | 7:0b7897232e93 | 521 | |
jjcarr2 | 7:0b7897232e93 | 522 | |
jjcarr2 | 7:0b7897232e93 | 523 | } |
jjcarr2 | 7:0b7897232e93 | 524 | |
jjcarr2 | 2:0351277ee5dd | 525 | |
jjcarr2 | 2:0351277ee5dd | 526 | void tools_section(float *location, float ¤t){ |
jjcarr2 | 2:0351277ee5dd | 527 | |
jjcarr2 | 7:0b7897232e93 | 528 | //location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field |
jjcarr2 | 7:0b7897232e93 | 529 | //Too Far from wall to see gap :( |
jjcarr2 | 7:0b7897232e93 | 530 | wall_follow(LEFT,FORWARD,TOOLS); |
jjcarr2 | 7:0b7897232e93 | 531 | |
jjcarr2 | 2:0351277ee5dd | 532 | current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; |
jjcarr2 | 2:0351277ee5dd | 533 | |
jjcarr2 | 2:0351277ee5dd | 534 | bt.printf("wavegap %f \t current %f \r\n",location[0],current); |
jjcarr2 | 2:0351277ee5dd | 535 | |
jjcarr2 | 2:0351277ee5dd | 536 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 537 | ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// |
jjcarr2 | 7:0b7897232e93 | 538 | alignWithWall(); |
jjcarr2 | 7:0b7897232e93 | 539 | wait_ms(200); |
jjcarr2 | 7:0b7897232e93 | 540 | |
jjcarr2 | 7:0b7897232e93 | 541 | rangeFinderLeft.startMeas(); |
jjcarr2 | 7:0b7897232e93 | 542 | wait_ms(100); |
jjcarr2 | 7:0b7897232e93 | 543 | rangeFinderLeft.getMeas(range); |
jjcarr2 | 7:0b7897232e93 | 544 | if(range < 10){ |
jjcarr2 | 7:0b7897232e93 | 545 | |
jjcarr2 | 2:0351277ee5dd | 546 | wall_follow2(LEFT,BACKWARD,TOOLS); |
jjcarr2 | 7:0b7897232e93 | 547 | |
jjcarr2 | 7:0b7897232e93 | 548 | leftEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 549 | rightEncoder.reset(); |
jjcarr2 | 7:0b7897232e93 | 550 | motors.setMotor0Speed(-0.9*MAX_SPEED); //right |
jjcarr2 | 7:0b7897232e93 | 551 | motors.setMotor1Speed(-0.9*MAX_SPEED); //left |
jjcarr2 | 7:0b7897232e93 | 552 | while(abs(leftEncoder.getPulses()*11.12/PPR) < 2); |
jjcarr2 | 7:0b7897232e93 | 553 | motors.stopBothMotors(); |
jjcarr2 | 7:0b7897232e93 | 554 | |
jjcarr2 | 2:0351277ee5dd | 555 | // forward |
jjcarr2 | 2:0351277ee5dd | 556 | |
jjcarr2 | 7:0b7897232e93 | 557 | /* leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 558 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 559 | motors.setMotor0Speed(-MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 560 | motors.setMotor1Speed(-MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 561 | while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); |
jjcarr2 | 2:0351277ee5dd | 562 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 563 | wait_ms(500);*/ |
jjcarr2 | 2:0351277ee5dd | 564 | current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; |
jjcarr2 | 2:0351277ee5dd | 565 | // REMOVED += because current should just be current pulses? |
jjcarr2 | 7:0b7897232e93 | 566 | /* |
jjcarr2 | 2:0351277ee5dd | 567 | wall_follow2(LEFT,FORWARD); |
jjcarr2 | 2:0351277ee5dd | 568 | // backward |
jjcarr2 | 2:0351277ee5dd | 569 | leftEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 570 | rightEncoder.reset(); |
jjcarr2 | 2:0351277ee5dd | 571 | motors.setMotor0Speed(MAX_SPEED); //right |
jjcarr2 | 2:0351277ee5dd | 572 | motors.setMotor1Speed(MAX_SPEED); //left |
jjcarr2 | 2:0351277ee5dd | 573 | while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300); |
jjcarr2 | 2:0351277ee5dd | 574 | motors.stopBothMotors(); |
jjcarr2 | 2:0351277ee5dd | 575 | current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/ |
jjcarr2 | 7:0b7897232e93 | 576 | |
jjcarr2 | 2:0351277ee5dd | 577 | |
jjcarr2 | 2:0351277ee5dd | 578 | leftTurn(); |
jjcarr2 | 2:0351277ee5dd | 579 | |
jjcarr2 | 2:0351277ee5dd | 580 | //Go over |
jjcarr2 | 2:0351277ee5dd | 581 | overBump(); |
jjcarr2 | 7:0b7897232e93 | 582 | } else { |
jjcarr2 | 7:0b7897232e93 | 583 | |
jjcarr2 | 7:0b7897232e93 | 584 | leftTurn(); |
jjcarr2 | 7:0b7897232e93 | 585 | //Go over |
jjcarr2 | 7:0b7897232e93 | 586 | overBump(); |
jjcarr2 | 7:0b7897232e93 | 587 | } |
jjcarr2 | 2:0351277ee5dd | 588 | |
jjcarr2 | 2:0351277ee5dd | 589 | } |
jjcarr2 | 2:0351277ee5dd | 590 |