Diff: main.cpp

Revision:

0:11047e265142

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 01 18:18:46 2015 +0000
@@ -0,0 +1,75 @@
+
+#include "mbed.h"
+#include "Serial_HL.h"
+
+// Testprogramm um zu sehen ob die RGB-Led richtig verlötet
+// ist und ob sie sich auch dimmen lässt
+
+SerialBLK pc(USBTX, USBRX);
+SvProtocol ua0(&pc);
+
+PwmOut servo1(p21),servo2(p22),servo3(p23);
+SerialBLK blue(p9, p10);
+int16_t stickVal1,stickVal2,stickVal3;
+
+void CommandHandler();
+
+int main()
+{
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+    servo3.period_ms(20);
+    
+    pc.format(8,SerialBLK::None,1);pc.baud(115200);blue.baud(115200);
+    ua0.SvMessage("ServoTest");
+    
+    Timer stw; stw.start();
+    while(1)
+    {
+        CommandHandler();
+        if((stw.read_ms()>100))
+        {
+            stw.reset();
+            if(ua0.acqON)
+            {
+             ua0.SvMessage(stickVal1);
+             ua0.WriteSvI16(1, stickVal1);
+             ua0.WriteSvI16(2, stickVal2);
+             ua0.WriteSvI16(3, stickVal3);   
+                }
+            }
+        }
+    
+    return 1;
+}
+
+
+void CommandHandler()
+{
+    uint8_t cmd;
+   
+
+    // Fragen ob überhaupt etwas im RX-Reg steht
+    if( !blue.IsDataAvail() )
+        return;
+
+    // wenn etwas im RX-Reg steht
+    // Kommando lesen
+    cmd = ua0.GetCommand();
+
+    if( cmd==2 ) {
+        // cmd2 hat 2 int16 Parameter
+        stickVal1 = ua0.ReadI16();
+        servo1.pulsewidth_us(1000+stickVal1);
+    }
+    if( cmd==3 ) {
+        // cmd2 hat 2 int16 Parameter
+        stickVal2 = ua0.ReadI16();
+        servo2.pulsewidth_us(1000+stickVal2);
+    }
+    if( cmd==4 ) {
+        // cmd2 hat 2 int16 Parameter
+        stickVal3 = ua0.ReadI16();
+        servo3.pulsewidth_us(1000+stickVal3);
+    }
+}
\ No newline at end of file