CAN.cpp

00001 #include "CAN.h"
00002 #include "used_leg_message.h"
00003 #include "CAN3.h"
00004 #include "mcp2515.h"
00005 
00006 
00007 
00008 //定义设备
00009 CAN pf_can(PB_8, PB_9);                                                         // CAN Rx pin name, CAN Tx pin name
00010 CAN df_can(PB_12, PB_13);                                                       // CAN Rx pin name, CAN Tx pin name
00011 // 定义CAN消息
00012 CANMessage pf_rxMsg, df_rxMsg;                                                  // 主控收到的CAN消息
00013 CANMessage pf_txMsg, df_txMsg;                                                    //主控发送的CAN消息
00014 
00015 SPI spi(PA_7,PA_6,PA_5);// mosi, miso, scl                           //SPI转can
00016 CAN3 df1_can(spi,PA_4); //spi, ncs, itr
00017 
00018 CANMessage df1_txMsg;                                                  // 主控收到的CAN消息
00019 CANMessage df1_rxMsg;                                                  // 主控发送的CAN消息
00020 
00021 //=================================函数=======================================//
00022 
00023 /// 将控制参数打包入CAN消息中
00024 /// CAN Command Packet Structure
00025 /// 16 bit position command, between -4*pi and 4*pi
00026 /// 12 bit velocity command, between -30 and + 30 rad/s
00027 /// 12 bit kp, between 0 and 500 N-m/rad
00028 /// 12 bit kd, between 0 and 100 N-m*s/rad
00029 /// 12 bit feed forward torque, between -18 and 18 N-m
00030 /// CAN Packet is 8 8-bit words
00031 /// Formatted as follows.  For each quantity, bit 0 is LSB
00032 /// 0: [position[15-8]]
00033 /// 1: [position[7-0]] 
00034 /// 2: [velocity[11-4]]
00035 /// 3: [velocity[3-0], kp[11-8]]
00036 /// 4: [kp[7-0]]
00037 /// 5: [kd[11-4]]
00038 /// 6: [kd[3-0], torque[11-8]]
00039 /// 7: [torque[7-0]]
00040 void pack_cmd(CANMessage * msg, joint_control joint){
00041      
00042      /// limit data to be within bounds ///
00043      float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
00044      float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
00045      float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
00046      float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
00047      float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
00048      /// convert floats to unsigned ints ///
00049      uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
00050      uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
00051      uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
00052      uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
00053      uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
00054      /// pack ints into the can buffer ///
00055      msg->data[0] = p_int>>8;                                       
00056      msg->data[1] = p_int&0xFF;
00057      msg->data[2] = v_int>>4;
00058      msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
00059      msg->data[4] = kp_int&0xFF;
00060      msg->data[5] = kd_int>>4;
00061      msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
00062      msg->data[7] = t_int&0xff;
00063 }
00064 
00065 
00066 // 多个控制器联合打包准备发送
00067 void PackAll(){
00068     pack_cmd(&pf_txMsg, a_control.pf); 
00069     pack_cmd(&df_txMsg, a_control.df); 
00070     pack_cmd(&df1_txMsg, a_control.df1); 
00071 }
00072 
00073 
00074 // 写联合打包的数据
00075 void WriteAll(){
00076     pf_can.write(pf_txMsg);
00077     wait(.00002);
00078     df_can.write(df_txMsg);
00079     wait(.00002);
00080     df1_can.write(&df1_txMsg);
00081     wait(.00002);
00082     
00083 }
00084 
00085 
00086 /// 提取信息并存入状态结构体
00087 /// CAN Reply Packet Structure
00088 /// 16 bit position, between -4*pi and 4*pi
00089 /// 12 bit velocity, between -30 and + 30 rad/s
00090 /// 12 bit current, between -40 and 40;
00091 /// CAN Packet is 5 8-bit words
00092 /// Formatted as follows.  For each quantity, bit 0 is LSB
00093 /// 0: [position[15-8]]
00094 /// 1: [position[7-0]] 
00095 /// 2: [velocity[11-4]]
00096 /// 3: [velocity[3-0], current[11-8]]
00097 /// 4: [current[7-0]]
00098 void unpack_reply(CANMessage msg, ankle_state * ankle){
00099     /// unpack ints from can buffer ///
00100     uint16_t id = msg.data[0];
00101     uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
00102     uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
00103     uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
00104     /// convert uints to floats ///
00105     float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
00106     float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
00107     float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
00108    
00109     
00110     if(id==0x01){
00111         ankle->pf.p = p;
00112         ankle->pf.v = v;
00113         ankle->pf.t = t;
00114         }
00115     else if(id==0x02){
00116         ankle->df.p = p;
00117         ankle->df.v = v;
00118         ankle->df.t = t;
00119         }
00120     else if(id==0x03){
00121         ankle->df1.p = p;
00122         ankle->df1.v = v;
00123         ankle->df1.t = t;
00124         }
00125 } 
00126 
00127 
00128 
00129 
00130