Diff: main.cpp

Revision:

11:6d5307ceb569

Parent:

10:8b7fce3bba86

Child:

12:ed085acbbde5

diff -r 8b7fce3bba86 -r 6d5307ceb569 main.cpp
--- a/main.cpp	Fri May 25 08:07:15 2018 +0000
+++ b/main.cpp	Tue Oct 02 00:57:35 2018 +0000
@@ -45,6 +45,7 @@
 #define ACT_MG_OFF      2
 #define Check_BMS_ON    3
 #define Check_BMS_OFF   4
+#define WAIT_BAT        5
 //Serial pc(USBTX,USBRX);
 Timer t;
 Serial              RS232(PA_9, PA_10);
@@ -114,7 +115,7 @@
 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
  
-#define PC_BAUDRATE 115200//38400     
+#define PC_BAUDRATE 38400     
  
 #define DEG_TO_RAD(x) ( x * 0.01745329 )
 #define RAD_TO_DEG(x) ( x * 57.29578 )
@@ -357,7 +358,7 @@
     //0xFF for action procedure
     if (msg.data == 0x21){
         error_code = 99;
-        State = ACT_MG_ON;
+        State = WAIT_BAT;//ACT_MG_ON;
     }
     
     if (msg.data == 0x20){
@@ -527,7 +528,7 @@
     DO_1 = 0;
     DO_2 = 0;
     DO_3 = 0;
-    DO_4 = 0;
+    DO_4 = 1;//for manual turn on the LiBattery, and turn on the main relay to open Load
     seq = 0;
     nh.initNode();
 //    nh.advertise(imu_pub);
@@ -539,6 +540,8 @@
     nh.advertise(Error_state_pub);
     nh.subscribe(cmd_vel_sub);
     nh.subscribe(ACT_sub);
+    
+    //==========================
     /*
     mpu.initialize();
     if (mpu.testConnection()) {
@@ -597,22 +600,24 @@
             DO_1 = 0;
             DO_2 = 0;
             
-            //cut off main power
-            DO_4 = 1;
-            wait_ms(100);
-            
-//            BMS ON
+            // if Battery is off, then do turn on
+//            if (BMS_state == 0){
+            //BMS ON
             DO_3 = 1;
-            wait(5);
+            wait(4);
             DO_3 = 0;
+//            }
             return 1;
 //        }
     }
     else if (state == 2){//MAG_OFF
-//            BMS OFF
+            // if Battery is on, then do trun off
+//            if (BMS_state == 1){
+            //BMS ON
             DO_3 = 1;
-            wait(5);
+            wait(4);
             DO_3 = 0;
+//            }
 
             //unLock triggrt
             DO_0 = 0;
@@ -655,6 +660,25 @@
     can1.frequency(500000);
     wait_ms(50);
     
+    //Lock triggrt
+    wait_ms(500);
+
+    DO_0 = 0;
+    DO_1 = 0;
+    DO_2 = 0;
+
+    DO_0 = 0;
+    DO_1 = 1;
+    DO_2 = 0;
+                        
+    wait_ms(500);
+
+    DO_0 = 0;
+    DO_1 = 0;
+    DO_2 = 0;
+    
+    wait_ms(500);
+    
     while(1){   
         seq++;
         motor_seq = seq;
@@ -673,10 +697,21 @@
                 counter = 0;
             break;
             
+            case WAIT_BAT:
+            if (sensorState == 0){
+                State = ACT_MG_ON;
+                counter = 0;
+            }
+            if (counter>1000){
+                State = IDLE;
+                error_code = Sensor_error;
+            }
+            break;
+            
             case ACT_MG_ON:
                 ret = MG_ACT(1);
                 if (ret){
-                     DO_4 = 0; 
+//                     DO_4 = 0; 
                      State = Check_BMS_ON;
                      counter = 0;
                 }
@@ -692,10 +727,12 @@
             break;
             
             case Check_BMS_ON:
-//                if (BMS_state == 1){
+                if (BMS_state == 1){
+                    // turn on parrel wire to share the UPS current
+                    DO_4 = 1;
                     State = IDLE;
                     error_code = 0;
-//                }
+                }
                 if (counter>100){
                     State = IDLE;
                     error_code = BMS_error;
@@ -708,6 +745,9 @@
             break;
             
             case ACT_MG_OFF:
+                // turn off parrel wire to avoid voltage feedback to UPS
+                DO_4 = 0;
+                
                 ret = MG_ACT(2);
                 if (ret){ 
                      State = Check_BMS_OFF;