Georg Lerm
/
LPC1768_m3pi_LineFollower_PID
s
Diff: main.cpp
- Revision:
- 0:78f9794620a3
- Child:
- 1:f1d9f7ef9c15
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu May 12 11:48:56 2011 +0000 @@ -0,0 +1,70 @@ +#include "mbed.h" +#include "m3pi.h" + +m3pi m3pi; + +// Minimum and maximum motor speeds +#define MAX 1.0 +#define MIN 0 + +// PID terms +#define P_TERM 1 +#define I_TERM 0 +#define D_TERM 20 + +int main() { + + m3pi.locate(0,1); + m3pi.printf("Line PID"); + + wait(2.0); + + m3pi.sensor_auto_calibrate(); + + float right; + float left; + float current_pos_of_line = 0.0; + float previous_pos_of_line = 0.0; + float derivative,proportional,integral = 0; + float power; + float speed = MAX; + + while (1) { + + // Get the position of the line. + current_pos_of_line = m3pi.line_position(); + proportional = current_pos_of_line; + + // Compute the derivative + derivative = current_pos_of_line - previous_pos_of_line; + + // Compute the integral + integral += proportional; + + // Remember the last position. + previous_pos_of_line = current_pos_of_line; + + // Compute the power + power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; + + // Compute new speeds + right = speed+power; + left = speed-power; + + // limit checks + if (right < MIN) + right = MIN; + else if (right > MAX) + right = MAX; + + if (left < MIN) + left = MIN; + else if (left > MAX) + left = MAX; + + // set speed + m3pi.left_motor(left); + m3pi.right_motor(right); + + } +} \ No newline at end of file