ABELI
/
LineMaster
Line
main.cpp@1:2e48f1a47d07, 2015-10-13 (annotated)
- Committer:
- MarkusSchieder
- Date:
- Tue Oct 13 16:46:33 2015 +0000
- Revision:
- 1:2e48f1a47d07
- Parent:
- 0:2271f1e56f17
adsf
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
markus2410 | 0:2271f1e56f17 | 1 | #include "mbed.h" |
markus2410 | 0:2271f1e56f17 | 2 | #include "Serial_HL.h" |
markus2410 | 0:2271f1e56f17 | 3 | #include "Bertl14.h" |
markus2410 | 0:2271f1e56f17 | 4 | #include "BertlObjects.h" |
markus2410 | 0:2271f1e56f17 | 5 | |
markus2410 | 0:2271f1e56f17 | 6 | SerialBLK pc(USBTX, USBRX); |
markus2410 | 0:2271f1e56f17 | 7 | SvProtocol ua0(&pc); |
markus2410 | 0:2271f1e56f17 | 8 | |
markus2410 | 0:2271f1e56f17 | 9 | void CommandHandler(); |
MarkusSchieder | 1:2e48f1a47d07 | 10 | void DriveUntilObstacle(); |
MarkusSchieder | 1:2e48f1a47d07 | 11 | void Turn180(); |
markus2410 | 0:2271f1e56f17 | 12 | void blinkvorne(); |
markus2410 | 0:2271f1e56f17 | 13 | void blinkhinten(); |
markus2410 | 0:2271f1e56f17 | 14 | Timer t1; |
markus2410 | 0:2271f1e56f17 | 15 | unsigned int led=1; |
markus2410 | 0:2271f1e56f17 | 16 | |
markus2410 | 0:2271f1e56f17 | 17 | int main(void) |
markus2410 | 0:2271f1e56f17 | 18 | { |
markus2410 | 0:2271f1e56f17 | 19 | InitBertl(); |
markus2410 | 0:2271f1e56f17 | 20 | pex.useISR=0; leds=9; |
markus2410 | 0:2271f1e56f17 | 21 | pex.ClearLeds(); |
markus2410 | 0:2271f1e56f17 | 22 | pc.format(8,SerialBLK::None,1); pc.baud(500000); // 115200 |
markus2410 | 0:2271f1e56f17 | 23 | |
markus2410 | 0:2271f1e56f17 | 24 | ua0.SvMessage("MotorTest1"); // Meldung zum PC senden |
markus2410 | 0:2271f1e56f17 | 25 | |
markus2410 | 0:2271f1e56f17 | 26 | Timer stw; stw.start(); |
markus2410 | 0:2271f1e56f17 | 27 | while(1) |
markus2410 | 0:2271f1e56f17 | 28 | { |
MarkusSchieder | 1:2e48f1a47d07 | 29 | t1.start(); |
MarkusSchieder | 1:2e48f1a47d07 | 30 | pex.ReadButtons(); |
MarkusSchieder | 1:2e48f1a47d07 | 31 | if( pex.IsButton(BTN_FLL) ) |
MarkusSchieder | 1:2e48f1a47d07 | 32 | { |
MarkusSchieder | 1:2e48f1a47d07 | 33 | DriveUntilObstacle(); |
MarkusSchieder | 1:2e48f1a47d07 | 34 | } |
MarkusSchieder | 1:2e48f1a47d07 | 35 | |
markus2410 | 0:2271f1e56f17 | 36 | |
MarkusSchieder | 1:2e48f1a47d07 | 37 | |
MarkusSchieder | 1:2e48f1a47d07 | 38 | |
MarkusSchieder | 1:2e48f1a47d07 | 39 | |
MarkusSchieder | 1:2e48f1a47d07 | 40 | CommandHandler(); |
markus2410 | 0:2271f1e56f17 | 41 | if( (stw.read_ms()>10) ) // 100Hz |
markus2410 | 0:2271f1e56f17 | 42 | { // dieser Teil wird mit 100Hz aufgerufen |
markus2410 | 0:2271f1e56f17 | 43 | stw.reset(); |
markus2410 | 0:2271f1e56f17 | 44 | if( ua0.acqON ) { |
markus2410 | 0:2271f1e56f17 | 45 | // nur wenn vom PC aus das Senden eingeschaltet wurde |
markus2410 | 0:2271f1e56f17 | 46 | // wird auch etwas gesendet |
markus2410 | 0:2271f1e56f17 | 47 | ua0.WriteSvI16(1, mL.encCnt); |
markus2410 | 0:2271f1e56f17 | 48 | ua0.WriteSvI16(2, mR.encCnt); |
markus2410 | 0:2271f1e56f17 | 49 | ua0.WriteSvI16(3, us.distCM*100.0); |
markus2410 | 0:2271f1e56f17 | 50 | us.StartMeas(); // for next cycle |
markus2410 | 0:2271f1e56f17 | 51 | } |
markus2410 | 0:2271f1e56f17 | 52 | } |
markus2410 | 0:2271f1e56f17 | 53 | } |
markus2410 | 0:2271f1e56f17 | 54 | return 1; |
markus2410 | 0:2271f1e56f17 | 55 | } |
markus2410 | 0:2271f1e56f17 | 56 | |
markus2410 | 0:2271f1e56f17 | 57 | void CommandHandler() |
markus2410 | 0:2271f1e56f17 | 58 | { |
markus2410 | 0:2271f1e56f17 | 59 | uint8_t cmd; |
markus2410 | 0:2271f1e56f17 | 60 | // Fragen ob überhaupt etwas im RX-Reg steht |
markus2410 | 0:2271f1e56f17 | 61 | if( !pc.IsDataAvail() ) |
markus2410 | 0:2271f1e56f17 | 62 | return; |
markus2410 | 0:2271f1e56f17 | 63 | // wenn etwas im RX-Reg steht |
markus2410 | 0:2271f1e56f17 | 64 | // Kommando lesen |
markus2410 | 0:2271f1e56f17 | 65 | cmd = ua0.GetCommand(); |
markus2410 | 0:2271f1e56f17 | 66 | |
markus2410 | 0:2271f1e56f17 | 67 | if( cmd==2 ) // Encoder auf 0 setzen |
markus2410 | 0:2271f1e56f17 | 68 | { |
markus2410 | 0:2271f1e56f17 | 69 | mL.encCnt=0; mR.encCnt=0; |
markus2410 | 0:2271f1e56f17 | 70 | ua0.SvMessage("Clear Encoder"); |
markus2410 | 0:2271f1e56f17 | 71 | } |
markus2410 | 0:2271f1e56f17 | 72 | if( cmd==3 ) // bei beiden Motoren Gas geben |
markus2410 | 0:2271f1e56f17 | 73 | { |
markus2410 | 0:2271f1e56f17 | 74 | mL.SetPow(ua0.ReadF()); |
markus2410 | 0:2271f1e56f17 | 75 | mR.SetPow(ua0.ReadF()); |
markus2410 | 0:2271f1e56f17 | 76 | ua0.SvMessage("Set Power"); |
markus2410 | 0:2271f1e56f17 | 77 | } |
markus2410 | 0:2271f1e56f17 | 78 | } |
markus2410 | 0:2271f1e56f17 | 79 | |
markus2410 | 0:2271f1e56f17 | 80 | |
MarkusSchieder | 1:2e48f1a47d07 | 81 | void DriveUntilObstacle() |
MarkusSchieder | 1:2e48f1a47d07 | 82 | { |
MarkusSchieder | 1:2e48f1a47d07 | 83 | |
MarkusSchieder | 1:2e48f1a47d07 | 84 | //pex.SetLeds(LED_FL1|LED_FL2); |
MarkusSchieder | 1:2e48f1a47d07 | 85 | mL.SetPow(0.5); |
MarkusSchieder | 1:2e48f1a47d07 | 86 | mR.SetPow(-0.5); |
MarkusSchieder | 1:2e48f1a47d07 | 87 | |
MarkusSchieder | 1:2e48f1a47d07 | 88 | while(1) |
MarkusSchieder | 1:2e48f1a47d07 | 89 | { |
MarkusSchieder | 1:2e48f1a47d07 | 90 | us.StartMeas(); |
MarkusSchieder | 1:2e48f1a47d07 | 91 | wait_ms(10); |
MarkusSchieder | 1:2e48f1a47d07 | 92 | if(led==0) |
MarkusSchieder | 1:2e48f1a47d07 | 93 | blinkvorne(); |
MarkusSchieder | 1:2e48f1a47d07 | 94 | else |
MarkusSchieder | 1:2e48f1a47d07 | 95 | blinkhinten(); |
MarkusSchieder | 1:2e48f1a47d07 | 96 | if ((us.distCM) < 5) |
MarkusSchieder | 1:2e48f1a47d07 | 97 | { |
MarkusSchieder | 1:2e48f1a47d07 | 98 | |
MarkusSchieder | 1:2e48f1a47d07 | 99 | Turn180(); |
MarkusSchieder | 1:2e48f1a47d07 | 100 | wait_ms(10); |
MarkusSchieder | 1:2e48f1a47d07 | 101 | } |
MarkusSchieder | 1:2e48f1a47d07 | 102 | |
MarkusSchieder | 1:2e48f1a47d07 | 103 | |
MarkusSchieder | 1:2e48f1a47d07 | 104 | } |
MarkusSchieder | 1:2e48f1a47d07 | 105 | |
markus2410 | 0:2271f1e56f17 | 106 | |
markus2410 | 0:2271f1e56f17 | 107 | |
markus2410 | 0:2271f1e56f17 | 108 | } |
markus2410 | 0:2271f1e56f17 | 109 | |
markus2410 | 0:2271f1e56f17 | 110 | |
MarkusSchieder | 1:2e48f1a47d07 | 111 | void Turn180() |
MarkusSchieder | 1:2e48f1a47d07 | 112 | |
markus2410 | 0:2271f1e56f17 | 113 | { |
MarkusSchieder | 1:2e48f1a47d07 | 114 | if(led==0) |
MarkusSchieder | 1:2e48f1a47d07 | 115 | led=1; |
MarkusSchieder | 1:2e48f1a47d07 | 116 | else |
MarkusSchieder | 1:2e48f1a47d07 | 117 | led=0; |
MarkusSchieder | 1:2e48f1a47d07 | 118 | mL.SetPow(0.5); |
MarkusSchieder | 1:2e48f1a47d07 | 119 | mR.SetPow(0.5); |
MarkusSchieder | 1:2e48f1a47d07 | 120 | mL.encCnt=0; |
MarkusSchieder | 1:2e48f1a47d07 | 121 | mR.encCnt=0; |
MarkusSchieder | 1:2e48f1a47d07 | 122 | |
MarkusSchieder | 1:2e48f1a47d07 | 123 | while(1) |
MarkusSchieder | 1:2e48f1a47d07 | 124 | { |
MarkusSchieder | 1:2e48f1a47d07 | 125 | if (mL.encCnt>45) |
MarkusSchieder | 1:2e48f1a47d07 | 126 | mL.SetPow(0); |
MarkusSchieder | 1:2e48f1a47d07 | 127 | if (mR.encCnt>45) |
MarkusSchieder | 1:2e48f1a47d07 | 128 | mR.SetPow(0); |
MarkusSchieder | 1:2e48f1a47d07 | 129 | |
MarkusSchieder | 1:2e48f1a47d07 | 130 | if (mR.encCnt>48 && mL.encCnt>48 ) |
MarkusSchieder | 1:2e48f1a47d07 | 131 | { |
MarkusSchieder | 1:2e48f1a47d07 | 132 | |
MarkusSchieder | 1:2e48f1a47d07 | 133 | DriveUntilObstacle(); |
MarkusSchieder | 1:2e48f1a47d07 | 134 | } |
MarkusSchieder | 1:2e48f1a47d07 | 135 | } |
markus2410 | 0:2271f1e56f17 | 136 | } |
MarkusSchieder | 1:2e48f1a47d07 | 137 | |
MarkusSchieder | 1:2e48f1a47d07 | 138 | void blinkvorne() |
markus2410 | 0:2271f1e56f17 | 139 | { |
MarkusSchieder | 1:2e48f1a47d07 | 140 | if(t1.read_ms() > 200) |
MarkusSchieder | 1:2e48f1a47d07 | 141 | { pex.ClearLeds(); |
MarkusSchieder | 1:2e48f1a47d07 | 142 | pex.SetLeds(LED_ALL_FRONT); |
MarkusSchieder | 1:2e48f1a47d07 | 143 | |
MarkusSchieder | 1:2e48f1a47d07 | 144 | } |
MarkusSchieder | 1:2e48f1a47d07 | 145 | |
MarkusSchieder | 1:2e48f1a47d07 | 146 | if(t1.read_ms() > 200) |
MarkusSchieder | 1:2e48f1a47d07 | 147 | { |
MarkusSchieder | 1:2e48f1a47d07 | 148 | pex.ClearLeds(); |
MarkusSchieder | 1:2e48f1a47d07 | 149 | t1.reset(); |
MarkusSchieder | 1:2e48f1a47d07 | 150 | } |
MarkusSchieder | 1:2e48f1a47d07 | 151 | |
MarkusSchieder | 1:2e48f1a47d07 | 152 | } |
MarkusSchieder | 1:2e48f1a47d07 | 153 | |
MarkusSchieder | 1:2e48f1a47d07 | 154 | void blinkhinten() |
markus2410 | 0:2271f1e56f17 | 155 | { |
MarkusSchieder | 1:2e48f1a47d07 | 156 | if(t1.read_ms() > 200) |
MarkusSchieder | 1:2e48f1a47d07 | 157 | { pex.ClearLeds(); |
MarkusSchieder | 1:2e48f1a47d07 | 158 | pex.SetLeds(LED_ALL_BACK); |
MarkusSchieder | 1:2e48f1a47d07 | 159 | |
MarkusSchieder | 1:2e48f1a47d07 | 160 | } |
MarkusSchieder | 1:2e48f1a47d07 | 161 | |
MarkusSchieder | 1:2e48f1a47d07 | 162 | if(t1.read_ms() > 200) |
MarkusSchieder | 1:2e48f1a47d07 | 163 | { |
MarkusSchieder | 1:2e48f1a47d07 | 164 | pex.ClearLeds(); |
MarkusSchieder | 1:2e48f1a47d07 | 165 | t1.reset(); |
MarkusSchieder | 1:2e48f1a47d07 | 166 | } |
MarkusSchieder | 1:2e48f1a47d07 | 167 | } |