ABELI / Mbed 2 deprecated LineMaster

Line

Dependencies:   BertlLib mbed

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?

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