Shao Rui / CH_Communicatuin_Test

123

Dependencies:   mbed-dev_spine

CAN/CAN.cpp@8:95a914f962bd, 2020-01-07 (annotated)

Committer:
shaorui
Date:
Tue Jan 07 09:23:24 2020 +0000
Revision:
8:95a914f962bd
Parent:
5:6a95726e45b0 
1234

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WXD 5:6a95726e45b0 1 #include "CAN.h"
shaorui 8:95a914f962bd 2 #include "used_leg_message.h"
WXD 5:6a95726e45b0 3
WXD 5:6a95726e45b0 4 //定义设备
shaorui 8:95a914f962bd 5 CAN ef_can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
shaorui 8:95a914f962bd 6 CAN wf_can(PB_12, PB_13); // CAN Rx pin name, CAN Tx pin name
WXD 5:6a95726e45b0 7 // 定义CAN消息
shaorui 8:95a914f962bd 8 CANMessage ef_rxMsg, wf_rxMsg; // 主控收到的CAN消息
shaorui 8:95a914f962bd 9 CANMessage EF_can, WF_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(){
shaorui 8:95a914f962bd 59 pack_cmd(&EF_can, a_control.ef);
shaorui 8:95a914f962bd 60 pack_cmd(&WF_can, a_control.wf);
WXD 5:6a95726e45b0 61 }
WXD 5:6a95726e45b0 62
WXD 5:6a95726e45b0 63
WXD 5:6a95726e45b0 64 // 写联合打包的数据
WXD 5:6a95726e45b0 65 void WriteAll(){
shaorui 8:95a914f962bd 66 ef_can.write(EF_can);
WXD 5:6a95726e45b0 67 wait(.00002);
shaorui 8:95a914f962bd 68 wf_can.write(WF_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){
shaorui 8:95a914f962bd 97 ankle->ef.p = p;
shaorui 8:95a914f962bd 98 ankle->ef.v = v;
shaorui 8:95a914f962bd 99 ankle->ef.t = t;
WXD 5:6a95726e45b0 100 }
WXD 5:6a95726e45b0 101 else if(id==0x0b){
shaorui 8:95a914f962bd 102 ankle->wf.p = p;
shaorui 8:95a914f962bd 103 ankle->wf.v = v;
shaorui 8:95a914f962bd 104 ankle->wf.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