Diff: main.cpp

Revision:

16:cc1ae2a289ee

Parent:

14:21f932d6069d

Child:

18:8806d24809c2

--- a/main.cpp	Wed May 22 05:46:45 2013 +0000
+++ b/main.cpp	Mon May 27 00:42:22 2013 +0000
@@ -30,6 +30,9 @@
 RobotLeg legC(p19, p11, p8, false);
 RobotLeg legD(p25, p24, p23, false);
 
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
 
 
 CmdHandler* legpos(Terminal* terminal, const char*)
@@ -137,6 +140,19 @@
         
         // Get delta-time
         deltaTime = deltaTimer.read();
+        
+        if ((radio.rx_controller>>25)&0x1) // reset
+        {
+            legA.reset(-0.6f);
+            while (legA.getStepping()) legA.update(T);
+            legB.reset(-0.1f);
+            while (legB.getStepping()) legB.update(T);
+            legC.reset(0.4f);
+            while (legC.getStepping()) legC.update(T);
+            legD.reset(0.9f);
+            while (legD.getStepping()) legD.update(T);
+        }
+        
         deltaTimer.reset();
         dataLog.push(deltaTime);