Program that combines a linefollower program with visible ligt communication.
Dependencies: m3pi_custom mbed
linefollower.cpp@0:1f5782fc5ca3, 2018-05-09 (annotated)
- Committer:
- bertgereels
- Date:
- Wed May 09 15:35:31 2018 +0000
- Revision:
- 0:1f5782fc5ca3
- Child:
- 1:243ec35fafcd
this is a test
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 | 0:1f5782fc5ca3 | 5 | namespace ProjectTwo{ |
bertgereels | 0:1f5782fc5ca3 | 6 | |
bertgereels | 0:1f5782fc5ca3 | 7 | LineFollower::LineFollower(){ |
bertgereels | 0:1f5782fc5ca3 | 8 | current_pos_of_line = 0.0; |
bertgereels | 0:1f5782fc5ca3 | 9 | previous_pos_of_line = 0.0; |
bertgereels | 0:1f5782fc5ca3 | 10 | derivative = 0; |
bertgereels | 0:1f5782fc5ca3 | 11 | proportional = 0; |
bertgereels | 0:1f5782fc5ca3 | 12 | integral = 0; |
bertgereels | 0:1f5782fc5ca3 | 13 | speed = MAX_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 14 | |
bertgereels | 0:1f5782fc5ca3 | 15 | m3pi.locate(0,1); |
bertgereels | 0:1f5782fc5ca3 | 16 | m3pi.printf("Line PID"); |
bertgereels | 0:1f5782fc5ca3 | 17 | |
bertgereels | 0:1f5782fc5ca3 | 18 | wait(2.0); |
bertgereels | 0:1f5782fc5ca3 | 19 | m3pi.cls(); |
bertgereels | 0:1f5782fc5ca3 | 20 | |
bertgereels | 0:1f5782fc5ca3 | 21 | CurrentLineFollowerState = STATE_INIT; |
bertgereels | 0:1f5782fc5ca3 | 22 | |
bertgereels | 0:1f5782fc5ca3 | 23 | disableStop = false; |
bertgereels | 0:1f5782fc5ca3 | 24 | foundIntersection = false; |
bertgereels | 0:1f5782fc5ca3 | 25 | } |
bertgereels | 0:1f5782fc5ca3 | 26 | |
bertgereels | 0:1f5782fc5ca3 | 27 | void LineFollower::followTheLine(void){ |
bertgereels | 0:1f5782fc5ca3 | 28 | switch(CurrentLineFollowerState){ |
bertgereels | 0:1f5782fc5ca3 | 29 | case STATE_INIT: |
bertgereels | 0:1f5782fc5ca3 | 30 | { |
bertgereels | 0:1f5782fc5ca3 | 31 | m3pi.sensor_auto_calibrate(); |
bertgereels | 0:1f5782fc5ca3 | 32 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 33 | break; |
bertgereels | 0:1f5782fc5ca3 | 34 | } |
bertgereels | 0:1f5782fc5ca3 | 35 | case STATE_COMPUTE: |
bertgereels | 0:1f5782fc5ca3 | 36 | { |
bertgereels | 0:1f5782fc5ca3 | 37 | // Get the position of the line. |
bertgereels | 0:1f5782fc5ca3 | 38 | current_pos_of_line = m3pi.line_position(); |
bertgereels | 0:1f5782fc5ca3 | 39 | proportional = current_pos_of_line; |
bertgereels | 0:1f5782fc5ca3 | 40 | |
bertgereels | 0:1f5782fc5ca3 | 41 | computeDerivative(); |
bertgereels | 0:1f5782fc5ca3 | 42 | |
bertgereels | 0:1f5782fc5ca3 | 43 | computeIntegral(); |
bertgereels | 0:1f5782fc5ca3 | 44 | |
bertgereels | 0:1f5782fc5ca3 | 45 | // Remember the last position. |
bertgereels | 0:1f5782fc5ca3 | 46 | previous_pos_of_line = current_pos_of_line; |
bertgereels | 0:1f5782fc5ca3 | 47 | |
bertgereels | 0:1f5782fc5ca3 | 48 | // Compute the power |
bertgereels | 0:1f5782fc5ca3 | 49 | computePower(); |
bertgereels | 0:1f5782fc5ca3 | 50 | |
bertgereels | 0:1f5782fc5ca3 | 51 | // Compute new speeds |
bertgereels | 0:1f5782fc5ca3 | 52 | computeNewSpeeds(); |
bertgereels | 0:1f5782fc5ca3 | 53 | |
bertgereels | 0:1f5782fc5ca3 | 54 | // limit checks |
bertgereels | 0:1f5782fc5ca3 | 55 | limitChecks(); |
bertgereels | 0:1f5782fc5ca3 | 56 | |
bertgereels | 0:1f5782fc5ca3 | 57 | checkForIntersection(); |
bertgereels | 0:1f5782fc5ca3 | 58 | |
bertgereels | 0:1f5782fc5ca3 | 59 | if(foundIntersection){ |
bertgereels | 0:1f5782fc5ca3 | 60 | CurrentLineFollowerState = STATE_STOP; |
bertgereels | 0:1f5782fc5ca3 | 61 | }else if(!foundIntersection){ |
bertgereels | 0:1f5782fc5ca3 | 62 | CurrentLineFollowerState = STATE_FORWARD; |
bertgereels | 0:1f5782fc5ca3 | 63 | } |
bertgereels | 0:1f5782fc5ca3 | 64 | |
bertgereels | 0:1f5782fc5ca3 | 65 | m3pi.cls(); |
bertgereels | 0:1f5782fc5ca3 | 66 | |
bertgereels | 0:1f5782fc5ca3 | 67 | break; |
bertgereels | 0:1f5782fc5ca3 | 68 | } |
bertgereels | 0:1f5782fc5ca3 | 69 | case STATE_FORWARD: |
bertgereels | 0:1f5782fc5ca3 | 70 | { |
bertgereels | 0:1f5782fc5ca3 | 71 | goForwards(); |
bertgereels | 0:1f5782fc5ca3 | 72 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 73 | break; |
bertgereels | 0:1f5782fc5ca3 | 74 | } |
bertgereels | 0:1f5782fc5ca3 | 75 | case STATE_LEFT: |
bertgereels | 0:1f5782fc5ca3 | 76 | { |
bertgereels | 0:1f5782fc5ca3 | 77 | turnLeft90(); |
bertgereels | 0:1f5782fc5ca3 | 78 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 79 | break; |
bertgereels | 0:1f5782fc5ca3 | 80 | } |
bertgereels | 0:1f5782fc5ca3 | 81 | case STATE_RIGHT: |
bertgereels | 0:1f5782fc5ca3 | 82 | { |
bertgereels | 0:1f5782fc5ca3 | 83 | turnRight90(); |
bertgereels | 0:1f5782fc5ca3 | 84 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 85 | break; |
bertgereels | 0:1f5782fc5ca3 | 86 | } |
bertgereels | 0:1f5782fc5ca3 | 87 | case STATE_BACKWARD: |
bertgereels | 0:1f5782fc5ca3 | 88 | { |
bertgereels | 0:1f5782fc5ca3 | 89 | CurrentLineFollowerState = STATE_COMPUTE; |
bertgereels | 0:1f5782fc5ca3 | 90 | break; |
bertgereels | 0:1f5782fc5ca3 | 91 | } |
bertgereels | 0:1f5782fc5ca3 | 92 | case STATE_STOP: |
bertgereels | 0:1f5782fc5ca3 | 93 | { |
bertgereels | 0:1f5782fc5ca3 | 94 | m3pi.stop(); |
bertgereels | 0:1f5782fc5ca3 | 95 | CurrentLineFollowerState = STATE_VLC; |
bertgereels | 0:1f5782fc5ca3 | 96 | break; |
bertgereels | 0:1f5782fc5ca3 | 97 | } |
bertgereels | 0:1f5782fc5ca3 | 98 | case STATE_VLC: |
bertgereels | 0:1f5782fc5ca3 | 99 | { |
bertgereels | 0:1f5782fc5ca3 | 100 | m3pi.cls(); |
bertgereels | 0:1f5782fc5ca3 | 101 | m3pi.locate(0,0); |
bertgereels | 0:1f5782fc5ca3 | 102 | m3pi.printf("VLC"); |
bertgereels | 0:1f5782fc5ca3 | 103 | m3pi.locate(0,1); |
bertgereels | 0:1f5782fc5ca3 | 104 | m3pi.printf("Activated"); |
bertgereels | 0:1f5782fc5ca3 | 105 | //CurrentLineFollowerState = STATE_COMPUTE; when communication is done, decide which direction to go |
bertgereels | 0:1f5782fc5ca3 | 106 | //Call to VLC obj jelle |
bertgereels | 0:1f5782fc5ca3 | 107 | //wait(1); |
bertgereels | 0:1f5782fc5ca3 | 108 | //CurrentLineFollowerState = STATE_LEFT; |
bertgereels | 0:1f5782fc5ca3 | 109 | |
bertgereels | 0:1f5782fc5ca3 | 110 | int index = decoder.decode(); |
bertgereels | 0:1f5782fc5ca3 | 111 | wait(1); //wait 1 sec before executing selected direction |
bertgereels | 0:1f5782fc5ca3 | 112 | switch(index){ |
bertgereels | 0:1f5782fc5ca3 | 113 | case 0:{ //links |
bertgereels | 0:1f5782fc5ca3 | 114 | CurrentLineFollowerState = STATE_LEFT; |
bertgereels | 0:1f5782fc5ca3 | 115 | break; |
bertgereels | 0:1f5782fc5ca3 | 116 | } |
bertgereels | 0:1f5782fc5ca3 | 117 | case 1:{ //rechts |
bertgereels | 0:1f5782fc5ca3 | 118 | CurrentLineFollowerState = STATE_RIGHT; |
bertgereels | 0:1f5782fc5ca3 | 119 | break; |
bertgereels | 0:1f5782fc5ca3 | 120 | } |
bertgereels | 0:1f5782fc5ca3 | 121 | case 2:{ //rechtdoor |
bertgereels | 0:1f5782fc5ca3 | 122 | CurrentLineFollowerState = STATE_FORWARD; |
bertgereels | 0:1f5782fc5ca3 | 123 | break; |
bertgereels | 0:1f5782fc5ca3 | 124 | } |
bertgereels | 0:1f5782fc5ca3 | 125 | case 3:{ //achteruit |
bertgereels | 0:1f5782fc5ca3 | 126 | CurrentLineFollowerState = STATE_BACKWARD; |
bertgereels | 0:1f5782fc5ca3 | 127 | break; |
bertgereels | 0:1f5782fc5ca3 | 128 | } |
bertgereels | 0:1f5782fc5ca3 | 129 | } |
bertgereels | 0:1f5782fc5ca3 | 130 | } |
bertgereels | 0:1f5782fc5ca3 | 131 | } |
bertgereels | 0:1f5782fc5ca3 | 132 | } |
bertgereels | 0:1f5782fc5ca3 | 133 | |
bertgereels | 0:1f5782fc5ca3 | 134 | float LineFollower::computeDerivative(void){ |
bertgereels | 0:1f5782fc5ca3 | 135 | return derivative = current_pos_of_line - previous_pos_of_line; |
bertgereels | 0:1f5782fc5ca3 | 136 | } |
bertgereels | 0:1f5782fc5ca3 | 137 | |
bertgereels | 0:1f5782fc5ca3 | 138 | float LineFollower::computeIntegral(void){ |
bertgereels | 0:1f5782fc5ca3 | 139 | return integral += proportional; |
bertgereels | 0:1f5782fc5ca3 | 140 | } |
bertgereels | 0:1f5782fc5ca3 | 141 | |
bertgereels | 0:1f5782fc5ca3 | 142 | float LineFollower::computePower(void){ |
bertgereels | 0:1f5782fc5ca3 | 143 | return power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
bertgereels | 0:1f5782fc5ca3 | 144 | } |
bertgereels | 0:1f5782fc5ca3 | 145 | |
bertgereels | 0:1f5782fc5ca3 | 146 | void LineFollower::computeNewSpeeds(void){ |
bertgereels | 0:1f5782fc5ca3 | 147 | right = speed+power; |
bertgereels | 0:1f5782fc5ca3 | 148 | left = speed-power; |
bertgereels | 0:1f5782fc5ca3 | 149 | } |
bertgereels | 0:1f5782fc5ca3 | 150 | |
bertgereels | 0:1f5782fc5ca3 | 151 | void LineFollower::limitChecks(void){ |
bertgereels | 0:1f5782fc5ca3 | 152 | if (right < MIN_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 153 | right = MIN_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 154 | else if (right > MAX_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 155 | right = MAX_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 156 | |
bertgereels | 0:1f5782fc5ca3 | 157 | if (left < MIN_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 158 | left = MIN_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 159 | else if (left > MAX_SPEED) |
bertgereels | 0:1f5782fc5ca3 | 160 | left = MAX_SPEED; |
bertgereels | 0:1f5782fc5ca3 | 161 | } |
bertgereels | 0:1f5782fc5ca3 | 162 | |
bertgereels | 0:1f5782fc5ca3 | 163 | void LineFollower::turnLeft90(void){ |
bertgereels | 0:1f5782fc5ca3 | 164 | m3pi.left_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 165 | m3pi.right_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 166 | wait(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 167 | m3pi.stop(); |
bertgereels | 0:1f5782fc5ca3 | 168 | m3pi.left(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 169 | wait(MAX_SPEED*1.5); |
bertgereels | 0:1f5782fc5ca3 | 170 | } |
bertgereels | 0:1f5782fc5ca3 | 171 | |
bertgereels | 0:1f5782fc5ca3 | 172 | void LineFollower::turnRight90(void){ |
bertgereels | 0:1f5782fc5ca3 | 173 | m3pi.left_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 174 | m3pi.right_motor(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 175 | wait(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 176 | m3pi.stop(); |
bertgereels | 0:1f5782fc5ca3 | 177 | m3pi.right(MAX_SPEED); |
bertgereels | 0:1f5782fc5ca3 | 178 | wait(MAX_SPEED*1.5); |
bertgereels | 0:1f5782fc5ca3 | 179 | } |
bertgereels | 0:1f5782fc5ca3 | 180 | |
bertgereels | 0:1f5782fc5ca3 | 181 | void LineFollower::goBackwards(void){ |
bertgereels | 0:1f5782fc5ca3 | 182 | m3pi.left_motor(-left); |
bertgereels | 0:1f5782fc5ca3 | 183 | m3pi.right_motor(-right); |
bertgereels | 0:1f5782fc5ca3 | 184 | } |
bertgereels | 0:1f5782fc5ca3 | 185 | |
bertgereels | 0:1f5782fc5ca3 | 186 | void LineFollower::goForwards(void){ |
bertgereels | 0:1f5782fc5ca3 | 187 | m3pi.left_motor(left); |
bertgereels | 0:1f5782fc5ca3 | 188 | m3pi.right_motor(right); |
bertgereels | 0:1f5782fc5ca3 | 189 | } |
bertgereels | 0:1f5782fc5ca3 | 190 | |
bertgereels | 0:1f5782fc5ca3 | 191 | void LineFollower::checkForIntersection(){ |
bertgereels | 0:1f5782fc5ca3 | 192 | unsigned int values[5]; |
bertgereels | 0:1f5782fc5ca3 | 193 | unsigned int* valuesPtr; |
bertgereels | 0:1f5782fc5ca3 | 194 | valuesPtr = values; |
bertgereels | 0:1f5782fc5ca3 | 195 | |
bertgereels | 0:1f5782fc5ca3 | 196 | m3pi.get_raw_values(valuesPtr); |
bertgereels | 0:1f5782fc5ca3 | 197 | |
bertgereels | 0:1f5782fc5ca3 | 198 | if(values[0] >= 2000 || values[4] >= 2000){ |
bertgereels | 0:1f5782fc5ca3 | 199 | foundIntersection = true; |
bertgereels | 0:1f5782fc5ca3 | 200 | }else{ |
bertgereels | 0:1f5782fc5ca3 | 201 | foundIntersection = false; |
bertgereels | 0:1f5782fc5ca3 | 202 | } |
bertgereels | 0:1f5782fc5ca3 | 203 | } |
bertgereels | 0:1f5782fc5ca3 | 204 | |
bertgereels | 0:1f5782fc5ca3 | 205 | } |