darron nielsen
/
m3pi_LineFollower_dpn
main.cpp
- Committer:
- microsat
- Date:
- 2010-11-11
- Revision:
- 0:301acbabd57a
- Child:
- 1:e046cce6ceb8
File content as of revision 0:301acbabd57a:
#include "mbed.h" #include "m3pi.h" BusOut leds(LED1,LED2,LED3,LED4); m3pi m3pi(p23,p9,p10); #define MAX 1.0 #define MIN 0 #define P_TERM 1 #define I_TERM 0 #define D_TERM 20 int main() { m3pi.locate(0,1); m3pi.printf("Line Flw"); wait(2.0); float right; float left; float position_of_line = 0.0; float prev_pos_of_line = 0.0; float derivative,proportional; float integral = 0; float power; m3pi.sensor_auto_calibrate(); float speed = MAX; while (1) { // Get the position of the line. position_of_line = m3pi.line_position(); proportional = position_of_line; // Compute the derivative derivative = position_of_line - prev_pos_of_line; // Compute the integral integral += proportional; power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; // Remember the last position. prev_pos_of_line = position_of_line; right = speed+power; left = speed-power; 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); } }