main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 BusOut leds(LED1,LED2,LED3,LED4);
00005 m3pi m3pi(p23,p9,p10);
00006 
00007 #define MAX 1.0
00008 #define MIN 0
00009 
00010 
00011 #define P_TERM 1
00012 #define I_TERM 0
00013 #define D_TERM 20
00014 
00015 int main() {
00016 
00017     m3pi.locate(0,1);
00018     m3pi.printf("Line Flw");
00019 
00020     wait(2.0);
00021 
00022     float right;
00023     float left;
00024     float position_of_line = 0.0;
00025     float prev_pos_of_line = 0.0;
00026     float derivative,proportional;
00027     float integral = 0;
00028     float power;
00029     m3pi.sensor_auto_calibrate();
00030     float speed = MAX;
00031     
00032     while (1) {
00033 
00034         // Get the position of the line.
00035         position_of_line = m3pi.line_position();
00036         proportional = position_of_line;
00037         // Compute the derivative
00038         derivative = position_of_line - prev_pos_of_line;
00039         // Compute the integral
00040         integral += proportional;
00041          // Remember the last position.
00042         prev_pos_of_line = position_of_line;
00043         // Compute
00044         power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
00045         
00046         //    Compute new speeds   
00047         right = speed+power;
00048         left  = speed-power;
00049         // limit checks
00050         if (right < MIN)
00051             right = MIN;
00052         else if (right > MAX)
00053             right = MAX;
00054             
00055         if (left < MIN)
00056             left = MIN;
00057         else if (left > MAX)
00058             left = MAX;
00059             
00060        // set speed 
00061         m3pi.left_motor(left);
00062         m3pi.right_motor(right);
00063 
00064 
00065     }
00066 }