Lab 4CHEL
/
Bertel_FollowLine
1 Sensor
Fork of BertlTemplate2 by
Bertel_FollowLine.cpp@2:bc32145e7819, 2016-03-07 (annotated)
- Committer:
- TFuchsbichler
- Date:
- Mon Mar 07 08:47:32 2016 +0000
- Revision:
- 2:bc32145e7819
- Child:
- 3:d0894bb57690
Followline mit 1 Sensor;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TFuchsbichler | 2:bc32145e7819 | 1 | |
TFuchsbichler | 2:bc32145e7819 | 2 | #include "mbed.h" |
TFuchsbichler | 2:bc32145e7819 | 3 | #include "Serial_HL.h" |
TFuchsbichler | 2:bc32145e7819 | 4 | #include "Bertl14.h" |
TFuchsbichler | 2:bc32145e7819 | 5 | #include "BertlObjects.h" |
TFuchsbichler | 2:bc32145e7819 | 6 | |
TFuchsbichler | 2:bc32145e7819 | 7 | |
TFuchsbichler | 2:bc32145e7819 | 8 | void FollowLine(); |
TFuchsbichler | 2:bc32145e7819 | 9 | void FindLine(); |
TFuchsbichler | 2:bc32145e7819 | 10 | void rightturn(); |
TFuchsbichler | 2:bc32145e7819 | 11 | void leftturn(); |
TFuchsbichler | 2:bc32145e7819 | 12 | void stop(); |
TFuchsbichler | 2:bc32145e7819 | 13 | void lefthard(); |
TFuchsbichler | 2:bc32145e7819 | 14 | void righthard(); |
TFuchsbichler | 2:bc32145e7819 | 15 | |
TFuchsbichler | 2:bc32145e7819 | 16 | int leftright; |
TFuchsbichler | 2:bc32145e7819 | 17 | BusOut boardPow(p30, P1_6,P1_7); |
TFuchsbichler | 2:bc32145e7819 | 18 | AnalogInHL ls1(p18), ls2(p16), ls3(p20), ls4(p19),ls5(p17); |
TFuchsbichler | 2:bc32145e7819 | 19 | |
TFuchsbichler | 2:bc32145e7819 | 20 | |
TFuchsbichler | 2:bc32145e7819 | 21 | Timer t; |
TFuchsbichler | 2:bc32145e7819 | 22 | |
TFuchsbichler | 2:bc32145e7819 | 23 | int main(void) |
TFuchsbichler | 2:bc32145e7819 | 24 | { |
TFuchsbichler | 2:bc32145e7819 | 25 | boardPow=3; |
TFuchsbichler | 2:bc32145e7819 | 26 | InitBertl(); |
TFuchsbichler | 2:bc32145e7819 | 27 | pex.ClearLeds(); |
TFuchsbichler | 2:bc32145e7819 | 28 | // Damit der Roboter nicht gleich loskoffert |
TFuchsbichler | 2:bc32145e7819 | 29 | leds = 9; |
TFuchsbichler | 2:bc32145e7819 | 30 | pex.WaitUntilFrontButtonPressed(); |
TFuchsbichler | 2:bc32145e7819 | 31 | leds=6; |
TFuchsbichler | 2:bc32145e7819 | 32 | wait(0.8); |
TFuchsbichler | 2:bc32145e7819 | 33 | leds=0; |
TFuchsbichler | 2:bc32145e7819 | 34 | // 2x Button-pressed jetzt gehts los |
TFuchsbichler | 2:bc32145e7819 | 35 | |
TFuchsbichler | 2:bc32145e7819 | 36 | while(1) { |
TFuchsbichler | 2:bc32145e7819 | 37 | wait(1); |
TFuchsbichler | 2:bc32145e7819 | 38 | FollowLine(); |
TFuchsbichler | 2:bc32145e7819 | 39 | |
TFuchsbichler | 2:bc32145e7819 | 40 | } |
TFuchsbichler | 2:bc32145e7819 | 41 | } |
TFuchsbichler | 2:bc32145e7819 | 42 | |
TFuchsbichler | 2:bc32145e7819 | 43 | |
TFuchsbichler | 2:bc32145e7819 | 44 | |
TFuchsbichler | 2:bc32145e7819 | 45 | |
TFuchsbichler | 2:bc32145e7819 | 46 | |
TFuchsbichler | 2:bc32145e7819 | 47 | void FollowLine() |
TFuchsbichler | 2:bc32145e7819 | 48 | { |
TFuchsbichler | 2:bc32145e7819 | 49 | |
TFuchsbichler | 2:bc32145e7819 | 50 | |
TFuchsbichler | 2:bc32145e7819 | 51 | while(1) { |
TFuchsbichler | 2:bc32145e7819 | 52 | if(ls2.Read()>600) { |
TFuchsbichler | 2:bc32145e7819 | 53 | t.reset(); |
TFuchsbichler | 2:bc32145e7819 | 54 | leftturn(); |
TFuchsbichler | 2:bc32145e7819 | 55 | leftright=1; |
TFuchsbichler | 2:bc32145e7819 | 56 | |
TFuchsbichler | 2:bc32145e7819 | 57 | } |
TFuchsbichler | 2:bc32145e7819 | 58 | |
TFuchsbichler | 2:bc32145e7819 | 59 | if(ls2.Read()<600) { |
TFuchsbichler | 2:bc32145e7819 | 60 | rightturn(); |
TFuchsbichler | 2:bc32145e7819 | 61 | leftright=0; |
TFuchsbichler | 2:bc32145e7819 | 62 | } |
TFuchsbichler | 2:bc32145e7819 | 63 | |
TFuchsbichler | 2:bc32145e7819 | 64 | |
TFuchsbichler | 2:bc32145e7819 | 65 | } |
TFuchsbichler | 2:bc32145e7819 | 66 | t.reset(); |
TFuchsbichler | 2:bc32145e7819 | 67 | t.stop(); |
TFuchsbichler | 2:bc32145e7819 | 68 | mL.SetPow(0); |
TFuchsbichler | 2:bc32145e7819 | 69 | mR.SetPow(0); |
TFuchsbichler | 2:bc32145e7819 | 70 | |
TFuchsbichler | 2:bc32145e7819 | 71 | } |
TFuchsbichler | 2:bc32145e7819 | 72 | |
TFuchsbichler | 2:bc32145e7819 | 73 | void rightturn() |
TFuchsbichler | 2:bc32145e7819 | 74 | { |
TFuchsbichler | 2:bc32145e7819 | 75 | mL.SetPow(0.2); |
TFuchsbichler | 2:bc32145e7819 | 76 | mR.SetPow(0.1); |
TFuchsbichler | 2:bc32145e7819 | 77 | |
TFuchsbichler | 2:bc32145e7819 | 78 | } |
TFuchsbichler | 2:bc32145e7819 | 79 | void leftturn() |
TFuchsbichler | 2:bc32145e7819 | 80 | { |
TFuchsbichler | 2:bc32145e7819 | 81 | mL.SetPow(0.1); |
TFuchsbichler | 2:bc32145e7819 | 82 | mR.SetPow(0.2); |
TFuchsbichler | 2:bc32145e7819 | 83 | } |
TFuchsbichler | 2:bc32145e7819 | 84 | |
TFuchsbichler | 2:bc32145e7819 | 85 | void stop() |
TFuchsbichler | 2:bc32145e7819 | 86 | { |
TFuchsbichler | 2:bc32145e7819 | 87 | mL.SetPow(0); |
TFuchsbichler | 2:bc32145e7819 | 88 | mR.SetPow(0); |
TFuchsbichler | 2:bc32145e7819 | 89 | } |
TFuchsbichler | 2:bc32145e7819 | 90 | |
TFuchsbichler | 2:bc32145e7819 | 91 | void righthard() |
TFuchsbichler | 2:bc32145e7819 | 92 | { |
TFuchsbichler | 2:bc32145e7819 | 93 | mL.SetPow(-0.4); |
TFuchsbichler | 2:bc32145e7819 | 94 | mR.SetPow(0.4); |
TFuchsbichler | 2:bc32145e7819 | 95 | } |
TFuchsbichler | 2:bc32145e7819 | 96 | |
TFuchsbichler | 2:bc32145e7819 | 97 | void lefthard() |
TFuchsbichler | 2:bc32145e7819 | 98 | { |
TFuchsbichler | 2:bc32145e7819 | 99 | mL.SetPow(-0.4); |
TFuchsbichler | 2:bc32145e7819 | 100 | mR.SetPow(0.4); |
TFuchsbichler | 2:bc32145e7819 | 101 | } |
TFuchsbichler | 2:bc32145e7819 | 102 | |
TFuchsbichler | 2:bc32145e7819 | 103 | void FindLine() |
TFuchsbichler | 2:bc32145e7819 | 104 | { |
TFuchsbichler | 2:bc32145e7819 | 105 | |
TFuchsbichler | 2:bc32145e7819 | 106 | while(1) { |
TFuchsbichler | 2:bc32145e7819 | 107 | |
TFuchsbichler | 2:bc32145e7819 | 108 | if(leftright==1) { |
TFuchsbichler | 2:bc32145e7819 | 109 | righthard(); |
TFuchsbichler | 2:bc32145e7819 | 110 | } |
TFuchsbichler | 2:bc32145e7819 | 111 | if(leftright==0) { |
TFuchsbichler | 2:bc32145e7819 | 112 | lefthard(); |
TFuchsbichler | 2:bc32145e7819 | 113 | } |
TFuchsbichler | 2:bc32145e7819 | 114 | if(ls2.Read()>500|ls3.Read()>500) { |
TFuchsbichler | 2:bc32145e7819 | 115 | stop(); |
TFuchsbichler | 2:bc32145e7819 | 116 | break; |
TFuchsbichler | 2:bc32145e7819 | 117 | } |
TFuchsbichler | 2:bc32145e7819 | 118 | } |
TFuchsbichler | 2:bc32145e7819 | 119 | |
TFuchsbichler | 2:bc32145e7819 | 120 | } |
TFuchsbichler | 2:bc32145e7819 | 121 | |
TFuchsbichler | 2:bc32145e7819 | 122 | |
TFuchsbichler | 2:bc32145e7819 | 123 | |
TFuchsbichler | 2:bc32145e7819 | 124 | |
TFuchsbichler | 2:bc32145e7819 | 125 |