Program that combines a linefollower program with visible ligt communication.
Dependencies: m3pi_custom mbed
linefollower.cpp@1:243ec35fafcd, 2018-05-16 (annotated)
- Committer:
- bertgereels
- Date:
- Wed May 16 19:12:20 2018 +0000
- Revision:
- 1:243ec35fafcd
- Parent:
- 0:1f5782fc5ca3
- Child:
- 2:21fb894dc9d6
Part two of the 'projectlab' course.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bertgereels | 0:1f5782fc5ca3 | 1 | #include "mbed.h" |
bertgereels | 0:1f5782fc5ca3 | 2 | #include "linefollower.h" |
bertgereels | 0:1f5782fc5ca3 | 3 | #include <string> |
bertgereels | 0:1f5782fc5ca3 | 4 | |
bertgereels | 1:243ec35fafcd | 5 | DigitalOut ledLinks(LED1); |
bertgereels | 1:243ec35fafcd | 6 | DigitalOut ledRechts(LED2); |
bertgereels | 1:243ec35fafcd | 7 | DigitalOut ledRechtdoor(LED3); |
bertgereels | 1:243ec35fafcd | 8 | DigitalOut ledAchteruit(LED4); |
bertgereels | 1:243ec35fafcd | 9 | |
bertgereels | 0:1f5782fc5ca3 | 10 | namespace ProjectTwo{ |
bertgereels | 1:243ec35fafcd | 11 | |
bertgereels | 0:1f5782fc5ca3 | 12 | LineFollower::LineFollower(){ |
bertgereels | 0:1f5782fc5ca3 | 13 | current_pos_of_line = 0.0; |
bertgereels | 0:1f5782fc5ca3 | 14 | previous_pos_of_line = 0.0; |
bertgereels | 0:1f5782fc5ca3 | 15 | derivative = 0; |
bertgereels | 0:1f5782fc5ca3 | 16 | proportional = 0; |
bertgereels | 0:1f5782fc5ca3 | 17 | integral = 0; |
bertgereels | 0:1f5782fc5ca3 | 18 | speed = MAX_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 19 | |
bertgereels | 0:1f5782fc5ca3 | 20 | m3pi.locate(0,1); |
bertgereels | 0:1f5782fc5ca3 | 21 | m3pi.printf("Line PID"); |
bertgereels | 0:1f5782fc5ca3 | 22 | wait(2.0); |
bertgereels | 0:1f5782fc5ca3 | 23 | m3pi.cls(); |
bertgereels | 0:1f5782fc5ca3 | 24 | |
bertgereels | 0:1f5782fc5ca3 | 25 | CurrentLineFollowerState = STATE_INIT; |
bertgereels | 1:243ec35fafcd | 26 | vlcDecoder = Manchester(); |
bertgereels | 0:1f5782fc5ca3 | 27 | } |
bertgereels | 0:1f5782fc5ca3 | 28 | |
bertgereels | 0:1f5782fc5ca3 | 29 | void LineFollower::followTheLine(void){ |
bertgereels | 0:1f5782fc5ca3 | 30 | switch(CurrentLineFollowerState){ |
bertgereels | 0:1f5782fc5ca3 | 31 | case STATE_INIT: |
bertgereels | 0:1f5782fc5ca3 | 32 | { |
bertgereels | 0:1f5782fc5ca3 | 33 | m3pi.sensor_auto_calibrate(); |
bertgereels | 0:1f5782fc5ca3 | 34 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 35 | break; |
bertgereels | 0:1f5782fc5ca3 | 36 | } |
bertgereels | 0:1f5782fc5ca3 | 37 | case STATE_COMPUTE: |
bertgereels | 0:1f5782fc5ca3 | 38 | { |
bertgereels | 1:243ec35fafcd | 39 | ledLinks = 0; |
bertgereels | 1:243ec35fafcd | 40 | ledRechts = 0; |
bertgereels | 1:243ec35fafcd | 41 | ledRechtdoor = 0; |
bertgereels | 1:243ec35fafcd | 42 | ledAchteruit = 0; |
bertgereels | 0:1f5782fc5ca3 | 43 | // Get the position of the line. |
bertgereels | 0:1f5782fc5ca3 | 44 | current_pos_of_line = m3pi.line_position(); |
bertgereels | 0:1f5782fc5ca3 | 45 | proportional = current_pos_of_line; |
bertgereels | 1:243ec35fafcd | 46 | |
bertgereels | 0:1f5782fc5ca3 | 47 | computeDerivative(); |
bertgereels | 0:1f5782fc5ca3 | 48 | |
bertgereels | 0:1f5782fc5ca3 | 49 | computeIntegral(); |
bertgereels | 0:1f5782fc5ca3 | 50 | |
bertgereels | 0:1f5782fc5ca3 | 51 | // Remember the last position. |
bertgereels | 0:1f5782fc5ca3 | 52 | previous_pos_of_line = current_pos_of_line; |
bertgereels | 0:1f5782fc5ca3 | 53 | |
bertgereels | 0:1f5782fc5ca3 | 54 | // Compute the power |
bertgereels | 0:1f5782fc5ca3 | 55 | computePower(); |
bertgereels | 0:1f5782fc5ca3 | 56 | |
bertgereels | 0:1f5782fc5ca3 | 57 | // Compute new speeds |
bertgereels | 0:1f5782fc5ca3 | 58 | computeNewSpeeds(); |
bertgereels | 0:1f5782fc5ca3 | 59 | |
bertgereels | 0:1f5782fc5ca3 | 60 | // limit checks |
bertgereels | 0:1f5782fc5ca3 | 61 | limitChecks(); |
bertgereels | 1:243ec35fafcd | 62 | |
bertgereels | 1:243ec35fafcd | 63 | if(checkForIntersection()){ |
bertgereels | 0:1f5782fc5ca3 | 64 | CurrentLineFollowerState = STATE_STOP; |
bertgereels | 1:243ec35fafcd | 65 | }else if(!checkForIntersection()){ |
bertgereels | 0:1f5782fc5ca3 | 66 | CurrentLineFollowerState = STATE_FORWARD; |
bertgereels | 0:1f5782fc5ca3 | 67 | } |
bertgereels | 0:1f5782fc5ca3 | 68 | |
bertgereels | 0:1f5782fc5ca3 | 69 | m3pi.cls(); |
bertgereels | 0:1f5782fc5ca3 | 70 | |
bertgereels | 0:1f5782fc5ca3 | 71 | break; |
bertgereels | 0:1f5782fc5ca3 | 72 | } |
bertgereels | 0:1f5782fc5ca3 | 73 | case STATE_FORWARD: |
bertgereels | 0:1f5782fc5ca3 | 74 | { |
bertgereels | 0:1f5782fc5ca3 | 75 | goForwards(); |
bertgereels | 0:1f5782fc5ca3 | 76 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 77 | break; |
bertgereels | 0:1f5782fc5ca3 | 78 | } |
bertgereels | 1:243ec35fafcd | 79 | case STATE_LONG_FORWARD: |
bertgereels | 1:243ec35fafcd | 80 | { |
bertgereels | 1:243ec35fafcd | 81 | int i = 0; |
bertgereels | 1:243ec35fafcd | 82 | do{ |
bertgereels | 1:243ec35fafcd | 83 | goForwards(); |
bertgereels | 1:243ec35fafcd | 84 | i ++; |
bertgereels | 1:243ec35fafcd | 85 | }while(i < 750); |
bertgereels | 1:243ec35fafcd | 86 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 1:243ec35fafcd | 87 | break; |
bertgereels | 1:243ec35fafcd | 88 | |
bertgereels | 1:243ec35fafcd | 89 | } |
bertgereels | 0:1f5782fc5ca3 | 90 | case STATE_LEFT: |
bertgereels | 0:1f5782fc5ca3 | 91 | { |
bertgereels | 0:1f5782fc5ca3 | 92 | turnLeft90(); |
bertgereels | 0:1f5782fc5ca3 | 93 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 94 | break; |
bertgereels | 0:1f5782fc5ca3 | 95 | } |
bertgereels | 0:1f5782fc5ca3 | 96 | case STATE_RIGHT: |
bertgereels | 0:1f5782fc5ca3 | 97 | { |
bertgereels | 0:1f5782fc5ca3 | 98 | turnRight90(); |
bertgereels | 0:1f5782fc5ca3 | 99 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 100 | break; |
bertgereels | 0:1f5782fc5ca3 | 101 | } |
bertgereels | 1:243ec35fafcd | 102 | case STATE_TURN180: |
bertgereels | 0:1f5782fc5ca3 | 103 | { |
bertgereels | 0:1f5782fc5ca3 | 104 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 1:243ec35fafcd | 105 | turnRight90(); |
bertgereels | 1:243ec35fafcd | 106 | turnRight90(); |
bertgereels | 0:1f5782fc5ca3 | 107 | break; |
bertgereels | 0:1f5782fc5ca3 | 108 | } |
bertgereels | 0:1f5782fc5ca3 | 109 | case STATE_STOP: |
bertgereels | 0:1f5782fc5ca3 | 110 | { |
bertgereels | 0:1f5782fc5ca3 | 111 | m3pi.stop(); |
bertgereels | 1:243ec35fafcd | 112 | m3pi.printf("K-punt"); |
bertgereels | 1:243ec35fafcd | 113 | |
bertgereels | 1:243ec35fafcd | 114 | bool searchForLight = true; |
bertgereels | 1:243ec35fafcd | 115 | int scanCounter = 0; |
bertgereels | 1:243ec35fafcd | 116 | while(searchForLight){ |
bertgereels | 1:243ec35fafcd | 117 | if(analogSensor.getVoltage() > 0.6){ |
bertgereels | 1:243ec35fafcd | 118 | CurrentLineFollowerState = STATE_VLC; |
bertgereels | 1:243ec35fafcd | 119 | searchForLight = false; |
bertgereels | 1:243ec35fafcd | 120 | } |
bertgereels | 1:243ec35fafcd | 121 | else{ |
bertgereels | 1:243ec35fafcd | 122 | Scan(); |
bertgereels | 1:243ec35fafcd | 123 | scanCounter++; |
bertgereels | 1:243ec35fafcd | 124 | } |
bertgereels | 1:243ec35fafcd | 125 | |
bertgereels | 1:243ec35fafcd | 126 | if(scanCounter == 2){ |
bertgereels | 1:243ec35fafcd | 127 | CurrentLineFollowerState = STATE_FORWARD; |
bertgereels | 1:243ec35fafcd | 128 | searchForLight = false; |
bertgereels | 1:243ec35fafcd | 129 | } |
bertgereels | 1:243ec35fafcd | 130 | wait(0.5); |
bertgereels | 1:243ec35fafcd | 131 | } |
bertgereels | 1:243ec35fafcd | 132 | |
bertgereels | 0:1f5782fc5ca3 | 133 | break; |
bertgereels | 0:1f5782fc5ca3 | 134 | } |
bertgereels | 0:1f5782fc5ca3 | 135 | case STATE_VLC: |
bertgereels | 0:1f5782fc5ca3 | 136 | { |
bertgereels | 0:1f5782fc5ca3 | 137 | m3pi.cls(); |
bertgereels | 0:1f5782fc5ca3 | 138 | m3pi.locate(0,0); |
bertgereels | 0:1f5782fc5ca3 | 139 | m3pi.printf("VLC"); |
bertgereels | 0:1f5782fc5ca3 | 140 | m3pi.locate(0,1); |
bertgereels | 1:243ec35fafcd | 141 | m3pi.printf("Activate"); |
bertgereels | 1:243ec35fafcd | 142 | wait(1); |
bertgereels | 1:243ec35fafcd | 143 | |
bertgereels | 1:243ec35fafcd | 144 | //retry decoding if detected value is equal to 5 |
bertgereels | 1:243ec35fafcd | 145 | do{ |
bertgereels | 1:243ec35fafcd | 146 | direction_index = vlcDecoder.decode(); |
bertgereels | 1:243ec35fafcd | 147 | }while(direction_index == 5); |
bertgereels | 0:1f5782fc5ca3 | 148 | |
bertgereels | 1:243ec35fafcd | 149 | wait(0.5); |
bertgereels | 1:243ec35fafcd | 150 | m3pi.cls(); |
bertgereels | 1:243ec35fafcd | 151 | m3pi.printf("DIR fnd"); |
bertgereels | 1:243ec35fafcd | 152 | |
bertgereels | 0:1f5782fc5ca3 | 153 | wait(1); //wait 1 sec before executing selected direction |
bertgereels | 1:243ec35fafcd | 154 | switch(direction_index){ |
bertgereels | 1:243ec35fafcd | 155 | case 0:{ //left |
bertgereels | 0:1f5782fc5ca3 | 156 | CurrentLineFollowerState = STATE_LEFT; |
bertgereels | 1:243ec35fafcd | 157 | ledLinks = 1; |
bertgereels | 0:1f5782fc5ca3 | 158 | break; |
bertgereels | 0:1f5782fc5ca3 | 159 | } |
bertgereels | 1:243ec35fafcd | 160 | case 1:{ //right |
bertgereels | 0:1f5782fc5ca3 | 161 | CurrentLineFollowerState = STATE_RIGHT; |
bertgereels | 1:243ec35fafcd | 162 | ledRechts = 1; |
bertgereels | 1:243ec35fafcd | 163 | break; |
bertgereels | 1:243ec35fafcd | 164 | } |
bertgereels | 1:243ec35fafcd | 165 | case 2:{ //forwards |
bertgereels | 1:243ec35fafcd | 166 | CurrentLineFollowerState = STATE_LONG_FORWARD; |
bertgereels | 1:243ec35fafcd | 167 | ledRechtdoor = 1; |
bertgereels | 0:1f5782fc5ca3 | 168 | break; |
bertgereels | 0:1f5782fc5ca3 | 169 | } |
bertgereels | 1:243ec35fafcd | 170 | case 3:{ //backwards |
bertgereels | 1:243ec35fafcd | 171 | CurrentLineFollowerState = STATE_TURN180; |
bertgereels | 1:243ec35fafcd | 172 | ledAchteruit = 1; |
bertgereels | 0:1f5782fc5ca3 | 173 | break; |
bertgereels | 0:1f5782fc5ca3 | 174 | } |
bertgereels | 1:243ec35fafcd | 175 | case 4:{ //ignore |
bertgereels | 1:243ec35fafcd | 176 | CurrentLineFollowerState = STATE_LONG_FORWARD; |
bertgereels | 0:1f5782fc5ca3 | 177 | break; |
bertgereels | 0:1f5782fc5ca3 | 178 | } |
bertgereels | 1:243ec35fafcd | 179 | default:{ |
bertgereels | 1:243ec35fafcd | 180 | CurrentLineFollowerState = STATE_LONG_FORWARD; |
bertgereels | 1:243ec35fafcd | 181 | m3pi.cls(); |
bertgereels | 1:243ec35fafcd | 182 | } |
bertgereels | 0:1f5782fc5ca3 | 183 | } |
bertgereels | 1:243ec35fafcd | 184 | |
bertgereels | 1:243ec35fafcd | 185 | break; |
bertgereels | 0:1f5782fc5ca3 | 186 | } |
bertgereels | 0:1f5782fc5ca3 | 187 | } |
bertgereels | 0:1f5782fc5ca3 | 188 | } |
bertgereels | 0:1f5782fc5ca3 | 189 | |
bertgereels | 0:1f5782fc5ca3 | 190 | float LineFollower::computeDerivative(void){ |
bertgereels | 0:1f5782fc5ca3 | 191 | return derivative = current_pos_of_line - previous_pos_of_line; |
bertgereels | 0:1f5782fc5ca3 | 192 | } |
bertgereels | 0:1f5782fc5ca3 | 193 | |
bertgereels | 0:1f5782fc5ca3 | 194 | float LineFollower::computeIntegral(void){ |
bertgereels | 0:1f5782fc5ca3 | 195 | return integral += proportional; |
bertgereels | 0:1f5782fc5ca3 | 196 | } |
bertgereels | 0:1f5782fc5ca3 | 197 | |
bertgereels | 0:1f5782fc5ca3 | 198 | float LineFollower::computePower(void){ |
bertgereels | 0:1f5782fc5ca3 | 199 | return power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
bertgereels | 0:1f5782fc5ca3 | 200 | } |
bertgereels | 0:1f5782fc5ca3 | 201 | |
bertgereels | 0:1f5782fc5ca3 | 202 | void LineFollower::computeNewSpeeds(void){ |
bertgereels | 0:1f5782fc5ca3 | 203 | right = speed+power; |
bertgereels | 0:1f5782fc5ca3 | 204 | left = speed-power; |
bertgereels | 0:1f5782fc5ca3 | 205 | } |
bertgereels | 0:1f5782fc5ca3 | 206 | |
bertgereels | 0:1f5782fc5ca3 | 207 | void LineFollower::limitChecks(void){ |
bertgereels | 0:1f5782fc5ca3 | 208 | if (right < MIN_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 209 | right = MIN_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 210 | else if (right > MAX_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 211 | right = MAX_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 212 | |
bertgereels | 0:1f5782fc5ca3 | 213 | if (left < MIN_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 214 | left = MIN_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 215 | else if (left > MAX_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 216 | left = MAX_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 217 | } |
bertgereels | 0:1f5782fc5ca3 | 218 | |
bertgereels | 0:1f5782fc5ca3 | 219 | void LineFollower::turnLeft90(void){ |
bertgereels | 0:1f5782fc5ca3 | 220 | m3pi.left_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 221 | m3pi.right_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 222 | wait(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 223 | m3pi.stop(); |
bertgereels | 0:1f5782fc5ca3 | 224 | m3pi.left(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 225 | wait(MAX_SPEED*1.5); |
bertgereels | 0:1f5782fc5ca3 | 226 | } |
bertgereels | 0:1f5782fc5ca3 | 227 | |
bertgereels | 0:1f5782fc5ca3 | 228 | void LineFollower::turnRight90(void){ |
bertgereels | 0:1f5782fc5ca3 | 229 | m3pi.left_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 230 | m3pi.right_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 231 | wait(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 232 | m3pi.stop(); |
bertgereels | 0:1f5782fc5ca3 | 233 | m3pi.right(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 234 | wait(MAX_SPEED*1.5); |
bertgereels | 0:1f5782fc5ca3 | 235 | } |
bertgereels | 0:1f5782fc5ca3 | 236 | |
bertgereels | 0:1f5782fc5ca3 | 237 | void LineFollower::goBackwards(void){ |
bertgereels | 0:1f5782fc5ca3 | 238 | m3pi.left_motor(-left); |
bertgereels | 0:1f5782fc5ca3 | 239 | m3pi.right_motor(-right); |
bertgereels | 0:1f5782fc5ca3 | 240 | } |
bertgereels | 0:1f5782fc5ca3 | 241 | |
bertgereels | 0:1f5782fc5ca3 | 242 | void LineFollower::goForwards(void){ |
bertgereels | 0:1f5782fc5ca3 | 243 | m3pi.left_motor(left); |
bertgereels | 0:1f5782fc5ca3 | 244 | m3pi.right_motor(right); |
bertgereels | 0:1f5782fc5ca3 | 245 | } |
bertgereels | 0:1f5782fc5ca3 | 246 | |
bertgereels | 1:243ec35fafcd | 247 | void LineFollower::Scan(void){ |
bertgereels | 1:243ec35fafcd | 248 | for(int i = 0; i <= 2; i++){ |
bertgereels | 1:243ec35fafcd | 249 | m3pi.stop(); |
bertgereels | 1:243ec35fafcd | 250 | m3pi.right_motor(MAX_SPEED); |
bertgereels | 1:243ec35fafcd | 251 | wait_ms(30); |
bertgereels | 1:243ec35fafcd | 252 | m3pi.stop(); |
bertgereels | 1:243ec35fafcd | 253 | m3pi.right_motor(-MAX_SPEED); |
bertgereels | 1:243ec35fafcd | 254 | wait_ms(20); |
bertgereels | 1:243ec35fafcd | 255 | m3pi.stop(); |
bertgereels | 1:243ec35fafcd | 256 | m3pi.left_motor(MAX_SPEED); |
bertgereels | 1:243ec35fafcd | 257 | wait_ms(20); |
bertgereels | 1:243ec35fafcd | 258 | m3pi.stop(); |
bertgereels | 1:243ec35fafcd | 259 | m3pi.left_motor(-MAX_SPEED); |
bertgereels | 1:243ec35fafcd | 260 | wait_ms(10); |
bertgereels | 1:243ec35fafcd | 261 | } |
bertgereels | 1:243ec35fafcd | 262 | m3pi.stop(); |
bertgereels | 1:243ec35fafcd | 263 | } |
bertgereels | 1:243ec35fafcd | 264 | |
bertgereels | 1:243ec35fafcd | 265 | bool LineFollower::checkForIntersection(){ |
bertgereels | 0:1f5782fc5ca3 | 266 | unsigned int values[5]; |
bertgereels | 0:1f5782fc5ca3 | 267 | unsigned int* valuesPtr; |
bertgereels | 0:1f5782fc5ca3 | 268 | valuesPtr = values; |
bertgereels | 0:1f5782fc5ca3 | 269 | |
bertgereels | 0:1f5782fc5ca3 | 270 | m3pi.get_raw_values(valuesPtr); |
bertgereels | 1:243ec35fafcd | 271 | |
bertgereels | 1:243ec35fafcd | 272 | //check 2 outermost IR sensors on both sides |
bertgereels | 1:243ec35fafcd | 273 | if((values[0] > 1900 && values[1] > 1900) || (values[3] > 1900 && values[4] > 1900)){ |
bertgereels | 1:243ec35fafcd | 274 | return true; |
bertgereels | 0:1f5782fc5ca3 | 275 | }else{ |
bertgereels | 1:243ec35fafcd | 276 | return false; |
bertgereels | 0:1f5782fc5ca3 | 277 | } |
bertgereels | 0:1f5782fc5ca3 | 278 | } |
bertgereels | 0:1f5782fc5ca3 | 279 | |
bertgereels | 0:1f5782fc5ca3 | 280 | } |