Joachim Schaeffer / Mbed 2 deprecated LineFollower2

a

Dependencies:   Roboter m3pi_js mbed

Fork of m3pi_LineFollower by Chris Styles

main.cpp@8:2cca24684c6d, 2017-01-11 (annotated)

Committer:
JoachimSch
Date:
Wed Jan 11 10:13:36 2017 +0000
Revision:
8:2cca24684c6d
Parent:
7:218e861ea777 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:eb1ece444e15 1 #include "mbed.h"
JoachimSch 8:2cca24684c6d 2 #include "m3pi_ng.h"
JoachimSch 8:2cca24684c6d 3 #include "LED.h"
JoachimSch 8:2cca24684c6d 4 #include "Umwandeln.h"
chris 0:eb1ece444e15 5
chris 6:8f46c6ac55ca 6 m3pi m3pi;
JoachimSch 8:2cca24684c6d 7 // Parameters that affect the performance
JoachimSch 8:2cca24684c6d 8 float speed = 0.7;
JoachimSch 8:2cca24684c6d 9 float correction = 0;
JoachimSch 8:2cca24684c6d 10 float threshold = 0.50;
JoachimSch 8:2cca24684c6d 11 float posalt = 0, posneu=0;
JoachimSch 8:2cca24684c6d 12 float k1=0.7, k2=0.8;
JoachimSch 8:2cca24684c6d 13 float im = 0.275;
JoachimSch 8:2cca24684c6d 14 LED B;
JoachimSch 8:2cca24684c6d 15 Ticker goback;
JoachimSch 8:2cca24684c6d 16 Timer t;
JoachimSch 8:2cca24684c6d 17 int sensors[5]={};
JoachimSch 8:2cca24684c6d 18 int a=0;
JoachimSch 8:2cca24684c6d 19
JoachimSch 8:2cca24684c6d 20 void pdreg()
JoachimSch 8:2cca24684c6d 21
JoachimSch 8:2cca24684c6d 22 {
JoachimSch 8:2cca24684c6d 23 posneu = m3pi.line_position(); // -1.0 is far left, 1.0 is far right, 0.0 in the middle
JoachimSch 8:2cca24684c6d 24 correction = k1*posneu+k2*(posneu-posalt);
JoachimSch 8:2cca24684c6d 25 if (correction >0)
JoachimSch 8:2cca24684c6d 26 {
JoachimSch 8:2cca24684c6d 27 m3pi.right_motor(speed+(im*correction));
JoachimSch 8:2cca24684c6d 28 m3pi.left_motor (speed-correction);
JoachimSch 8:2cca24684c6d 29 }
JoachimSch 8:2cca24684c6d 30 else
JoachimSch 8:2cca24684c6d 31 {
JoachimSch 8:2cca24684c6d 32 m3pi.right_motor(speed+correction);
JoachimSch 8:2cca24684c6d 33 m3pi.left_motor(speed-(im*correction));
JoachimSch 8:2cca24684c6d 34 }
JoachimSch 8:2cca24684c6d 35
JoachimSch 8:2cca24684c6d 36 posalt = posneu;
JoachimSch 8:2cca24684c6d 37 }
JoachimSch 8:2cca24684c6d 38
JoachimSch 8:2cca24684c6d 39 void setup ()
JoachimSch 8:2cca24684c6d 40 {
JoachimSch 8:2cca24684c6d 41 //Displayausgabe
JoachimSch 8:2cca24684c6d 42 m3pi.locate(0,0);
JoachimSch 8:2cca24684c6d 43 m3pi.printf("PARTYROB");
JoachimSch 8:2cca24684c6d 44 m3pi.locate(0,1);
JoachimSch 8:2cca24684c6d 45 m3pi.printf("FLOWFLOW");
JoachimSch 8:2cca24684c6d 46 wait(2.0);
JoachimSch 8:2cca24684c6d 47 m3pi.sensor_auto_calibrate();
JoachimSch 8:2cca24684c6d 48 }
chris 0:eb1ece444e15 49
chris 0:eb1ece444e15 50 int main() {
chris 0:eb1ece444e15 51
JoachimSch 8:2cca24684c6d 52 setup ();
chris 0:eb1ece444e15 53
JoachimSch 8:2cca24684c6d 54 goback.attach(&pdreg,0.05);
chris 1:5ddd3faed06d 55
JoachimSch 8:2cca24684c6d 56 while(1)
JoachimSch 8:2cca24684c6d 57 {
JoachimSch 8:2cca24684c6d 58 m3pi.calibrated_sensor(sensors);
JoachimSch 8:2cca24684c6d 59 if ((sensors[0]>400) && (sensors[4]>400))
JoachimSch 8:2cca24684c6d 60 {
JoachimSch 8:2cca24684c6d 61 goback.detach();
JoachimSch 8:2cca24684c6d 62 B.Blau_Blinken(2,0.1);
JoachimSch 8:2cca24684c6d 63 m3pi.stop();
JoachimSch 8:2cca24684c6d 64 wait(1);
JoachimSch 8:2cca24684c6d 65 }
JoachimSch 8:2cca24684c6d 66 goback.attach(&pdreg,0.005);
JoachimSch 8:2cca24684c6d 67 }
JoachimSch 8:2cca24684c6d 68
JoachimSch 8:2cca24684c6d 69 }