Revision 5:d5c2e8f852fd, committed 2018-04-06

Comitter:

59010050

Date:

Fri Apr 06 08:33:48 2018 +0000

Parent:

4:6c8b844d291f

Commit message:

latest

Changed in this revision

--- a/main.cpp	Wed Apr 04 17:14:22 2018 +0000
+++ b/main.cpp	Fri Apr 06 08:33:48 2018 +0000
@@ -6,6 +6,7 @@
 Serial pc(USBTX, USBRX);
 //Serial bt(A7,A2);
 Timer timer1; 
+Timer timerwalk;
 Thread thread1;         
 Thread thread2;
 
@@ -36,6 +37,7 @@
 Servo Servo4(D10);
 
 int value;
+int  walking_time; 
 
 void servo_Right();
 void move();
@@ -49,7 +51,7 @@
 float down_degree = 0.00 ;
 float up_degree = 0.00 ; 
 float stepmin = 1;
-float round = 10;
+float round = 5;
 float waittime = 0.001 ; 
  
 float pos_down_left = 1400.00;
@@ -75,14 +77,12 @@
 int main() {
     pc.baud(1000000); 
     //pc.printf("malin");
-    //getvalue();
     timer1.start(); // start timer counting
     //if (pc.getc() == '1')
-    //{
-      pc.printf("ma"); 
+     //{
       thread2.start(servo);
       //thread1.start(IMU); 
-    //}   
+     //}   
 }
 void cal_step_down(){
     pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); 
@@ -97,14 +97,14 @@
         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(){    
@@ -120,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(){
@@ -187,8 +187,10 @@
             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");
+            pc.printf("Finish \n");
+            walking_time = timerwalk.read_ms(); 
             thread1.terminate();
+            pc.printf("Walking time = %d  \n", walking_time); 
             break;         
         }
     }
@@ -253,6 +255,8 @@
     attitude_setup();
     getvalue();
     pc.printf("start\n"); 
+    thread1.start(IMU); 
+    timerwalk.start(); // start timer counting
     //pc.printf("%f \n",down_degree );
     //pc.printf("%f \n",up_degree );
     cal_step_down();
@@ -267,10 +271,11 @@
     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");
+    pc.printf("case 5 = 90-75 \n");
+    pc.printf("case 6 = 80-45 \n");
+    pc.printf("case 7 = 85-45 \n");
+    pc.printf("case 8 = 95-45 \n");
+    pc.printf("case 9 = 100-45 \n");
         value = pc.getc() ; 
         switch (value) {
          case '1': { 
@@ -294,21 +299,26 @@
             break;
             }
         case '5': {
+            down_degree = 90.00 ;
+            up_degree = 75.00 ; 
+            break;
+            }
+        case '6': {
             down_degree = 80.00 ;
             up_degree = 45.00 ; 
             break;
             }
-        case '6': {
+        case '7': {
             down_degree = 85.00 ;
             up_degree = 45.00 ; 
             break;
             }
-        case '7': {
+        case '8': {
             down_degree = 95.00 ;
             up_degree = 45.00 ; 
             break;
             }
-        case '8': {
+        case '9': {
             down_degree = 100.00 ;
             up_degree = 45.00 ; 
             break;