Diff: main.cpp

Revision:

9:159bad8bae03

Parent:

8:79ca11e0129d

Child:

10:3b952ea7fad4

--- a/main.cpp	Mon Jul 18 03:31:39 2016 +0000
+++ b/main.cpp	Mon Jul 18 09:36:16 2016 +0000
@@ -108,14 +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)
     
-    mkdir("/sd/20160718", 0777);
-    dataPtr = fopen("/sd/20160718/data.dat", "w");
+//    mkdir("/sd/20160718", 0777);
+//    dataPtr = fopen("/sd/20160718/data.dat", "w");
+    dataPtr = fopen("/sd/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();
@@ -133,23 +133,16 @@
                           ready_to_go();
                           pc.printf("go\r\n");
                           pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
-                          dataPtr = fopen("/sd/20160718/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;
-                          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;
+                          break;   ///stop
                 case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break;     ///left
                 case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break;    ///right
                 case 'f': gamma_ref = 0.0; roll_ref = 0.0; break;   ///forward
-                case 'c': gamma_ref = 0.0; reset_offset(); break;   ///clear
+                case 'c': gamma_ref = 0.0; reset_offset(); break;   ///calibrate
+                case 'h': fclose(dataPtr); interrupt = 1; break;     ///halt
                 default: break;
             }
             BTCharR = 0;
@@ -205,8 +198,8 @@
             
             dphi_hat = 0.0;
             phi_hat = 0.0;
-            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
-            yaw_angle_old = yaw_angle;
+//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+//            yaw_angle_old = yaw_angle;
         }
         else if(sys_state == M_PD)
         {
@@ -215,8 +208,8 @@
 
             dphi_hat = 0.0;
             phi_hat = 0.0;
-            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
-            yaw_angle_old = yaw_angle;
+//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+//            yaw_angle_old = yaw_angle;
 
             count2ztc_m++;
             if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
@@ -229,8 +222,8 @@
             calc_Phi(K_mphi);
             calc_Gamma(u,15,b_h);
 
-            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
-            yaw_angle_old = yaw_angle;
+//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+//            yaw_angle_old = yaw_angle;
         }
         else if(sys_state == H_PD)
         {
@@ -239,8 +232,8 @@
 
             dphi_hat = 0.0;
             phi_hat = 0.0;
-            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
-            yaw_angle_old = yaw_angle;
+//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+//            yaw_angle_old = yaw_angle;
 
 //            count2ztc_h++;
             if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;}
@@ -253,8 +246,8 @@
             calc_Phi(K_hphi);
             calc_Gamma(u,15,b_h);
 
-            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
-            yaw_angle_old = yaw_angle;
+//            yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+//            yaw_angle_old = yaw_angle;
         }
         else
         {
@@ -323,8 +316,8 @@
             T_total = (float)sample_time*count_isr;
 
             ///Integrate Ax to get Vx
-            Vx = Vx_old - axm_f*sample_time;
-            Vx_old = 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;}
 
             fprintf_data(1);
@@ -453,13 +446,13 @@
 //        fprintf(dataPtr, "dRoll\t");
 //        fprintf(dataPtr, "dPhi_hat\t");
         fprintf(dataPtr, "Phi_hat\t");
-//        fprintf(dataPtr, "dYaw\t");
-        fprintf(dataPtr, "Yaw\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");
+        fprintf(dataPtr, "Gamma_rad\n");
+//        fprintf(dataPtr, "Speed-X\n");
     }
     else
     {
@@ -470,13 +463,13 @@
 //        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, "%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);
+        fprintf(dataPtr, "%f\n", gamma_rad);
+//        fprintf(dataPtr, "%f\n", Vx);
     }
 }