Diff: main.cpp

Revision:

7:b8413464d111

Parent:

6:bd469c945e41

Child:

8:79ca11e0129d

--- a/main.cpp	Sun Jul 10 06:23:02 2016 +0000
+++ b/main.cpp	Sun Jul 10 09:41:33 2016 +0000
@@ -72,6 +72,7 @@
 float Vx = 0.0;
 float Vx_old = 0.0;
 
+unsigned char fall_down = 0;
 unsigned char pedal_state = 0;
 unsigned char sys_state = S_S;
 unsigned int count2ztc_h = 0;
@@ -79,12 +80,15 @@
 unsigned int count2straight = 0;
 unsigned int count_isr = 0;
 float T_total = 0.0;
+
+FILE *dataPtr;
 //********************************//
 
 bool resetting = 0;
 
 void timer1_interrupt(void);
 void timer2_interrupt(void);
+void fprintf_data(unsigned char mode);
 void reset_offset(void);
 void ready_to_go(void);
 void attimeout(void);
@@ -104,8 +108,14 @@
     timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
     timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
     
-//    SD_card.format(8, 3);
-//    fprintf_data(0);
+    mkdir("/sd/20160710", 0777);
+    dataPtr = fopen("/sd/20160710/data.dat", "w");
+    if(dataPtr == NULL) {
+        error("Could not open file for write\r\n");
+        interrupt = 1;
+    }
+    fprintf_data(0);
+    fclose(dataPtr);
     pc.printf("System ready.\r\n");
     
     reset_pos();
@@ -123,10 +133,17 @@
                           ready_to_go();
                           pc.printf("go\r\n");
                           pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
+                          dataPtr = fopen("/sd/20160710/data.dat", "a");
+                          if(dataPtr == NULL) {
+                            error("Could not open file for write\r\n");
+                            interrupt = 1;
+                            break;
+                          }
                           s = 1; break;        ///start
                 case 'a': pc.printf("stop\r\n");
                           pedal_state = 0; sys_state = S_S; Vx = 0.0; Vx_old = 0.0; gamma_ref = 0.0; count2ztc_h = 0; count2ztc_m = 0;
-                          s = 0; state_count = 0; break;   ///stop
+                          s = 0; state_count = 0;
+                          fclose(dataPtr); break;   ///stop
 //                case 'm': pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0; if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;} break;
 //                case 'h': pedal_state = 3; sys_state = H_PD; gamma_ref = 0.0; if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;} break;
                 case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break;     ///left
@@ -162,6 +179,9 @@
     
     if(cosroll != 0)
     {
+        if(roll_angle >= 0.1745f || roll_angle <= -0.1745f)fall_down = 1;
+        else fall_down = 0;
+        
         droll_angle = lpf(u1, droll_angle_old, 62.8);
         droll_angle_old = droll_angle;
         dyaw_angle = lpf(u3/cosroll, dyaw_angle_old, 3.1416);
@@ -307,7 +327,7 @@
             Vx_old = Vx;
 //            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
@@ -418,6 +438,48 @@
     }
 }
 
+void fprintf_data(unsigned char mode)
+{
+    if(mode == 0)
+    {
+        fprintf(dataPtr, "(not weighted)\nK_l: %f, %f\nK_m: %f, %f\nK_phi: %f, %f, %f\n", K_l[0], K_l[1], K_m[0], K_m[1], K_mphi[0], K_mphi[1], K_mphi[2]);
+        fprintf(dataPtr, "Gyro offset: %d %d %d\n", Gyro_axis_zero[0], Gyro_axis_zero[1], Gyro_axis_zero[2]);
+        fprintf(dataPtr, "Acce offset: %d %d %d\n", Acce_axis_zero[0], Acce_axis_zero[1], Acce_axis_zero[2]);
+        fprintf(dataPtr, "Magn offset: %d %d %d\n", Magn_axis_zero[0], Magn_axis_zero[1], Magn_axis_zero[2]);
+        fprintf(dataPtr, "sys_state\t");
+        fprintf(dataPtr, "time\t");
+        fprintf(dataPtr, "fall\t");
+        fprintf(dataPtr, "Roll\t");
+//        fprintf(dataPtr, "dRoll\t");
+//        fprintf(dataPtr, "dPhi_hat\t");
+        fprintf(dataPtr, "Phi_hat\t");
+//        fprintf(dataPtr, "dYaw\t");
+        fprintf(dataPtr, "Yaw\t");
+//        fprintf(dataPtr, "Pedal_step\t");
+//        fprintf(dataPtr, "Ctrl_out\t");
+        fprintf(dataPtr, "Gamma_ref\t");
+        fprintf(dataPtr, "Gamma_rad\t");
+        fprintf(dataPtr, "Speed-X\n");
+    }
+    else
+    {
+        fprintf(dataPtr, "%d\t", sys_state);
+        fprintf(dataPtr, "%f\t", T_total);
+        fprintf(dataPtr, "%d\t", fall_down);
+        fprintf(dataPtr, "%f\t", roll_angle);
+//        fprintf(dataPtr, "%f\t", droll_angle);
+//        fprintf(dataPtr, "%f\t", dphi_hat);
+        fprintf(dataPtr, "%f\t", phi_hat);
+//        fprintf(dataPtr, "%f\t", dyaw_angle);
+        fprintf(dataPtr, "%f\t", yaw_angle);
+//        fprintf(dataPtr, "%d\t", pedal_step);
+//        fprintf(dataPtr, "%f\t", u);
+        fprintf(dataPtr, "%f\t", gamma_ref);
+        fprintf(dataPtr, "%f\t", gamma_rad);
+        fprintf(dataPtr, "%f\n", Vx);
+    }
+}
+
 void reset_offset(void)
 {
     pc.printf("Reseting gyro and accel-X offset...\r\n");