Diff: main.cpp

Revision:

1:852156b5cca1

Parent:

0:3af6fae90816

--- a/main.cpp	Sun Apr 01 14:20:53 2018 +0000
+++ b/main.cpp	Mon Apr 02 10:18:44 2018 +0000
@@ -1,65 +1,121 @@
 #include "mbed.h"
 #include "Servo.h"
 #include "rtos.h"
+#include "attitude.h"
 
 Serial pc(USBTX, USBRX);
- 
+//Serial bt(A7,A2);
+Timer timer1; 
+Thread thread1;         
+
+Thread thread2;
+
+void IMU()
+{
+    while(1) {
+    if (timer1.read_us()  >=1000)// read time in ms
+        {
+            attitude_get();
+            pc.printf(" %f \t", ax*10 ); 
+            pc.printf("  %f \t", ay*10 ); 
+            pc.printf("  %f \t", az*10 - 10); //cm/s*s
+
+            pc.printf(" %f \t", gx ); 
+            pc.printf("  %f \t", gy );  
+            pc.printf("  %f  \t", gz );  //deg/s    */
+            pc.printf("%.0f\t  %.0f \t   \n\r",  roll,   pitch ); 
+
+            timer1.reset(); // reset timer 
+            }
+            }
+}
+
 Servo Servo1(D6);
 Servo Servo2(D8);
 Servo Servo3(D9);
 Servo Servo4(D10);
 
 void myservoLeft();
+void move();
 
 int pos_down = 1400;
 int pos_up = 1000;
 int pos_down_end = 1700;
-int pos_up_end = 1700;
+int pos_up_end = ;  
+int final_posdown = 1700 ;             
+int final_posup = 1466.66;                                                                                                                    
+int state_count = 1;
+int round_count = 1;
 
 int main() {
-    pc.baud(9600);
-    wait(5);
+    pc.baud(1000000); 
+    pc.printf("malin");
+    
+    timer1.start(); // start timer counting
+    if (pc.getc() == '1')
+    {
+        thread2.start(move);
+        thread1.start(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");
     while(1){
-        myservoLeft();
-        if(pos_down <= pos_down_end and pos_up == 1000){
+        
+        if(state_count == 1){
             Servo1.SetPosition(pos_down);
-            pos_down = pos_down+2;
+            Servo3.SetPosition(pos_down);
+            pos_down = pos_down+5;
+            wait(0.01);
+            if(pos_down >= pos_down_end+5 and pos_up >= 1000){
+                state_count = 2;
+            }
         }
-        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
+        else if(state_count == 2){
             Servo2.SetPosition(pos_up);
-            pos_up = pos_up+2;
+            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){
+                state_count = 3;
+            }
         }
-        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
+        else if(state_count == 3){
             Servo1.SetPosition(pos_down);
-            pos_down = pos_down-2;
+            Servo3.SetPosition(pos_down);
+            pos_down = pos_down-5;
+            wait(0.01);
+            if(pos_down <= 1395 and pos_up >= pos_up_end+5){
+                state_count = 4;
+            }
         }
-        else if(pos_down == 1398 and pos_up > 1000){
+        else if(state_count == 4){
             Servo2.SetPosition(pos_up);
-            pos_up = pos_up-2;
-        }           
+            Servo4.SetPosition(pos_up);
+            pos_up = pos_up-5;
+            wait(0.01); 
+            if(pos_down <= 1395 and pos_up <= 995){
+                state_count = 0;
+            }       
+        }
+         else if (state_count == 0 and round_count < 10){
+            round_count = round_count+1;
+            state_count = 1;
+            pos_down = 1400;
+            pos_up = 1000;
+        }          
+         else {
+             pc.printf("stop"); 
+             } 
     }
-}
-
-void myservoLeft() 
-{   
-        if(pos_down <= pos_down_end and pos_up == 1000){
-            Servo3.SetPosition(pos_down);
-            pos_down = pos_down+2;
-        }
-        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
-            Servo4.SetPosition(pos_up);
-            pos_up = pos_up+2;
-        }
-        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
-            Servo3.SetPosition(pos_down);
-            pos_down = pos_down-2;
-        }
-        else if(pos_down == 1398 and pos_up > 1000){
-            Servo4.SetPosition(pos_up);
-            pos_up = pos_up-2;
-        }           
-}
+}
\ No newline at end of file