darron nielsen / Mbed 2 deprecated m3pi_LineFollower_dpn

Dependencies:   mbed

main.cpp@1:e046cce6ceb8, 2010-11-11 (annotated)

Committer:
microsat
Date:
Thu Nov 11 14:56:38 2010 +0000
Revision:
1:e046cce6ceb8
Parent:
0:301acbabd57a 
added comments to my code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
microsat 0:301acbabd57a 1 #include "mbed.h"
microsat 0:301acbabd57a 2 #include "m3pi.h"
microsat 0:301acbabd57a 3
microsat 0:301acbabd57a 4 BusOut leds(LED1,LED2,LED3,LED4);
microsat 0:301acbabd57a 5 m3pi m3pi(p23,p9,p10);
microsat 0:301acbabd57a 6
microsat 0:301acbabd57a 7 #define MAX 1.0
microsat 0:301acbabd57a 8 #define MIN 0
microsat 1:e046cce6ceb8 9
microsat 1:e046cce6ceb8 10
microsat 0:301acbabd57a 11 #define P_TERM 1
microsat 0:301acbabd57a 12 #define I_TERM 0
microsat 0:301acbabd57a 13 #define D_TERM 20
microsat 0:301acbabd57a 14
microsat 0:301acbabd57a 15 int main() {
microsat 0:301acbabd57a 16
microsat 0:301acbabd57a 17 m3pi.locate(0,1);
microsat 0:301acbabd57a 18 m3pi.printf("Line Flw");
microsat 0:301acbabd57a 19
microsat 0:301acbabd57a 20 wait(2.0);
microsat 0:301acbabd57a 21
microsat 0:301acbabd57a 22 float right;
microsat 1:e046cce6ceb8 23 float left;
microsat 1:e046cce6ceb8 24 float position_of_line = 0.0;
microsat 0:301acbabd57a 25 float prev_pos_of_line = 0.0;
microsat 1:e046cce6ceb8 26 float derivative,proportional;
microsat 1:e046cce6ceb8 27 float integral = 0;
microsat 1:e046cce6ceb8 28 float power;
microsat 0:301acbabd57a 29 m3pi.sensor_auto_calibrate();
microsat 0:301acbabd57a 30 float speed = MAX;
microsat 1:e046cce6ceb8 31
microsat 0:301acbabd57a 32 while (1) {
microsat 0:301acbabd57a 33
microsat 0:301acbabd57a 34 // Get the position of the line.
microsat 0:301acbabd57a 35 position_of_line = m3pi.line_position();
microsat 1:e046cce6ceb8 36 proportional = position_of_line;
microsat 1:e046cce6ceb8 37 // Compute the derivative
microsat 0:301acbabd57a 38 derivative = position_of_line - prev_pos_of_line;
microsat 1:e046cce6ceb8 39 // Compute the integral
microsat 1:e046cce6ceb8 40 integral += proportional;
microsat 1:e046cce6ceb8 41 // Remember the last position.
microsat 1:e046cce6ceb8 42 prev_pos_of_line = position_of_line;
microsat 1:e046cce6ceb8 43 // Compute
microsat 0:301acbabd57a 44 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
microsat 1:e046cce6ceb8 45
microsat 1:e046cce6ceb8 46 // Compute new speeds
microsat 0:301acbabd57a 47 right = speed+power;
microsat 0:301acbabd57a 48 left = speed-power;
microsat 1:e046cce6ceb8 49 // limit checks
microsat 0:301acbabd57a 50 if (right < MIN)
microsat 0:301acbabd57a 51 right = MIN;
microsat 0:301acbabd57a 52 else if (right > MAX)
microsat 0:301acbabd57a 53 right = MAX;
microsat 0:301acbabd57a 54
microsat 0:301acbabd57a 55 if (left < MIN)
microsat 0:301acbabd57a 56 left = MIN;
microsat 0:301acbabd57a 57 else if (left > MAX)
microsat 0:301acbabd57a 58 left = MAX;
microsat 0:301acbabd57a 59
microsat 0:301acbabd57a 60 // set speed
microsat 0:301acbabd57a 61 m3pi.left_motor(left);
microsat 0:301acbabd57a 62 m3pi.right_motor(right);
microsat 0:301acbabd57a 63
microsat 0:301acbabd57a 64
microsat 0:301acbabd57a 65 }
microsat 0:301acbabd57a 66 }