An example PID line following program

Embed: (wiki syntax)

« Back to documentation index

main.cpp Source File

# 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 }
```