yezhong yezhong / yezhong_main_controller_copy_3_

1233

Dependencies:   cJSON_lib mcp2515- mbed-dev-f303 esp8266 yezhong_main_controller_copy_3

CAN/CAN.cpp@0:e923de71caa5, 2022-06-24 (annotated)

Committer:
yezhong
Date:
Fri Jun 24 07:58:26 2022 +0000
Revision:
0:e923de71caa5
111111

Who changed what in which revision?

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