Diff: main.cpp

Revision:

12:60593247555d

Parent:

10:3b952ea7fad4

Child:

13:51ef67cd4fd6

--- a/main.cpp	Wed Jan 18 15:48:47 2017 +0000
+++ b/main.cpp	Wed Feb 22 09:49:44 2017 +0000
@@ -48,6 +48,14 @@
 DigitalOut pin_pc2(PC_2);
 DigitalOut pin_pc3(PC_3);
 
+// Modification********************
+DigitalOut tim1(PA_0);
+DigitalOut tim2(PA_1);
+DigitalOut tim3(PA_2);
+DigitalOut tim4(PA_3);
+DigitalOut tim5(PA_4);
+//**********************************
+
 Serial pc(D1, D0);
 Serial BT(PC_6, PA_12);
 
@@ -166,15 +174,19 @@
 
 void timer1_interrupt(void)
 {
+    // Modify
+    tim1 = 1;
+    //********************
 //    pin_pc2 = !pin_pc2;
     V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
     
 //    pc.printf("%d\t%d\t%d\r\n", filted_sensor_data(INDEX_ACCE_X, 1.8), filted_sensor_data(INDEX_ACCE_Y, 1.8), filted_sensor_data(INDEX_ACCE_Z, 1.8));
-    
+//    tim2 = 1;
     roll_fusion(axm,aym,azm,u3,u1,1);
     x1_hat_estimat(axm,aym,azm,u3,u2,Alpha);
     m_x1_hat(mx,my,mz,u3,u2,Alpha);
     m_x2_hat(mx,my,mz,u3,u1,Alpha);
+//    tim2 = 0;
     
     if(cosroll != 0)
     {
@@ -218,7 +230,7 @@
 //            yaw_angle_old = yaw_angle;
 
             count2ztc_m++;
-            if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
+            if(count2ztc_m >= CNT2ZTCm){sys_state = H_PD;}
         }
         else if(sys_state == M_ZTC)
         {
@@ -276,11 +288,13 @@
 //        printf("%d   Roll:%9.6f   dRoll:%9.6f   u:%9.6f   phi_hat:%9.6f   gamma:%9.6f\r\n", sys_state, roll_angle, droll_angle, u, phi_hat, gamma_rad);
 
         ///Show results or Send datas
-        gamma_degree = (short int)(gamma_rad*114.59f);
+//        gamma_degree = (short int)(gamma_rad*114.59f);
+        gamma_degree = 0;
         if(gamma_degree > 40)gamma_degree = 40;
         else if(gamma_degree < -40)gamma_degree = -40;
         else ;
         
+        
         ///Steering
         if(s == 1)
         {
@@ -315,6 +329,7 @@
             else  lookuptable_steering( gamma_degree + HANDLE_START_BAL);
         }
         
+        
         ///print file
         if(sys_state >= 1)
         {
@@ -324,9 +339,9 @@
             ///Integrate Ax to get Vx
 //            Vx = Vx_old - axm_f*sample_time;
 //            Vx_old = Vx;
-//            if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}
+            if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}
 
-            fprintf_data(1);
+//            fprintf_data(1);
         }
     }
     else
@@ -334,10 +349,16 @@
         roll_fusion(0,0,0,0,0,1);
         gamma_degree = 0;
     }
+    // Modify
+    tim1 = 0;
+    //********************
 }
 
 void timer2_interrupt(void)
 {
+    // Modify
+    
+    //********************
 //    pin_pc3 = !pin_pc3;
     if(s == 1)           // bluetooth start 
     {
@@ -435,6 +456,10 @@
             }
         }
     }
+    // Modify
+//    tim2 = 0;
+//    tim1 = 0;
+    //********************
 }
 
 void fprintf_data(unsigned char mode)