TUB-MRT / Mbed 2 deprecated m3pi_LineFollower_PID

An example PID line following program

Dependencies:   m3pi mbed

Fork of m3pi_LineFollower_PID by Chris Styles

main.cpp@1:c0b4f4f6952e, 2016-06-15 (annotated)

Committer:
dpfeiffer
Date:
Wed Jun 15 15:18:23 2016 +0000
Revision:
1:c0b4f4f6952e
Parent:
0:78f9794620a3 
functional PID controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:78f9794620a3 1 #include "mbed.h"
chris 0:78f9794620a3 2 #include "m3pi.h"
chris 0:78f9794620a3 3
chris 0:78f9794620a3 4 m3pi m3pi;
chris 0:78f9794620a3 5
dpfeiffer 1:c0b4f4f6952e 6 DigitalIn m3pi_IN[] = {(p12)};
dpfeiffer 1:c0b4f4f6952e 7
chris 0:78f9794620a3 8 // Minimum and maximum motor speeds
dpfeiffer 1:c0b4f4f6952e 9 #define MAX 0.8
chris 0:78f9794620a3 10 #define MIN 0
chris 0:78f9794620a3 11
chris 0:78f9794620a3 12 // PID terms
chris 0:78f9794620a3 13 #define P_TERM 1
chris 0:78f9794620a3 14 #define I_TERM 0
chris 0:78f9794620a3 15 #define D_TERM 20
chris 0:78f9794620a3 16
chris 0:78f9794620a3 17 int main() {
chris 0:78f9794620a3 18
chris 0:78f9794620a3 19 m3pi.locate(0,1);
chris 0:78f9794620a3 20 m3pi.printf("Line PID");
chris 0:78f9794620a3 21
chris 0:78f9794620a3 22 wait(2.0);
chris 0:78f9794620a3 23
chris 0:78f9794620a3 24 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 25
chris 0:78f9794620a3 26 float right;
chris 0:78f9794620a3 27 float left;
chris 0:78f9794620a3 28 float current_pos_of_line = 0.0;
chris 0:78f9794620a3 29 float previous_pos_of_line = 0.0;
chris 0:78f9794620a3 30 float derivative,proportional,integral = 0;
chris 0:78f9794620a3 31 float power;
dpfeiffer 1:c0b4f4f6952e 32 float speed = 0.6;
chris 0:78f9794620a3 33
dpfeiffer 1:c0b4f4f6952e 34 m3pi_IN[0].mode(PullUp);
chris 0:78f9794620a3 35 while (1) {
dpfeiffer 1:c0b4f4f6952e 36 while (m3pi_IN[0]==0){
dpfeiffer 1:c0b4f4f6952e 37 m3pi.stop();
dpfeiffer 1:c0b4f4f6952e 38 }
chris 0:78f9794620a3 39
chris 0:78f9794620a3 40 // Get the position of the line.
chris 0:78f9794620a3 41 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 42 proportional = current_pos_of_line;
chris 0:78f9794620a3 43
chris 0:78f9794620a3 44 // Compute the derivative
chris 0:78f9794620a3 45 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 46
chris 0:78f9794620a3 47 // Compute the integral
chris 0:78f9794620a3 48 integral += proportional;
chris 0:78f9794620a3 49
chris 0:78f9794620a3 50 // Remember the last position.
chris 0:78f9794620a3 51 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 52
chris 0:78f9794620a3 53 // Compute the power
chris 0:78f9794620a3 54 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 55
chris 0:78f9794620a3 56 // Compute new speeds
chris 0:78f9794620a3 57 right = speed+power;
chris 0:78f9794620a3 58 left = speed-power;
chris 0:78f9794620a3 59
chris 0:78f9794620a3 60 // limit checks
chris 0:78f9794620a3 61 if (right < MIN)
chris 0:78f9794620a3 62 right = MIN;
chris 0:78f9794620a3 63 else if (right > MAX)
chris 0:78f9794620a3 64 right = MAX;
chris 0:78f9794620a3 65
chris 0:78f9794620a3 66 if (left < MIN)
chris 0:78f9794620a3 67 left = MIN;
chris 0:78f9794620a3 68 else if (left > MAX)
chris 0:78f9794620a3 69 left = MAX;
chris 0:78f9794620a3 70
chris 0:78f9794620a3 71 // set speed
chris 0:78f9794620a3 72 m3pi.left_motor(left);
chris 0:78f9794620a3 73 m3pi.right_motor(right);
chris 0:78f9794620a3 74
chris 0:78f9794620a3 75 }
chris 0:78f9794620a3 76 }