![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
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 @@ +