Diff: BertlCard.cpp

Revision:

1:69daffc2b325

Parent:

0:46edd470c018

Child:

2:90bc6c045ffd

--- a/BertlCard.cpp	Wed Oct 14 14:39:03 2015 +0000
+++ b/BertlCard.cpp	Thu Nov 19 08:12:24 2015 +0000
@@ -1,8 +1,9 @@
-
 #include "mbed.h"
 #include "Serial_HL.h"
 #include "Bertl14.h"
-#include "BertlObjects.h"
+#include "BertlObjects.cpp"
+
+// V 3.0
 
 SerialBLK pc(USBTX, USBRX);
 // SerialBLK pc(p9, p10); // BlueTooth
@@ -12,72 +13,87 @@
 BusOut boardPow(p30,      P1_6, P1_7);
 
 void CommandHandler();
-
-float gForw=0.0, gTurn=0.0;
+void SwitchLeds();
 
 int main(void)
 {
+    boardPow=1;
+    wait_ms(10);
     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
+    ua0.SvMessage("BertlKart_1");
 
     Timer stw;
     stw.start();
     while(1) {
         CommandHandler();
-        if( (stw.read_ms()>10) ) { // 100Hz
-            // dieser Teil wird mit 100Hz aufgerufen
+        if( (stw.read_ms()>10) ) {
+            SwitchLeds();
             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);
+                ua0.WriteSvI16(1, mL.encCnt);
+                ua0.WriteSvI16(2, mR.encCnt);
             }
         }
     }
     return 1;
 }
 
+int16_t ledsOn = 0;
+void SwitchLeds()
+{
+    pex.ReadButtons();
+    if( pex.IsAnyBackButton() ) {
+        if( ledsOn ) {
+            pex.ClearLeds();
+            ledsOn=0;
+        } else {
+            pex.SetLedPort(LED_BR2|LED_BL1);
+            ledsOn=1;
+        }
+        wait_ms(500);
+    }
+}
+
 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);
+            pex.SetLedPort(LED_BR2|LED_BL1);
         else
             pex.ClearLeds();
     }
+    if( cmd==3 ) { // Rev. Brake
+        int16_t dir = pc.GetChar();
+        if( dir>0 ) { // 1 Brake for Forw.
+            mL.SetPow(-0.3);
+            mR.SetPow(-0.3);
+        } else { // 0 Brake for Backw.
+            mL.SetPow(0.3);
+            mR.SetPow(0.3);
+        }
+        wait_ms(50);
+        mL.SetPow(0);
+        mR.SetPow(0);
+        ua0.SvMessage("Rev Brake");
+    }
     if( cmd==4 ) {
-        mL.SetPow(ua0.ReadF());
-        mR.SetPow(ua0.ReadF());
+        mL.SetPow2(ua0.ReadF());
+        mR.SetPow2(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);
+        mL.SetPow(ua0.ReadF());
+        mR.SetPow(ua0.ReadF());
     }
 }
 
 
-