Bert Gereels / Mbed 2 deprecated ProjectTwo

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?

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