Bert Gereels / Mbed 2 deprecated ProjectTwo

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?

UserRevisionLine numberNew 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 }