Diff: main.cpp

Revision:

5:2290732b2782

Parent:

4:b0967990e390

Child:

6:bd469c945e41

--- a/main.cpp	Wed Jul 06 06:54:15 2016 +0000
+++ b/main.cpp	Wed Jul 06 13:22:03 2016 +0000
@@ -85,6 +85,7 @@
 void timer1_interrupt(void);
 void timer2_interrupt(void);
 void reset_offset(void);
+void ready_to_go(void);
 void attimeout(void);
 
 int main() {
@@ -94,6 +95,7 @@
     pc.baud(115200);
     setup_spi_sensor();
     init_Sensors();
+    
     BT.baud(115200);
     
     reset_offset();
@@ -117,12 +119,12 @@
             switch(BTCharR)
             {
                 case 's': reset_offset();
-                          lookuptable_pedaling(FIRST_POS);
-                          lookuptable_steering(HANDLE_STRAIGHT);
-                          wait(1.5f);
+                          ready_to_go();
+                          pc.printf("go\r\n");
                           pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
                           s = 1; break;        ///start
-                case 'a': pedal_state = 0; sys_state = S_S; Vx = 0.0; Vx_old = 0.0; gamma_ref = 0.0; count2ztc_h = 0; count2ztc_m = 0;
+                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
 //                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;
@@ -236,6 +238,7 @@
         else
         {
             u = 0.0;
+//            calc_PD(K_l, 0.0);
             calc_Gamma(u,15,b_h);
             dphi_hat = 0.0;
             phi_hat = 0.0;
@@ -280,9 +283,13 @@
                 }
                 else
                 {
-                    lookuptable_steering(HANDLE_START);
-//                    lookuptable_steering(HANDLE_STRAIGHT);
-//                    lookuptable_steering( gamma_degree );
+                    if(!resetting)
+                    {
+                        lookuptable_steering(HANDLE_START);
+//                        lookuptable_steering(HANDLE_STRAIGHT);
+//                        lookuptable_steering( gamma_degree );
+                    }
+                    else ;
                 }
             }
             else  lookuptable_steering( gamma_degree + HANDLE_START_BAL);
@@ -363,7 +370,8 @@
         {
             if(state_change == 2)
             {
-//                pc.printf("\nm");
+                pc.printf("m\r\n");
+//                BT.printf("m\r\n");
                 pedal_state = 2;
                 sys_state = M_PD;
                 gamma_ref = 0.0;
@@ -391,7 +399,7 @@
             }
             else
             {
-//                reset_pos();
+                reset_pos();
             }
         }
         else
@@ -424,7 +432,24 @@
 //    pc.printf("%d\r\n", Acce_axis_zero[0]);
 }
 
+void ready_to_go(void)
+{
+    resetting = 1;
+    lookuptable_pedaling(FIRST_POS);
+    lookuptable_steering(HANDLE_STRAIGHT);
+    timeout.attach(&attimeout, 1.0f);
+    while(resetting)
+    {
+        wait_ms(100);
+    }
+    timeout.detach();
+}
+
 void attimeout(void)
 {
     resetting = 0;
-}
\ No newline at end of file
+}
+//void attimeout2(void)
+//{
+//    preparing = 0;
+//}
\ No newline at end of file