main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 m3pi m3pi;
00005 
00006 // Minimum and maximum motor speeds
00007 #define MAX 1.0
00008 #define MIN 0
00009 
00010 // PID terms
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 PID");
00019 
00020     wait(2.0);
00021 
00022     m3pi.sensor_auto_calibrate();
00023 
00024     float right;
00025     float left;
00026     float current_pos_of_line = 0.0;
00027     float previous_pos_of_line = 0.0;
00028     float derivative,proportional,integral = 0;
00029     float power;
00030     float speed = MAX;
00031     
00032     while (1) {
00033 
00034         // Get the position of the line.
00035         current_pos_of_line = m3pi.line_position();        
00036         proportional = current_pos_of_line;
00037         
00038         // Compute the derivative
00039         derivative = current_pos_of_line - previous_pos_of_line;
00040         
00041         // Compute the integral
00042         integral += proportional;
00043         
00044         // Remember the last position.
00045         previous_pos_of_line = current_pos_of_line;
00046         
00047         // Compute the power
00048         power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
00049         
00050         // Compute new speeds   
00051         right = speed+power;
00052         left  = speed-power;
00053         
00054         // limit checks
00055         if (right < MIN)
00056             right = MIN;
00057         else if (right > MAX)
00058             right = MAX;
00059             
00060         if (left < MIN)
00061             left = MIN;
00062         else if (left > MAX)
00063             left = MAX;
00064             
00065        // set speed 
00066         m3pi.left_motor(left);
00067         m3pi.right_motor(right);
00068 
00069     }
00070 }