MotorModule.cpp

00001 
00002 #include "MotorModule.h"
00003 
00004 
00005 /// CAN Command Packet Structure ///
00006 /// 16 bit position command, between -4*pi and 4*pi
00007 /// 12 bit velocity command, between -30 and + 30 rad/s
00008 /// 12 bit kp, between 0 and 500 N-m/rad
00009 /// 12 bit kd, between 0 and 100 N-m*s/rad
00010 /// 12 bit feed forward torque, between -18 and 18 N-m
00011 /// CAN Packet is 8 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], kp[11-8]]
00017 /// 4: [kp[7-0]]
00018 /// 5: [kd[11-4]]
00019 /// 6: [kd[3-0], torque[11-8]]
00020 /// 7: [torque[7-0]]
00021 
00022 void pack_cmd(MotorStruct * motor){
00023      /// limit data to be within bounds ///
00024      motor->control.p_des = fminf(fmaxf(P_MIN, motor->control.p_des), P_MAX);                    
00025      motor->control.v_des = fminf(fmaxf(V_MIN, motor->control.v_des), V_MAX);
00026      motor->control.kp = fminf(fmaxf(KP_MIN, motor->control.kp), KP_MAX);
00027      motor->control.kd = fminf(fmaxf(KD_MIN, motor->control.kd), KD_MAX);
00028      motor->control.i_ff = fminf(fmaxf(I_MIN, motor->control.i_ff), I_MAX);
00029      /// convert floats to unsigned ints ///
00030      int p_int = float_to_uint(motor->control.p_des, P_MIN, P_MAX, 16);            
00031      int v_int = float_to_uint(motor->control.v_des, V_MIN, V_MAX, 12);
00032      int kp_int = float_to_uint(motor->control.kp, KP_MIN, KP_MAX, 12);
00033      int kd_int = float_to_uint(motor->control.kd, KD_MIN, KD_MAX, 12);
00034      int t_int = float_to_uint(motor->control.i_ff, I_MIN, I_MAX, 12);
00035      /// pack ints into the can buffer ///
00036      motor->txMsg.data[0] = p_int>>8;                                       
00037      motor->txMsg.data[1] = p_int&0xFF;
00038      motor->txMsg.data[2] = v_int>>4;
00039      motor->txMsg.data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
00040      motor->txMsg.data[4] = kp_int&0xFF;
00041      motor->txMsg.data[5] = kd_int>>4;
00042      motor->txMsg.data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
00043      motor->txMsg.data[7] = t_int&0xff;
00044      }
00045      
00046 /// CAN Reply Packet Structure ///
00047 /// 16 bit position, between -4*pi and 4*pi
00048 /// 12 bit velocity, between -30 and + 30 rad/s
00049 /// 12 bit current, between -40 and 40;
00050 /// CAN Packet is 5 8-bit words
00051 /// Formatted as follows.  For each quantity, bit 0 is LSB
00052 /// 0: [position[15-8]]
00053 /// 1: [position[7-0]] 
00054 /// 2: [velocity[11-4]]
00055 /// 3: [velocity[3-0], current[11-8]]
00056 /// 4: [current[7-0]]
00057 
00058 void unpack_reply(MotorStruct * motor){
00059     /// unpack ints from can buffer ///
00060     int id = motor->rxMsg.data[0];
00061     int p_int = (motor->rxMsg.data[1]<<8)|motor->rxMsg.data[2];
00062     int v_int = (motor->rxMsg.data[3]<<4)|(motor->rxMsg.data[4]>>4);
00063     int i_int = ((motor->rxMsg.data[4]&0xF)<<8)|motor->rxMsg.data[5];
00064     /// convert unsigned ints to floats ///
00065     float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
00066     float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
00067     float i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
00068     
00069     motor->state.position = p;
00070     motor->state.velocity = v;
00071     motor->state.current = i;  
00072     } 
00073     
00074 
00075 void enable_motor(MotorStruct * motor, CAN * can)
00076 {
00077     motor->txMsg.data[0] = 0xFF;
00078     motor->txMsg.data[1] = 0xFF;
00079     motor->txMsg.data[2] = 0xFF;
00080     motor->txMsg.data[3] = 0xFF;
00081     motor->txMsg.data[4] = 0xFF;
00082     motor->txMsg.data[5] = 0xFF;
00083     motor->txMsg.data[6] = 0xFF;
00084     motor->txMsg.data[7] = 0xFC;  
00085     can->write(motor->txMsg); 
00086 }
00087 void disable_motor(MotorStruct * motor, CAN * can)
00088 {
00089     motor->txMsg.data[0] = 0xFF;
00090     motor->txMsg.data[1] = 0xFF;
00091     motor->txMsg.data[2] = 0xFF;
00092     motor->txMsg.data[3] = 0xFF;
00093     motor->txMsg.data[4] = 0xFF;
00094     motor->txMsg.data[5] = 0xFF;
00095     motor->txMsg.data[6] = 0xFF;
00096     motor->txMsg.data[7] = 0xFD;  
00097     can->write(motor->txMsg); 
00098 }
00099 
00100 void zero_motor(MotorStruct * motor, CAN * can)
00101 {
00102     motor->txMsg.data[0] = 0xFF;
00103     motor->txMsg.data[1] = 0xFF;
00104     motor->txMsg.data[2] = 0xFF;
00105     motor->txMsg.data[3] = 0xFF;
00106     motor->txMsg.data[4] = 0xFF;
00107     motor->txMsg.data[5] = 0xFF;
00108     motor->txMsg.data[6] = 0xFF;
00109     motor->txMsg.data[7] = 0xFE;  
00110     can->write(motor->txMsg); 
00111 }