Basic Line Following Program motors: p25, p10 inputs: p15, p16 Current settings: lineToSpeedFactor = 10 / 3 speedModifier = -4
Dependencies: mbed
Fork of RenBuggyLineFollower by
This small program lets a buggy follow a line (black line on white background)
main.cpp@2:2439f5a4a93d, 2016-07-14 (annotated)
- Committer:
- DanArgust
- Date:
- Thu Jul 14 10:55:33 2016 +0000
- Revision:
- 2:2439f5a4a93d
- Parent:
- 0:466ee63955df
- Child:
- 3:936111f70e37
added 'searching' functionality
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DanArgust | 0:466ee63955df | 1 | /********************************************************* |
DanArgust | 0:466ee63955df | 2 | *RenBuggyLineFollower * |
DanArgust | 0:466ee63955df | 3 | *Author: Dan Argust * |
DanArgust | 0:466ee63955df | 4 | * * |
DanArgust | 0:466ee63955df | 5 | *This program demonstates the use of two sensors to * |
DanArgust | 0:466ee63955df | 6 | *detect and follow a thin black line. * |
DanArgust | 0:466ee63955df | 7 | *********************************************************/ |
DanArgust | 0:466ee63955df | 8 | |
DanArgust | 2:2439f5a4a93d | 9 | #include "mbed.h" |
DanArgust | 0:466ee63955df | 10 | |
DanArgust | 0:466ee63955df | 11 | AnalogIn ain0(p15); |
DanArgust | 0:466ee63955df | 12 | AnalogIn ain1(p16); |
DanArgust | 0:466ee63955df | 13 | |
DanArgust | 0:466ee63955df | 14 | PwmOut pwm0(p25); |
DanArgust | 0:466ee63955df | 15 | PwmOut pwm1(p10); |
DanArgust | 0:466ee63955df | 16 | |
DanArgust | 0:466ee63955df | 17 | float Lmotor; |
DanArgust | 0:466ee63955df | 18 | float Rmotor; |
DanArgust | 0:466ee63955df | 19 | float newLine; |
DanArgust | 0:466ee63955df | 20 | float oldLine; |
DanArgust | 2:2439f5a4a93d | 21 | bool searching; |
DanArgust | 2:2439f5a4a93d | 22 | int counter; |
DanArgust | 0:466ee63955df | 23 | |
DanArgust | 0:466ee63955df | 24 | void getMotorSpeeds(float a,float b){ |
DanArgust | 0:466ee63955df | 25 | float inputs[2] = {a,b}; |
DanArgust | 0:466ee63955df | 26 | float max_speed = 1.0; |
DanArgust | 0:466ee63955df | 27 | Lmotor = 0; |
DanArgust | 0:466ee63955df | 28 | Rmotor = 0; |
DanArgust | 0:466ee63955df | 29 | newLine = 0; |
DanArgust | 2:2439f5a4a93d | 30 | float line_to_speed_factor = max_speed/0.3; |
DanArgust | 0:466ee63955df | 31 | for(int i = 0;i<2;++i){ |
DanArgust | 0:466ee63955df | 32 | newLine += inputs[i]*(i+1); |
DanArgust | 0:466ee63955df | 33 | } |
DanArgust | 0:466ee63955df | 34 | float sum = 0; |
DanArgust | 0:466ee63955df | 35 | for (int i = 0;i<2;++i){ |
DanArgust | 0:466ee63955df | 36 | sum += inputs[i]; |
DanArgust | 0:466ee63955df | 37 | } |
DanArgust | 0:466ee63955df | 38 | newLine = newLine / sum; |
DanArgust | 0:466ee63955df | 39 | if((a<0.2)&&(b<0.2)){ |
DanArgust | 0:466ee63955df | 40 | if(oldLine>1.5) |
DanArgust | 0:466ee63955df | 41 | newLine = 2; |
DanArgust | 0:466ee63955df | 42 | else |
DanArgust | 0:466ee63955df | 43 | newLine = 1; |
DanArgust | 2:2439f5a4a93d | 44 | searching = true |
DanArgust | 2:2439f5a4a93d | 45 | } |
DanArgust | 2:2439f5a4a93d | 46 | if(searching){ |
DanArgust | 2:2439f5a4a93d | 47 | if(abs(a - b)<0.2){ |
DanArgust | 2:2439f5a4a93d | 48 | newLine = oldLine; |
DanArgust | 2:2439f5a4a93d | 49 | } |
DanArgust | 2:2439f5a4a93d | 50 | counter++ |
DanArgust | 2:2439f5a4a93d | 51 | if(counter>300){ |
DanArgust | 2:2439f5a4a93d | 52 | newLine = 3 - oldLine; |
DanArgust | 2:2439f5a4a93d | 53 | counter = 0; |
DanArgust | 2:2439f5a4a93d | 54 | } |
DanArgust | 2:2439f5a4a93d | 55 | else{ |
DanArgust | 2:2439f5a4a93d | 56 | searching = false; |
DanArgust | 2:2439f5a4a93d | 57 | } |
DanArgust | 0:466ee63955df | 58 | } |
DanArgust | 0:466ee63955df | 59 | oldLine = newLine; |
DanArgust | 2:2439f5a4a93d | 60 | Lmotor = newLine*line_to_speed_factor-4; |
DanArgust | 2:2439f5a4a93d | 61 | Rmotor = 2-(newLine*line_to_speed_factor-4); |
DanArgust | 0:466ee63955df | 62 | if(Lmotor>max_speed){ |
DanArgust | 0:466ee63955df | 63 | Lmotor = max_speed; |
DanArgust | 0:466ee63955df | 64 | } |
DanArgust | 0:466ee63955df | 65 | if(Rmotor>max_speed){ |
DanArgust | 0:466ee63955df | 66 | Rmotor = max_speed; |
DanArgust | 0:466ee63955df | 67 | } |
DanArgust | 0:466ee63955df | 68 | } |
DanArgust | 0:466ee63955df | 69 | |
DanArgust | 0:466ee63955df | 70 | int main() |
DanArgust | 0:466ee63955df | 71 | { |
DanArgust | 2:2439f5a4a93d | 72 | searching = false |
DanArgust | 2:2439f5a4a93d | 73 | counter = 0 |
DanArgust | 0:466ee63955df | 74 | while(true){ |
DanArgust | 0:466ee63955df | 75 | getMotorSpeeds(ain0.read(),ain1.read()); |
DanArgust | 0:466ee63955df | 76 | pwm0 = Lmotor; |
DanArgust | 0:466ee63955df | 77 | pwm1 = Rmotor; |
DanArgust | 0:466ee63955df | 78 | wait_ms(10); |
DanArgust | 0:466ee63955df | 79 | } |
DanArgust | 0:466ee63955df | 80 | } |