Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
CAN/CAN_com.cpp@76:4fd876d4cf2b, 22 months ago (annotated)
- Committer:
- adimmit
- Date:
- Tue Oct 18 16:22:24 2022 +0000
- Revision:
- 76:4fd876d4cf2b
- Parent:
- 70:2ea6c555a2cb
added some position data logging;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 42:738fa01b0346 | 1 | #include "CAN_com.h" |
benkatz | 42:738fa01b0346 | 2 | |
benkatz | 42:738fa01b0346 | 3 | |
benkatz | 55:c4c9fec8539c | 4 | #define P_MIN -12.5f |
benkatz | 55:c4c9fec8539c | 5 | #define P_MAX 12.5f |
elijahsj | 70:2ea6c555a2cb | 6 | #define V_MIN -65.0f |
elijahsj | 70:2ea6c555a2cb | 7 | #define V_MAX 65.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 |
elijahsj | 67:96a4052a1705 | 11 | #define KD_MAX 10.0f |
elijahsj | 66:3947255b18c5 | 12 | #define T_MIN -72.0f |
elijahsj | 66:3947255b18c5 | 13 | #define T_MAX 72.0f |
benkatz | 42:738fa01b0346 | 14 | |
benkatz | 42:738fa01b0346 | 15 | |
benkatz | 42:738fa01b0346 | 16 | /// CAN Reply Packet Structure /// |
benkatz | 42:738fa01b0346 | 17 | /// 16 bit position, between -4*pi and 4*pi |
benkatz | 42:738fa01b0346 | 18 | /// 12 bit velocity, between -30 and + 30 rad/s |
benkatz | 42:738fa01b0346 | 19 | /// 12 bit current, between -40 and 40; |
benkatz | 42:738fa01b0346 | 20 | /// CAN Packet is 5 8-bit words |
benkatz | 42:738fa01b0346 | 21 | /// Formatted as follows. For each quantity, bit 0 is LSB |
benkatz | 42:738fa01b0346 | 22 | /// 0: [position[15-8]] |
benkatz | 42:738fa01b0346 | 23 | /// 1: [position[7-0]] |
benkatz | 42:738fa01b0346 | 24 | /// 2: [velocity[11-4]] |
benkatz | 42:738fa01b0346 | 25 | /// 3: [velocity[3-0], current[11-8]] |
benkatz | 42:738fa01b0346 | 26 | /// 4: [current[7-0]] |
benkatz | 42:738fa01b0346 | 27 | void pack_reply(CANMessage *msg, float p, float v, float t){ |
elijahsj | 70:2ea6c555a2cb | 28 | int p_int = float_to_uint(p, P_MIN, P_MAX, 16); |
benkatz | 42:738fa01b0346 | 29 | int v_int = float_to_uint(v, V_MIN, V_MAX, 12); |
benkatz | 42:738fa01b0346 | 30 | int t_int = float_to_uint(t, -T_MAX, T_MAX, 12); |
benkatz | 42:738fa01b0346 | 31 | msg->data[0] = CAN_ID; |
benkatz | 42:738fa01b0346 | 32 | msg->data[1] = p_int>>8; |
benkatz | 42:738fa01b0346 | 33 | msg->data[2] = p_int&0xFF; |
benkatz | 42:738fa01b0346 | 34 | msg->data[3] = v_int>>4; |
benkatz | 42:738fa01b0346 | 35 | msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); |
benkatz | 42:738fa01b0346 | 36 | msg->data[5] = t_int&0xFF; |
benkatz | 42:738fa01b0346 | 37 | } |
benkatz | 42:738fa01b0346 | 38 | |
benkatz | 42:738fa01b0346 | 39 | /// CAN Command Packet Structure /// |
benkatz | 42:738fa01b0346 | 40 | /// 16 bit position command, between -4*pi and 4*pi |
benkatz | 42:738fa01b0346 | 41 | /// 12 bit velocity command, between -30 and + 30 rad/s |
benkatz | 42:738fa01b0346 | 42 | /// 12 bit kp, between 0 and 500 N-m/rad |
benkatz | 42:738fa01b0346 | 43 | /// 12 bit kd, between 0 and 100 N-m*s/rad |
benkatz | 42:738fa01b0346 | 44 | /// 12 bit feed forward torque, between -18 and 18 N-m |
benkatz | 42:738fa01b0346 | 45 | /// CAN Packet is 8 8-bit words |
benkatz | 42:738fa01b0346 | 46 | /// Formatted as follows. For each quantity, bit 0 is LSB |
benkatz | 42:738fa01b0346 | 47 | /// 0: [position[15-8]] |
benkatz | 42:738fa01b0346 | 48 | /// 1: [position[7-0]] |
benkatz | 42:738fa01b0346 | 49 | /// 2: [velocity[11-4]] |
benkatz | 42:738fa01b0346 | 50 | /// 3: [velocity[3-0], kp[11-8]] |
benkatz | 42:738fa01b0346 | 51 | /// 4: [kp[7-0]] |
benkatz | 42:738fa01b0346 | 52 | /// 5: [kd[11-4]] |
benkatz | 42:738fa01b0346 | 53 | /// 6: [kd[3-0], torque[11-8]] |
benkatz | 42:738fa01b0346 | 54 | /// 7: [torque[7-0]] |
benkatz | 42:738fa01b0346 | 55 | void unpack_cmd(CANMessage msg, ControllerStruct * controller){ |
benkatz | 42:738fa01b0346 | 56 | int p_int = (msg.data[0]<<8)|msg.data[1]; |
benkatz | 42:738fa01b0346 | 57 | int v_int = (msg.data[2]<<4)|(msg.data[3]>>4); |
benkatz | 42:738fa01b0346 | 58 | int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; |
benkatz | 42:738fa01b0346 | 59 | int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); |
benkatz | 42:738fa01b0346 | 60 | int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; |
benkatz | 42:738fa01b0346 | 61 | |
benkatz | 42:738fa01b0346 | 62 | controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); |
benkatz | 42:738fa01b0346 | 63 | controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); |
benkatz | 42:738fa01b0346 | 64 | controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); |
benkatz | 42:738fa01b0346 | 65 | controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); |
benkatz | 42:738fa01b0346 | 66 | controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); |
benkatz | 42:738fa01b0346 | 67 | //printf("Received "); |
benkatz | 42:738fa01b0346 | 68 | //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 | 69 | //printf("\n\r"); |
benkatz | 42:738fa01b0346 | 70 | } |
benkatz | 42:738fa01b0346 | 71 |