Weber Yang
start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of
AGV_0411
by 
Weber Yang
Revision 10:8b7fce3bba86, committed 2018-05-25
- Comitter:
- WeberYang
- Date:
- Fri May 25 08:07:15 2018 +0000
- Parent:
- 9:e10bd4b460d7
- Child:
- 11:6d5307ceb569
- Commit message:
- DEMO_version01
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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