qiu dehua / Mbed 2 deprecated GT_MOTOR_24NM_V03

初定版本

Dependencies:   mbed Hobbyking_Cheetah FastPWM3

CAN/CAN_com.cpp@55:d57335792cb7, 2022-05-10 (annotated)

Committer:
qiudehua
Date:
Tue May 10 02:10:43 2022 +0000
Revision:
55:d57335792cb7
Parent:
54:59575833d16f 
GT_MOTOR_24NM_V03

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 42:738fa01b0346 1 #include "CAN_com.h"
benkatz 42:738fa01b0346 2
benkatz 42:738fa01b0346 3
benkatz 54:59575833d16f 4 #define P_MIN -95.5f
benkatz 54:59575833d16f 5 #define P_MAX 95.5f
benkatz 42:738fa01b0346 6 #define V_MIN -45.0f
benkatz 42:738fa01b0346 7 #define V_MAX 45.0f
benkatz 42:738fa01b0346 8 #define KP_MIN 0.0f
benkatz 42:738fa01b0346 9 #define KP_MAX 500.0f
benkatz 42:738fa01b0346 10 #define KD_MIN 0.0f
benkatz 42:738fa01b0346 11 #define KD_MAX 5.0f
benkatz 42:738fa01b0346 12 #define T_MIN -18.0f
benkatz 42:738fa01b0346 13 #define T_MAX 18.0f
benkatz 42:738fa01b0346 14
qiudehua 55:d57335792cb7 15 /*
qiudehua 55:d57335792cb7 16 //为保证设置参数的最小分辨率,可启用下列参数
qiudehua 55:d57335792cb7 17 //注意：必须保证上、下转换时,MIN/MAX的一致,否则数据会出错!!!
qiudehua 55:d57335792cb7 18 #define P_MIN -150f
qiudehua 55:d57335792cb7 19 #define P_MAX 150f
qiudehua 55:d57335792cb7 20 #define V_MIN -270.0f
qiudehua 55:d57335792cb7 21 #define V_MAX 270.0f
qiudehua 55:d57335792cb7 22 #define KP_MIN 0.0f
qiudehua 55:d57335792cb7 23 #define KP_MAX 50.0f
qiudehua 55:d57335792cb7 24 #define KD_MIN 0.0f
qiudehua 55:d57335792cb7 25 #define KD_MAX 5.0f
qiudehua 55:d57335792cb7 26 #define T_MIN -5.0f
qiudehua 55:d57335792cb7 27 #define T_MAX 5.0f
qiudehua 55:d57335792cb7 28 */
benkatz 42:738fa01b0346 29
benkatz 42:738fa01b0346 30 /// CAN Reply Packet Structure ///
benkatz 42:738fa01b0346 31 /// 16 bit position, between -4*pi and 4*pi
benkatz 42:738fa01b0346 32 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 33 /// 12 bit current, between -40 and 40;
benkatz 42:738fa01b0346 34 /// CAN Packet is 5 8-bit words
benkatz 42:738fa01b0346 35 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 36 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 37 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 38 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 39 /// 3: [velocity[3-0], current[11-8]]
benkatz 42:738fa01b0346 40 /// 4: [current[7-0]]
benkatz 42:738fa01b0346 41 void pack_reply(CANMessage *msg, float p, float v, float t){
benkatz 42:738fa01b0346 42 int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
benkatz 42:738fa01b0346 43 int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
benkatz 42:738fa01b0346 44 int t_int = float_to_uint(t, -T_MAX, T_MAX, 12);
benkatz 42:738fa01b0346 45 msg->data[0] = CAN_ID;
benkatz 42:738fa01b0346 46 msg->data[1] = p_int>>8;
benkatz 42:738fa01b0346 47 msg->data[2] = p_int&0xFF;
benkatz 42:738fa01b0346 48 msg->data[3] = v_int>>4;
benkatz 42:738fa01b0346 49 msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
benkatz 42:738fa01b0346 50 msg->data[5] = t_int&0xFF;
benkatz 42:738fa01b0346 51 }
benkatz 42:738fa01b0346 52
benkatz 42:738fa01b0346 53 /// CAN Command Packet Structure ///
benkatz 42:738fa01b0346 54 /// 16 bit position command, between -4*pi and 4*pi
benkatz 42:738fa01b0346 55 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 56 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 42:738fa01b0346 57 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 42:738fa01b0346 58 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 42:738fa01b0346 59 /// CAN Packet is 8 8-bit words
benkatz 42:738fa01b0346 60 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 61 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 62 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 63 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 64 /// 3: [velocity[3-0], kp[11-8]]
benkatz 42:738fa01b0346 65 /// 4: [kp[7-0]]
benkatz 42:738fa01b0346 66 /// 5: [kd[11-4]]
benkatz 42:738fa01b0346 67 /// 6: [kd[3-0], torque[11-8]]
benkatz 42:738fa01b0346 68 /// 7: [torque[7-0]]
benkatz 42:738fa01b0346 69 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
benkatz 42:738fa01b0346 70 int p_int = (msg.data[0]<<8)|msg.data[1];
benkatz 42:738fa01b0346 71 int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
benkatz 42:738fa01b0346 72 int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
benkatz 42:738fa01b0346 73 int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
benkatz 42:738fa01b0346 74 int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
benkatz 42:738fa01b0346 75
benkatz 42:738fa01b0346 76 controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 42:738fa01b0346 77 controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
qiudehua 55:d57335792cb7 78 controller->v_des /= GR; // plan A&B&C, modify by 20210630
qiudehua 55:d57335792cb7 79 controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
qiudehua 55:d57335792cb7 80 controller->kp *= GR; // plan A&B&C, modify by 20210630
benkatz 42:738fa01b0346 81 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
qiudehua 55:d57335792cb7 82 //controller->kd /= GR; // plan B&C, modify by 20210630
benkatz 42:738fa01b0346 83 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
qiudehua 55:d57335792cb7 84 controller->t_ff *= GR; // plan A&C, modify by 20210630
benkatz 42:738fa01b0346 85 //printf("Received ");
benkatz 42:738fa01b0346 86 //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
benkatz 42:738fa01b0346 87 //printf("\n\r");
benkatz 42:738fa01b0346 88 }
benkatz 42:738fa01b0346 89