![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
KreuHatzl
main.cpp@0:3aa1e3850dcc, 2015-10-21 (annotated)
- Committer:
- MarkusSchieder
- Date:
- Wed Oct 21 17:39:45 2015 +0000
- Revision:
- 0:3aa1e3850dcc
KreuHatzl
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MarkusSchieder | 0:3aa1e3850dcc | 1 | |
MarkusSchieder | 0:3aa1e3850dcc | 2 | #include "mbed.h" |
MarkusSchieder | 0:3aa1e3850dcc | 3 | #include "Serial_HL.h" |
MarkusSchieder | 0:3aa1e3850dcc | 4 | #include "Bertl14.h" |
MarkusSchieder | 0:3aa1e3850dcc | 5 | #include "BertlObjects.h" |
MarkusSchieder | 0:3aa1e3850dcc | 6 | |
MarkusSchieder | 0:3aa1e3850dcc | 7 | void DriveUntilObstacle(); |
MarkusSchieder | 0:3aa1e3850dcc | 8 | void Turn180(); |
MarkusSchieder | 0:3aa1e3850dcc | 9 | void BlinkTask(); |
MarkusSchieder | 0:3aa1e3850dcc | 10 | Timer t1; |
MarkusSchieder | 0:3aa1e3850dcc | 11 | int i,j; |
MarkusSchieder | 0:3aa1e3850dcc | 12 | |
MarkusSchieder | 0:3aa1e3850dcc | 13 | int main(void) |
MarkusSchieder | 0:3aa1e3850dcc | 14 | { |
MarkusSchieder | 0:3aa1e3850dcc | 15 | i=0; |
MarkusSchieder | 0:3aa1e3850dcc | 16 | j=0; |
MarkusSchieder | 0:3aa1e3850dcc | 17 | t1.start(); |
MarkusSchieder | 0:3aa1e3850dcc | 18 | InitBertl(); |
MarkusSchieder | 0:3aa1e3850dcc | 19 | pex.ClearLeds(); |
MarkusSchieder | 0:3aa1e3850dcc | 20 | leds = 9; |
MarkusSchieder | 0:3aa1e3850dcc | 21 | pex.WaitUntilFrontButtonPressed(); |
MarkusSchieder | 0:3aa1e3850dcc | 22 | leds=6; |
MarkusSchieder | 0:3aa1e3850dcc | 23 | wait(0.8); |
MarkusSchieder | 0:3aa1e3850dcc | 24 | pex.WaitUntilFrontButtonPressed(); |
MarkusSchieder | 0:3aa1e3850dcc | 25 | leds=0; |
MarkusSchieder | 0:3aa1e3850dcc | 26 | |
MarkusSchieder | 0:3aa1e3850dcc | 27 | while(1) { |
MarkusSchieder | 0:3aa1e3850dcc | 28 | DriveUntilObstacle(); |
MarkusSchieder | 0:3aa1e3850dcc | 29 | wait(1); |
MarkusSchieder | 0:3aa1e3850dcc | 30 | Turn180(); |
MarkusSchieder | 0:3aa1e3850dcc | 31 | wait(1); |
MarkusSchieder | 0:3aa1e3850dcc | 32 | } |
MarkusSchieder | 0:3aa1e3850dcc | 33 | return 1; |
MarkusSchieder | 0:3aa1e3850dcc | 34 | } |
MarkusSchieder | 0:3aa1e3850dcc | 35 | |
MarkusSchieder | 0:3aa1e3850dcc | 36 | void DriveUntilObstacle() |
MarkusSchieder | 0:3aa1e3850dcc | 37 | { |
MarkusSchieder | 0:3aa1e3850dcc | 38 | // Motoren ein |
MarkusSchieder | 0:3aa1e3850dcc | 39 | mL.SetPow(0.3); |
MarkusSchieder | 0:3aa1e3850dcc | 40 | mR.SetPow(0.3); |
MarkusSchieder | 0:3aa1e3850dcc | 41 | // in einer While-Schleife warten bis Distanz kleiner MinDistance |
MarkusSchieder | 0:3aa1e3850dcc | 42 | while(1) { |
MarkusSchieder | 0:3aa1e3850dcc | 43 | //mL.SetPow(0.5); |
MarkusSchieder | 0:3aa1e3850dcc | 44 | //mR.SetPow(0.5); |
MarkusSchieder | 0:3aa1e3850dcc | 45 | us.StartMeas(); |
MarkusSchieder | 0:3aa1e3850dcc | 46 | wait_ms(10); |
MarkusSchieder | 0:3aa1e3850dcc | 47 | BlinkTask(); |
MarkusSchieder | 0:3aa1e3850dcc | 48 | if ((us.distCM*100.0) < 500.0) { |
MarkusSchieder | 0:3aa1e3850dcc | 49 | // Motoren aus |
MarkusSchieder | 0:3aa1e3850dcc | 50 | if (i==0){ |
MarkusSchieder | 0:3aa1e3850dcc | 51 | i=1; |
MarkusSchieder | 0:3aa1e3850dcc | 52 | } |
MarkusSchieder | 0:3aa1e3850dcc | 53 | else { |
MarkusSchieder | 0:3aa1e3850dcc | 54 | i=0; |
MarkusSchieder | 0:3aa1e3850dcc | 55 | } |
MarkusSchieder | 0:3aa1e3850dcc | 56 | mL.SetPow(0.0); |
MarkusSchieder | 0:3aa1e3850dcc | 57 | mR.SetPow(0.0); |
MarkusSchieder | 0:3aa1e3850dcc | 58 | pex.ClearLeds(); |
MarkusSchieder | 0:3aa1e3850dcc | 59 | break; |
MarkusSchieder | 0:3aa1e3850dcc | 60 | } |
MarkusSchieder | 0:3aa1e3850dcc | 61 | } |
MarkusSchieder | 0:3aa1e3850dcc | 62 | } |
MarkusSchieder | 0:3aa1e3850dcc | 63 | |
MarkusSchieder | 0:3aa1e3850dcc | 64 | void Turn180() |
MarkusSchieder | 0:3aa1e3850dcc | 65 | { |
MarkusSchieder | 0:3aa1e3850dcc | 66 | if (j==0){ |
MarkusSchieder | 0:3aa1e3850dcc | 67 | // Motoren auf 0.5 bis 0.7 |
MarkusSchieder | 0:3aa1e3850dcc | 68 | mL.SetPow(0.7); |
MarkusSchieder | 0:3aa1e3850dcc | 69 | mR.SetPow(-0.7); |
MarkusSchieder | 0:3aa1e3850dcc | 70 | j=1; |
MarkusSchieder | 0:3aa1e3850dcc | 71 | pex.SetLeds(LED_FR1|LED_BR1); |
MarkusSchieder | 0:3aa1e3850dcc | 72 | } |
MarkusSchieder | 0:3aa1e3850dcc | 73 | else{ |
MarkusSchieder | 0:3aa1e3850dcc | 74 | mL.SetPow(-0.7); |
MarkusSchieder | 0:3aa1e3850dcc | 75 | mR.SetPow(0.7); |
MarkusSchieder | 0:3aa1e3850dcc | 76 | j=0; |
MarkusSchieder | 0:3aa1e3850dcc | 77 | pex.SetLeds(LED_FL1|LED_BL1); |
MarkusSchieder | 0:3aa1e3850dcc | 78 | } |
MarkusSchieder | 0:3aa1e3850dcc | 79 | mL.encCnt=mR.encCnt=0; |
MarkusSchieder | 0:3aa1e3850dcc | 80 | |
MarkusSchieder | 0:3aa1e3850dcc | 81 | // in einer While-Schleife warten bis |
MarkusSchieder | 0:3aa1e3850dcc | 82 | // bei !!!beiden!!! Motoren die Ticks für eine 180° Drehung erreicht sind |
MarkusSchieder | 0:3aa1e3850dcc | 83 | while (1) { |
MarkusSchieder | 0:3aa1e3850dcc | 84 | if (mL.encCnt>20) { |
MarkusSchieder | 0:3aa1e3850dcc | 85 | mL.SetPow(0.0); |
MarkusSchieder | 0:3aa1e3850dcc | 86 | } |
MarkusSchieder | 0:3aa1e3850dcc | 87 | if (mR.encCnt>20) { |
MarkusSchieder | 0:3aa1e3850dcc | 88 | mR.SetPow(0.0); |
MarkusSchieder | 0:3aa1e3850dcc | 89 | } |
MarkusSchieder | 0:3aa1e3850dcc | 90 | if (mL.encCnt>20 && mR.encCnt>20) |
MarkusSchieder | 0:3aa1e3850dcc | 91 | pex.ClearLeds(); |
MarkusSchieder | 0:3aa1e3850dcc | 92 | break; |
MarkusSchieder | 0:3aa1e3850dcc | 93 | } |
MarkusSchieder | 0:3aa1e3850dcc | 94 | // Motoren ausschalten |
MarkusSchieder | 0:3aa1e3850dcc | 95 | mL.SetPow(0.0); |
MarkusSchieder | 0:3aa1e3850dcc | 96 | mR.SetPow(0.0); |
MarkusSchieder | 0:3aa1e3850dcc | 97 | |
MarkusSchieder | 0:3aa1e3850dcc | 98 | } |
MarkusSchieder | 0:3aa1e3850dcc | 99 | |
MarkusSchieder | 0:3aa1e3850dcc | 100 | void BlinkTask() |
MarkusSchieder | 0:3aa1e3850dcc | 101 | { |
MarkusSchieder | 0:3aa1e3850dcc | 102 | |
MarkusSchieder | 0:3aa1e3850dcc | 103 | if (t1.read_ms()>100) { |
MarkusSchieder | 0:3aa1e3850dcc | 104 | if (i==0){ |
MarkusSchieder | 0:3aa1e3850dcc | 105 | pex.SetLeds(LED_FR1|LED_FL1); |
MarkusSchieder | 0:3aa1e3850dcc | 106 | } |
MarkusSchieder | 0:3aa1e3850dcc | 107 | else { |
MarkusSchieder | 0:3aa1e3850dcc | 108 | pex.SetLeds(LED_BR1|LED_BL1); |
MarkusSchieder | 0:3aa1e3850dcc | 109 | } |
MarkusSchieder | 0:3aa1e3850dcc | 110 | } |
MarkusSchieder | 0:3aa1e3850dcc | 111 | if (t1.read_ms()>200){ |
MarkusSchieder | 0:3aa1e3850dcc | 112 | pex.ClearLeds(); |
MarkusSchieder | 0:3aa1e3850dcc | 113 | t1.reset(); |
MarkusSchieder | 0:3aa1e3850dcc | 114 | } |
MarkusSchieder | 0:3aa1e3850dcc | 115 | } |
MarkusSchieder | 0:3aa1e3850dcc | 116 |