Revision 12:60593247555d, committed 2017-02-22

Comitter:

cpul5338

Date:

Wed Feb 22 09:49:44 2017 +0000

Parent:

11:45a641da473d

Commit message:

robot bike

Changed in this revision

diff -r 45a641da473d -r 60593247555d SPI_9dSensor.h
--- a/SPI_9dSensor.h	Wed Jan 18 15:48:47 2017 +0000
+++ b/SPI_9dSensor.h	Wed Feb 22 09:49:44 2017 +0000
@@ -22,23 +22,25 @@
 #define INDEX_MAGN_Y    8
 #define INDEX_MAGN_Z    9
 
-#define GX_offset   35
-#define GY_offset   -28
-#define GZ_offset   151
-#define AX_offset   -715///-908
-#define AY_offset   -501
-#define AZ_offset   215
+// sensor correction
+#define GX_offset   50.095//35
+#define GY_offset   56.249//same
+#define GZ_offset   1.862//151
+#define AX_offset   -651.6106//-715///-908
+#define AY_offset   -697.9139//-501
+#define AZ_offset   218.8919//215
 #define MX_offset   136
 #define MY_offset   -108
 #define MZ_offset   -102
 
+// Gain of Gyro and Acce
 #define Mag_backgnd 0.36435f ///gauss, Magnetic field in Hsinchu
-#define Gyro_gainx  0.001065f///2000dps/2^15 = 61 miliDegree/s = 0.001065rad/s
-#define Gyro_gainy  0.001065f///0.001212716
-#define Gyro_gainz  0.001065f///0.0012375596
-#define Acce_gainx  -0.002394f///=8*9.81/2^15
-#define Acce_gainy  -0.002394f///=8*9.81/2^15
-#define Acce_gainz  -0.002394f///=8*9.81/2^15
+#define Gyro_gainx  0.002422966362920f///2000dps/2^15 = 61 miliDegree/s = 0.001065rad/s
+#define Gyro_gainy  0.002422966362920f///0.001212716
+#define Gyro_gainz  0.002422966362920f///0.0012375596
+#define Acce_gainx  -0.002404245422390f///=8*9.81/2^15
+#define Acce_gainy  -0.002344293490135f///=8*9.81/2^15
+#define Acce_gainz  -0.002307390759185f///=8*9.81/2^15
 #define Magn_gain   0.000244f ///8gauss/2^15 = 0.000244gauss
 
 extern SPI sensor_LSM9D;

diff -r 45a641da473d -r 60593247555d main.cpp
--- 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)