Sungwoo Kim
LIGHT2
Diff: CAN/function_CAN.cpp
- Revision:
- 48:889798ff9329
- Parent:
- 47:fdcb8bd86fd6
- Child:
- 49:e7bcfc244d40
diff -r fdcb8bd86fd6 -r 889798ff9329 CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp Tue Jan 21 13:39:27 2020 +0000
+++ b/CAN/function_CAN.cpp Wed Jan 29 13:50:24 2020 +0000
@@ -20,6 +20,7 @@
// variables
uint8_t can_index = 0;
+extern DigitalOut LED;
/*******************************************************************************
* CAN functions
@@ -245,11 +246,6 @@
D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
- //spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
- //spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
-
} else if (msg.data[1] == 1) {
P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
@@ -257,10 +253,7 @@
D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
- //spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
- //spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
+
} else if (msg.data[1] == 2) {
P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
@@ -268,9 +261,6 @@
ROM_RESET_DATA();
- //spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
- //spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
- //spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
} else if (msg.data[1] == 3) {
K_SPRING = (int16_t) (msg.data[2] | msg.data[3] << 8);
D_DAMPER = (int16_t) (msg.data[4] | msg.data[5] << 8);
@@ -487,7 +477,7 @@
}
case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
- TORQUE_SENSOR_PULSE_PER_TORQUE = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f);
ROM_RESET_DATA();
//spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) TORQUE_SENSOR_PULSE_PER_TORQUE);
@@ -1058,8 +1048,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
- temp_msg.data[1] = (uint8_t) ENC_PULSE_PER_POSITION;
- temp_msg.data[2] = (uint8_t) (ENC_PULSE_PER_POSITION >> 8);
+ int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION * 10.0f);
+ temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position;
+ temp_msg.data[2] = (uint8_t) (temp_enc_pulse_per_position >> 8);
can.write(temp_msg);
}
@@ -1070,8 +1061,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
- temp_msg.data[1] = (uint8_t) TORQUE_SENSOR_PULSE_PER_TORQUE;
- temp_msg.data[2] = (uint8_t) (TORQUE_SENSOR_PULSE_PER_TORQUE >> 8);
+ int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f);
+ temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
+ temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
can.write(temp_msg);
}
@@ -1121,16 +1113,23 @@
void CAN_TX_DDV_VALVE_DEADZONE(void) {
CANMessage temp_msg;
+ float temp_valve_deadzone_minus = 0.0f;
+ float temp_valve_deadzone_plus = 0.0f;
+ float temp_ddv_center = 0.0f;
+
+ temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+ temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+ temp_ddv_center = 0.0f;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE;
- temp_msg.data[1] = (uint8_t) VALVE_DEADZONE_MINUS;
- temp_msg.data[2] = (uint8_t) ((int) (VALVE_DEADZONE_MINUS) >> 8);
- temp_msg.data[3] = (uint8_t) (VALVE_DEADZONE_PLUS);
- temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
- temp_msg.data[5] = (uint8_t) (DDV_CENTER);
- temp_msg.data[6] = (uint8_t) ((int) (DDV_CENTER) >> 8);
+ temp_msg.data[1] = (uint8_t) temp_valve_deadzone_minus;
+ temp_msg.data[2] = (uint8_t) ((int) (temp_valve_deadzone_minus) >> 8);
+ temp_msg.data[3] = (uint8_t) (temp_valve_deadzone_plus);
+ temp_msg.data[4] = (uint8_t) ((int) (temp_valve_deadzone_plus) >> 8);
+ temp_msg.data[5] = (uint8_t) (temp_ddv_center);
+ temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
can.write(temp_msg);
}
@@ -1249,15 +1248,23 @@
void CAN_TX_DDV_VALVE_MAX_MIN_POS(void) {
CANMessage temp_msg;
+ float temp_valve_max_pos = 0.0f;
+ float temp_valve_min_pos = 0.0f;
+ float temp_ddv_center = 0.0f;
+
+ temp_valve_max_pos = 10000.0f;
+ temp_valve_min_pos = -10000.0f;
+ temp_ddv_center = 0.0f;
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
- temp_msg.data[1] = (uint8_t) VALVE_MAX_POS;
- temp_msg.data[2] = (uint8_t) ((int) (VALVE_MAX_POS) >> 8);
- temp_msg.data[3] = (uint8_t) (VALVE_MIN_POS);
- temp_msg.data[4] = (uint8_t) ((int) (VALVE_MIN_POS) >> 8);
- temp_msg.data[5] = (uint8_t) (DDV_CENTER);
- temp_msg.data[6] = (uint8_t) ((int) (DDV_CENTER) >> 8);
+ temp_msg.data[1] = (uint8_t) temp_valve_max_pos;
+ temp_msg.data[2] = (uint8_t) ((int) (temp_valve_max_pos) >> 8);
+ temp_msg.data[3] = (uint8_t) (temp_valve_min_pos);
+ temp_msg.data[4] = (uint8_t) ((int) (temp_valve_min_pos) >> 8);
+ temp_msg.data[5] = (uint8_t) (temp_ddv_center);
+ temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
can.write(temp_msg);
}
@@ -1281,13 +1288,13 @@
can.write(temp_msg);
}
-void CAN_TX_TORQUE(int16_t t_torque) {
+void CAN_TX_TORQUE(int16_t t_valve_pos) {
CANMessage temp_msg;
temp_msg.id = CID_TX_TORQUE;
temp_msg.len = 2;
- temp_msg.data[0] = (uint8_t) t_torque;
- temp_msg.data[1] = (uint8_t) (t_torque >> 8);
+ temp_msg.data[0] = (uint8_t) t_valve_pos;
+ temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
can.write(temp_msg);
}
@@ -1321,7 +1328,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_VALVE_POSITION;
- temp_msg.len = 8;
+ temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) t_valve_pos;
temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
temp_msg.data[2] = (uint8_t) t_ref_valve_pos;
@@ -1336,3 +1343,4 @@
+