darron nielsen / Mbed 2 deprecated m3pi_LineFollower_dpn

Dependencies:   mbed

main.cpp@0:301acbabd57a, 2010-11-11 (annotated)

Committer:
microsat
Date:
Thu Nov 11 05:45:44 2010 +0000
Revision:
0:301acbabd57a
Child:
1:e046cce6ceb8
need to work more on the terms. right now it more a PD controller than a PID

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 0:301acbabd57a 9 #define P_TERM 1
microsat 0:301acbabd57a 10 #define I_TERM 0
microsat 0:301acbabd57a 11 #define D_TERM 20
microsat 0:301acbabd57a 12
microsat 0:301acbabd57a 13 int main() {
microsat 0:301acbabd57a 14
microsat 0:301acbabd57a 15 m3pi.locate(0,1);
microsat 0:301acbabd57a 16 m3pi.printf("Line Flw");
microsat 0:301acbabd57a 17
microsat 0:301acbabd57a 18 wait(2.0);
microsat 0:301acbabd57a 19
microsat 0:301acbabd57a 20 float right;
microsat 0:301acbabd57a 21 float left;
microsat 0:301acbabd57a 22 float position_of_line = 0.0;
microsat 0:301acbabd57a 23 float prev_pos_of_line = 0.0;
microsat 0:301acbabd57a 24 float derivative,proportional;
microsat 0:301acbabd57a 25 float integral = 0;
microsat 0:301acbabd57a 26 float power;
microsat 0:301acbabd57a 27 m3pi.sensor_auto_calibrate();
microsat 0:301acbabd57a 28 float speed = MAX;
microsat 0:301acbabd57a 29
microsat 0:301acbabd57a 30 while (1) {
microsat 0:301acbabd57a 31
microsat 0:301acbabd57a 32 // Get the position of the line.
microsat 0:301acbabd57a 33 position_of_line = m3pi.line_position();
microsat 0:301acbabd57a 34 proportional = position_of_line;
microsat 0:301acbabd57a 35 // Compute the derivative
microsat 0:301acbabd57a 36 derivative = position_of_line - prev_pos_of_line;
microsat 0:301acbabd57a 37 // Compute the integral
microsat 0:301acbabd57a 38 integral += proportional;
microsat 0:301acbabd57a 39
microsat 0:301acbabd57a 40 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
microsat 0:301acbabd57a 41 // Remember the last position.
microsat 0:301acbabd57a 42 prev_pos_of_line = position_of_line;
microsat 0:301acbabd57a 43 right = speed+power;
microsat 0:301acbabd57a 44 left = speed-power;
microsat 0:301acbabd57a 45
microsat 0:301acbabd57a 46 if (right < MIN)
microsat 0:301acbabd57a 47 right = MIN;
microsat 0:301acbabd57a 48 else if (right > MAX)
microsat 0:301acbabd57a 49 right = MAX;
microsat 0:301acbabd57a 50
microsat 0:301acbabd57a 51 if (left < MIN)
microsat 0:301acbabd57a 52 left = MIN;
microsat 0:301acbabd57a 53 else if (left > MAX)
microsat 0:301acbabd57a 54 left = MAX;
microsat 0:301acbabd57a 55
microsat 0:301acbabd57a 56 // set speed
microsat 0:301acbabd57a 57 m3pi.left_motor(left);
microsat 0:301acbabd57a 58 m3pi.right_motor(right);
microsat 0:301acbabd57a 59
microsat 0:301acbabd57a 60
microsat 0:301acbabd57a 61 }
microsat 0:301acbabd57a 62 }