MotorTest.cpp

00001 
00002 #include "mbed.h"
00003 #include "Serial_HL.h"
00004 #include "Bertl14.h"
00005 #include "BertlObjects.h"
00006  
00007 SerialBLK pc(USBTX, USBRX);
00008 SvProtocol ua0(&pc);
00009 
00010 V 2.0
00011 
00012 void CommandHandler();
00013 
00014 int main(void)
00015 {
00016   InitBertl();
00017     pex.useISR=0; leds=9;
00018     pex.ClearLeds();
00019   pc.format(8,SerialBLK::None,1); pc.baud(500000); // 115200
00020   
00021   ua0.SvMessage("MotorTest1"); // Meldung zum PC senden
00022   
00023   Timer stw; stw.start();
00024   while(1)
00025   {
00026     CommandHandler();
00027     if( (stw.read_ms()>10) ) // 100Hz
00028     { // dieser Teil wird mit 100Hz aufgerufen
00029       stw.reset();
00030       if( ua0.acqON ) {
00031         // nur wenn vom PC aus das Senden eingeschaltet wurde
00032         // wird auch etwas gesendet
00033         ua0.WriteSvI16(1, mL.encCnt);
00034         ua0.WriteSvI16(2, mR.encCnt);
00035         ua0.WriteSvI16(3, us.distCM*100.0);
00036         us.StartMeas(); // for next cycle
00037       }
00038     }
00039   }
00040   return 1;
00041 }
00042 
00043 void CommandHandler()
00044 {
00045   uint8_t cmd;
00046   // Fragen ob überhaupt etwas im RX-Reg steht
00047   if( !pc.IsDataAvail() )
00048     return;
00049   // wenn etwas im RX-Reg steht
00050     // Kommando lesen
00051     cmd = ua0.GetCommand();
00052   
00053   if( cmd==2 ) // Encoder auf 0 setzen
00054     {
00055     mL.encCnt=0; mR.encCnt=0;
00056     ua0.SvMessage("Clear Encoder");
00057   }
00058   if( cmd==3 ) // bei beiden Motoren Gas geben
00059   {
00060     mL.SetPow(ua0.ReadF());
00061     mR.SetPow(ua0.ReadF());
00062     ua0.SvMessage("Set Power");
00063   }
00064 }
00065 
00066