![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Line
main.cpp
- Committer:
- MarkusSchieder
- Date:
- 2015-10-13
- Revision:
- 1:2e48f1a47d07
- Parent:
- 0:2271f1e56f17
File content as of revision 1:2e48f1a47d07:
#include "mbed.h" #include "Serial_HL.h" #include "Bertl14.h" #include "BertlObjects.h" SerialBLK pc(USBTX, USBRX); SvProtocol ua0(&pc); void CommandHandler(); void DriveUntilObstacle(); void Turn180(); void blinkvorne(); void blinkhinten(); Timer t1; unsigned int led=1; int main(void) { InitBertl(); pex.useISR=0; leds=9; pex.ClearLeds(); pc.format(8,SerialBLK::None,1); pc.baud(500000); // 115200 ua0.SvMessage("MotorTest1"); // Meldung zum PC senden Timer stw; stw.start(); while(1) { t1.start(); pex.ReadButtons(); if( pex.IsButton(BTN_FLL) ) { DriveUntilObstacle(); } CommandHandler(); if( (stw.read_ms()>10) ) // 100Hz { // dieser Teil wird mit 100Hz aufgerufen stw.reset(); if( ua0.acqON ) { // nur wenn vom PC aus das Senden eingeschaltet wurde // wird auch etwas gesendet ua0.WriteSvI16(1, mL.encCnt); ua0.WriteSvI16(2, mR.encCnt); ua0.WriteSvI16(3, us.distCM*100.0); us.StartMeas(); // for next cycle } } } return 1; } void CommandHandler() { uint8_t cmd; // Fragen ob überhaupt etwas im RX-Reg steht if( !pc.IsDataAvail() ) return; // wenn etwas im RX-Reg steht // Kommando lesen cmd = ua0.GetCommand(); if( cmd==2 ) // Encoder auf 0 setzen { mL.encCnt=0; mR.encCnt=0; ua0.SvMessage("Clear Encoder"); } if( cmd==3 ) // bei beiden Motoren Gas geben { mL.SetPow(ua0.ReadF()); mR.SetPow(ua0.ReadF()); ua0.SvMessage("Set Power"); } } void DriveUntilObstacle() { //pex.SetLeds(LED_FL1|LED_FL2); mL.SetPow(0.5); mR.SetPow(-0.5); while(1) { us.StartMeas(); wait_ms(10); if(led==0) blinkvorne(); else blinkhinten(); if ((us.distCM) < 5) { Turn180(); wait_ms(10); } } } void Turn180() { if(led==0) led=1; else led=0; mL.SetPow(0.5); mR.SetPow(0.5); mL.encCnt=0; mR.encCnt=0; while(1) { if (mL.encCnt>45) mL.SetPow(0); if (mR.encCnt>45) mR.SetPow(0); if (mR.encCnt>48 && mL.encCnt>48 ) { DriveUntilObstacle(); } } } void blinkvorne() { if(t1.read_ms() > 200) { pex.ClearLeds(); pex.SetLeds(LED_ALL_FRONT); } if(t1.read_ms() > 200) { pex.ClearLeds(); t1.reset(); } } void blinkhinten() { if(t1.read_ms() > 200) { pex.ClearLeds(); pex.SetLeds(LED_ALL_BACK); } if(t1.read_ms() > 200) { pex.ClearLeds(); t1.reset(); } }