hh
Embed:
(wiki syntax)
Show/hide line numbers
CAN_com.cpp
00001 #include "CAN_com.h" 00002 00003 00004 #define P_MIN -95.5f 00005 #define P_MAX 95.5f 00006 #define V_MIN -45.0f 00007 #define V_MAX 45.0f 00008 #define KP_MIN 0.0f 00009 #define KP_MAX 500.0f 00010 #define KD_MIN 0.0f 00011 #define KD_MAX 5.0f 00012 #define T_MIN -18.0f 00013 #define T_MAX 18.0f 00014 00015 00016 /// CAN Reply Packet Structure /// 00017 /// 16 bit position, between -4*pi and 4*pi 00018 /// 12 bit velocity, between -30 and + 30 rad/s 00019 /// 12 bit current, between -40 and 40; 00020 /// CAN Packet is 5 8-bit words 00021 /// Formatted as follows. For each quantity, bit 0 is LSB 00022 /// 0: [position[15-8]] 00023 /// 1: [position[7-0]] 00024 /// 2: [velocity[11-4]] 00025 /// 3: [velocity[3-0], current[11-8]] 00026 /// 4: [current[7-0]] 00027 void pack_reply(CANMessage *msg, float p, float v, float t){ 00028 int p_int = float_to_uint(p, P_MIN, P_MAX, 16); 00029 int v_int = float_to_uint(v, V_MIN, V_MAX, 12); 00030 int t_int = float_to_uint(t, -T_MAX, T_MAX, 12); 00031 msg->data[0] = CAN_ID; 00032 msg->data[1] = p_int>>8; 00033 msg->data[2] = p_int&0xFF; 00034 msg->data[3] = v_int>>4; 00035 msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); 00036 msg->data[5] = t_int&0xFF; 00037 } 00038 00039 /// CAN Command Packet Structure /// 00040 /// 16 bit position command, between -4*pi and 4*pi 00041 /// 12 bit velocity command, between -30 and + 30 rad/s 00042 /// 12 bit kp, between 0 and 500 N-m/rad 00043 /// 12 bit kd, between 0 and 100 N-m*s/rad 00044 /// 12 bit feed forward torque, between -18 and 18 N-m 00045 /// CAN Packet is 8 8-bit words 00046 /// Formatted as follows. For each quantity, bit 0 is LSB 00047 /// 0: [position[15-8]] 00048 /// 1: [position[7-0]] 00049 /// 2: [velocity[11-4]] 00050 /// 3: [velocity[3-0], kp[11-8]] 00051 /// 4: [kp[7-0]] 00052 /// 5: [kd[11-4]] 00053 /// 6: [kd[3-0], torque[11-8]] 00054 /// 7: [torque[7-0]] 00055 void unpack_cmd(CANMessage msg, ControllerStruct * controller){ 00056 int p_int = (msg.data[0]<<8)|msg.data[1]; 00057 int v_int = (msg.data[2]<<4)|(msg.data[3]>>4); 00058 int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; 00059 int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); 00060 int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; 00061 00062 controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); 00063 controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); 00064 controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); 00065 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); 00066 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); 00067 //printf("Received "); 00068 //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); 00069 //printf("\n\r"); 00070 } 00071
Generated on Fri Jul 22 2022 01:43:47 by 1.7.2