Tools Section Navigation Calibrated slightly
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos
Fork of NavigationTest by
main.cpp@1:801f0b9a862a, 2014-03-13 (annotated)
- Committer:
- Fairy_Paolina
- Date:
- Thu Mar 13 20:45:56 2014 +0000
- Revision:
- 1:801f0b9a862a
- Parent:
- 0:ff94cc47fef7
- Child:
- 2:0351277ee5dd
- Child:
- 3:3d0be48abcf2
revised;
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" |
Fairy_Paolina | 0:ff94cc47fef7 | 9 | |
Fairy_Paolina | 0:ff94cc47fef7 | 10 | #define PIN_TRIGGER (p12) |
Fairy_Paolina | 0:ff94cc47fef7 | 11 | #define PIN_ECHO (p11) |
Fairy_Paolina | 0:ff94cc47fef7 | 12 | #define PULSE_PER_REV (1192) |
Fairy_Paolina | 0:ff94cc47fef7 | 13 | #define WHEEL_CIRCUM (12.56637) |
Fairy_Paolina | 0:ff94cc47fef7 | 14 | #define DIST_PER_PULSE (0.01054225722682) |
Fairy_Paolina | 0:ff94cc47fef7 | 15 | #define MTRS_TO_INCH (39.3701) |
Fairy_Paolina | 1:801f0b9a862a | 16 | #define MAX_SPEED (0.3*127) |
Fairy_Paolina | 0:ff94cc47fef7 | 17 | |
Fairy_Paolina | 0:ff94cc47fef7 | 18 | float range, pid_return; |
Fairy_Paolina | 0:ff94cc47fef7 | 19 | void errFunction(void); |
Fairy_Paolina | 0:ff94cc47fef7 | 20 | bool cRc; |
Fairy_Paolina | 0:ff94cc47fef7 | 21 | |
Fairy_Paolina | 0:ff94cc47fef7 | 22 | //Hardware Initialization |
Fairy_Paolina | 0:ff94cc47fef7 | 23 | Serial bt(p13,p14); |
Fairy_Paolina | 0:ff94cc47fef7 | 24 | Serial pc(USBTX,USBRX); |
Fairy_Paolina | 0:ff94cc47fef7 | 25 | HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO ); |
Fairy_Paolina | 1:801f0b9a862a | 26 | PID pid1(2.0,2.0,0.0,0.02); |
Fairy_Paolina | 0:ff94cc47fef7 | 27 | PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); |
Fairy_Paolina | 1:801f0b9a862a | 28 | QEI leftEncoder(p17,p18,NC,1192,QEI::X4_ENCODING); |
Fairy_Paolina | 1:801f0b9a862a | 29 | //QEI rightEncoder(p19,p20,NC,1192,QEI::X4_ENCODING); |
Fairy_Paolina | 1:801f0b9a862a | 30 | //InterruptIn encoder(p29); |
Fairy_Paolina | 1:801f0b9a862a | 31 | |
Fairy_Paolina | 0:ff94cc47fef7 | 32 | |
Fairy_Paolina | 0:ff94cc47fef7 | 33 | //Functions |
Fairy_Paolina | 0:ff94cc47fef7 | 34 | |
Fairy_Paolina | 0:ff94cc47fef7 | 35 | void wall_follow(void); |
Fairy_Paolina | 1:801f0b9a862a | 36 | void wall_follow2(int *currentLocation); |
Fairy_Paolina | 0:ff94cc47fef7 | 37 | void wall_follow3(int ¤tLocation, int &WaveOpening); |
Fairy_Paolina | 1:801f0b9a862a | 38 | void leftTurn(void); |
Fairy_Paolina | 1:801f0b9a862a | 39 | void rightTurn(void); |
Fairy_Paolina | 0:ff94cc47fef7 | 40 | void us_distance(void); |
Fairy_Paolina | 0:ff94cc47fef7 | 41 | |
Fairy_Paolina | 1:801f0b9a862a | 42 | //Variables |
Fairy_Paolina | 0:ff94cc47fef7 | 43 | |
Fairy_Paolina | 0:ff94cc47fef7 | 44 | int main(void){ |
Fairy_Paolina | 1:801f0b9a862a | 45 | int location=0; |
Fairy_Paolina | 0:ff94cc47fef7 | 46 | |
Fairy_Paolina | 0:ff94cc47fef7 | 47 | pc.baud(115200); |
Fairy_Paolina | 1:801f0b9a862a | 48 | bt.baud(115200); |
Fairy_Paolina | 1:801f0b9a862a | 49 | motors.begin(); |
Fairy_Paolina | 0:ff94cc47fef7 | 50 | |
Fairy_Paolina | 1:801f0b9a862a | 51 | /*motors.setMotor0Speed(MAX_SPEED); //left |
Fairy_Paolina | 1:801f0b9a862a | 52 | motors.setMotor1Speed(MAX_SPEED); //right |
Fairy_Paolina | 1:801f0b9a862a | 53 | wait_ms(350); |
Fairy_Paolina | 1:801f0b9a862a | 54 | */ |
Fairy_Paolina | 1:801f0b9a862a | 55 | //wall_follow(); |
Fairy_Paolina | 1:801f0b9a862a | 56 | //wall_follow2(&location); |
Fairy_Paolina | 1:801f0b9a862a | 57 | pc.printf("%d\n\r",location); |
Fairy_Paolina | 0:ff94cc47fef7 | 58 | |
Fairy_Paolina | 0:ff94cc47fef7 | 59 | motors.stopBothMotors(); |
Fairy_Paolina | 1:801f0b9a862a | 60 | leftTurn(); |
Fairy_Paolina | 1:801f0b9a862a | 61 | wait(1); |
Fairy_Paolina | 1:801f0b9a862a | 62 | rightTurn(); |
Fairy_Paolina | 0:ff94cc47fef7 | 63 | |
Fairy_Paolina | 0:ff94cc47fef7 | 64 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 65 | |
Fairy_Paolina | 0:ff94cc47fef7 | 66 | void errFunction(void){ |
Fairy_Paolina | 0:ff94cc47fef7 | 67 | //Nothing |
Fairy_Paolina | 0:ff94cc47fef7 | 68 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 69 | |
Fairy_Paolina | 0:ff94cc47fef7 | 70 | void us_distance(void) |
Fairy_Paolina | 0:ff94cc47fef7 | 71 | { |
Fairy_Paolina | 0:ff94cc47fef7 | 72 | pc.printf("Ultra Sonic\n\r"); |
Fairy_Paolina | 0:ff94cc47fef7 | 73 | rangeFinder.startMeas(); |
Fairy_Paolina | 0:ff94cc47fef7 | 74 | wait_us(20); |
Fairy_Paolina | 0:ff94cc47fef7 | 75 | if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID)) |
Fairy_Paolina | 0:ff94cc47fef7 | 76 | { |
Fairy_Paolina | 0:ff94cc47fef7 | 77 | pc.printf("Range = %f\n\r", range); |
Fairy_Paolina | 0:ff94cc47fef7 | 78 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 79 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 80 | |
Fairy_Paolina | 0:ff94cc47fef7 | 81 | void wall_follow(void) |
Fairy_Paolina | 0:ff94cc47fef7 | 82 | { |
Fairy_Paolina | 0:ff94cc47fef7 | 83 | while(1){ |
Fairy_Paolina | 0:ff94cc47fef7 | 84 | |
Fairy_Paolina | 1:801f0b9a862a | 85 | pid1.setInputLimits(5.75, 6); |
Fairy_Paolina | 1:801f0b9a862a | 86 | pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); |
Fairy_Paolina | 1:801f0b9a862a | 87 | pid1.setSetPoint(6.0); |
Fairy_Paolina | 0:ff94cc47fef7 | 88 | |
Fairy_Paolina | 0:ff94cc47fef7 | 89 | rangeFinder.startMeas(); |
Fairy_Paolina | 1:801f0b9a862a | 90 | wait_ms(20); |
Fairy_Paolina | 0:ff94cc47fef7 | 91 | if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) |
Fairy_Paolina | 0:ff94cc47fef7 | 92 | { |
Fairy_Paolina | 1:801f0b9a862a | 93 | pc.printf("Range = %f\n\r", range); |
Fairy_Paolina | 0:ff94cc47fef7 | 94 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 95 | pid1.setProcessValue(range); |
Fairy_Paolina | 0:ff94cc47fef7 | 96 | pid_return = pid1.compute(); |
Fairy_Paolina | 1:801f0b9a862a | 97 | pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 98 | |
Fairy_Paolina | 0:ff94cc47fef7 | 99 | if(pid_return > 0){ |
Fairy_Paolina | 0:ff94cc47fef7 | 100 | motors.setMotor0Speed(MAX_SPEED - pid_return);//left |
Fairy_Paolina | 0:ff94cc47fef7 | 101 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 102 | }else if(pid_return < 0){ |
Fairy_Paolina | 0:ff94cc47fef7 | 103 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 104 | motors.setMotor1Speed(MAX_SPEED + pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 105 | }else{ |
Fairy_Paolina | 0:ff94cc47fef7 | 106 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 107 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 108 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 109 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 110 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 111 | |
Fairy_Paolina | 0:ff94cc47fef7 | 112 | /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ |
Fairy_Paolina | 0:ff94cc47fef7 | 113 | |
Fairy_Paolina | 1:801f0b9a862a | 114 | void wall_follow2(int *currentLocation) |
Fairy_Paolina | 0:ff94cc47fef7 | 115 | { |
Fairy_Paolina | 0:ff94cc47fef7 | 116 | int SeeWaveGap = false; |
Fairy_Paolina | 1:801f0b9a862a | 117 | int count=0; |
Fairy_Paolina | 1:801f0b9a862a | 118 | |
Fairy_Paolina | 0:ff94cc47fef7 | 119 | while(1){ |
Fairy_Paolina | 0:ff94cc47fef7 | 120 | |
Fairy_Paolina | 1:801f0b9a862a | 121 | pid1.setInputLimits(0.0, 6.0); |
Fairy_Paolina | 1:801f0b9a862a | 122 | pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); |
Fairy_Paolina | 1:801f0b9a862a | 123 | pid1.setSetPoint(6.0); |
Fairy_Paolina | 0:ff94cc47fef7 | 124 | |
Fairy_Paolina | 0:ff94cc47fef7 | 125 | rangeFinder.startMeas(); |
Fairy_Paolina | 1:801f0b9a862a | 126 | wait_ms(20); |
Fairy_Paolina | 0:ff94cc47fef7 | 127 | if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) |
Fairy_Paolina | 0:ff94cc47fef7 | 128 | { |
Fairy_Paolina | 1:801f0b9a862a | 129 | bt.printf("Range = %f\n", range); |
Fairy_Paolina | 0:ff94cc47fef7 | 130 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 131 | |
Fairy_Paolina | 0:ff94cc47fef7 | 132 | /*************CHECK FOR WAVE OPENING*****************/ |
Fairy_Paolina | 1:801f0b9a862a | 133 | /* If after 60 ms the ultrasonic still sees 20+ cm */ |
Fairy_Paolina | 0:ff94cc47fef7 | 134 | /* then robot is at wave opening */ |
Fairy_Paolina | 0:ff94cc47fef7 | 135 | |
Fairy_Paolina | 1:801f0b9a862a | 136 | pc.printf("range %f\r\n",range); |
Fairy_Paolina | 1:801f0b9a862a | 137 | if(range > 20){ |
Fairy_Paolina | 0:ff94cc47fef7 | 138 | currentLocation++; |
Fairy_Paolina | 1:801f0b9a862a | 139 | bt.printf("saw gap \r\n"); |
Fairy_Paolina | 1:801f0b9a862a | 140 | |
Fairy_Paolina | 1:801f0b9a862a | 141 | if(SeeWaveGap){ |
Fairy_Paolina | 1:801f0b9a862a | 142 | motors.stopBothMotors(); |
Fairy_Paolina | 1:801f0b9a862a | 143 | bt.printf("wavegap\r\n"); |
Fairy_Paolina | 1:801f0b9a862a | 144 | // AT WAVE OPENING!!!! |
Fairy_Paolina | 1:801f0b9a862a | 145 | |
Fairy_Paolina | 1:801f0b9a862a | 146 | break; |
Fairy_Paolina | 1:801f0b9a862a | 147 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 148 | SeeWaveGap = true; |
Fairy_Paolina | 0:ff94cc47fef7 | 149 | } |
Fairy_Paolina | 1:801f0b9a862a | 150 | |
Fairy_Paolina | 0:ff94cc47fef7 | 151 | |
Fairy_Paolina | 0:ff94cc47fef7 | 152 | |
Fairy_Paolina | 0:ff94cc47fef7 | 153 | pid1.setProcessValue(range); |
Fairy_Paolina | 0:ff94cc47fef7 | 154 | pid_return = pid1.compute(); |
Fairy_Paolina | 1:801f0b9a862a | 155 | bt.printf("Range: %f\n PID: %f\r\n", range, pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 156 | |
Fairy_Paolina | 0:ff94cc47fef7 | 157 | if(pid_return > 0){ |
Fairy_Paolina | 0:ff94cc47fef7 | 158 | motors.setMotor0Speed(MAX_SPEED - pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 159 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 160 | }else if(pid_return < 0){ |
Fairy_Paolina | 0:ff94cc47fef7 | 161 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 162 | motors.setMotor1Speed(MAX_SPEED + pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 163 | }else{ |
Fairy_Paolina | 0:ff94cc47fef7 | 164 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 165 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 166 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 167 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 168 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 169 | |
Fairy_Paolina | 0:ff94cc47fef7 | 170 | |
Fairy_Paolina | 0:ff94cc47fef7 | 171 | /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */ |
Fairy_Paolina | 0:ff94cc47fef7 | 172 | /* MEANT FOR RETURNING FROM OIL RIGS */ |
Fairy_Paolina | 0:ff94cc47fef7 | 173 | |
Fairy_Paolina | 0:ff94cc47fef7 | 174 | void wall_follow3(int ¤tLocation, int &WaveOpening) |
Fairy_Paolina | 0:ff94cc47fef7 | 175 | { |
Fairy_Paolina | 0:ff94cc47fef7 | 176 | while(1){ |
Fairy_Paolina | 0:ff94cc47fef7 | 177 | |
Fairy_Paolina | 0:ff94cc47fef7 | 178 | |
Fairy_Paolina | 1:801f0b9a862a | 179 | pid1.setInputLimits(5.75, 6); |
Fairy_Paolina | 1:801f0b9a862a | 180 | pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); |
Fairy_Paolina | 1:801f0b9a862a | 181 | pid1.setSetPoint(6.0); |
Fairy_Paolina | 0:ff94cc47fef7 | 182 | |
Fairy_Paolina | 0:ff94cc47fef7 | 183 | rangeFinder.startMeas(); |
Fairy_Paolina | 0:ff94cc47fef7 | 184 | wait_ms(100); |
Fairy_Paolina | 0:ff94cc47fef7 | 185 | if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) |
Fairy_Paolina | 0:ff94cc47fef7 | 186 | { |
Fairy_Paolina | 0:ff94cc47fef7 | 187 | //bt.printf("Range = %f\n", range); |
Fairy_Paolina | 0:ff94cc47fef7 | 188 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 189 | |
Fairy_Paolina | 0:ff94cc47fef7 | 190 | /*************CHECK FOR WAVE OPENING*****************/ |
Fairy_Paolina | 0:ff94cc47fef7 | 191 | /* If after 100 ms the ultrasonic still sees 20+ cm */ |
Fairy_Paolina | 0:ff94cc47fef7 | 192 | /* then robot is at wave opening */ |
Fairy_Paolina | 0:ff94cc47fef7 | 193 | |
Fairy_Paolina | 0:ff94cc47fef7 | 194 | |
Fairy_Paolina | 0:ff94cc47fef7 | 195 | if(range > 20 ){ |
Fairy_Paolina | 0:ff94cc47fef7 | 196 | currentLocation--; |
Fairy_Paolina | 0:ff94cc47fef7 | 197 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 198 | |
Fairy_Paolina | 0:ff94cc47fef7 | 199 | if( currentLocation == WaveOpening){ |
Fairy_Paolina | 0:ff94cc47fef7 | 200 | // AT WAVE OPENING!!!! |
Fairy_Paolina | 0:ff94cc47fef7 | 201 | |
Fairy_Paolina | 0:ff94cc47fef7 | 202 | break; |
Fairy_Paolina | 0:ff94cc47fef7 | 203 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 204 | |
Fairy_Paolina | 0:ff94cc47fef7 | 205 | |
Fairy_Paolina | 0:ff94cc47fef7 | 206 | pid1.setProcessValue(range); |
Fairy_Paolina | 0:ff94cc47fef7 | 207 | pid_return = pid1.compute(); |
Fairy_Paolina | 0:ff94cc47fef7 | 208 | bt.printf("Range: %f\n PID: %f", range, pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 209 | |
Fairy_Paolina | 0:ff94cc47fef7 | 210 | if(pid_return > 0){ |
Fairy_Paolina | 0:ff94cc47fef7 | 211 | motors.setMotor0Speed(MAX_SPEED - pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 212 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 213 | }else if(pid_return < 0){ |
Fairy_Paolina | 0:ff94cc47fef7 | 214 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 215 | motors.setMotor1Speed(MAX_SPEED + pid_return); |
Fairy_Paolina | 0:ff94cc47fef7 | 216 | }else{ |
Fairy_Paolina | 0:ff94cc47fef7 | 217 | motors.setMotor0Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 218 | motors.setMotor1Speed(MAX_SPEED); |
Fairy_Paolina | 0:ff94cc47fef7 | 219 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 220 | } |
Fairy_Paolina | 0:ff94cc47fef7 | 221 | } |
Fairy_Paolina | 1:801f0b9a862a | 222 | |
Fairy_Paolina | 1:801f0b9a862a | 223 | void rightTurn(void) |
Fairy_Paolina | 1:801f0b9a862a | 224 | { |
Fairy_Paolina | 1:801f0b9a862a | 225 | leftEncoder.reset(); |
Fairy_Paolina | 1:801f0b9a862a | 226 | //rightEncoder.reset(); |
Fairy_Paolina | 1:801f0b9a862a | 227 | motors.setMotor0Speed(-0.4*127); |
Fairy_Paolina | 1:801f0b9a862a | 228 | motors.setMotor1Speed(0.4*127); |
Fairy_Paolina | 1:801f0b9a862a | 229 | while(leftEncoder.getPulses()<1400); |
Fairy_Paolina | 1:801f0b9a862a | 230 | motors.stopBothMotors(); |
Fairy_Paolina | 1:801f0b9a862a | 231 | } |
Fairy_Paolina | 1:801f0b9a862a | 232 | |
Fairy_Paolina | 1:801f0b9a862a | 233 | void leftTurn(void) |
Fairy_Paolina | 1:801f0b9a862a | 234 | { |
Fairy_Paolina | 1:801f0b9a862a | 235 | leftEncoder.reset(); |
Fairy_Paolina | 1:801f0b9a862a | 236 | //rightEncoder.reset(); |
Fairy_Paolina | 1:801f0b9a862a | 237 | motors.setMotor0Speed(0.4*127); |
Fairy_Paolina | 1:801f0b9a862a | 238 | motors.setMotor1Speed(-0.4*127); |
Fairy_Paolina | 1:801f0b9a862a | 239 | while(leftEncoder.getPulses()>-1500); |
Fairy_Paolina | 1:801f0b9a862a | 240 | motors.stopBothMotors(); |
Fairy_Paolina | 1:801f0b9a862a | 241 | } |