Winnie Liu / Hobbyking_Cheetah_modify1105

hobbyking_cheetah source code modified 2020/12/15

Dependencies:   mbed-dev-f303 FastPWM3

CAN/CAN_com.cpp@55:71a6e5fe5805, 2020-12-15 (annotated)

Committer:
WinnieLiu
Date:
Tue Dec 15 07:28:27 2020 +0000
Revision:
55:71a6e5fe5805
Parent:
54:59575833d16f 
2020/12/15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 42:738fa01b0346 1 #include "CAN_com.h"
benkatz 42:738fa01b0346 2
WinnieLiu 55:71a6e5fe5805 3 /* -----
WinnieLiu 55:71a6e5fe5805 4 移除速度控制，把位置控制改成PID
WinnieLiu 55:71a6e5fe5805 5
WinnieLiu 55:71a6e5fe5805 6 log file:
WinnieLiu 55:71a6e5fe5805 7 11/05: 改position的上下限(0~359.9999)，移除velocity command，新增ki command
WinnieLiu 55:71a6e5fe5805 8 11/24: 更改can reply data packet structure(position: 2bytes -> 4bytes)
WinnieLiu 55:71a6e5fe5805 9 11/26: 更改can reply data packet structure(position: 32bits -> 28bits)
WinnieLiu 55:71a6e5fe5805 10 >> 因為會overflow
WinnieLiu 55:71a6e5fe5805 11 -----*/
WinnieLiu 55:71a6e5fe5805 12
benkatz 42:738fa01b0346 13
WinnieLiu 55:71a6e5fe5805 14
WinnieLiu 55:71a6e5fe5805 15 #define P_MIN 0.0f
WinnieLiu 55:71a6e5fe5805 16 #define P_MAX 6.283185f //359.9999 deg
WinnieLiu 55:71a6e5fe5805 17 #define P_REPLY_MIN -21360*2*PI
WinnieLiu 55:71a6e5fe5805 18 #define P_REPLY_MAX 21360*2*PI //rad
WinnieLiu 55:71a6e5fe5805 19 //#define P_MIN -95.5f
WinnieLiu 55:71a6e5fe5805 20 //#define P_MAX 95.5f
benkatz 42:738fa01b0346 21 #define V_MIN -45.0f
benkatz 42:738fa01b0346 22 #define V_MAX 45.0f
benkatz 42:738fa01b0346 23 #define KP_MIN 0.0f
benkatz 42:738fa01b0346 24 #define KP_MAX 500.0f
WinnieLiu 55:71a6e5fe5805 25 #define KI_MIN 0.0f
WinnieLiu 55:71a6e5fe5805 26 #define KI_MAX 10.0f
benkatz 42:738fa01b0346 27 #define KD_MIN 0.0f
benkatz 42:738fa01b0346 28 #define KD_MAX 5.0f
WinnieLiu 55:71a6e5fe5805 29 #define T_MIN -40.0f
WinnieLiu 55:71a6e5fe5805 30 #define T_MAX 40.0f
benkatz 42:738fa01b0346 31
benkatz 42:738fa01b0346 32
benkatz 42:738fa01b0346 33 /// CAN Reply Packet Structure ///
benkatz 42:738fa01b0346 34 /// 16 bit position, between -4*pi and 4*pi
benkatz 42:738fa01b0346 35 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 36 /// 12 bit current, between -40 and 40;
benkatz 42:738fa01b0346 37 /// CAN Packet is 5 8-bit words
benkatz 42:738fa01b0346 38 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 39 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 40 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 41 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 42 /// 3: [velocity[3-0], current[11-8]]
benkatz 42:738fa01b0346 43 /// 4: [current[7-0]]
benkatz 42:738fa01b0346 44 void pack_reply(CANMessage *msg, float p, float v, float t){
WinnieLiu 55:71a6e5fe5805 45 int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 28);
WinnieLiu 55:71a6e5fe5805 46 //int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
benkatz 42:738fa01b0346 47 int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
WinnieLiu 55:71a6e5fe5805 48 int t_int = float_to_uint(t, T_MIN, T_MAX, 12);
WinnieLiu 55:71a6e5fe5805 49
benkatz 42:738fa01b0346 50 msg->data[0] = CAN_ID;
WinnieLiu 55:71a6e5fe5805 51 msg->data[1] = p_int >> 20;
WinnieLiu 55:71a6e5fe5805 52 msg->data[2] = (p_int>>12)&0xFF;
WinnieLiu 55:71a6e5fe5805 53 msg->data[3] = (p_int>>4)&0xFF;
WinnieLiu 55:71a6e5fe5805 54 msg->data[4] = (((p_int)&0xF)<<4)+(t_int>>8);
WinnieLiu 55:71a6e5fe5805 55 msg->data[5] = (t_int&0xFF);
WinnieLiu 55:71a6e5fe5805 56
WinnieLiu 55:71a6e5fe5805 57 /*
WinnieLiu 55:71a6e5fe5805 58 for(int i = 1 ; i < 6 ; i++)
WinnieLiu 55:71a6e5fe5805 59 printf("%x ",msg->data[i]);
WinnieLiu 55:71a6e5fe5805 60 printf("\n\r");
WinnieLiu 55:71a6e5fe5805 61 */
WinnieLiu 55:71a6e5fe5805 62
WinnieLiu 55:71a6e5fe5805 63 /*
WinnieLiu 55:71a6e5fe5805 64 msg->data[1] = p_int>>24;
WinnieLiu 55:71a6e5fe5805 65 msg->data[2] = (p_int>>16)&0xFF;
WinnieLiu 55:71a6e5fe5805 66 msg->data[3] = (p_int<<8)&0xFF;
WinnieLiu 55:71a6e5fe5805 67 msg->data[4] = p_int&0xFF;
WinnieLiu 55:71a6e5fe5805 68 msg->data[5] = v_int>>4;
WinnieLiu 55:71a6e5fe5805 69 msg->data[6] = ((v_int&0xF)<<4) + (t_int>>8);
WinnieLiu 55:71a6e5fe5805 70 msg->data[7] = t_int&0xFF;
WinnieLiu 55:71a6e5fe5805 71 */
WinnieLiu 55:71a6e5fe5805 72 /*
benkatz 42:738fa01b0346 73 msg->data[1] = p_int>>8;
benkatz 42:738fa01b0346 74 msg->data[2] = p_int&0xFF;
benkatz 42:738fa01b0346 75 msg->data[3] = v_int>>4;
benkatz 42:738fa01b0346 76 msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
benkatz 42:738fa01b0346 77 msg->data[5] = t_int&0xFF;
WinnieLiu 55:71a6e5fe5805 78 */
benkatz 42:738fa01b0346 79 }
benkatz 42:738fa01b0346 80
benkatz 42:738fa01b0346 81 /// CAN Command Packet Structure ///
benkatz 42:738fa01b0346 82 /// 16 bit position command, between -4*pi and 4*pi
benkatz 42:738fa01b0346 83 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 84 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 42:738fa01b0346 85 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 42:738fa01b0346 86 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 42:738fa01b0346 87 /// CAN Packet is 8 8-bit words
benkatz 42:738fa01b0346 88 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 89 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 90 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 91 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 92 /// 3: [velocity[3-0], kp[11-8]]
benkatz 42:738fa01b0346 93 /// 4: [kp[7-0]]
benkatz 42:738fa01b0346 94 /// 5: [kd[11-4]]
benkatz 42:738fa01b0346 95 /// 6: [kd[3-0], torque[11-8]]
benkatz 42:738fa01b0346 96 /// 7: [torque[7-0]]
benkatz 42:738fa01b0346 97 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
benkatz 42:738fa01b0346 98 int p_int = (msg.data[0]<<8)|msg.data[1];
WinnieLiu 55:71a6e5fe5805 99 //int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
WinnieLiu 55:71a6e5fe5805 100 //int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
WinnieLiu 55:71a6e5fe5805 101 int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
WinnieLiu 55:71a6e5fe5805 102 int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
benkatz 42:738fa01b0346 103 int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
benkatz 42:738fa01b0346 104 int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
benkatz 42:738fa01b0346 105
benkatz 42:738fa01b0346 106 controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
WinnieLiu 55:71a6e5fe5805 107 //controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 42:738fa01b0346 108 controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
WinnieLiu 55:71a6e5fe5805 109 controller->ki = uint_to_float(ki_int, KI_MIN, KI_MAX, 12);
benkatz 42:738fa01b0346 110 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
benkatz 42:738fa01b0346 111 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
benkatz 42:738fa01b0346 112 //printf("Received ");
WinnieLiu 55:71a6e5fe5805 113 //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->kp, controller->ki, controller->kd, controller->t_ff, controller->i_q_ref);
benkatz 42:738fa01b0346 114 //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 115 //printf("\n\r");
WinnieLiu 55:71a6e5fe5805 116 }
benkatz 42:738fa01b0346 117