s

Dependencies:   mbed m3pi

Committer:
chris
Date:
Thu May 12 11:48:56 2011 +0000
Revision:
0:78f9794620a3
Child:
1:f1d9f7ef9c15

        

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