CAN.cpp

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