pan zhan / panzhan_main_controller_continuous

12

Dependencies:   mbed-dev_spine

CAN/CAN.cpp@0:dd5d4837292c, 2021-06-09 (annotated)

Committer:
panzhan
Date:
Wed Jun 09 01:41:45 2021 +0000
Revision:
0:dd5d4837292c
continuous motion

Who changed what in which revision?

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