Yi-Ting Hsieh / Hobbyking_Cheetah_test0223

Test the set param mode

Dependencies:   FastPWM3

CAN/CAN_com.cpp@55:6ab83cbbbca7, 2021-03-26 (annotated)

Committer:
eatinghsieh
Date:
Fri Mar 26 05:28:00 2021 +0000
Revision:
55:6ab83cbbbca7
Parent:
54:59575833d16f 
Test set parameter mode

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
eatinghsieh 55:6ab83cbbbca7 15 #define kp_L_MAX 125.0f
eatinghsieh 55:6ab83cbbbca7 16 #define kp_L_MIN 0.0f
eatinghsieh 55:6ab83cbbbca7 17 #define kp_M1_MAX 250.0f
eatinghsieh 55:6ab83cbbbca7 18 #define kp_M1_MIN 126.0f
eatinghsieh 55:6ab83cbbbca7 19 #define kp_M2_MAX 375.0f
eatinghsieh 55:6ab83cbbbca7 20 #define kp_M2_MIN 251.0f
eatinghsieh 55:6ab83cbbbca7 21 #define kp_H_MAX 500.0f
eatinghsieh 55:6ab83cbbbca7 22 #define kp_H_MIN 376.0f
eatinghsieh 55:6ab83cbbbca7 23 #define range_L_MAX 256.0f
eatinghsieh 55:6ab83cbbbca7 24 #define range_L_MIN 256.0f
eatinghsieh 55:6ab83cbbbca7 25 #define range_M_MAX 256.0f
eatinghsieh 55:6ab83cbbbca7 26 #define range_M_MIN 256.0f
eatinghsieh 55:6ab83cbbbca7 27 #define range_H_MAX 256.0f
eatinghsieh 55:6ab83cbbbca7 28 #define range_H_MIN 256.0f
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 }
eatinghsieh 55:6ab83cbbbca7 52
eatinghsieh 55:6ab83cbbbca7 53 /// CAN Command Packet Structure ///
eatinghsieh 55:6ab83cbbbca7 54 /// 16 bit position command, between -4*pi and 4*pi
eatinghsieh 55:6ab83cbbbca7 55 /// 12 bit velocity command, between -30 and + 30 rad/s
eatinghsieh 55:6ab83cbbbca7 56 /// 12 bit kp, between 0 and 500 N-m/rad
eatinghsieh 55:6ab83cbbbca7 57 /// 12 bit kd, between 0 and 100 N-m*s/rad
eatinghsieh 55:6ab83cbbbca7 58 /// 12 bit feed forward torque, between -18 and 18 N-m
eatinghsieh 55:6ab83cbbbca7 59 /// CAN Packet is 8 8-bit words
eatinghsieh 55:6ab83cbbbca7 60 /// Formatted as follows. For each quantity, bit 0 is LSB
eatinghsieh 55:6ab83cbbbca7 61 /// 0: [position[15-8]]
eatinghsieh 55:6ab83cbbbca7 62 /// 1: [position[7-0]]
eatinghsieh 55:6ab83cbbbca7 63 /// 2: [velocity[11-4]]
eatinghsieh 55:6ab83cbbbca7 64 /// 3: [velocity[3-0], kp[11-8]]
eatinghsieh 55:6ab83cbbbca7 65 /// 4: [kp[7-0]]
eatinghsieh 55:6ab83cbbbca7 66 /// 5: [kd[11-4]]
eatinghsieh 55:6ab83cbbbca7 67 /// 6: [kd[3-0], torque[11-8]]
eatinghsieh 55:6ab83cbbbca7 68 /// 7: [torque[7-0]]
eatinghsieh 55:6ab83cbbbca7 69 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
eatinghsieh 55:6ab83cbbbca7 70 int p_int = (msg.data[0]<<16)|(msg.data[1]<<8)|msg.data[2];
eatinghsieh 55:6ab83cbbbca7 71 int v_int = (msg.data[3]<<12)|(msg.data[4]<<4)|(msg.data[5]>>4);
eatinghsieh 55:6ab83cbbbca7 72 int t_int = ((msg.data[5]&0xF)<<16)|(msg.data[6]<<8)|msg.data[7];
eatinghsieh 55:6ab83cbbbca7 73
eatinghsieh 55:6ab83cbbbca7 74 controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 24);
eatinghsieh 55:6ab83cbbbca7 75 controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 20);
eatinghsieh 55:6ab83cbbbca7 76 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 20);
eatinghsieh 55:6ab83cbbbca7 77 //printf("Received ");
eatinghsieh 55:6ab83cbbbca7 78 //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
eatinghsieh 55:6ab83cbbbca7 79 //printf("\n\r");
eatinghsieh 55:6ab83cbbbca7 80 }
eatinghsieh 55:6ab83cbbbca7 81
eatinghsieh 55:6ab83cbbbca7 82 void unpack_param(CANMessage msg, ControllerStruct * controller){
eatinghsieh 55:6ab83cbbbca7 83 int kp_L_int = msg.data[0];
eatinghsieh 55:6ab83cbbbca7 84 int kp_M1_int = msg.data[1];
eatinghsieh 55:6ab83cbbbca7 85 int kp_M2_int = msg.data[2];
eatinghsieh 55:6ab83cbbbca7 86 int kp_H_int = msg.data[3];
eatinghsieh 55:6ab83cbbbca7 87 int kd_int = msg.data[4];
eatinghsieh 55:6ab83cbbbca7 88 int range_L_int = msg.data[5];
eatinghsieh 55:6ab83cbbbca7 89 int range_M_int = msg.data[6];
eatinghsieh 55:6ab83cbbbca7 90 int range_H_int = msg.data[7];
eatinghsieh 55:6ab83cbbbca7 91
benkatz 42:738fa01b0346 92
eatinghsieh 55:6ab83cbbbca7 93 controller->kp_L = uint_to_float(kp_L_int, kp_L_MIN, kp_L_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 94 controller->kp_M1 = uint_to_float(kp_M1_int, kp_M1_MIN, kp_M1_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 95 controller->kp_M2 = uint_to_float(kp_M2_int, kp_M2_MIN, kp_M2_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 96 controller->kp_H = uint_to_float(kp_H_int, kp_H_MIN, kp_H_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 97 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 98 controller->range_L = uint_to_float(range_L_int, range_L_MIN, range_L_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 99 controller->range_M = uint_to_float(range_M_int, range_M_MIN, range_M_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 100 controller->range_H = uint_to_float(range_H_int, range_H_MIN, range_H_MAX, 8);
eatinghsieh 55:6ab83cbbbca7 101 //printf("Received ");
eatinghsieh 55:6ab83cbbbca7 102 //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
eatinghsieh 55:6ab83cbbbca7 103 //printf("\n\r");
eatinghsieh 55:6ab83cbbbca7 104 }
eatinghsieh 55:6ab83cbbbca7 105 /*
benkatz 42:738fa01b0346 106 /// CAN Command Packet Structure ///
benkatz 42:738fa01b0346 107 /// 16 bit position command, between -4*pi and 4*pi
benkatz 42:738fa01b0346 108 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 109 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 42:738fa01b0346 110 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 42:738fa01b0346 111 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 42:738fa01b0346 112 /// CAN Packet is 8 8-bit words
benkatz 42:738fa01b0346 113 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 114 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 115 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 116 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 117 /// 3: [velocity[3-0], kp[11-8]]
benkatz 42:738fa01b0346 118 /// 4: [kp[7-0]]
benkatz 42:738fa01b0346 119 /// 5: [kd[11-4]]
benkatz 42:738fa01b0346 120 /// 6: [kd[3-0], torque[11-8]]
benkatz 42:738fa01b0346 121 /// 7: [torque[7-0]]
benkatz 42:738fa01b0346 122 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
benkatz 42:738fa01b0346 123 int p_int = (msg.data[0]<<8)|msg.data[1];
benkatz 42:738fa01b0346 124 int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
benkatz 42:738fa01b0346 125 int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
benkatz 42:738fa01b0346 126 int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
benkatz 42:738fa01b0346 127 int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
benkatz 42:738fa01b0346 128
benkatz 42:738fa01b0346 129 controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 42:738fa01b0346 130 controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 42:738fa01b0346 131 controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
benkatz 42:738fa01b0346 132 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
benkatz 42:738fa01b0346 133 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
benkatz 42:738fa01b0346 134 //printf("Received ");
benkatz 42:738fa01b0346 135 //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 136 //printf("\n\r");
eatinghsieh 55:6ab83cbbbca7 137 }*/
benkatz 42:738fa01b0346 138