m3pi LineFollowing

Line following programs use the reflective sensors under the front of the 3pi to detect a difference in reflectivity, usually contrast between a black line on a white background (or vice versa!). The changes in reflections are used to control the motors to keep the line in the center of the sensors.

Reflection sensors

There are 5 sensors on the 3pi, one in the centre, one either side close to the centre and another one either side futher out. This give good resolution near the centre and a wide range.

The m3pi library contains the following function:

float line_position(void);

This reads current state of the sensors and returns a calibrated and normalised floating point number representing where a black line might be with respect to the centre of the 3pi.

  • 0.0 = a line is detected in the centre
  • 1.0 = a line is detected at right edge
  • -1.0 = a line is detected at the left edge, or no line is detected at all

Basic line Following

Note that this is a very (very) crude algorithm, with three parameter that affect the behaviour:

  • The initial speed of the m3pi
  • How far off the centre the m3pi is before corrective action is taken
  • How aggressive the corrective action is

Import program

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 m3pi m3pi;
00005 
00006 int main() {
00007 
00008     // Parameters that affect the performance
00009     float speed = 0.2;
00010     float correction = 0.1;   
00011     float threshold = 0.5;
00012 
00013     m3pi.locate(0,1);
00014     m3pi.printf("Line Flw");
00015 
00016     wait(2.0);
00017     
00018     m3pi.sensor_auto_calibrate();
00019     
00020     while (1) {
00021 
00022         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
00023         float position_of_line = m3pi.line_position();
00024 
00025         // Line is more than the threshold to the right, slow the left motor
00026         if (position_of_line > threshold) {
00027             m3pi.right_motor(speed);
00028             m3pi.left_motor(speed-correction);
00029         }
00030 
00031         // Line is more than 50% to the left, slow the right motor
00032         else if (position_of_line < -threshold) {
00033             m3pi.left_motor(speed);
00034             m3pi.right_motor(speed-correction);
00035         }
00036 
00037         // Line is in the middle
00038         else {
00039             m3pi.forward(speed);
00040         }
00041     }
00042 }

PID Line Following

Import program

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 }