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 }