Diff: CAN/CAN.cpp

Revision:

0:d80c66cb1b3a

diff -r 000000000000 -r d80c66cb1b3a CAN/CAN.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.cpp	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,112 @@
+#include "CAN.h"
+#include "used_leg_message.h"
+
+
+
+//定义设备
+CAN knee_can(PB_8, PB_9);                                                         // CAN Rx pin name, CAN Tx pin name
+CAN ankle_can(PB_12, PB_13);                                                       // CAN Rx pin name, CAN Tx pin name
+// 定义CAN消息
+CANMessage knee_rxMsg, ankle_rxMsg;                                                  // 主控收到的CAN消息
+CANMessage knee_txMsg, ankle_txMsg;                                           // 主控发送的CAN消息
+
+//=================================函数=======================================//
+
+/// 将控制参数打包入CAN消息中
+/// CAN Command Packet Structure
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+void pack_cmd(CANMessage * msg, joint_control joint){
+     
+     /// limit data to be within bounds ///
+     float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
+     float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
+     float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
+     float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
+     float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
+     /// convert floats to unsigned ints ///
+     uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
+     uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
+     uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
+     uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
+     uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
+     /// pack ints into the can buffer ///
+     msg->data[0] = p_int>>8;                                       
+     msg->data[1] = p_int&0xFF;
+     msg->data[2] = v_int>>4;
+     msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
+     msg->data[4] = kp_int&0xFF;
+     msg->data[5] = kd_int>>4;
+     msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
+     msg->data[7] = t_int&0xff;
+}
+
+
+// 多个控制器联合打包准备发送
+void PackAll(){
+    pack_cmd(&knee_txMsg, a_control.knee); 
+    pack_cmd(&ankle_txMsg, a_control.ankle); 
+}
+
+
+// 写联合打包的数据
+void WriteAll(){
+    knee_can.write(knee_txMsg);
+    wait(.00002);
+    ankle_can.write(ankle_txMsg);
+    wait(.00002);
+}
+
+
+/// 提取信息并存入状态结构体
+/// CAN Reply Packet Structure
+/// 16 bit position, between -4*pi and 4*pi
+/// 12 bit velocity, between -30 and + 30 rad/s
+/// 12 bit current, between -40 and 40;
+/// CAN Packet is 5 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], current[11-8]]
+/// 4: [current[7-0]]
+void unpack_reply(CANMessage msg, leg_state * state){
+    /// unpack ints from can buffer ///
+    uint16_t id = msg.data[0];
+    uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
+    uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
+    uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
+    /// convert uints to floats ///
+    float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
+    float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
+    float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
+    
+    if(id==0x01){
+        state->knee_state.p = p;
+        state->knee_state.v = v;
+        state->knee_state.t = t;
+        }
+    else if(id==0x01){
+        state->ankle_state.p = p;
+        state->ankle_state.v = v;
+        state->ankle_state.t = t;
+        }
+} 
+
+
+
+
+