Georg Lerm
/
LPC1768_m3pi_LineFollower_PID
s
main.cpp
- Committer:
- Georg
- Date:
- 2020-09-14
- Revision:
- 1:f1d9f7ef9c15
- Parent:
- 0:78f9794620a3
File content as of revision 1:f1d9f7ef9c15:
#include "mbed.h" #include "m3pi.h" m3pi m3pi; // Minimum and maximum motor speeds #define MAX 0.40 #define MIN 0.00 // PID terms #define P_TERM 1 #define I_TERM 0 #define D_TERM 20 /* char fugue[] = "! O5 L16 agafaea dac+adaea fa<aa<bac#a dac#adaea f" "O6 dcd<b-d<ad<g d<f+d<gd<ad<b- d<dd<ed<f+d<g d<f+d<gd<ad" "L8 MS <b-d<b-d MLe-<ge-<g MSc<ac<a ML d<fd<f O5 MS b-gb-g" "ML >c#e>c#e MS afaf ML gc#gc# MS fdfd ML e<b-e<b-" "O6 L16ragafaea dac#adaea fa<aa<bac#a dac#adaea faeadaca" "<b-acadg<b-g egdgcg<b-g <ag<b-gcf<af dfcf<b-f<af" "<gf<af<b-e<ge c#e<b-e<ae<ge <fe<ge<ad<fd" "O5 e>ee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d" "e>d>c#>db>d>c#b >c#agaegfe f O6 dc#dfdc#<b c#4"; */ int main() { m3pi.leds(0x00); m3pi.locate(0,1); m3pi.printf("Line PID"); wait(2.0); m3pi.cls(); m3pi.locate(0,0); m3pi.printf("Battery="); m3pi.locate(0,1); m3pi.printf("%.3f V",m3pi.battery()); wait(2.0); m3pi.sensor_auto_calibrate(); // m3pi.play(fugue,sizeof(fugue)); //wait(20); float right; float left; float current_pos_of_line = 0.0; float previous_pos_of_line = 0.0; float derivative,proportional,integral = 0; float power; float speed = MAX; while (1) { // Get the position of the line. current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; // Compute the derivative derivative = current_pos_of_line - previous_pos_of_line; // Compute the integral integral += proportional; // Remember the last position. previous_pos_of_line = current_pos_of_line; // Compute the power power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; // Compute new speeds right = speed+power; left = speed-power; // limit checks if (right < MIN) right = MIN; else if (right > MAX) right = MAX; if (left < MIN) left = MIN; else if (left > MAX) left = MAX; // set speed m3pi.left_motor(left); m3pi.right_motor(right); } }