the new engineer´s
/
LAB_6_BingBong_Bertl
BingBong
Fork of BertlTemplate2 by
Diff: BertlTemplate.cpp
- Revision:
- 4:3aca65c975f0
- Parent:
- 3:57fc35fcd060
--- a/BertlTemplate.cpp Mon May 02 15:49:14 2016 +0000 +++ b/BertlTemplate.cpp Mon May 30 15:46:55 2016 +0000 @@ -1,87 +1,100 @@ - + #include "mbed.h" #include "Serial_HL.h" #include "Bertl14.h" #include "BertlObjects.h" +SerialBLK pc(USBTX, USBRX); +SvProtocol ua0(&pc); + // main=2^0 LS ENC 2^2 BusOut boardPow(p30, P1_6, P1_7); -Timer t1,t2; +// 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 + + +void rightCurve() +{ + mL.SetPow(0.3); //rechts + mR.SetPow(0.0); +} -void BlinkTask_Forward() +void leftCurve() +{ + mL.SetPow(0.0); //links + mR.SetPow(0.3); +} + +void straight() +{ + mL.SetPow(0.3); //gerade + mR.SetPow(0.3); +} + +void stop() { - if(t1.read_ms()>100) { - t1.reset(); - pex.ToggleLeds(LED_ALL_FRONT);} - + mL.SetPow(0.0); + mR.SetPow(0.0); + wait_ms(1000); } -void BlinkTask_Backward() + +const int LS_THRSH = 400; + +void LineFollow() { - if(t2.read_ms()>200) { - t2.reset(); - pex.ToggleLeds(LED_ALL_BACK); - + while(1){ + if (ls1.Read() > LS_THRSH || ls2.Read() > LS_THRSH ) + { + leftCurve(); + } + + else if(ls4.Read() > LS_THRSH || ls5.Read() > LS_THRSH ) + { + rightCurve(); + } + else if (ls3.Read() > LS_THRSH) + { + straight(); + } + else if (ls1.Read() < LS_THRSH || ls2.Read() < LS_THRSH || ls3.Read() < LS_THRSH || ls4.Read() < LS_THRSH || ls5.Read() < LS_THRSH) + { + stop(); + return; + } + else + straight(); } } -void ForwardUntilWall() + +void TurnUntilLine() { - mL.SetPow2(0.3); - mR.SetPow2(0.3); //Leistung 30% .. 0.3 - + mL.SetPow2(-0.3); + mR.SetPow2(0.3); - while(1) { - pex.ReadButtons(); - if (pex.IsButton (BTN_FM)) - break; - BlinkTask_Forward(); + while(1){ + + if (ls3.Read()>400){ + mL.SetPow2(0.0); + mR.SetPow2(0.0); + return; + } } - mL.SetPow2(0.0); - mR.SetPow2(0.0); - pex.ClearLeds(); } -void BackwardUntilWall() -{ - - mL.SetPow2(-0.3); - mR.SetPow2(-0.3); //Leistung 30% .. 0.3 - - - while(1) { - pex.ReadButtons(); - if (pex.IsButton (BTN_BM)) - break; - BlinkTask_Backward(); - } - mL.SetPow2(0.0); - mR.SetPow2(0.0); - pex.ClearLeds(); -} - - int main(void) { - boardPow=1; - wait_ms(10); - InitBertl(); - pex.useISR=0; - leds=0; - pex.ClearLeds(); - - pex.WaitUntilFrontButtonPressed(); - t1.start(); - t2.start(); - + pc.format(8,SerialBLK::None,1); + pc.baud(115200); // 115200 + boardPow = 3; + while(1) { - - ForwardUntilWall(); - BackwardUntilWall(); - + LineFollow(); + wait_ms(1000); + TurnUntilLine(); + wait_ms(1000); } - return 1; - } - -