Martin Werluschnig
/
BertlTemplate2_Linienfahren
BertlTemplate2_Linienfahren
Diff: BertlTemplate_Linienfahren.cpp
- Revision:
- 5:b1a4eed391a6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BertlTemplate_Linienfahren.cpp Thu Nov 15 17:50:41 2018 +0000 @@ -0,0 +1,66 @@ + +#include "mbed.h" +#include "Serial_HL.h" +#include "Bertl14.h" +#include "BertlObjects.h" + +// main=2^0 LS ENC 2^2 +BusOut boardPow(p30, P1_6, P1_7); + +// ls5 nur beim Betrl15 +// AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17); // B14 + +AnalogInHL ls1(p18), ls2(p16), ls3(p20), ls4(p19), ls5(p17); // B15 + +int main(void) +{ + boardPow=3; wait_ms(10); + InitBertl(); + pex.useISR=0; leds=9; + pex.ClearLeds(); + + bool rechts = 0;//flag, welche kurve zuletzt dran war + bool links = 0; + + while(1) + { + wait_ms(10); + + if(ls1.Read()>600) + { + mL.SetPow(0.3); mR.SetPow(0.1);//rechtskrve + rechts = 1; + } + else if(ls4.Read()>600) + { + mL.SetPow(0.1); mR.SetPow(0.3);//linkskurve + links = 1; + } + else if(ls1.Read()<200 && ls4.Read()<200 && ls2.Read()>200 && ls3.Read()>200) + { + mL.SetPow(0.3); mR.SetPow(0.3);//geradeaus + rechts = 0; + links = 0; + } + else if(ls1.Read()<200 && ls4.Read()<200 && ls2.Read()<200 && ls3.Read()<200 && ls5.Read()<200)//wenn alle unter 200, je nachdem was die letzte kurve war --> andere richtung + { + if(links == 1) + { + mL.SetPow(0.1); mR.SetPow(0.3);//linkskurve + rechts = 0;//zurücksetzen + } + else if (rechts == 1) + { + mL.SetPow(0.3); mR.SetPow(0.1);//rechtskurve + links = 0; + } + else + { + mL.SetPow(0.3); mR.SetPow(0.3);//geradeaus + } + } + } + return 1; +} + +