Georg Lerm / Mbed 2 deprecated LPC1768_m3pi_LineFollower_PID

s

Dependencies:   mbed m3pi

main.cpp@1:f1d9f7ef9c15, 2020-09-14 (annotated)

Committer:
Georg
Date:
Mon Sep 14 10:35:40 2020 +0000
Revision:
1:f1d9f7ef9c15
Parent:
0:78f9794620a3 
c

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
Georg 1:f1d9f7ef9c15 7 #define MAX 0.40
Georg 1:f1d9f7ef9c15 8 #define MIN 0.00
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
Georg 1:f1d9f7ef9c15 15 /*
Georg 1:f1d9f7ef9c15 16 char fugue[] =
Georg 1:f1d9f7ef9c15 17 "! O5 L16 agafaea dac+adaea fa<aa<bac#a dac#adaea f"
Georg 1:f1d9f7ef9c15 18 "O6 dcd<b-d<ad<g d<f+d<gd<ad<b- d<dd<ed<f+d<g d<f+d<gd<ad"
Georg 1:f1d9f7ef9c15 19 "L8 MS <b-d<b-d MLe-<ge-<g MSc<ac<a ML d<fd<f O5 MS b-gb-g"
Georg 1:f1d9f7ef9c15 20 "ML >c#e>c#e MS afaf ML gc#gc# MS fdfd ML e<b-e<b-"
Georg 1:f1d9f7ef9c15 21 "O6 L16ragafaea dac#adaea fa<aa<bac#a dac#adaea faeadaca"
Georg 1:f1d9f7ef9c15 22 "<b-acadg<b-g egdgcg<b-g <ag<b-gcf<af dfcf<b-f<af"
Georg 1:f1d9f7ef9c15 23 "<gf<af<b-e<ge c#e<b-e<ae<ge <fe<ge<ad<fd"
Georg 1:f1d9f7ef9c15 24 "O5 e>ee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d"
Georg 1:f1d9f7ef9c15 25 "e>d>c#>db>d>c#b >c#agaegfe f O6 dc#dfdc#<b c#4";
Georg 1:f1d9f7ef9c15 26 */
Georg 1:f1d9f7ef9c15 27
Georg 1:f1d9f7ef9c15 28
chris 0:78f9794620a3 29 int main() {
Georg 1:f1d9f7ef9c15 30 m3pi.leds(0x00);
chris 0:78f9794620a3 31 m3pi.locate(0,1);
chris 0:78f9794620a3 32 m3pi.printf("Line PID");
chris 0:78f9794620a3 33
chris 0:78f9794620a3 34 wait(2.0);
Georg 1:f1d9f7ef9c15 35 m3pi.cls();
Georg 1:f1d9f7ef9c15 36 m3pi.locate(0,0);
Georg 1:f1d9f7ef9c15 37 m3pi.printf("Battery=");
Georg 1:f1d9f7ef9c15 38 m3pi.locate(0,1);
Georg 1:f1d9f7ef9c15 39 m3pi.printf("%.3f V",m3pi.battery());
Georg 1:f1d9f7ef9c15 40 wait(2.0);
chris 0:78f9794620a3 41
chris 0:78f9794620a3 42 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 43
Georg 1:f1d9f7ef9c15 44
Georg 1:f1d9f7ef9c15 45 // m3pi.play(fugue,sizeof(fugue));
Georg 1:f1d9f7ef9c15 46
Georg 1:f1d9f7ef9c15 47 //wait(20);
chris 0:78f9794620a3 48 float right;
chris 0:78f9794620a3 49 float left;
chris 0:78f9794620a3 50 float current_pos_of_line = 0.0;
chris 0:78f9794620a3 51 float previous_pos_of_line = 0.0;
chris 0:78f9794620a3 52 float derivative,proportional,integral = 0;
chris 0:78f9794620a3 53 float power;
chris 0:78f9794620a3 54 float speed = MAX;
chris 0:78f9794620a3 55
chris 0:78f9794620a3 56 while (1) {
chris 0:78f9794620a3 57
chris 0:78f9794620a3 58 // Get the position of the line.
chris 0:78f9794620a3 59 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 60 proportional = current_pos_of_line;
chris 0:78f9794620a3 61
chris 0:78f9794620a3 62 // Compute the derivative
chris 0:78f9794620a3 63 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 64
chris 0:78f9794620a3 65 // Compute the integral
chris 0:78f9794620a3 66 integral += proportional;
chris 0:78f9794620a3 67
chris 0:78f9794620a3 68 // Remember the last position.
chris 0:78f9794620a3 69 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 70
chris 0:78f9794620a3 71 // Compute the power
chris 0:78f9794620a3 72 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 73
chris 0:78f9794620a3 74 // Compute new speeds
chris 0:78f9794620a3 75 right = speed+power;
chris 0:78f9794620a3 76 left = speed-power;
chris 0:78f9794620a3 77
chris 0:78f9794620a3 78 // limit checks
chris 0:78f9794620a3 79 if (right < MIN)
chris 0:78f9794620a3 80 right = MIN;
chris 0:78f9794620a3 81 else if (right > MAX)
chris 0:78f9794620a3 82 right = MAX;
chris 0:78f9794620a3 83
chris 0:78f9794620a3 84 if (left < MIN)
chris 0:78f9794620a3 85 left = MIN;
chris 0:78f9794620a3 86 else if (left > MAX)
chris 0:78f9794620a3 87 left = MAX;
chris 0:78f9794620a3 88
chris 0:78f9794620a3 89 // set speed
chris 0:78f9794620a3 90 m3pi.left_motor(left);
chris 0:78f9794620a3 91 m3pi.right_motor(right);
chris 0:78f9794620a3 92
chris 0:78f9794620a3 93 }
chris 0:78f9794620a3 94 }