CAN_com.cpp

00001 #include "CAN_com.h"
00002 
00003 
00004 
00005  
00006 
00007 /// CAN Reply Packet Structure ///
00008 /// 16 bit position, between -4*pi and 4*pi
00009 /// 12 bit velocity, between -30 and + 30 rad/s
00010 /// 12 bit current, between -40 and 40;
00011 /// CAN Packet is 5 8-bit words
00012 /// Formatted as follows.  For each quantity, bit 0 is LSB
00013 /// 0: [position[15-8]]
00014 /// 1: [position[7-0]] 
00015 /// 2: [velocity[11-4]]
00016 /// 3: [velocity[3-0], current[11-8]]
00017 /// 4: [current[7-0]]
00018 void pack_reply(CANMessage *msg, float p, float v, float t){
00019     int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
00020     int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
00021     int t_int = float_to_uint(t, -T_MAX, T_MAX, 12);
00022     msg->data[0] = CAN_ID;
00023     msg->data[1] = p_int>>8;
00024     msg->data[2] = p_int&0xFF;
00025     msg->data[3] = v_int>>4;
00026     msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
00027     msg->data[5] = t_int&0xFF;
00028     }
00029     
00030 /// CAN Command Packet Structure ///
00031 /// 16 bit position command, between -4*pi and 4*pi
00032 /// 12 bit velocity command, between -30 and + 30 rad/s
00033 /// 12 bit kp, between 0 and 500 N-m/rad
00034 /// 12 bit kd, between 0 and 100 N-m*s/rad
00035 /// 12 bit feed forward torque, between -18 and 18 N-m
00036 /// CAN Packet is 8 8-bit words
00037 /// Formatted as follows.  For each quantity, bit 0 is LSB
00038 /// 0: [position[15-8]]
00039 /// 1: [position[7-0]] 
00040 /// 2: [velocity[11-4]]
00041 /// 3: [velocity[3-0], kp[11-8]]
00042 /// 4: [kp[7-0]]
00043 /// 5: [kd[11-4]]
00044 /// 6: [kd[3-0], torque[11-8]]
00045 /// 7: [torque[7-0]]
00046 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
00047         int p_int = (msg.data[0]<<8)|msg.data[1];
00048         int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
00049         int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
00050         int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
00051         int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
00052         
00053         controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
00054         controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
00055         controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
00056         controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
00057         controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
00058     //printf("Received   ");
00059     //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
00060     //printf("\n\r");
00061     }
00062