Motor control for robots. More compact, less object-oriented revision.
Dependencies: FastPWM3 mbed-dev-f303
Fork of Hobbyking_Cheetah_V1 by
CAN_com.cpp
00001 #include "CAN_com.h" 00002 00003 00004 00005 00006 00007 /// CAN Reply Packet Structure /// 00008 /// 16 bit position, between -4*pi and 4*pi 00009 /// 12 bit velocity, between -30 and + 30 rad/s 00010 /// 12 bit current, between -40 and 40; 00011 /// CAN Packet is 5 8-bit words 00012 /// Formatted as follows. For each quantity, bit 0 is LSB 00013 /// 0: [position[15-8]] 00014 /// 1: [position[7-0]] 00015 /// 2: [velocity[11-4]] 00016 /// 3: [velocity[3-0], current[11-8]] 00017 /// 4: [current[7-0]] 00018 void pack_reply(CANMessage *msg, float p, float v, float t){ 00019 int p_int = float_to_uint(p, P_MIN, P_MAX, 16); 00020 int v_int = float_to_uint(v, V_MIN, V_MAX, 12); 00021 int t_int = float_to_uint(t, -T_MAX, T_MAX, 12); 00022 msg->data[0] = CAN_ID; 00023 msg->data[1] = p_int>>8; 00024 msg->data[2] = p_int&0xFF; 00025 msg->data[3] = v_int>>4; 00026 msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); 00027 msg->data[5] = t_int&0xFF; 00028 } 00029 00030 /// CAN Command Packet Structure /// 00031 /// 16 bit position command, between -4*pi and 4*pi 00032 /// 12 bit velocity command, between -30 and + 30 rad/s 00033 /// 12 bit kp, between 0 and 500 N-m/rad 00034 /// 12 bit kd, between 0 and 100 N-m*s/rad 00035 /// 12 bit feed forward torque, between -18 and 18 N-m 00036 /// CAN Packet is 8 8-bit words 00037 /// Formatted as follows. For each quantity, bit 0 is LSB 00038 /// 0: [position[15-8]] 00039 /// 1: [position[7-0]] 00040 /// 2: [velocity[11-4]] 00041 /// 3: [velocity[3-0], kp[11-8]] 00042 /// 4: [kp[7-0]] 00043 /// 5: [kd[11-4]] 00044 /// 6: [kd[3-0], torque[11-8]] 00045 /// 7: [torque[7-0]] 00046 void unpack_cmd(CANMessage msg, ControllerStruct * controller){ 00047 int p_int = (msg.data[0]<<8)|msg.data[1]; 00048 int v_int = (msg.data[2]<<4)|(msg.data[3]>>4); 00049 int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; 00050 int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); 00051 int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; 00052 00053 controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); 00054 controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); 00055 controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); 00056 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); 00057 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); 00058 //printf("Received "); 00059 //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); 00060 //printf("\n\r"); 00061 } 00062
Generated on Tue Jul 12 2022 13:17:46 by 1.7.2