Diff: MotorTest.cpp

Revision:

1:ef44156a6e44

Parent:

0:9ac39e20730c

diff -r 9ac39e20730c -r ef44156a6e44 MotorTest.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorTest.cpp	Tue Feb 24 11:28:43 2015 +0000
@@ -0,0 +1,64 @@
+
+#include "mbed.h"
+#include "Serial_HL.h"
+#include "Bertl14.h"
+#include "BertlObjects.h"
+ 
+SerialBLK pc(USBTX, USBRX);
+SvProtocol ua0(&pc);
+
+void CommandHandler();
+
+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)
+  {
+    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");
+  }
+}
+
+