![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 7:e9086c72bb22
- Parent:
- 4:58c8081de776
- Child:
- 11:82d8768d7351
--- a/CAN/function_CAN.cpp Tue Aug 20 10:40:27 2019 +0000 +++ b/CAN/function_CAN.cpp Tue Aug 20 12:27:19 2019 +0000 @@ -1,4 +1,3 @@ -#include "mbed.h" #include "function_CAN.h" // CAN ID Setting Variables @@ -6,18 +5,26 @@ 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_VOUT = 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_VOUT = 1500; int CID_TX_VALVE_POSITION = 1600; -extern CAN can; -extern CANMessage msg; +// Board Information +int BNO = 0; +int CONTROL_MODE = 0; +double P_GAIN_JOINT_POSITION = 0.0; +double I_GAIN_JOINT_POSITION = 0.0; +double D_GAIN_JOINT_POSITION = 0.0; +double P_GAIN_JOINT_TORQUE = 0.0; +double I_GAIN_JOINT_TORQUE = 0.0; +double D_GAIN_JOINT_TORQUE = 0.0; + void ReadCMD(char CMD) { @@ -38,10 +45,28 @@ can.read(msg); unsigned int address = msg.id; - if(address==100){ + if(address==CID_RX_CMD){ unsigned int CMD = msg.data[0]; ReadCMD(CMD); + } else if(address==CID_RX_REF_POSITION) { + int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24); + int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24); + pos.ref = (double)temp_pos; + vel.ref = (double)temp_vel; + } else if(address==CID_RX_REF_TORQUE) { + int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8); + torq.ref = (double)temp_torq; + } else if(address==CID_RX_REF_PRES_DIFF) { +// int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8); +// torq.ref = (double)temp_presdiff; + } else if(address==CID_RX_REF_VOUT) { + int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8); + Vout.ref = (double)temp_PWM; + } else if(address==CID_RX_REF_VALVE_POSITION) { + int16_t temp_cur = (int16_t) (msg.data[0] | msg.data[1] << 8); + cur.ref = (double)temp_cur; } + } /****************************************************************************** @@ -98,7 +123,7 @@ can.write(temp_msg); } - +*/ inline void CAN_TX_CONTROL_MODE(void) { CANMessage temp_msg; @@ -109,7 +134,7 @@ can.write(temp_msg); } - +/* inline void CAN_TX_JOINT_ENC_DIR(void) { CANMessage temp_msg; @@ -173,17 +198,18 @@ 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; + long sendPgain=0, sendIgain=0, sendDgain=0; if (t_type == 0) { - sendPgain = (long) (P_GAIN_VALVE_POSITION); - sendIgain = (long) (I_GAIN_VALVE_POSITION); - sendDgain = (long) (D_GAIN_VALVE_POSITION); +// 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); @@ -210,6 +236,7 @@ can.write(temp_msg); } +/* inline void CAN_TX_VALVE_DEADZONE(void) { CANMessage temp_msg;