Diff: main.cpp

Revision:

3:98ef5105926e

Parent:

2:436ed0069b61

Child:

4:6c8b844d291f

--- a/main.cpp	Mon Apr 02 11:46:08 2018 +0000
+++ b/main.cpp	Wed Apr 04 14:47:57 2018 +0000
@@ -7,7 +7,6 @@
 //Serial bt(A7,A2);
 Timer timer1; 
 Thread thread1;         
-
 Thread thread2;
 
 void IMU()
@@ -16,14 +15,15 @@
     if (timer1.read_us()  >=1000)// read time in ms
         {
             attitude_get();
+            //pc.printf("IMU \n");
             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", 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 ); 
+            pc.printf("%.0f\t  %.0f \t  %.0f \n\r",  roll,   pitch, yaw); 
 
             timer1.reset(); // reset timer 
             }
@@ -35,136 +35,278 @@
 Servo Servo3(D9);
 Servo Servo4(D10);
 
-void servoleft();
-void move();
+int value;
 
-int pos_down = 1400;
-int pos_up = 1000;
-int pos_down_end = 1700;
-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;
+void servo_Right();
+void move();
+void cal_step_down();
+void cal_step_up();
+void servo();
+void getvalue();
+ 
+float pos_down_start = 1400.00;
+float pos_up_start = 1000.00;
+float down_degree = 80.00000000;
+float up_degree = 15.00000000 ; 
+float stepmin = 1;
+float round = 10;
+float waittime = 0.001 ; 
+ 
+float pos_down_left = 1400.00;
+float pos_up_left = 1000.00;
+float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down  //90 ใน80 นอก(45)+7 ใน85 นอก+5
+float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5
+float state_count_left = 1;
+float round_count_left = 1;
+float step_down_left;
+float step_up_left;
+ 
+float pos_down_right = 1400.00;
+float pos_up_right = 1000.00;
+float pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)))  ; //right down   //99
+float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5
+float state_count_right = 1;
+float round_count_right = 1;
+float step_up_right;
+float step_down_right;
+ 
+
+
 int main() {
     pc.baud(1000000); 
     //pc.printf("malin");
-    
+    //getvalue();
     timer1.start(); // start timer counting
     if (pc.getc() == '1')
     {
-        thread2.start(move);
-        //thread1.start(IMU); 
-    }
-    
+      pc.printf("ma"); 
+      thread2.start(servo);
+      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 cal_step_down(){
+    if (pos_down_end_right > pos_down_end_left){
+        step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
+        step_down_left = stepmin;
+    } else if (pos_down_end_right < pos_down_end_left){
+        step_down_right = stepmin;
+        step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
+    } else{
+        step_down_right = stepmin;
+        step_down_left = stepmin;
     }
-void move() {
-    attitude_setup();
+    /*pc.printf("pos_down_right");
+    pc.printf("%f\n",pos_down_end_right);
+    pc.printf("pos_down_left");
+    pc.printf("%f\n",pos_down_end_left);
+    pc.printf("step_down_right");
+    pc.printf("%f\n",step_down_right);
+    pc.printf("step_down_left");
+    pc.printf("%f\n",step_down_left);    */
+}
+ 
+void cal_step_up(){    
+    if (pos_up_end_right > pos_up_end_left){
+        step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
+        step_up_left = stepmin;
+    } else if (pos_up_end_right < pos_up_end_left){
+        step_up_right = stepmin;
+        step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
+    } else{
+        step_up_right = stepmin;
+        step_up_left = stepmin;
+    }
+    /*pc.printf("pos_up_right");
+    pc.printf("%f\n",pos_up_end_right);
+    pc.printf("pos_up_left");
+    pc.printf("%f\n",pos_up_end_left);
+    pc.printf("step_up_right");
+    pc.printf("%f\n",step_up_right);;
+    pc.printf("step_up_left");
+    pc.printf("%f\n",step_up_left);  */  
+}
+ 
+void move(){
     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){
-    servoleft();        
-        if(state_count == 1){
-            Servo3.SetPosition(pos_down);
-            pos_down = pos_down+5;
-            wait(0.01);
-            pc.printf("right %d\n",pos_down);
-            if(pos_down == pos_down_end+5 and pos_up == 1000){
-                state_count = 2;
+    while(1) {
+        servo_Right();
+        if(state_count_left == 1) {
+            Servo1.SetPosition(pos_down_left);
+            wait(waittime);
+            pos_down_left = pos_down_left + step_down_left;
+            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
+                state_count_left = 2;
+            }
+            /*pc.printf("LAD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LAP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if(state_count_left == 2) {
+            Servo2.SetPosition(pos_up_left);
+            wait(waittime);
+            pos_up_left = pos_up_left + step_up_left;
+            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
+                state_count_left = 3;
+            }
+            /*pc.printf("LBD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LBP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if(state_count_left == 3) {
+            Servo1.SetPosition(pos_down_left);
+            wait(waittime);
+            pos_down_left = pos_down_left - step_down_left;
+            if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
+                state_count_left = 4;
+            }
+            /*pc.printf("LCD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LCP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if(state_count_left == 4) {
+            Servo2.SetPosition(pos_up_left);
+            wait(waittime);
+            pos_up_left = pos_up_left - step_up_left;
+            if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
+                state_count_left = 0;
             }
+            /*pc.printf("LDD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LDP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if (state_count_left == 0 and round_count_left < round) {
+            round_count_left = round_count_left+1;
+            state_count_left = 1;
+            pos_down_left = pos_down_start;
+            pos_up_left = pos_up_start;
+        } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){
+            pc.printf("Finish");
+            thread1.terminate();
+            break;         
         }
-        else if(state_count == 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;
-            }
+    }
+}
+ 
+void servo_Right()
+{
+    if(state_count_right == 1) {
+        Servo3.SetPosition(pos_down_right);
+        wait(waittime);
+        pos_down_right = pos_down_right + step_down_right;
+        if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
+            state_count_right = 2;
+        }
+        /*pc.printf("RAD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RAP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if(state_count_right == 2) {
+        Servo4.SetPosition(pos_up_right);
+        wait(waittime);
+        pos_up_right = pos_up_right + step_up_right;
+        if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
+            state_count_right = 3;
+        }
+        /*pc.printf("RBD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RBP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if(state_count_right == 3) {
+        Servo3.SetPosition(pos_down_right);
+        wait(waittime);
+        pos_down_right = pos_down_right - step_down_right;
+        if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
+            state_count_right = 4;
         }
-        else if(state_count == 3){
-            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;
-            }
+        /*pc.printf("RCD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RCP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if(state_count_right == 4) {
+        Servo4.SetPosition(pos_up_right);
+        wait(waittime);
+        pos_up_right = pos_up_right - step_up_right;
+        if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
+            state_count_right = 0;
         }
-        else if(state_count == 4){
-            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"); 
-             } 
+        /*pc.printf("RDD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RDP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if (state_count_right == 0 and round_count_right < round) {
+        round_count_right = round_count_right+1;
+        state_count_right = 1;
+        pos_down_right = pos_down_start;
+        pos_up_right = pos_up_start;
     }
-}
\ No newline at end of file
+}
+            
+    
+void servo() {
+    attitude_setup();
+    pc.printf("start\n"); 
+    cal_step_down();
+    cal_step_up();
+    move();
+}
+
+void getvalue() {
+    pc.printf("case 1 = 90-15 \n");
+    pc.printf("case 2 = 90-30 \n");
+    pc.printf("case 3 = 90-45 \n");
+    pc.printf("case 4 = 90-60 \n");
+    pc.printf("case 5 = 80-45 \n");
+    pc.printf("case 6 = 85-45 \n");
+    pc.printf("case 7 = 95-45 \n");
+    pc.printf("case 8 = 100-45 \n");
+        value = pc.getc() ; 
+        switch (value) {
+         case '1': { 
+            down_degree = 90.00 ;
+            up_degree = 15.00 ; 
+            break;
+            }
+        case '2': {
+            down_degree = 90.00 ;
+            up_degree = 30.00 ; 
+            break;
+            }
+        case '3': {
+            down_degree = 90.00 ;
+            up_degree = 45.00 ; 
+            break;
+            }
+        case '4': {
+            down_degree = 90.00 ;
+            up_degree = 60.00 ; 
+            break;
+            }
+        case '5': {
+            down_degree = 80.00 ;
+            up_degree = 45.00 ; 
+            break;
+            }
+        case '6': {
+            down_degree = 85.00 ;
+            up_degree = 45.00 ; 
+            break;
+            }
+        case '7': {
+            down_degree = 95.00 ;
+            up_degree = 45.00 ; 
+            break;
+            }
+        case '8': {
+            down_degree = 100.00 ;
+            up_degree = 45.00 ; 
+            break;
+            }
+        break;
+}
+    pc.printf("%f \n",down_degree );
+    pc.printf("%f \n",up_degree );
+    
+    }
\ No newline at end of file