![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
Diff: main.cpp
- Revision:
- 10:8b7fce3bba86
- Parent:
- 9:e10bd4b460d7
- Child:
- 11:6d5307ceb569
diff -r e10bd4b460d7 -r 8b7fce3bba86 main.cpp --- a/main.cpp Wed May 23 01:54:11 2018 +0000 +++ b/main.cpp Fri May 25 08:07:15 2018 +0000 @@ -55,6 +55,7 @@ DigitalOut DO_1(PC_6); DigitalOut DO_2(PC_8); DigitalOut DO_3(PC_9); +DigitalOut DO_4(PA_12); DigitalIn DI_0(PB_13); //CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name @@ -103,10 +104,10 @@ uint32_t seq; //========define ACT_state return code============================================ -#define Satndby 0x00 -#define Busy 0x01 -#define Sensor_error 0x10 -#define BMS_error 0x11 +#define Satndby 0 +#define Busy 1 +#define Sensor_error 2 +#define BMS_error 3 //======================================================== #define IMU_FIFO_RATE_DIVIDER 0x09 #define IMU_SAMPLE_RATE_DIVIDER 4 @@ -348,8 +349,9 @@ std_msgs::Int16 DO; //DO_0 MAG_1 //DO_1,MAG_2 -//DO_2,UPS +//DO_2,MAG_3 //DO_3,BMS +//DO_4,MainRelay int State; void DO_ACT(const std_msgs::Int16 &msg){ //0xFF for action procedure @@ -363,36 +365,69 @@ State = ACT_MG_OFF; } - if (msg.data<0x20){ - if (msg.data & 0x01){ - DO_0 = 1; - } - else{ + if (msg.data == 0x00){ + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + DO_3 = 0; + DO_4 = 0; + } + if (msg.data == 0x40){ + //BMS trigger + DO_3 = 1; + wait(3); + DO_3 = 0; + } + if (msg.data == 0x50){ + //Main Relay off + DO_4 = 0; + } + if (msg.data == 0x51){ + //Main Relay on + DO_4 = 1; + } + if (msg.data == 0x31){ + //Lock triggrt DO_0 = 0; - } - - if (msg.data & 0x02){ - DO_1 = 1; - } - else{ DO_1 = 0; - } - - if (msg.data & 0x04){ - DO_2 = 1; - } - else{ + DO_2 = 0; + + DO_0 = 0; + DO_1 = 1; + DO_2 = 0; + + wait_ms(500); + + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + } + + if (msg.data == 0x30){ + //unLock triggrt + DO_0 = 0; + DO_1 = 0; DO_2 = 0; - } - - if (msg.data & 0x08){ - DO_3 = 1; - } - else{ - DO_3 = 0; - } - + + DO_0 = 1; + DO_1 = 0; + DO_2 = 0; + + DO_0 = 1; + DO_1 = 0; + DO_2 = 1; + + wait_ms(500); + + DO_0 = 1; + DO_1 = 0; + DO_2 = 0; + + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; } + } ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT); //====================================================================== @@ -490,7 +525,9 @@ bool Init() { DO_0 = 0; DO_1 = 0; - + DO_2 = 0; + DO_3 = 0; + DO_4 = 0; seq = 0; nh.initNode(); // nh.advertise(imu_pub); @@ -544,27 +581,62 @@ { // int ret; if (state == 1){ //MAG_ON - if (sensorState == 1){ //Battery on-position +// if (sensorState == 0){ //Battery on-position //Lock triggrt + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + + DO_0 = 0; DO_1 = 1; - wait_ms(100); - DO_1 = 0; + DO_2 = 0; + + wait_ms(500); - DO_2 = 1; - wait(3); + DO_0 = 0; + DO_1 = 0; DO_2 = 0; + + //cut off main power + DO_4 = 1; + wait_ms(100); + +// BMS ON + DO_3 = 1; + wait(5); + DO_3 = 0; return 1; - } +// } } else if (state == 2){//MAG_OFF +// BMS OFF + DO_3 = 1; + wait(5); + DO_3 = 0; + //unLock triggrt - DO_2 = 1; - wait_ms(100); + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + + DO_0 = 1; + DO_1 = 0; DO_2 = 0; + DO_0 = 1; + DO_1 = 0; DO_2 = 1; - wait(3); + + wait_ms(500); + + DO_0 = 1; + DO_1 = 0; DO_2 = 0; + + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + return 1; } return 0; @@ -582,9 +654,6 @@ wait_ms(50); can1.frequency(500000); wait_ms(50); - DO_2 = 1; //let UPS ON - - while(1){ seq++; @@ -606,24 +675,35 @@ case ACT_MG_ON: ret = MG_ACT(1); - if (ret){ + if (ret){ + DO_4 = 0; State = Check_BMS_ON; counter = 0; } if (counter>10){ State = IDLE; error_code = Sensor_error; + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + DO_3 = 0; + DO_4 = 0; } break; case Check_BMS_ON: - if (BMS_state == 1){ +// if (BMS_state == 1){ State = IDLE; error_code = 0; - } - if (counter>10){ +// } + if (counter>100){ State = IDLE; error_code = BMS_error; + DO_0 = 0; + DO_1 = 0; + DO_2 = 0; + DO_3 = 0; + DO_4 = 0; } break; @@ -644,18 +724,19 @@ State = IDLE; error_code = 0; } - if (counter>10){ + if (counter>100){ State = IDLE; error_code = BMS_error; } break; - } ACT_state.data = state_code; ACT_state_pub.publish( &ACT_state); + wait_ms(10); Error_state.data = error_code; Error_state_pub.publish( &Error_state); + wait_ms(10); //============DI================== int reading = DI_0; if (reading == lastsensorState) { @@ -671,22 +752,22 @@ lastsensorState = reading; DI_pub.publish( &DI); - + wait_ms(10); //========================================= if (can1.read(rxMsg) && (t.read_ms() > 5000)) { if(rxMsg.id == CAN_DATA){ BMS_state = 1; SOC = rxMsg.data[0]>>1; Tempert = rxMsg.data[1]-50; - RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + rxMsg.data[2]; - Current = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4]; + RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2]; + Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4]; MaxCellV = rxMsg.data[6]; MinCellV = rxMsg.data[7]; BTState.header.stamp = nh.now(); BTState.header.frame_id = 0; BTState.header.seq = seq; BTState.voltage = RackVoltage*0.1; - BTState.current = Current; + BTState.current = Current; BTState.design_capacity = 80; BTState.percentage = SOC; BT_pub.publish( &BTState ); @@ -704,7 +785,8 @@ } Sendmessage(motor_rpm_l,motor_rpm_r); - wait_ms(100); + wait_ms(70); + //wait_ms(100); nh.spinOnce(); } } \ No newline at end of file