Diff: BertlCard.cpp

Revision:

0:46edd470c018

Child:

1:69daffc2b325

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BertlCard.cpp	Wed Oct 14 14:39:03 2015 +0000
@@ -0,0 +1,83 @@
+
+#include "mbed.h"
+#include "Serial_HL.h"
+#include "Bertl14.h"
+#include "BertlObjects.h"
+
+SerialBLK pc(USBTX, USBRX);
+// SerialBLK pc(p9, p10); // BlueTooth
+SvProtocol ua0(&pc);
+
+//              main=2^0  LS    ENC 2^2
+BusOut boardPow(p30,      P1_6, P1_7);
+
+void CommandHandler();
+
+float gForw=0.0, gTurn=0.0;
+
+int main(void)
+{
+    InitBertl();
+    pex.useISR=0;
+    leds=0;
+    pex.ClearLeds();
+    // pex.SetLedPort(LED_FL1|LED_FR1);
+    pex.SetLedPort(LED_BR2|LED_BL1);
+    pc.format(8,SerialBLK::None,1);
+    pc.baud(115200); // 115200
+    boardPow = 1;
+    ua0.SvMessage("BertlKart_5"); // 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, gForw*100);
+                ua0.WriteSvI16(2, gTurn*100);
+            }
+        }
+    }
+    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 ) {
+        if( ua0.ReadI16() )
+            pex.SetLedPort(LED_FL1|LED_FR1);
+        else
+            pex.ClearLeds();
+    }
+    if( cmd==4 ) {
+        mL.SetPow(ua0.ReadF());
+        mR.SetPow(ua0.ReadF());
+    }
+    if( cmd==5 ) {
+        gForw = ua0.ReadF();
+        // gForw = -ua0.ReadF(); // Wetz
+        mL.SetPow(gForw+gTurn);
+        mR.SetPow(gForw-gTurn);
+    } else if( cmd==6 ) {
+        gTurn = ua0.ReadF();
+        // gTurn = -ua0.ReadF(); // Wetz
+        mL.SetPow(gForw+gTurn);
+        mR.SetPow(gForw-gTurn);
+    }
+}
+
+
+