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?
User | Revision | Line number | New 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 |