Diff: main.cpp

Revision:

1:353c3b1ad238

Parent:

0:aadc51983820

--- a/main.cpp	Tue Oct 06 17:18:01 2015 +0000
+++ b/main.cpp	Wed Oct 07 18:14:48 2015 +0000
@@ -8,19 +8,44 @@
 SerialBLK pc(p13,p14);
 SvProtocol ua0(&pc);
 DigitalOut led1(LED1);
+Ticker t1;
  
 PwmOut servo1(p21),servo2(p22),servo3(p23);
 int16_t stickVal1,stickVal2,stickVal3;
+int speed=1000;
  
 void CommandHandler();
+
+void morespeed() {
+    if (stickVal1 == 1)
+    {   if(speed < 2000)
+            {
+            speed = speed + 50;
+            servo1.pulsewidth_us(speed);
+            servo2.pulsewidth_us(speed);
+            ua0.SvMessage("+100");
+            }
+        }
+    if (stickVal1 == 2)
+    {
+        if(speed > 1000)
+            {
+            speed = speed - 50;
+            servo1.pulsewidth_us(speed);
+            servo2.pulsewidth_us(speed);
+            ua0.SvMessage("-100");
+            }
+        }
+}
  
 int main()
-{
+{   
     servo1.period_ms(20);
     servo2.period_ms(20);
     servo3.period_ms(20);
+    t1.attach(&morespeed, 3.0);
     
-    pc.baud(9600);
+     pc.format(8,SerialBLK::None,1);pc.baud(9600);
     ua0.SvMessage("ServoTest");
     
     Timer stw; stw.start();
@@ -58,10 +83,12 @@
  
     if( cmd==2 ) {
         // cmd2 hat 2 int16 Parameter
-
-    led1 = 1;
+        
+        led1 = 1;
         stickVal1 = ua0.ReadI16();
-        servo1.pulsewidth_us(1000+stickVal1);
+        //servo1.pulsewidth_us(1000+stickVal1);
+        //servo2.pulsewidth_us(1000+stickVal1);
+        //ua0.SvMessage("Gerade");
     }
     if( cmd==3 ) {
         // cmd2 hat 2 int16 Parameter
@@ -71,7 +98,20 @@
     if( cmd==4 ) {
         // cmd2 hat 2 int16 Parameter
         stickVal3 = ua0.ReadI16();
-        servo3.pulsewidth_us(1000+stickVal3);
+        //servo3.pulsewidth_us(1000+stickVal3);
+        if(stickVal3 == 1)
+            {servo3.pulsewidth_us(1000);
+            ua0.SvMessage("Links");
+            }
+        if(stickVal3 == 2)
+            {servo3.pulsewidth_us(1500);
+            ua0.SvMessage("Gerade");
+            }
+        if(stickVal3 == 3)
+            {servo3.pulsewidth_us(1650);
+            ua0.SvMessage("Rechts");
+            }
+        
     }
 }