Diff: main.cpp

Revision:

0:afeca64a6543

Child:

1:dfa0f59e8d2c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 17 17:19:45 2018 +0000
@@ -0,0 +1,66 @@
+
+#include "MMouse18.h"
+#include "Serial_HL.h"
+
+SerialBLK pc(USBTX, USBRX);
+// SerialBLK pc(p28, p27);
+SvProtocol ua0(&pc);
+
+void CommandHandler();
+void CheckButton();
+
+Motor mL(p22,p24,p23); Motor mR(p26, p21, p25);
+Encoder encL(p15,p16); Encoder encR(p30,p29);
+
+DigitalIn sw(p5);
+
+BusOut leds(LED1,LED2,LED3,LED4);
+
+int main(void)
+{
+    pc.format(8,SerialBLK::None,1);
+    pc.baud(115200);
+    mL.SetPow(0); mR.SetPow(0);
+    encL.Init(ENC_A,ENC_RISE); encR.Init(ENC_A,ENC_RISE);
+    
+    ua0.SvMessage("MotTest1"); 
+    
+    Timer stw;
+    stw.start();
+    while(1) {
+        CheckButton();
+        CommandHandler();  
+        if( stw.read_ms()>10 ) { 
+            stw.reset();
+            if( ua0.acqON ) {
+                ua0.WriteSvI16(1, encL.cnt);
+                ua0.WriteSvI16(2, encR.cnt);
+            }
+        }
+    }
+    return 1;
+}
+
+void CommandHandler()
+{
+    uint8_t cmd;
+    if( !pc.IsDataAvail() )
+        return;
+    cmd = ua0.GetCommand();
+    if( cmd==2 ) {
+        mL.SetPow(ua0.ReadF()); mR.SetPow(ua0.ReadF());
+        ua0.SvMessage("Set Pow");
+    }
+    if( cmd==3 ) {
+        encL.cnt=encR.cnt=0;
+        ua0.SvMessage("Reset Cnt");
+    }
+}
+
+void CheckButton()
+{
+    if( sw==1 )
+        leds=0xF;
+    else
+        leds=0;
+}
\ No newline at end of file