Yeseong Jeong
/
HydraulicControlBoard_Start
20210203
Diff: CAN/function_CAN.cpp
- Revision:
- 2:a1c0a37df760
- Child:
- 3:aa28672362bb
diff -r e04e563be5ce -r a1c0a37df760 CAN/function_CAN.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CAN/function_CAN.cpp Tue Aug 20 07:38:55 2019 +0000 @@ -0,0 +1,347 @@ +#include "mbed.h" +#include "function_CAN.h" + +// CAN ID Setting Variables +int CID_RX_CMD = 100; +int CID_RX_REF_POSITION = 200; +int CID_RX_REF_TORQUE = 300; +int CID_RX_REF_PRES_DIFF = 400; +int CID_RX_REF_PWM = 500; +int CID_RX_REF_VALVE_POSITION = 600; + +int CID_TX_INFO = 1100; +int CID_TX_POSITION = 1200; +int CID_TX_TORQUE = 1300; +int CID_TX_PRES = 1400; +int CID_TX_PWM = 1500; +int CID_TX_VALVE_POSITION = 1600; + +extern CAN can; +extern CANMessage msg; + +void ReadCMD(char CMD) +{ + switch(CMD){ + case CRX_ASK_INFO: + CAN_TX_INFO(); + break; + case CRX_ASK_BNO: + CAN_TX_BNO(); + break; + default: + break; + } +} + +void CAN_RX_HANDLER() +{ + can.read(msg); + unsigned int address = msg.id; + + if(address==100){ + unsigned int CMD = msg.data[0]; + ReadCMD(CMD); + } +} + +/****************************************************************************** + Information Transmission Functions +*******************************************************************************/ + +inline void CAN_TX_INFO(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 7; + temp_msg.data[0] = (unsigned char) CTX_SEND_INFO; + temp_msg.data[1] = (unsigned char) BNO; + temp_msg.data[2] = (unsigned char) CAN_FREQ; + temp_msg.data[3] = (unsigned char) (CAN_FREQ >> 8); + temp_msg.data[4] = (unsigned char) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]); + temp_msg.data[5] = (unsigned char) CONTROL_MODE; + temp_msg.data[6] = (unsigned char) OPERATING_MODE; + + can.write(temp_msg); +} + +inline void CAN_TX_BNO(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 2; + temp_msg.data[0] = (unsigned char) CTX_SEND_BNO; + temp_msg.data[1] = (unsigned char) BNO; + + can.write(temp_msg); +} + +inline void CAN_TX_OPERATING_MODE(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 2; + temp_msg.data[0] = (unsigned char) CTX_SEND_OPERATING_MODE; + temp_msg.data[1] = (unsigned char) OPERATING_MODE; + + can.write(temp_msg); +} + + +inline void CAN_TX_CAN_FREQ(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ; + temp_msg.data[1] = (uint8_t) CAN_FREQ; + temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_CONTROL_MODE(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 2; + temp_msg.data[0] = (unsigned char) CTX_SEND_CONTROL_MODE; + temp_msg.data[1] = (unsigned char) CONTROL_MODE; + + can.write(temp_msg); +} + +inline void CAN_TX_JOINT_ENC_DIR(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_JOINT_ENC_DIR; + temp_msg.data[1] = (unsigned char) DIR_JOINT_ENC; + temp_msg.data[2] = (unsigned char) (DIR_JOINT_ENC >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_VALVE_DIR(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DIR; + temp_msg.data[1] = (unsigned char) DIR_VALVE; + temp_msg.data[2] = (unsigned char) (DIR_VALVE >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_VALVE_ENC_DIR(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_ENC_DIR; + temp_msg.data[1] = (unsigned char) DIR_VALVE_ENC; + temp_msg.data[2] = (unsigned char) (DIR_VALVE_ENC >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_VOLTAGE_SUPPLY(void) { + long send_voltage_supply = (long) (SUPPLY_VOLTAGE * 10); + + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_SUPPLY; + temp_msg.data[1] = (unsigned char) (send_voltage_supply); + temp_msg.data[2] = (unsigned char) (send_voltage_supply >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_VOLTAGE_VALVE(void) { + long send_voltage_valve = (long) (VALVE_VOLTAGE_LIMIT * 10); + + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_VALVE; + temp_msg.data[1] = (unsigned char) send_voltage_valve; + temp_msg.data[2] = (unsigned char) (send_voltage_valve >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_PID_GAIN(int t_type) { + // t_type = 0 : valve position control gain + // t_type = 1 : joint position control gain + // t_type = 2 : joint torque control gain + + long sendPgain, sendIgain, sendDgain; + if (t_type == 0) { + sendPgain = (long) (P_GAIN_VALVE_POSITION); + sendIgain = (long) (I_GAIN_VALVE_POSITION); + sendDgain = (long) (D_GAIN_VALVE_POSITION); + } else if (t_type == 1) { + sendPgain = (long) (P_GAIN_JOINT_POSITION); + sendIgain = (long) (I_GAIN_JOINT_POSITION); + sendDgain = (long) (D_GAIN_JOINT_POSITION); + } else if (t_type == 2) { + sendPgain = (long) (P_GAIN_JOINT_TORQUE); + sendIgain = (long) (I_GAIN_JOINT_TORQUE); + sendDgain = (long) (D_GAIN_JOINT_TORQUE); + } + + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 8; + temp_msg.data[0] = (unsigned char) CTX_SEND_PID_GAIN; + temp_msg.data[1] = (unsigned char) t_type; + temp_msg.data[2] = (unsigned char) sendPgain; + temp_msg.data[3] = (unsigned char) (sendPgain >> 8); + temp_msg.data[4] = (unsigned char) sendIgain; + temp_msg.data[5] = (unsigned char) (sendIgain >> 8); + temp_msg.data[6] = (unsigned char) sendDgain; + temp_msg.data[7] = (unsigned char) (sendDgain >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_VALVE_DEADZONE(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 7; + temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DEADZONE; + temp_msg.data[1] = (unsigned char) (int) (VALVE_CENTER); + temp_msg.data[2] = (unsigned char) ((int) (VALVE_CENTER) >> 8); + temp_msg.data[3] = (unsigned char) (int) (VALVE_DEADZONE_PLUS); + temp_msg.data[4] = (unsigned char) ((int) (VALVE_DEADZONE_PLUS) >> 8); + temp_msg.data[5] = (unsigned char) (int) (VALVE_DEADZONE_MINUS); + temp_msg.data[6] = (unsigned char) ((int) (VALVE_DEADZONE_MINUS) >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_ENC_PULSE_PER_POSITION(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_PULSE_PER_POSITION; + temp_msg.data[1] = (unsigned char) ENC_PULSE_PER_POSITION; + temp_msg.data[2] = (unsigned char) (ENC_PULSE_PER_POSITION >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE; + temp_msg.data[1] = (unsigned char) TORQUE_SENSOR_PULSE_PER_TORQUE; + temp_msg.data[2] = (unsigned char) (TORQUE_SENSOR_PULSE_PER_TORQUE >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 5; + temp_msg.data[0] = (unsigned char) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR; + temp_msg.data[1] = (unsigned char) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.); + temp_msg.data[2] = (unsigned char) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.) >> 8); + temp_msg.data[3] = (unsigned char) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.); + temp_msg.data[4] = (unsigned char) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.) >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_HOMEPOS_OFFSET(void) { + long send_homepos_offset; + send_homepos_offset = (long) (HOMEPOS_OFFSET); + + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_OFFSET; + temp_msg.data[1] = (unsigned char) send_homepos_offset; + temp_msg.data[2] = (unsigned char) (send_homepos_offset >> 8); + + can.write(temp_msg); +} + +/****************************************************************************** + Sensor & State Transmission Functions +*******************************************************************************/ + +inline void CAN_TX_POSITION(long t_pos, long t_vel) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_POSITION; + temp_msg.len = 8; + temp_msg.data[0] = (unsigned char) t_pos; + temp_msg.data[1] = (unsigned char) (t_pos >> 8); + temp_msg.data[2] = (unsigned char) (t_pos >> 16); + temp_msg.data[3] = (unsigned char) (t_pos >> 24); + temp_msg.data[4] = (unsigned char) t_vel; + temp_msg.data[5] = (unsigned char) (t_vel >> 8); + temp_msg.data[6] = (unsigned char) (t_vel >> 16); + temp_msg.data[7] = (unsigned char) (t_vel >> 24); + + can.write(temp_msg); +} + +inline void CAN_TX_TORQUE(int16_t t_torque) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_TORQUE; + temp_msg.len = 2; + temp_msg.data[0] = (unsigned char) t_torque; + temp_msg.data[1] = (unsigned char) (t_torque >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_PRES; + temp_msg.len = 8; + temp_msg.data[0] = (unsigned char) t_pres_a; + temp_msg.data[1] = (unsigned char) (t_pres_a >> 8); + temp_msg.data[2] = (unsigned char) t_pres_b; + temp_msg.data[3] = (unsigned char) (t_pres_b >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_PWM(int16_t t_pwm) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_POSITION; + temp_msg.len = 8; + temp_msg.data[0] = (unsigned char) t_pwm; + temp_msg.data[1] = (unsigned char) (t_pwm >> 8); + + can.write(temp_msg); +} + +inline void CAN_TX_VALVE_POSITION(int16_t t_valve_pos) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_VALVE_POSITION; + temp_msg.len = 8; + temp_msg.data[0] = (unsigned char) t_valve_pos; + temp_msg.data[1] = (unsigned char) (t_valve_pos >> 8); + + can.write(temp_msg); +}