Bert Gereels / Mbed 2 deprecated ProjectTwo

Program that combines a linefollower program with visible ligt communication.

Dependencies:   m3pi_custom mbed

linefollower.cpp@2:21fb894dc9d6, 2018-05-16 (annotated)

Committer:
bertgereels
Date:
Wed May 16 19:18:11 2018 +0000
Revision:
2:21fb894dc9d6
Parent:
1:243ec35fafcd 
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 2:21fb894dc9d6 144 //retry decoding if detected value is equal to 5 = faulty stop
bertgereels 1:243ec35fafcd 145 direction_index = vlcDecoder.decode();
bertgereels 1:243ec35fafcd 146 }while(direction_index == 5);
bertgereels 0:1f5782fc5ca3 147
bertgereels 1:243ec35fafcd 148 wait(0.5);
bertgereels 1:243ec35fafcd 149 m3pi.cls();
bertgereels 1:243ec35fafcd 150 m3pi.printf("DIR fnd");
bertgereels 1:243ec35fafcd 151
bertgereels 0:1f5782fc5ca3 152 wait(1); //wait 1 sec before executing selected direction
bertgereels 1:243ec35fafcd 153 switch(direction_index){
bertgereels 1:243ec35fafcd 154 case 0:{ //left
bertgereels 0:1f5782fc5ca3 155 CurrentLineFollowerState = STATE_LEFT;
bertgereels 1:243ec35fafcd 156 ledLinks = 1;
bertgereels 0:1f5782fc5ca3 157 break;
bertgereels 0:1f5782fc5ca3 158 }
bertgereels 1:243ec35fafcd 159 case 1:{ //right
bertgereels 0:1f5782fc5ca3 160 CurrentLineFollowerState = STATE_RIGHT;
bertgereels 1:243ec35fafcd 161 ledRechts = 1;
bertgereels 1:243ec35fafcd 162 break;
bertgereels 1:243ec35fafcd 163 }
bertgereels 1:243ec35fafcd 164 case 2:{ //forwards
bertgereels 1:243ec35fafcd 165 CurrentLineFollowerState = STATE_LONG_FORWARD;
bertgereels 1:243ec35fafcd 166 ledRechtdoor = 1;
bertgereels 0:1f5782fc5ca3 167 break;
bertgereels 0:1f5782fc5ca3 168 }
bertgereels 1:243ec35fafcd 169 case 3:{ //backwards
bertgereels 1:243ec35fafcd 170 CurrentLineFollowerState = STATE_TURN180;
bertgereels 1:243ec35fafcd 171 ledAchteruit = 1;
bertgereels 0:1f5782fc5ca3 172 break;
bertgereels 0:1f5782fc5ca3 173 }
bertgereels 1:243ec35fafcd 174 case 4:{ //ignore
bertgereels 1:243ec35fafcd 175 CurrentLineFollowerState = STATE_LONG_FORWARD;
bertgereels 0:1f5782fc5ca3 176 break;
bertgereels 0:1f5782fc5ca3 177 }
bertgereels 1:243ec35fafcd 178 default:{
bertgereels 1:243ec35fafcd 179 CurrentLineFollowerState = STATE_LONG_FORWARD;
bertgereels 1:243ec35fafcd 180 m3pi.cls();
bertgereels 1:243ec35fafcd 181 }
bertgereels 0:1f5782fc5ca3 182 }
bertgereels 1:243ec35fafcd 183
bertgereels 1:243ec35fafcd 184 break;
bertgereels 0:1f5782fc5ca3 185 }
bertgereels 0:1f5782fc5ca3 186 }
bertgereels 0:1f5782fc5ca3 187 }
bertgereels 0:1f5782fc5ca3 188
bertgereels 0:1f5782fc5ca3 189 float LineFollower::computeDerivative(void){
bertgereels 0:1f5782fc5ca3 190 return derivative = current_pos_of_line - previous_pos_of_line;
bertgereels 0:1f5782fc5ca3 191 }
bertgereels 0:1f5782fc5ca3 192
bertgereels 0:1f5782fc5ca3 193 float LineFollower::computeIntegral(void){
bertgereels 0:1f5782fc5ca3 194 return integral += proportional;
bertgereels 0:1f5782fc5ca3 195 }
bertgereels 0:1f5782fc5ca3 196
bertgereels 0:1f5782fc5ca3 197 float LineFollower::computePower(void){
bertgereels 0:1f5782fc5ca3 198 return power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
bertgereels 0:1f5782fc5ca3 199 }
bertgereels 0:1f5782fc5ca3 200
bertgereels 0:1f5782fc5ca3 201 void LineFollower::computeNewSpeeds(void){
bertgereels 0:1f5782fc5ca3 202 right = speed+power;
bertgereels 0:1f5782fc5ca3 203 left = speed-power;
bertgereels 0:1f5782fc5ca3 204 }
bertgereels 0:1f5782fc5ca3 205
bertgereels 0:1f5782fc5ca3 206 void LineFollower::limitChecks(void){
bertgereels 0:1f5782fc5ca3 207 if (right < MIN_SPEED)
bertgereels 0:1f5782fc5ca3 208 right = MIN_SPEED;
bertgereels 0:1f5782fc5ca3 209 else if (right > MAX_SPEED)
bertgereels 0:1f5782fc5ca3 210 right = MAX_SPEED;
bertgereels 0:1f5782fc5ca3 211
bertgereels 0:1f5782fc5ca3 212 if (left < MIN_SPEED)
bertgereels 0:1f5782fc5ca3 213 left = MIN_SPEED;
bertgereels 0:1f5782fc5ca3 214 else if (left > MAX_SPEED)
bertgereels 0:1f5782fc5ca3 215 left = MAX_SPEED;
bertgereels 0:1f5782fc5ca3 216 }
bertgereels 0:1f5782fc5ca3 217
bertgereels 0:1f5782fc5ca3 218 void LineFollower::turnLeft90(void){
bertgereels 0:1f5782fc5ca3 219 m3pi.left_motor(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 220 m3pi.right_motor(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 221 wait(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 222 m3pi.stop();
bertgereels 0:1f5782fc5ca3 223 m3pi.left(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 224 wait(MAX_SPEED*1.5);
bertgereels 0:1f5782fc5ca3 225 }
bertgereels 0:1f5782fc5ca3 226
bertgereels 0:1f5782fc5ca3 227 void LineFollower::turnRight90(void){
bertgereels 0:1f5782fc5ca3 228 m3pi.left_motor(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 229 m3pi.right_motor(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 230 wait(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 231 m3pi.stop();
bertgereels 0:1f5782fc5ca3 232 m3pi.right(MAX_SPEED);
bertgereels 0:1f5782fc5ca3 233 wait(MAX_SPEED*1.5);
bertgereels 0:1f5782fc5ca3 234 }
bertgereels 0:1f5782fc5ca3 235
bertgereels 0:1f5782fc5ca3 236 void LineFollower::goBackwards(void){
bertgereels 0:1f5782fc5ca3 237 m3pi.left_motor(-left);
bertgereels 0:1f5782fc5ca3 238 m3pi.right_motor(-right);
bertgereels 0:1f5782fc5ca3 239 }
bertgereels 0:1f5782fc5ca3 240
bertgereels 0:1f5782fc5ca3 241 void LineFollower::goForwards(void){
bertgereels 0:1f5782fc5ca3 242 m3pi.left_motor(left);
bertgereels 0:1f5782fc5ca3 243 m3pi.right_motor(right);
bertgereels 0:1f5782fc5ca3 244 }
bertgereels 0:1f5782fc5ca3 245
bertgereels 1:243ec35fafcd 246 void LineFollower::Scan(void){
bertgereels 1:243ec35fafcd 247 for(int i = 0; i <= 2; i++){
bertgereels 1:243ec35fafcd 248 m3pi.stop();
bertgereels 1:243ec35fafcd 249 m3pi.right_motor(MAX_SPEED);
bertgereels 1:243ec35fafcd 250 wait_ms(30);
bertgereels 1:243ec35fafcd 251 m3pi.stop();
bertgereels 1:243ec35fafcd 252 m3pi.right_motor(-MAX_SPEED);
bertgereels 1:243ec35fafcd 253 wait_ms(20);
bertgereels 1:243ec35fafcd 254 m3pi.stop();
bertgereels 1:243ec35fafcd 255 m3pi.left_motor(MAX_SPEED);
bertgereels 1:243ec35fafcd 256 wait_ms(20);
bertgereels 1:243ec35fafcd 257 m3pi.stop();
bertgereels 1:243ec35fafcd 258 m3pi.left_motor(-MAX_SPEED);
bertgereels 1:243ec35fafcd 259 wait_ms(10);
bertgereels 1:243ec35fafcd 260 }
bertgereels 1:243ec35fafcd 261 m3pi.stop();
bertgereels 1:243ec35fafcd 262 }
bertgereels 1:243ec35fafcd 263
bertgereels 1:243ec35fafcd 264 bool LineFollower::checkForIntersection(){
bertgereels 0:1f5782fc5ca3 265 unsigned int values[5];
bertgereels 0:1f5782fc5ca3 266 unsigned int* valuesPtr;
bertgereels 0:1f5782fc5ca3 267 valuesPtr = values;
bertgereels 0:1f5782fc5ca3 268
bertgereels 0:1f5782fc5ca3 269 m3pi.get_raw_values(valuesPtr);
bertgereels 1:243ec35fafcd 270
bertgereels 1:243ec35fafcd 271 //check 2 outermost IR sensors on both sides
bertgereels 1:243ec35fafcd 272 if((values[0] > 1900 && values[1] > 1900) || (values[3] > 1900 && values[4] > 1900)){
bertgereels 1:243ec35fafcd 273 return true;
bertgereels 0:1f5782fc5ca3 274 }else{
bertgereels 1:243ec35fafcd 275 return false;
bertgereels 0:1f5782fc5ca3 276 }
bertgereels 0:1f5782fc5ca3 277 }
bertgereels 0:1f5782fc5ca3 278
bertgereels 0:1f5782fc5ca3 279 }