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@0:466ee63955df, 2016-07-14 (annotated)
- Committer:
- DanArgust
- Date:
- Thu Jul 14 09:09:52 2016 +0000
- Revision:
- 0:466ee63955df
- Child:
- 2:2439f5a4a93d
First commit; Basic Line follower using two sensors; This commit contains highly sensitive algorithm and exhibits twitchy behaviour
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 | 0:466ee63955df | 9 | #include "mbed.h" //"mbed.h" is a library that makes it easier to program microcontrollers |
DanArgust | 0:466ee63955df | 10 | #include "USBSerial.h" |
DanArgust | 0:466ee63955df | 11 | //#include "TimedMovement.h" //"TimedMovement.h" contains the functions that we will use to move the buggy |
DanArgust | 0:466ee63955df | 12 | |
DanArgust | 0:466ee63955df | 13 | AnalogIn ain0(p15); |
DanArgust | 0:466ee63955df | 14 | AnalogIn ain1(p16); |
DanArgust | 0:466ee63955df | 15 | |
DanArgust | 0:466ee63955df | 16 | PwmOut pwm0(p25); |
DanArgust | 0:466ee63955df | 17 | PwmOut pwm1(p10); |
DanArgust | 0:466ee63955df | 18 | |
DanArgust | 0:466ee63955df | 19 | float Lmotor; |
DanArgust | 0:466ee63955df | 20 | float Rmotor; |
DanArgust | 0:466ee63955df | 21 | float newLine; |
DanArgust | 0:466ee63955df | 22 | float oldLine; |
DanArgust | 0:466ee63955df | 23 | |
DanArgust | 0:466ee63955df | 24 | //USBSerial USB; |
DanArgust | 0:466ee63955df | 25 | Ticker timer; |
DanArgust | 0:466ee63955df | 26 | |
DanArgust | 0:466ee63955df | 27 | void getMotorSpeeds(float a,float b){ |
DanArgust | 0:466ee63955df | 28 | float inputs[2] = {a,b}; |
DanArgust | 0:466ee63955df | 29 | float max_speed = 1.0; |
DanArgust | 0:466ee63955df | 30 | Lmotor = 0; |
DanArgust | 0:466ee63955df | 31 | Rmotor = 0; |
DanArgust | 0:466ee63955df | 32 | newLine = 0; |
DanArgust | 0:466ee63955df | 33 | float line_to_speed_factor = 5; |
DanArgust | 0:466ee63955df | 34 | for(int i = 0;i<2;++i){ |
DanArgust | 0:466ee63955df | 35 | newLine += inputs[i]*(i+1); |
DanArgust | 0:466ee63955df | 36 | } |
DanArgust | 0:466ee63955df | 37 | float sum = 0; |
DanArgust | 0:466ee63955df | 38 | for (int i = 0;i<2;++i){ |
DanArgust | 0:466ee63955df | 39 | sum += inputs[i]; |
DanArgust | 0:466ee63955df | 40 | } |
DanArgust | 0:466ee63955df | 41 | newLine = newLine / sum; |
DanArgust | 0:466ee63955df | 42 | if((a<0.2)&&(b<0.2)){ |
DanArgust | 0:466ee63955df | 43 | if(oldLine>1.5) |
DanArgust | 0:466ee63955df | 44 | newLine = 2; |
DanArgust | 0:466ee63955df | 45 | else |
DanArgust | 0:466ee63955df | 46 | newLine = 1; |
DanArgust | 0:466ee63955df | 47 | } |
DanArgust | 0:466ee63955df | 48 | oldLine = newLine; |
DanArgust | 0:466ee63955df | 49 | /*USB.printf("\033[2J"); |
DanArgust | 0:466ee63955df | 50 | USB.printf("\033[0;0H"); |
DanArgust | 0:466ee63955df | 51 | USB.printf("input a = %f\n\rinput b = %f\n\r",a,b); |
DanArgust | 0:466ee63955df | 52 | USB.printf("%f\n",line);*/ |
DanArgust | 0:466ee63955df | 53 | Lmotor = newLine*line_to_speed_factor-6.5; |
DanArgust | 0:466ee63955df | 54 | Rmotor = 2-(newLine*line_to_speed_factor-6.5); |
DanArgust | 0:466ee63955df | 55 | if(Lmotor>max_speed){ |
DanArgust | 0:466ee63955df | 56 | Lmotor = max_speed; |
DanArgust | 0:466ee63955df | 57 | } |
DanArgust | 0:466ee63955df | 58 | if(Rmotor>max_speed){ |
DanArgust | 0:466ee63955df | 59 | Rmotor = max_speed; |
DanArgust | 0:466ee63955df | 60 | } |
DanArgust | 0:466ee63955df | 61 | } |
DanArgust | 0:466ee63955df | 62 | |
DanArgust | 0:466ee63955df | 63 | /*void PrintOutputs(){ |
DanArgust | 0:466ee63955df | 64 | USB.printf("\033[2J"); |
DanArgust | 0:466ee63955df | 65 | USB.printf("\033[0;0H"); |
DanArgust | 0:466ee63955df | 66 | USB.printf("Left : %f\n\r",Lmotor); |
DanArgust | 0:466ee63955df | 67 | USB.printf("Right : %f\n\r",Rmotor); |
DanArgust | 0:466ee63955df | 68 | }*/ |
DanArgust | 0:466ee63955df | 69 | |
DanArgust | 0:466ee63955df | 70 | int main() |
DanArgust | 0:466ee63955df | 71 | { |
DanArgust | 0:466ee63955df | 72 | //timer.attach(&PrintOutputs,0.1); |
DanArgust | 0:466ee63955df | 73 | while(true){ |
DanArgust | 0:466ee63955df | 74 | getMotorSpeeds(ain0.read(),ain1.read()); |
DanArgust | 0:466ee63955df | 75 | pwm0 = Lmotor; |
DanArgust | 0:466ee63955df | 76 | pwm1 = Rmotor; |
DanArgust | 0:466ee63955df | 77 | wait_ms(10); |
DanArgust | 0:466ee63955df | 78 | } |
DanArgust | 0:466ee63955df | 79 | } |