modified 0511
Dependencies: mbed-dev FastPWM3
CAN/CAN_com.cpp
- Committer:
- WinnieLiu
- Date:
- 2022-03-07
- Revision:
- 62:d43fcdd2d48b
- Parent:
- 60:f009e39e913e
- Child:
- 65:cb3497b549e9
File content as of revision 62:d43fcdd2d48b:
#include "CAN_com.h" /* //----- origin ----- #define P_MIN -95.5f #define P_MAX 95.5f #define V_MIN -45.0f #define V_MAX 45.0f #define KP_MIN 0.0f #define KP_MAX 500.0f #define KD_MIN 0.0f #define KD_MAX 5.0f #define T_MIN -18.0f #define T_MAX 18.0f //-------------------- */ /// CAN Reply Packet Structure /// /// 16 bit position, between -4*pi and 4*pi /// 12 bit velocity, between -30 and + 30 rad/s /// 12 bit current, between -40 and 40; /// CAN Packet is 5 8-bit words /// Formatted as follows. For each quantity, bit 0 is LSB /// 0: [position[15-8]] /// 1: [position[7-0]] /// 2: [velocity[11-4]] /// 3: [velocity[3-0], current[11-8]] /// 4: [current[7-0]] void pack_reply(CANMessage *msg, float p, float v, float t, int version, int calibrate_finish, int state){ /* // ----- origin -----(p,v,t) int p_int = float_to_uint(p, P_MIN, P_MAX, 16); int v_int = float_to_uint(v, V_MIN, V_MAX, 12); int t_int = float_to_uint(t, -T_MAX, T_MAX, 12); msg->data[0] = CAN_ID; msg->data[1] = p_int>>8; msg->data[2] = p_int&0xFF; msg->data[3] = v_int>>4; msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); msg->data[5] = t_int&0xFF; // -------------------------- */ // ----- tune PID parameters ----- int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 16); int v_int = float_to_uint(v, V_MIN, V_MAX, 12); int t_int = float_to_uint(t, T_MIN, T_MAX, 12); msg->data[0] = CAN_ID; msg->data[1] = p_int>>8; msg->data[2] = p_int&0xFF; msg->data[3] = v_int>>4; msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); msg->data[5] = t_int&0xFF; msg->data[6] = (version<<4) + (calibrate_finish&0xF); msg->data[7] = state; //printf("%x\n\r",msg->data[6]); /* // ----- fixed pid controller parameters(set at menu) ----- int p_int = float_to_uint(p,P_REPLY_MIN,P_REPLY_MAX,16); int t_int = float_to_uint(t, T_MIN, T_MAX, 12); msg->data[0] = CAN_ID; msg->data[1] = p_int >> 8; msg->data[2] = p_int&0xFF; msg->data[3] = t_int >> 8; msg->data[4] = t_int&0xFF; //printf("position: %.3f ", p); //printf("\n\r"); */ /* // ----- changable kp with fixed kd ----- int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 16); int v_int = float_to_uint(v, V_MIN, V_MAX, 12); int t_int = float_to_uint(t, -T_MAX, T_MAX, 12); msg->data[0] = CAN_ID; msg->data[1] = p_int>>8; msg->data[2] = p_int&0xFF; msg->data[3] = v_int>>4; msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); msg->data[5] = t_int&0xFF; // -------------------------- */ } /// CAN Command Packet Structure /// /// 16 bit position command, between -4*pi and 4*pi /// 12 bit velocity command, between -30 and + 30 rad/s /// 12 bit kp, between 0 and 500 N-m/rad /// 12 bit kd, between 0 and 100 N-m*s/rad /// 12 bit feed forward torque, between -18 and 18 N-m /// CAN Packet is 8 8-bit words /// Formatted as follows. For each quantity, bit 0 is LSB /// 0: [position[15-8]] /// 1: [position[7-0]] /// 2: [velocity[11-4]] /// 3: [velocity[3-0], kp[11-8]] /// 4: [kp[7-0]] /// 5: [kd[11-4]] /// 6: [kd[3-0], torque[11-8]] /// 7: [torque[7-0]] void unpack_cmd(CANMessage msg, ControllerStruct * controller){ /* // ----- origin -----(p,v,kp,kd,t) int p_int = (msg.data[0]<<8)|msg.data[1]; int v_int = (msg.data[2]<<4)|(msg.data[3]>>4); int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; // -------------------------------- */ /* // ----- origin -----(p,v,kp,kd,t) controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); // ------------------------------- */ // ----- tune PID parameters ---- (p,kp,ki,kd,t) int p_int = (msg.data[0]<<8)|msg.data[1]; int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4); int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); controller->ki = uint_to_float(ki_int, KI_MIN, KI_MAX, 12); controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); /* // ----- fixed pid controller parameters(set at menu) ----- // only send position command and torque command int p_int = (msg.data[0]<<8)|msg.data[1]; int t_int = (msg.data[2]<<8)|msg.data[3]; controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); // */ /* // ----- changable kp with fixed kd ----- int p_int = (msg.data[0]<<8)|msg.data[1]; int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4); int t_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); */ //printf("Received "); //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->kp, controller->ki, controller->kd, controller->t_ff, controller->i_q_ref); //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); //printf("\n\r"); }