Martin Gurtner / HKC_MiniCheetah

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

CAN/CAN_com.cpp@59:8fb0145aa933, 2021-01-22 (annotated)

Committer:
MartinGurtner
Date:
Fri Jan 22 12:44:34 2021 +0000
Revision:
59:8fb0145aa933
Parent:
58:312f8ec71d74 
communication improved - floats are correctly converted to uints

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 55:c4c9fec8539c 4 #define P_MAX 12.5f
MartinGurtner 59:8fb0145aa933 5 #define V_MAX 270.0f
MartinGurtner 59:8fb0145aa933 6 #define KP_MAX 50.0f
MartinGurtner 59:8fb0145aa933 7 #define KD_MAX 0.5f
MartinGurtner 58:312f8ec71d74 8 #define T_MAX 3.0f
benkatz 42:738fa01b0346 9
benkatz 42:738fa01b0346 10
benkatz 42:738fa01b0346 11 /// CAN Reply Packet Structure ///
benkatz 42:738fa01b0346 12 /// 16 bit position, between -4*pi and 4*pi
benkatz 42:738fa01b0346 13 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 14 /// 12 bit current, between -40 and 40;
benkatz 42:738fa01b0346 15 /// CAN Packet is 5 8-bit words
benkatz 42:738fa01b0346 16 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 17 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 18 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 19 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 20 /// 3: [velocity[3-0], current[11-8]]
benkatz 42:738fa01b0346 21 /// 4: [current[7-0]]
benkatz 42:738fa01b0346 22 void pack_reply(CANMessage *msg, float p, float v, float t){
MartinGurtner 59:8fb0145aa933 23
MartinGurtner 59:8fb0145aa933 24 int p_int = float_to_uint_symmetric<16>(p, P_MAX);
MartinGurtner 59:8fb0145aa933 25 int v_int = float_to_uint_symmetric<12>(v, V_MAX);
MartinGurtner 59:8fb0145aa933 26 int t_int = float_to_uint_symmetric<12>(t, T_MAX);
MartinGurtner 59:8fb0145aa933 27
benkatz 42:738fa01b0346 28 msg->data[0] = CAN_ID;
benkatz 42:738fa01b0346 29 msg->data[1] = p_int>>8;
benkatz 42:738fa01b0346 30 msg->data[2] = p_int&0xFF;
benkatz 42:738fa01b0346 31 msg->data[3] = v_int>>4;
benkatz 42:738fa01b0346 32 msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
benkatz 42:738fa01b0346 33 msg->data[5] = t_int&0xFF;
benkatz 42:738fa01b0346 34 }
benkatz 42:738fa01b0346 35
benkatz 42:738fa01b0346 36 /// CAN Command Packet Structure ///
benkatz 42:738fa01b0346 37 /// 16 bit position command, between -4*pi and 4*pi
benkatz 42:738fa01b0346 38 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 42:738fa01b0346 39 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 42:738fa01b0346 40 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 42:738fa01b0346 41 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 42:738fa01b0346 42 /// CAN Packet is 8 8-bit words
benkatz 42:738fa01b0346 43 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 42:738fa01b0346 44 /// 0: [position[15-8]]
benkatz 42:738fa01b0346 45 /// 1: [position[7-0]]
benkatz 42:738fa01b0346 46 /// 2: [velocity[11-4]]
benkatz 42:738fa01b0346 47 /// 3: [velocity[3-0], kp[11-8]]
benkatz 42:738fa01b0346 48 /// 4: [kp[7-0]]
benkatz 42:738fa01b0346 49 /// 5: [kd[11-4]]
benkatz 42:738fa01b0346 50 /// 6: [kd[3-0], torque[11-8]]
benkatz 42:738fa01b0346 51 /// 7: [torque[7-0]]
benkatz 42:738fa01b0346 52 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
benkatz 42:738fa01b0346 53 int p_int = (msg.data[0]<<8)|msg.data[1];
benkatz 42:738fa01b0346 54 int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
benkatz 42:738fa01b0346 55 int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
benkatz 42:738fa01b0346 56 int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
benkatz 42:738fa01b0346 57 int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
benkatz 42:738fa01b0346 58
MartinGurtner 59:8fb0145aa933 59 controller->p_des = uint_to_float_symmetric<16>(p_int, P_MAX);
MartinGurtner 59:8fb0145aa933 60 controller->v_des = uint_to_float_symmetric<12>(v_int, V_MAX);
MartinGurtner 59:8fb0145aa933 61 controller->kp = uint_to_float_positive<12>(kp_int, KP_MAX);
MartinGurtner 59:8fb0145aa933 62 controller->kd = uint_to_float_positive<12>(kd_int, KD_MAX);
MartinGurtner 59:8fb0145aa933 63 controller->t_ff = uint_to_float_symmetric<12>(t_int, T_MAX);
benkatz 42:738fa01b0346 64 //printf("Received ");
benkatz 42:738fa01b0346 65 //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 66 //printf("\n\r");
benkatz 42:738fa01b0346 67 }
benkatz 42:738fa01b0346 68