Lab 4CHEL / Mbed 2 deprecated Bertel_FollowLine

1 Sensor

Dependencies:   BertlLib mbed

Fork of BertlTemplate2 by michael hollegha

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?

UserRevisionLine numberNew 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