Program that combines a linefollower program with visible ligt communication.
Dependencies: m3pi_custom mbed
linefollower.cpp
- Committer:
- bertgereels
- Date:
- 2018-05-16
- Revision:
- 2:21fb894dc9d6
- Parent:
- 1:243ec35fafcd
File content as of revision 2:21fb894dc9d6:
#include "mbed.h" #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; derivative = 0; proportional = 0; integral = 0; speed = MAX_SPEED; m3pi.locate(0,1); m3pi.printf("Line PID"); wait(2.0); m3pi.cls(); CurrentLineFollowerState = STATE_INIT; vlcDecoder = Manchester(); } void LineFollower::followTheLine(void){ switch(CurrentLineFollowerState){ case STATE_INIT: { m3pi.sensor_auto_calibrate(); CurrentLineFollowerState = STATE_COMPUTE; break; } 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(); // Remember the last position. previous_pos_of_line = current_pos_of_line; // Compute the power computePower(); // Compute new speeds computeNewSpeeds(); // limit checks limitChecks(); if(checkForIntersection()){ CurrentLineFollowerState = STATE_STOP; }else if(!checkForIntersection()){ CurrentLineFollowerState = STATE_FORWARD; } m3pi.cls(); break; } case STATE_FORWARD: { goForwards(); 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(); CurrentLineFollowerState = STATE_COMPUTE; break; } case STATE_RIGHT: { turnRight90(); CurrentLineFollowerState = STATE_COMPUTE; break; } case STATE_TURN180: { CurrentLineFollowerState = STATE_COMPUTE; turnRight90(); turnRight90(); break; } case STATE_STOP: { m3pi.stop(); 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: { m3pi.cls(); m3pi.locate(0,0); m3pi.printf("VLC"); m3pi.locate(0,1); m3pi.printf("Activate"); wait(1); //retry decoding if detected value is equal to 5 = faulty stop direction_index = vlcDecoder.decode(); }while(direction_index == 5); wait(0.5); m3pi.cls(); m3pi.printf("DIR fnd"); wait(1); //wait 1 sec before executing selected direction switch(direction_index){ case 0:{ //left CurrentLineFollowerState = STATE_LEFT; ledLinks = 1; break; } case 1:{ //right CurrentLineFollowerState = STATE_RIGHT; ledRechts = 1; break; } case 2:{ //forwards CurrentLineFollowerState = STATE_LONG_FORWARD; ledRechtdoor = 1; break; } case 3:{ //backwards CurrentLineFollowerState = STATE_TURN180; ledAchteruit = 1; break; } case 4:{ //ignore CurrentLineFollowerState = STATE_LONG_FORWARD; break; } default:{ CurrentLineFollowerState = STATE_LONG_FORWARD; m3pi.cls(); } } break; } } } float LineFollower::computeDerivative(void){ return derivative = current_pos_of_line - previous_pos_of_line; } float LineFollower::computeIntegral(void){ return integral += proportional; } float LineFollower::computePower(void){ return power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; } void LineFollower::computeNewSpeeds(void){ right = speed+power; left = speed-power; } void LineFollower::limitChecks(void){ if (right < MIN_SPEED) right = MIN_SPEED; else if (right > MAX_SPEED) right = MAX_SPEED; if (left < MIN_SPEED) left = MIN_SPEED; else if (left > MAX_SPEED) left = MAX_SPEED; } void LineFollower::turnLeft90(void){ m3pi.left_motor(MAX_SPEED); m3pi.right_motor(MAX_SPEED); wait(MAX_SPEED); m3pi.stop(); m3pi.left(MAX_SPEED); wait(MAX_SPEED*1.5); } void LineFollower::turnRight90(void){ m3pi.left_motor(MAX_SPEED); m3pi.right_motor(MAX_SPEED); wait(MAX_SPEED); m3pi.stop(); m3pi.right(MAX_SPEED); wait(MAX_SPEED*1.5); } void LineFollower::goBackwards(void){ m3pi.left_motor(-left); m3pi.right_motor(-right); } void LineFollower::goForwards(void){ m3pi.left_motor(left); m3pi.right_motor(right); } 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); //check 2 outermost IR sensors on both sides if((values[0] > 1900 && values[1] > 1900) || (values[3] > 1900 && values[4] > 1900)){ return true; }else{ return false; } } }