hobbyking_cheetah source code modified 2020/12/15
Dependencies: mbed-dev-f303 FastPWM3
Diff: CAN/CAN_com.cpp
- Revision:
- 55:71a6e5fe5805
- Parent:
- 54:59575833d16f
--- a/CAN/CAN_com.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/CAN/CAN_com.cpp Tue Dec 15 07:28:27 2020 +0000 @@ -1,16 +1,33 @@ #include "CAN_com.h" +/* ----- +移除速度控制,把位置控制改成PID + +log file: +11/05: 改position的上下限(0~359.9999),移除velocity command,新增ki command +11/24: 更改can reply data packet structure(position: 2bytes -> 4bytes) +11/26: 更改can reply data packet structure(position: 32bits -> 28bits) +>> 因為會overflow +-----*/ + - #define P_MIN -95.5f - #define P_MAX 95.5f + + #define P_MIN 0.0f + #define P_MAX 6.283185f //359.9999 deg + #define P_REPLY_MIN -21360*2*PI + #define P_REPLY_MAX 21360*2*PI //rad + //#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 KI_MIN 0.0f + #define KI_MAX 10.0f #define KD_MIN 0.0f #define KD_MAX 5.0f - #define T_MIN -18.0f - #define T_MAX 18.0f + #define T_MIN -40.0f + #define T_MAX 40.0f /// CAN Reply Packet Structure /// @@ -25,15 +42,40 @@ /// 3: [velocity[3-0], current[11-8]] /// 4: [current[7-0]] void pack_reply(CANMessage *msg, float p, float v, float t){ - int p_int = float_to_uint(p, P_MIN, P_MAX, 16); + int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 28); + //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); + int t_int = float_to_uint(t, T_MIN, T_MAX, 12); + msg->data[0] = CAN_ID; + msg->data[1] = p_int >> 20; + msg->data[2] = (p_int>>12)&0xFF; + msg->data[3] = (p_int>>4)&0xFF; + msg->data[4] = (((p_int)&0xF)<<4)+(t_int>>8); + msg->data[5] = (t_int&0xFF); + + /* + for(int i = 1 ; i < 6 ; i++) + printf("%x ",msg->data[i]); + printf("\n\r"); + */ + + /* + msg->data[1] = p_int>>24; + msg->data[2] = (p_int>>16)&0xFF; + msg->data[3] = (p_int<<8)&0xFF; + msg->data[4] = p_int&0xFF; + msg->data[5] = v_int>>4; + msg->data[6] = ((v_int&0xF)<<4) + (t_int>>8); + msg->data[7] = t_int&0xFF; + */ + /* 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 /// @@ -54,18 +96,22 @@ /// 7: [torque[7-0]] void unpack_cmd(CANMessage msg, ControllerStruct * controller){ 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 v_int = (msg.data[2]<<4)|(msg.data[3]>>4); + //int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; + 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->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); + //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->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); //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"); - } +}