![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Program that combines a linefollower program with visible ligt communication.
Dependencies: m3pi_custom mbed
Diff: linefollower.cpp
- Revision:
- 1:243ec35fafcd
- Parent:
- 0:1f5782fc5ca3
- Child:
- 2:21fb894dc9d6
diff -r 1f5782fc5ca3 -r 243ec35fafcd linefollower.cpp --- a/linefollower.cpp Wed May 09 15:35:31 2018 +0000 +++ b/linefollower.cpp Wed May 16 19:12:20 2018 +0000 @@ -2,8 +2,13 @@ #include "linefollower.h" #include <string> +DigitalOut ledLinks(LED1); +DigitalOut ledRechts(LED2); +DigitalOut ledRechtdoor(LED3); +DigitalOut ledAchteruit(LED4); + namespace ProjectTwo{ - + LineFollower::LineFollower(){ current_pos_of_line = 0.0; previous_pos_of_line = 0.0; @@ -14,14 +19,11 @@ m3pi.locate(0,1); m3pi.printf("Line PID"); - wait(2.0); m3pi.cls(); CurrentLineFollowerState = STATE_INIT; - - disableStop = false; - foundIntersection = false; + vlcDecoder = Manchester(); } void LineFollower::followTheLine(void){ @@ -34,10 +36,14 @@ } case STATE_COMPUTE: { + ledLinks = 0; + ledRechts = 0; + ledRechtdoor = 0; + ledAchteruit = 0; // Get the position of the line. current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; - + computeDerivative(); computeIntegral(); @@ -53,12 +59,10 @@ // limit checks limitChecks(); - - checkForIntersection(); - - if(foundIntersection){ + + if(checkForIntersection()){ CurrentLineFollowerState = STATE_STOP; - }else if(!foundIntersection){ + }else if(!checkForIntersection()){ CurrentLineFollowerState = STATE_FORWARD; } @@ -72,6 +76,17 @@ CurrentLineFollowerState = STATE_COMPUTE; break; } + case STATE_LONG_FORWARD: + { + int i = 0; + do{ + goForwards(); + i ++; + }while(i < 750); + CurrentLineFollowerState = STATE_COMPUTE; + break; + + } case STATE_LEFT: { turnLeft90(); @@ -84,15 +99,37 @@ CurrentLineFollowerState = STATE_COMPUTE; break; } - case STATE_BACKWARD: + case STATE_TURN180: { CurrentLineFollowerState = STATE_COMPUTE; + turnRight90(); + turnRight90(); break; } case STATE_STOP: { m3pi.stop(); - CurrentLineFollowerState = STATE_VLC; + m3pi.printf("K-punt"); + + bool searchForLight = true; + int scanCounter = 0; + while(searchForLight){ + if(analogSensor.getVoltage() > 0.6){ + CurrentLineFollowerState = STATE_VLC; + searchForLight = false; + } + else{ + Scan(); + scanCounter++; + } + + if(scanCounter == 2){ + CurrentLineFollowerState = STATE_FORWARD; + searchForLight = false; + } + wait(0.5); + } + break; } case STATE_VLC: @@ -101,32 +138,51 @@ m3pi.locate(0,0); m3pi.printf("VLC"); m3pi.locate(0,1); - m3pi.printf("Activated"); - //CurrentLineFollowerState = STATE_COMPUTE; when communication is done, decide which direction to go - //Call to VLC obj jelle - //wait(1); - //CurrentLineFollowerState = STATE_LEFT; + m3pi.printf("Activate"); + wait(1); + + //retry decoding if detected value is equal to 5 + do{ + direction_index = vlcDecoder.decode(); + }while(direction_index == 5); - int index = decoder.decode(); + wait(0.5); + m3pi.cls(); + m3pi.printf("DIR fnd"); + wait(1); //wait 1 sec before executing selected direction - switch(index){ - case 0:{ //links + switch(direction_index){ + case 0:{ //left CurrentLineFollowerState = STATE_LEFT; + ledLinks = 1; break; } - case 1:{ //rechts + case 1:{ //right CurrentLineFollowerState = STATE_RIGHT; + ledRechts = 1; + break; + } + case 2:{ //forwards + CurrentLineFollowerState = STATE_LONG_FORWARD; + ledRechtdoor = 1; break; } - case 2:{ //rechtdoor - CurrentLineFollowerState = STATE_FORWARD; + case 3:{ //backwards + CurrentLineFollowerState = STATE_TURN180; + ledAchteruit = 1; break; } - case 3:{ //achteruit - CurrentLineFollowerState = STATE_BACKWARD; + case 4:{ //ignore + CurrentLineFollowerState = STATE_LONG_FORWARD; break; } + default:{ + CurrentLineFollowerState = STATE_LONG_FORWARD; + m3pi.cls(); + } } + + break; } } } @@ -188,17 +244,36 @@ m3pi.right_motor(right); } - void LineFollower::checkForIntersection(){ + void LineFollower::Scan(void){ + for(int i = 0; i <= 2; i++){ + m3pi.stop(); + m3pi.right_motor(MAX_SPEED); + wait_ms(30); + m3pi.stop(); + m3pi.right_motor(-MAX_SPEED); + wait_ms(20); + m3pi.stop(); + m3pi.left_motor(MAX_SPEED); + wait_ms(20); + m3pi.stop(); + m3pi.left_motor(-MAX_SPEED); + wait_ms(10); + } + m3pi.stop(); + } + + bool LineFollower::checkForIntersection(){ unsigned int values[5]; unsigned int* valuesPtr; valuesPtr = values; m3pi.get_raw_values(valuesPtr); - - if(values[0] >= 2000 || values[4] >= 2000){ - foundIntersection = true; + + //check 2 outermost IR sensors on both sides + if((values[0] > 1900 && values[1] > 1900) || (values[3] > 1900 && values[4] > 1900)){ + return true; }else{ - foundIntersection = false; + return false; } }