Diff: main.cpp

Revision:

2:436ed0069b61

Parent:

1:852156b5cca1

Child:

3:98ef5105926e

diff -r 852156b5cca1 -r 436ed0069b61 main.cpp
--- a/main.cpp	Mon Apr 02 10:18:44 2018 +0000
+++ b/main.cpp	Mon Apr 02 11:46:08 2018 +0000
@@ -35,76 +35,125 @@
 Servo Servo3(D9);
 Servo Servo4(D10);
 
-void myservoLeft();
+void servoleft();
 void move();
 
 int pos_down = 1400;
 int pos_up = 1000;
 int pos_down_end = 1700;
-int pos_up_end = ;  
-int final_posdown = 1700 ;             
-int final_posup = 1466.66;                                                                                                                    
+int pos_up_end = 1500;  
+int edit_up = 1400;
+                                                                                                                   
 int state_count = 1;
+int state_count2 = 1;
 int round_count = 1;
-
+int round_count2 = 1;
 int main() {
     pc.baud(1000000); 
-    pc.printf("malin");
+    //pc.printf("malin");
     
     timer1.start(); // start timer counting
     if (pc.getc() == '1')
     {
         thread2.start(move);
-        thread1.start(IMU); 
+        //thread1.start(IMU); 
     }
     
 }
+void servoleft(){
+        
+        if(state_count2 == 1){
+            Servo1.SetPosition(pos_down);
+            //pos_down = pos_down+5;
+            wait(0.01);
+            pc.printf("left %d\n",pos_down);
+            if(pos_down == pos_down_end+5 and pos_up == 1000){
+                state_count2 = 2;
+            }
+        }
+        else if(state_count2 == 2){
+            Servo2.SetPosition(pos_up);
+            //pos_up = pos_up+5;
+            wait(0.01);
+            pc.printf("stage2");
+            if(pos_down == pos_down_end+5 and pos_up == edit_up+5){
+                state_count2 = 3;
+            }
+        }
+        else if(state_count2 == 3){
+            Servo1.SetPosition(pos_down);
+            //pos_down = pos_down-5;
+            wait(0.01);
+            pc.printf("stage3");
+            if(pos_down == 1395 and pos_up == edit_up+5){
+                state_count2 = 4;
+            }
+        }
+        else if(state_count2 == 4){
+            Servo2.SetPosition(pos_up);
+            //pos_up = pos_up-5;
+            wait(0.01); 
+            pc.printf("stage4");
+            if(pos_down == 1395 and pos_up == 995){
+                state_count2 = 0;
+            }       
+        }
+         else if (state_count2 == 0 and round_count2 < 10){
+            round_count2 = round_count2+1;
+            state_count2 = 1;
+            pos_down = 1400;
+            pos_up = 1000;
+        }          
+         else {
+             pc.printf("1stop"); 
+             //thread1.stop(IMU);
+             } 
+    
+
+    }
 void move() {
     attitude_setup();
     Servo1.Enable(1000,2000);
     Servo2.Enable(1000,2000);
     Servo3.Enable(1000,2000);
     Servo4.Enable(1000,2000);
-    pc.printf("start");
-    pc.printf("start");
-    pc.printf("start");
-    pc.printf("start");
+    //pc.printf("start");
+    //pc.printf("start");
+    //pc.printf("start");
+    //pc.printf("start");
     pc.printf("start");
     while(1){
-        
+    servoleft();        
         if(state_count == 1){
-            Servo1.SetPosition(pos_down);
             Servo3.SetPosition(pos_down);
             pos_down = pos_down+5;
             wait(0.01);
-            if(pos_down >= pos_down_end+5 and pos_up >= 1000){
+            pc.printf("right %d\n",pos_down);
+            if(pos_down == pos_down_end+5 and pos_up == 1000){
                 state_count = 2;
             }
         }
         else if(state_count == 2){
-            Servo2.SetPosition(pos_up);
             Servo4.SetPosition(pos_up);
             pos_up = pos_up+5;
             wait(0.01);
-            if(pos_down >= pos_down_end+5 and pos_up >= pos_up_end+5){
+            if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){
                 state_count = 3;
             }
         }
         else if(state_count == 3){
-            Servo1.SetPosition(pos_down);
             Servo3.SetPosition(pos_down);
             pos_down = pos_down-5;
             wait(0.01);
-            if(pos_down <= 1395 and pos_up >= pos_up_end+5){
+            if(pos_down == 1395 and pos_up == pos_up_end+5){
                 state_count = 4;
             }
         }
         else if(state_count == 4){
-            Servo2.SetPosition(pos_up);
             Servo4.SetPosition(pos_up);
             pos_up = pos_up-5;
             wait(0.01); 
-            if(pos_down <= 1395 and pos_up <= 995){
+            if(pos_down == 1395 and pos_up == 995){
                 state_count = 0;
             }       
         }