![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
1 Sensor
Fork of BertlTemplate2 by
Bertel_FollowLine.cpp@3:d0894bb57690, 2016-03-07 (annotated)
- Committer:
- TFuchsbichler
- Date:
- Mon Mar 07 08:52:11 2016 +0000
- Revision:
- 3:d0894bb57690
- Parent:
- 2:bc32145e7819
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 | |
TFuchsbichler | 2:bc32145e7819 | 22 | int main(void) |
TFuchsbichler | 2:bc32145e7819 | 23 | { |
TFuchsbichler | 2:bc32145e7819 | 24 | boardPow=3; |
TFuchsbichler | 2:bc32145e7819 | 25 | InitBertl(); |
TFuchsbichler | 2:bc32145e7819 | 26 | pex.ClearLeds(); |
TFuchsbichler | 2:bc32145e7819 | 27 | // Damit der Roboter nicht gleich loskoffert |
TFuchsbichler | 2:bc32145e7819 | 28 | leds = 9; |
TFuchsbichler | 2:bc32145e7819 | 29 | pex.WaitUntilFrontButtonPressed(); |
TFuchsbichler | 2:bc32145e7819 | 30 | leds=6; |
TFuchsbichler | 2:bc32145e7819 | 31 | wait(0.8); |
TFuchsbichler | 2:bc32145e7819 | 32 | leds=0; |
TFuchsbichler | 2:bc32145e7819 | 33 | // 2x Button-pressed jetzt gehts los |
TFuchsbichler | 2:bc32145e7819 | 34 | |
TFuchsbichler | 2:bc32145e7819 | 35 | while(1) { |
TFuchsbichler | 2:bc32145e7819 | 36 | wait(1); |
TFuchsbichler | 2:bc32145e7819 | 37 | FollowLine(); |
TFuchsbichler | 2:bc32145e7819 | 38 | |
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 | void FollowLine() |
TFuchsbichler | 2:bc32145e7819 | 47 | { |
TFuchsbichler | 2:bc32145e7819 | 48 | |
TFuchsbichler | 2:bc32145e7819 | 49 | |
TFuchsbichler | 2:bc32145e7819 | 50 | while(1) { |
TFuchsbichler | 2:bc32145e7819 | 51 | if(ls2.Read()>600) { |
TFuchsbichler | 2:bc32145e7819 | 52 | t.reset(); |
TFuchsbichler | 2:bc32145e7819 | 53 | leftturn(); |
TFuchsbichler | 2:bc32145e7819 | 54 | leftright=1; |
TFuchsbichler | 2:bc32145e7819 | 55 | |
TFuchsbichler | 2:bc32145e7819 | 56 | } |
TFuchsbichler | 2:bc32145e7819 | 57 | |
TFuchsbichler | 2:bc32145e7819 | 58 | if(ls2.Read()<600) { |
TFuchsbichler | 2:bc32145e7819 | 59 | rightturn(); |
TFuchsbichler | 2:bc32145e7819 | 60 | leftright=0; |
TFuchsbichler | 2:bc32145e7819 | 61 | } |
TFuchsbichler | 2:bc32145e7819 | 62 | |
TFuchsbichler | 2:bc32145e7819 | 63 | |
TFuchsbichler | 2:bc32145e7819 | 64 | } |
TFuchsbichler | 2:bc32145e7819 | 65 | |
TFuchsbichler | 2:bc32145e7819 | 66 | } |
TFuchsbichler | 2:bc32145e7819 | 67 | |
TFuchsbichler | 2:bc32145e7819 | 68 | void rightturn() |
TFuchsbichler | 2:bc32145e7819 | 69 | { |
TFuchsbichler | 2:bc32145e7819 | 70 | mL.SetPow(0.2); |
TFuchsbichler | 2:bc32145e7819 | 71 | mR.SetPow(0.1); |
TFuchsbichler | 2:bc32145e7819 | 72 | |
TFuchsbichler | 2:bc32145e7819 | 73 | } |
TFuchsbichler | 2:bc32145e7819 | 74 | void leftturn() |
TFuchsbichler | 2:bc32145e7819 | 75 | { |
TFuchsbichler | 2:bc32145e7819 | 76 | mL.SetPow(0.1); |
TFuchsbichler | 2:bc32145e7819 | 77 | mR.SetPow(0.2); |
TFuchsbichler | 2:bc32145e7819 | 78 | } |
TFuchsbichler | 2:bc32145e7819 | 79 | |
TFuchsbichler | 2:bc32145e7819 | 80 | void stop() |
TFuchsbichler | 2:bc32145e7819 | 81 | { |
TFuchsbichler | 2:bc32145e7819 | 82 | mL.SetPow(0); |
TFuchsbichler | 2:bc32145e7819 | 83 | mR.SetPow(0); |
TFuchsbichler | 2:bc32145e7819 | 84 | } |
TFuchsbichler | 2:bc32145e7819 | 85 | |
TFuchsbichler | 2:bc32145e7819 | 86 | void righthard() |
TFuchsbichler | 2:bc32145e7819 | 87 | { |
TFuchsbichler | 2:bc32145e7819 | 88 | mL.SetPow(-0.4); |
TFuchsbichler | 2:bc32145e7819 | 89 | mR.SetPow(0.4); |
TFuchsbichler | 2:bc32145e7819 | 90 | } |
TFuchsbichler | 2:bc32145e7819 | 91 | |
TFuchsbichler | 2:bc32145e7819 | 92 | void lefthard() |
TFuchsbichler | 2:bc32145e7819 | 93 | { |
TFuchsbichler | 2:bc32145e7819 | 94 | mL.SetPow(-0.4); |
TFuchsbichler | 2:bc32145e7819 | 95 | mR.SetPow(0.4); |
TFuchsbichler | 2:bc32145e7819 | 96 | } |
TFuchsbichler | 2:bc32145e7819 | 97 | |
TFuchsbichler | 2:bc32145e7819 | 98 | void FindLine() |
TFuchsbichler | 2:bc32145e7819 | 99 | { |
TFuchsbichler | 2:bc32145e7819 | 100 | |
TFuchsbichler | 2:bc32145e7819 | 101 | while(1) { |
TFuchsbichler | 2:bc32145e7819 | 102 | |
TFuchsbichler | 2:bc32145e7819 | 103 | if(leftright==1) { |
TFuchsbichler | 2:bc32145e7819 | 104 | righthard(); |
TFuchsbichler | 2:bc32145e7819 | 105 | } |
TFuchsbichler | 2:bc32145e7819 | 106 | if(leftright==0) { |
TFuchsbichler | 2:bc32145e7819 | 107 | lefthard(); |
TFuchsbichler | 2:bc32145e7819 | 108 | } |
TFuchsbichler | 2:bc32145e7819 | 109 | if(ls2.Read()>500|ls3.Read()>500) { |
TFuchsbichler | 2:bc32145e7819 | 110 | stop(); |
TFuchsbichler | 2:bc32145e7819 | 111 | break; |
TFuchsbichler | 2:bc32145e7819 | 112 | } |
TFuchsbichler | 2:bc32145e7819 | 113 | } |
TFuchsbichler | 2:bc32145e7819 | 114 | |
TFuchsbichler | 2:bc32145e7819 | 115 | } |
TFuchsbichler | 2:bc32145e7819 | 116 | |
TFuchsbichler | 2:bc32145e7819 | 117 | |
TFuchsbichler | 2:bc32145e7819 | 118 | |
TFuchsbichler | 2:bc32145e7819 | 119 | |
TFuchsbichler | 2:bc32145e7819 | 120 |