Diff: main.cpp

Revision:

1:13164a15fbf6

Parent:

0:43d21d5145d3

Child:

2:18835f8732ad

--- a/main.cpp	Tue Apr 10 13:42:04 2018 +0000
+++ b/main.cpp	Thu Apr 19 01:54:55 2018 +0000
@@ -2,6 +2,7 @@
 #include "Servo.h"
 #include "rtos.h"
 #include "attitude.h"
+#include "math.h"
 
 Serial pc(USBTX, USBRX);
 
@@ -19,7 +20,8 @@
 void IMU();
 void Cal_si();
 void Avg();
-void Cal_Cg();
+void Cal_Cg_linear();
+void Cal_Cg_log();
 void move();
 void receive_hormone();
 void cal_step_down(); //calculate step
@@ -35,13 +37,17 @@
 float roll_data[10];
 float Si = 0.00 ;
 float Cg = 0.00;
+float Cg_down = 0.00;
+float Cg_up = 0;
+float standardDeviation = 0;
+float sd = 0; 
  
 float pos_down_start = 1400.00;
 float pos_up_start = 1000.00;
 float down_degree = 90.00;
 float up_degree = 45.00; 
 float stepmin = 1;
-float round = 200;
+float round = 5;
 float waittime = 0.001 ; 
  
 float pos_down_left = 1400.00;
@@ -92,10 +98,17 @@
 }
 
 void receive_hormone(){
-    down_degree = 90.00*(1.00-(0.5*Cg));
-    up_degree = 45.00*(1.00+(0.7*Cg));
-    if(down_degree < 80){
-        down_degree = 80;
+    down_degree = 90.00*(1.00-(0.06*Cg_down)); 
+    /*pc.printf("%f \t",Si);
+    pc.printf("down\t"); 
+    pc.printf("%f \t",down_degree);
+    pc.printf("%f\t",Cg);*/
+    up_degree = 45.00*(1.00+(0.7*Cg_up));
+    /*pc.printf("up\t"); 
+    pc.printf("%f \t",up_degree);
+    pc.printf("%f\n",Cg);*/
+    if(down_degree < 85){
+        down_degree = 85;
         if(up_degree > 75){
         up_degree = 75;
         }
@@ -153,10 +166,19 @@
 }
  
 void move(){
-    Servo1.Enable(1000,2000);
-    Servo2.Enable(1000,2000);
-    Servo3.Enable(1000,2000);
-    Servo4.Enable(1000,2000);
+    Servo1.Enable(1000,20000);
+    Servo2.Enable(1000,20000);
+    Servo3.Enable(1000,20000);
+    Servo4.Enable(1000,20000);
+    
+    /*pc.printf("Si\t"); 
+    pc.printf("%f \t",Si);
+    pc.printf("down\t"); 
+    pc.printf("%f \t",down_degree);
+    pc.printf("%f\t",Cg);
+    pc.printf("up\t"); 
+    pc.printf("%f \t",up_degree);
+    pc.printf("%f\n",Cg);*/
     while(1) {
         cal_step_down();
         cal_step_up();
@@ -210,7 +232,6 @@
             state_count_left = 1;
             pos_down_left = pos_down_start;
             pos_up_left = pos_up_start;
-            //receive_hormone();
         } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){
             thread1.terminate();
             pc.printf("Finish \n");
@@ -279,6 +300,12 @@
 ///////////////////////////Control IMU////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////
 void IMU(){
+    pc.printf("roll\t");
+    pc.printf("Si\t");
+    pc.printf("Cg\t");
+    pc.printf("down\t");
+    pc.printf("up\n"); 
+    
     while(1) {
         if (timer1.read_us()  >=10000)// read time in ms
         {
@@ -288,7 +315,14 @@
             //pc.printf("  %f \t", ay*10 ); 
             //pc.printf("  %f \t", az*10-10); //cm/s*s
 
-            pc.printf("%f\t  %.0f \t  %.0f \n\r",  roll,   pitch, yaw);
+            //pc.printf("%f\t  %.0f \t  %.0f \n\r",  roll,   pitch, yaw);
+            
+            //pc.printf("up\t"); 
+            //pc.printf("%f \t",up_degree);
+            //pc.printf("%f\t",Cg);
+            //pc.printf("down\t"); 
+            //pc.printf("%f \t",down_degree);
+            //pc.printf("%f\n",Cg);
          
             timer1.reset(); // reset timer 
             Cal_si();
@@ -299,17 +333,21 @@
 void Cal_si(){
     if(state_count_left == 4 or state_count_right == 4){
         roll_data[i] = roll;
-        pc.printf("%f\n",roll_data[i]);
+        //pc.printf("%f\n",roll_data[i]);
         Avg();
         //pc.printf("Avg ");
         //pc.printf("%f\n",avg);
-        Si = avg/4;
-        sum = sum - roll_data[i];
-        //pc.printf("Si ");
-        //pc.printf("%f\n",Si);
-        Cal_Cg();
-        //pc.printf("Cg ");
-        //pc.printf("%f\n",Cg);
+        Cal_Cg_linear();
+        //pc.printf("roll\t");
+        pc.printf("%f\t",roll_data[i]);
+        //pc.printf("Si\t");
+        pc.printf("%f\t",Si);
+        //pc.printf("Cg\t");
+        pc.printf("%f\t",Cg);
+        //pc.printf("down\t"); 
+        pc.printf("%f \t",down_degree);
+        //pc.printf("up\t"); 
+        pc.printf("%f \n",up_degree);
         if(i == 9){
             i = 0;
         }else{
@@ -321,18 +359,26 @@
 void Avg(){
     sum = sum + roll;
     avg = sum/10;
+    standardDeviation = (roll - avg)*(roll-avg); 
+    sd = sqrt(standardDeviation/10);
     if(avg < 0){
         avg = avg*(-1);
     }
 }
 
-void Cal_Cg(){
+
+
+void Cal_Cg_linear(){
+    Si = sd/0.6;
+    sum = sum - roll_data[i];
     if(Si > 0){
-        Cg = (0.9*Si)+(0.3*Cg);
+        Cg = (0.8*Si)+(0.5*Cg);
         if(Cg > 1){
             Cg = 1;
         }
     }else{
         Cg = 0.00;
     }
+    Cg_down = 1/0.1*exp(2*(Cg-2.081))-0.15;
+    Cg_up = (0.8*log(Cg+0.4))+0.734;
 }
\ No newline at end of file