chen xin / CH_Communicatuin_Test2

1

CAN/CAN.cpp@5:6a95726e45b0, 2019-10-08 (annotated)

Committer:
WXD
Date:
Tue Oct 08 04:35:23 2019 +0000
Revision:
5:6a95726e45b0
Child:
8:95a914f962bd
qqq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WXD 5:6a95726e45b0 1 #include "CAN.h"
WXD 5:6a95726e45b0 2
WXD 5:6a95726e45b0 3
WXD 5:6a95726e45b0 4 //定义设备
WXD 5:6a95726e45b0 5 CAN pf_can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
WXD 5:6a95726e45b0 6 CAN df_can(PB_12, PB_13); // CAN Rx pin name, CAN Tx pin name
WXD 5:6a95726e45b0 7 // 定义CAN消息
WXD 5:6a95726e45b0 8 CANMessage pf_rxMsg, df_rxMsg; // 主控收到的CAN消息
WXD 5:6a95726e45b0 9 CANMessage PF_can, DF_can; // 主控发送的CAN消息
WXD 5:6a95726e45b0 10
WXD 5:6a95726e45b0 11
WXD 5:6a95726e45b0 12 //=================================函数=======================================//
WXD 5:6a95726e45b0 13
WXD 5:6a95726e45b0 14 /// 将控制参数打包入CAN消息中
WXD 5:6a95726e45b0 15 /// CAN Command Packet Structure
WXD 5:6a95726e45b0 16 /// 16 bit position command, between -4*pi and 4*pi
WXD 5:6a95726e45b0 17 /// 12 bit velocity command, between -30 and + 30 rad/s
WXD 5:6a95726e45b0 18 /// 12 bit kp, between 0 and 500 N-m/rad
WXD 5:6a95726e45b0 19 /// 12 bit kd, between 0 and 100 N-m*s/rad
WXD 5:6a95726e45b0 20 /// 12 bit feed forward torque, between -18 and 18 N-m
WXD 5:6a95726e45b0 21 /// CAN Packet is 8 8-bit words
WXD 5:6a95726e45b0 22 /// Formatted as follows. For each quantity, bit 0 is LSB
WXD 5:6a95726e45b0 23 /// 0: [position[15-8]]
WXD 5:6a95726e45b0 24 /// 1: [position[7-0]]
WXD 5:6a95726e45b0 25 /// 2: [velocity[11-4]]
WXD 5:6a95726e45b0 26 /// 3: [velocity[3-0], kp[11-8]]
WXD 5:6a95726e45b0 27 /// 4: [kp[7-0]]
WXD 5:6a95726e45b0 28 /// 5: [kd[11-4]]
WXD 5:6a95726e45b0 29 /// 6: [kd[3-0], torque[11-8]]
WXD 5:6a95726e45b0 30 /// 7: [torque[7-0]]
WXD 5:6a95726e45b0 31 void pack_cmd(CANMessage * msg, joint_control joint){
WXD 5:6a95726e45b0 32
WXD 5:6a95726e45b0 33 /// limit data to be within bounds ///
WXD 5:6a95726e45b0 34 float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);
WXD 5:6a95726e45b0 35 float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
WXD 5:6a95726e45b0 36 float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
WXD 5:6a95726e45b0 37 float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
WXD 5:6a95726e45b0 38 float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
WXD 5:6a95726e45b0 39 /// convert floats to unsigned ints ///
WXD 5:6a95726e45b0 40 uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
WXD 5:6a95726e45b0 41 uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
WXD 5:6a95726e45b0 42 uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
WXD 5:6a95726e45b0 43 uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
WXD 5:6a95726e45b0 44 uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
WXD 5:6a95726e45b0 45 /// pack ints into the can buffer ///
WXD 5:6a95726e45b0 46 msg->data[0] = p_int>>8;
WXD 5:6a95726e45b0 47 msg->data[1] = p_int&0xFF;
WXD 5:6a95726e45b0 48 msg->data[2] = v_int>>4;
WXD 5:6a95726e45b0 49 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
WXD 5:6a95726e45b0 50 msg->data[4] = kp_int&0xFF;
WXD 5:6a95726e45b0 51 msg->data[5] = kd_int>>4;
WXD 5:6a95726e45b0 52 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
WXD 5:6a95726e45b0 53 msg->data[7] = t_int&0xff;
WXD 5:6a95726e45b0 54 }
WXD 5:6a95726e45b0 55
WXD 5:6a95726e45b0 56
WXD 5:6a95726e45b0 57 // 多个控制器联合打包准备发送
WXD 5:6a95726e45b0 58 void PackAll(){
WXD 5:6a95726e45b0 59 pack_cmd(&PF_can, a_control.pf);
WXD 5:6a95726e45b0 60 pack_cmd(&DF_can, a_control.df);
WXD 5:6a95726e45b0 61 }
WXD 5:6a95726e45b0 62
WXD 5:6a95726e45b0 63
WXD 5:6a95726e45b0 64 // 写联合打包的数据
WXD 5:6a95726e45b0 65 void WriteAll(){
WXD 5:6a95726e45b0 66 pf_can.write(PF_can);
WXD 5:6a95726e45b0 67 wait(.00002);
WXD 5:6a95726e45b0 68 df_can.write(DF_can);
WXD 5:6a95726e45b0 69 wait(.00002);
WXD 5:6a95726e45b0 70 }
WXD 5:6a95726e45b0 71
WXD 5:6a95726e45b0 72
WXD 5:6a95726e45b0 73 /// 提取信息并存入状态结构体
WXD 5:6a95726e45b0 74 /// CAN Reply Packet Structure
WXD 5:6a95726e45b0 75 /// 16 bit position, between -4*pi and 4*pi
WXD 5:6a95726e45b0 76 /// 12 bit velocity, between -30 and + 30 rad/s
WXD 5:6a95726e45b0 77 /// 12 bit current, between -40 and 40;
WXD 5:6a95726e45b0 78 /// CAN Packet is 5 8-bit words
WXD 5:6a95726e45b0 79 /// Formatted as follows. For each quantity, bit 0 is LSB
WXD 5:6a95726e45b0 80 /// 0: [position[15-8]]
WXD 5:6a95726e45b0 81 /// 1: [position[7-0]]
WXD 5:6a95726e45b0 82 /// 2: [velocity[11-4]]
WXD 5:6a95726e45b0 83 /// 3: [velocity[3-0], current[11-8]]
WXD 5:6a95726e45b0 84 /// 4: [current[7-0]]
WXD 5:6a95726e45b0 85 void unpack_reply(CANMessage msg, ankle_state * ankle){
WXD 5:6a95726e45b0 86 /// unpack ints from can buffer ///
WXD 5:6a95726e45b0 87 uint16_t id = msg.data[0];
WXD 5:6a95726e45b0 88 uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
WXD 5:6a95726e45b0 89 uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
WXD 5:6a95726e45b0 90 uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
WXD 5:6a95726e45b0 91 /// convert uints to floats ///
WXD 5:6a95726e45b0 92 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
WXD 5:6a95726e45b0 93 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
WXD 5:6a95726e45b0 94 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
WXD 5:6a95726e45b0 95
WXD 5:6a95726e45b0 96 if(id==0x0a){
WXD 5:6a95726e45b0 97 ankle->pf.p = p;
WXD 5:6a95726e45b0 98 ankle->pf.v = v;
WXD 5:6a95726e45b0 99 ankle->pf.t = t;
WXD 5:6a95726e45b0 100 }
WXD 5:6a95726e45b0 101 else if(id==0x0b){
WXD 5:6a95726e45b0 102 ankle->df.p = p;
WXD 5:6a95726e45b0 103 ankle->df.v = v;
WXD 5:6a95726e45b0 104 ankle->df.t = t;
WXD 5:6a95726e45b0 105 }
WXD 5:6a95726e45b0 106 }
WXD 5:6a95726e45b0 107
WXD 5:6a95726e45b0 108
WXD 5:6a95726e45b0 109
WXD 5:6a95726e45b0 110
WXD 5:6a95726e45b0 111