Diff: main.cpp

Revision:

4:6c8b844d291f

Parent:

3:98ef5105926e

Child:

5:d5c2e8f852fd

--- a/main.cpp	Wed Apr 04 14:47:57 2018 +0000
+++ b/main.cpp	Wed Apr 04 17:14:22 2018 +0000
@@ -46,8 +46,8 @@
  
 float pos_down_start = 1400.00;
 float pos_up_start = 1000.00;
-float down_degree = 80.00000000;
-float up_degree = 15.00000000 ; 
+float down_degree = 0.00 ;
+float up_degree = 0.00 ; 
 float stepmin = 1;
 float round = 10;
 float waittime = 0.001 ; 
@@ -77,14 +77,16 @@
     //pc.printf("malin");
     //getvalue();
     timer1.start(); // start timer counting
-    if (pc.getc() == '1')
-    {
+    //if (pc.getc() == '1')
+    //{
       pc.printf("ma"); 
       thread2.start(servo);
-      thread1.start(IMU); 
-    }   
+      //thread1.start(IMU); 
+    //}   
 }
 void cal_step_down(){
+    pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); 
+    pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)))  ; 
     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;
@@ -95,17 +97,19 @@
         step_down_right = stepmin;
         step_down_left = stepmin;
     }
-    /*pc.printf("pos_down_right");
+    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);    */
+    pc.printf("%f\n",step_down_left);    
 }
  
 void cal_step_up(){    
+    pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); 
+    pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); 
     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;
@@ -116,14 +120,14 @@
         step_up_right = stepmin;
         step_up_left = stepmin;
     }
-    /*pc.printf("pos_up_right");
+    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);  */  
+    pc.printf("%f\n",step_up_left);  
 }
  
 void move(){
@@ -247,9 +251,14 @@
     
 void servo() {
     attitude_setup();
+    getvalue();
     pc.printf("start\n"); 
+    //pc.printf("%f \n",down_degree );
+    //pc.printf("%f \n",up_degree );
     cal_step_down();
     cal_step_up();
+    //pc.printf("%f \n",down_degree );
+    //pc.printf("%f \n",up_degree );
     move();
 }
 
@@ -306,7 +315,7 @@
             }
         break;
 }
-    pc.printf("%f \n",down_degree );
-    pc.printf("%f \n",up_degree );
+    //pc.printf("%f \n",down_degree );
+    //pc.printf("%f \n",up_degree );
     
     }
\ No newline at end of file