![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_0411 by
Diff: main.cpp
- Revision:
- 11:6d5307ceb569
- Parent:
- 10:8b7fce3bba86
--- 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;