CAN_com.cpp

00001 #include "CAN_com.h"
00002 
00003 /* -----
00004 移除速度控制，把位置控制改成PID
00005 
00006 log file:
00007 11/05: 改position的上下限(0~359.9999)，移除velocity command，新增ki command
00008 11/24: 更改can reply data packet structure(position: 2bytes -> 4bytes)
00009 11/26: 更改can reply data packet structure(position: 32bits -> 28bits)
00010 >> 因為會overflow
00011 -----*/
00012 
00013 
00014 
00015  #define P_MIN 0.0f
00016  #define P_MAX 6.283185f //359.9999 deg
00017  #define P_REPLY_MIN -21360*2*PI
00018  #define P_REPLY_MAX 21360*2*PI //rad
00019  //#define P_MIN -95.5f
00020  //#define P_MAX 95.5f
00021  #define V_MIN -45.0f
00022  #define V_MAX 45.0f
00023  #define KP_MIN 0.0f
00024  #define KP_MAX 500.0f
00025  #define KI_MIN 0.0f
00026  #define KI_MAX 10.0f
00027  #define KD_MIN 0.0f
00028  #define KD_MAX 5.0f
00029  #define T_MIN -40.0f
00030  #define T_MAX 40.0f
00031  
00032 
00033 /// CAN Reply Packet Structure ///
00034 /// 16 bit position, between -4*pi and 4*pi
00035 /// 12 bit velocity, between -30 and + 30 rad/s
00036 /// 12 bit current, between -40 and 40;
00037 /// CAN Packet is 5 8-bit words
00038 /// Formatted as follows.  For each quantity, bit 0 is LSB
00039 /// 0: [position[15-8]]
00040 /// 1: [position[7-0]] 
00041 /// 2: [velocity[11-4]]
00042 /// 3: [velocity[3-0], current[11-8]]
00043 /// 4: [current[7-0]]
00044 void pack_reply(CANMessage *msg, float p, float v, float t){
00045     int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 28);
00046     //int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
00047     int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
00048     int t_int = float_to_uint(t, T_MIN, T_MAX, 12);
00049     
00050     msg->data[0] = CAN_ID;
00051     msg->data[1] = p_int >> 20;
00052     msg->data[2] = (p_int>>12)&0xFF;
00053     msg->data[3] = (p_int>>4)&0xFF;
00054     msg->data[4] = (((p_int)&0xF)<<4)+(t_int>>8);
00055     msg->data[5] = (t_int&0xFF);
00056     
00057     /*
00058     for(int i = 1 ; i < 6 ; i++)
00059         printf("%x  ",msg->data[i]);
00060     printf("\n\r");
00061     */
00062 
00063     /*
00064     msg->data[1] = p_int>>24;
00065     msg->data[2] = (p_int>>16)&0xFF;
00066     msg->data[3] = (p_int<<8)&0xFF;
00067     msg->data[4] = p_int&0xFF;
00068     msg->data[5] = v_int>>4;
00069     msg->data[6] = ((v_int&0xF)<<4) + (t_int>>8);
00070     msg->data[7] = t_int&0xFF;
00071     */
00072     /*
00073     msg->data[1] = p_int>>8;
00074     msg->data[2] = p_int&0xFF;
00075     msg->data[3] = v_int>>4;
00076     msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
00077     msg->data[5] = t_int&0xFF;
00078     */
00079     }
00080     
00081 /// CAN Command Packet Structure ///
00082 /// 16 bit position command, between -4*pi and 4*pi
00083 /// 12 bit velocity command, between -30 and + 30 rad/s
00084 /// 12 bit kp, between 0 and 500 N-m/rad
00085 /// 12 bit kd, between 0 and 100 N-m*s/rad
00086 /// 12 bit feed forward torque, between -18 and 18 N-m
00087 /// CAN Packet is 8 8-bit words
00088 /// Formatted as follows.  For each quantity, bit 0 is LSB
00089 /// 0: [position[15-8]]
00090 /// 1: [position[7-0]] 
00091 /// 2: [velocity[11-4]]
00092 /// 3: [velocity[3-0], kp[11-8]]
00093 /// 4: [kp[7-0]]
00094 /// 5: [kd[11-4]]
00095 /// 6: [kd[3-0], torque[11-8]]
00096 /// 7: [torque[7-0]]
00097 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
00098         int p_int = (msg.data[0]<<8)|msg.data[1];
00099         //int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
00100         //int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
00101         int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
00102         int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; 
00103         int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
00104         int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
00105         
00106         controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
00107         //controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
00108         controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
00109         controller->ki = uint_to_float(ki_int, KI_MIN, KI_MAX, 12);
00110         controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
00111         controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
00112     //printf("Received   ");
00113     //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->kp, controller->ki, controller->kd, controller->t_ff, controller->i_q_ref);
00114     //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
00115     //printf("\n\r");
00116 }
00117