Diff: main.cpp

Revision:

8:f17874479d80

Parent:

7:3935c7dcc9c5

--- a/main.cpp	Thu Jan 28 15:23:43 2016 +0000
+++ b/main.cpp	Wed Feb 03 17:48:58 2016 +0000
@@ -536,7 +536,7 @@
             if(a == true && b == true && c == true && d == true) {
                 float LHipAngle,LKneeAngle;
                 float RHipAngle,RKneeAngle;
-                                
+
                 bcom.getMotorPos(LEFT_SIDE,&LHipAngle,&LKneeAngle);
                 wait_ms(90);
                 bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle);
@@ -742,6 +742,11 @@
 
 int main()
 {
+    Timer counterUP;
+    Timer counterLOW;
+
+    char state[2];
+    float temp[2];
     pc.baud(115200);
     pc.printf("Start\n");
 
@@ -751,7 +756,49 @@
         while(!button);
         SwMode();
     }
+    sync_communicate.stop();
 
+    counterUP.start();
+    counterLOW.start();
+
+    state[0] =0;
+    state[1] =0;
+
+    temp[0] =20;
+    temp[1] =5;
+
+    while(1) {
+
+        if(counterUP.read_ms() > 1400) {
+            if(state[0] ==0) {
+                temp[0] = 20;
+                state[0]=1;
+            } else {
+                temp[0] = 50;
+                state[0]=0;
+            }
+            counterUP.reset();
+
+        }
+
+        if(counterLOW.read_ms() > 700) {
+            if(state[1] ==0) {
+                temp[1] = 5;
+                state[1]=1;
+            } else {
+                temp[1] = 90;
+                state[1]=0;
+            }
+
+            counterLOW.reset();
+
+        }
+
+
+        bcom.setMotorPos(LEFT_SIDE,temp[0],temp[1]);
+        bcom.setMotorPos(RIGHT_SIDE,temp[0],temp[1]);
+
+    }
 }