![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
CAN/function_CAN.cpp
- Committer:
- jobuuu
- Date:
- 2019-08-20
- Revision:
- 2:a1c0a37df760
- Child:
- 3:aa28672362bb
File content as of revision 2:a1c0a37df760:
#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); }