Revision 26:2b865c00d7e9, committed 2017-05-01

Comitter:

benkatz

Date:

Mon May 01 15:22:58 2017 +0000

Parent:

25:f5741040c4bb

Child:

27:501fee691e0e

Commit message:

- Added CAN Send/Receive; - Updated to most recent MBED (Broke SPI 16-bit read/write. Hacked to work with 2 8-bit writes now); - Removed most sine/cos calculations; - Fixed sign error in autocalibration routine

Changed in this revision

--- a/CANnucleo.lib	Sun Apr 09 03:05:52 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/hudakz/code/CANnucleo/#353237492903

--- a/Calibration/calibration.cpp	Sun Apr 09 03:05:52 2017 +0000
+++ b/Calibration/calibration.cpp	Mon May 01 15:22:58 2017 +0000
@@ -31,7 +31,8 @@
     //ps->ZeroPosition();
     ps->Sample(); 
     wait_us(1000);
-    float theta_start = ps->GetMechPosition();                                  //get initial rotor position
+    //float theta_start = ps->GetMechPosition();                                  //get initial rotor position
+    float theta_start;
     controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
     controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
     controller->i_a = -controller->i_b - controller->i_c;
@@ -49,6 +50,7 @@
         TIM1->CCR1 = 0x708*(1.0f-dtc_w);
        ps->Sample();                                                            //sample position sensor
        theta_actual = ps->GetMechPosition();
+       if(theta_ref==0){theta_start = theta_actual;}
        if(sample_counter > 200){
            sample_counter = 0 ;
         printf("%.4f   %.4f\n\r", theta_ref/(NPP), theta_actual);
@@ -107,6 +109,7 @@
     controller->i_a = -controller->i_b - controller->i_c;
     dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
     float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
+    printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
     for(int i = 0; i<n; i++){                                                   // rotate forwards
        for(int j = 0; j<n2; j++){   
         theta_ref += delta;
@@ -131,7 +134,7 @@
         printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
        //theta_ref += delta;
         }
-    printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
+    
     for(int i = 0; i<n; i++){                                                   // rotate backwards
        for(int j = 0; j<n2; j++){
        theta_ref -= delta;
@@ -208,6 +211,7 @@
                 }
             lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
             printf("%d   %d   %d   %f\n\r", i, ind, lut[ind],  cogging_current[i]);
+            wait(.001);
             }
             
         ps->WriteLUT(lut);                                                      // write lookup table to position sensor object

--- a/Config/current_controller_config.h	Sun Apr 09 03:05:52 2017 +0000
+++ b/Config/current_controller_config.h	Mon May 01 15:22:58 2017 +0000
@@ -6,11 +6,13 @@
 #define K_SCALE 0.0001f            // K_loop/Loop BW (Hz)
 #define KI_D 0.06f                  // 1/samples
 #define KI_Q 0.06f                  // 1/samples
-#define V_BUS 24.0f                 // Volts
+#define V_BUS 17.0f                 // Volts
 
 #define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples
 #define Q_INT_LIM V_BUS/(K_Q*KI_Q)  // Amps*samples
 
+#define I_MAX 40.0f
+
 
 
 #endif

--- a/Config/motor_config.h	Sun Apr 09 03:05:52 2017 +0000
+++ b/Config/motor_config.h	Mon May 01 15:22:58 2017 +0000
@@ -6,6 +6,8 @@
 #define L_Q 0.00003f            //Henries
 #define KT .07f                 //N-m per peak phase amp (= RMS amps/1.5)
 #define NPP 21                  //Number of pole pairs
+#define GR 6.0f                 //Gear ratio
+#define KT_OUT 0.45f            //Effective output torque constatnt
 
 #define WB KT/NPP               //Webers.  
 

--- a/Config/user_config.h	Sun Apr 09 03:05:52 2017 +0000
+++ b/Config/user_config.h	Mon May 01 15:22:58 2017 +0000
@@ -8,9 +8,12 @@
 #define M_OFFSET                __float_reg[1]                                  // Encoder mechanical offset
 #define I_BW                    __float_reg[2]                                  // Current loop bandwidth
 #define TORQUE_LIMIT            __float_reg[3]                                  // Torque limit (current limit = torque_limit/(kt*gear ratio))
+#define THETA_MIN               __float_reg[4]                                  // Minimum position setpoint
+#define THETA_MAX               __float_reg[5]                                  // Maximum position setpoint
 
 #define PHASE_ORDER             __int_reg[0]                                    // Phase swapping during calibration
 #define CAN_ID                  __int_reg[1]                                    // CAN bus ID
+#define CAN_MASTER              __int_reg[2]                                    // CAN bus "master" ID
 #define ENCODER_LUT             __int_reg[4]                                    // Encoder offset LUT - 128 elements long
 
 extern float __float_reg[];

--- a/FOC/foc.cpp	Sun Apr 09 03:05:52 2017 +0000
+++ b/FOC/foc.cpp	Mon May 01 15:22:58 2017 +0000
@@ -1,8 +1,8 @@
 #include "user_config.h"
 #include "foc.h"
 
-//#include "FastMath.h"
-//using namespace FastMath;
+#include "FastMath.h"
+using namespace FastMath;
 
 
 void abc( float theta, float d, float q, float *a, float *b, float *c){
@@ -10,18 +10,22 @@
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
 
-    *a = d*cosf(-theta) + q*sinf(-theta);
-    *b = d*cosf((2.0f*PI/3.0f)-theta) + q*sinf((2.0f*PI/3.0f)-theta);
-    *c =  d*cosf((-2.0f*PI/3.0f)-theta) + q*sinf((-2.0f*PI/3.0f)-theta);
+    *a = d*cosf(theta) - q*sinf(theta);
+    *b = d*cosf(theta - (2.0f*PI/3.0f)) - q*sinf(theta - (2.0f*PI/3.0f));
+    *c =  d*cosf(theta + (2.0f*PI/3.0f)) - q*sinf(theta +(2.0f*PI/3.0f));
     }
     
+    
 void dq0(float theta, float a, float b, float c, float *d, float *q){
     /// DQ0 Transform ///
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
     
-    *d = (2.0f/3.0f)*(a*cosf(-theta) + b*cosf((2.0f*PI/3.0f)-theta) + c*cosf((-2.0f*PI/3.0f)-theta));
-    *q = (2.0f/3.0f)*(a*sinf(-theta) + b*sinf((2.0f*PI/3.0f)-theta) + c*sinf((-2.0f*PI/3.0f)-theta));
+    //float cos = cosf(theta);
+    //float sin = sinf(theta);
+    
+    *d = (2.0f/3.0f)*(a*cosf(theta) + b*cosf(theta - (2.0f*PI/3.0f)) + c*cosf(theta + (2.0f*PI/3.0f)));
+    *q = (2.0f/3.0f)*(-a*sinf(theta) - b*sinf(theta - (2.0f*PI/3.0f)) - c*sinf(theta + (2.0f*PI/3.0f)));
     }
     
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
@@ -31,7 +35,7 @@
     float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
     *dtc_u = fminf(fmaxf(((u - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
     *dtc_v = fminf(fmaxf(((v - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
-    *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+    *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);  
     
     }
 
@@ -40,7 +44,10 @@
     int adc2_offset = 0;
     int n = 1024;
     for (int i = 0; i<n; i++){                                                  // Average n samples of the ADC
-        ADC1->CR2  |= 0x40000000; 
+        TIM1->CCR3 = 0x708*(1.0f);                                               // Write duty cycles
+        TIM1->CCR2 = 0x708*(1.0f);
+        TIM1->CCR1 = 0x708*(1.0f);
+        ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
         wait(.001);
         adc2_offset += ADC2->DR;
         adc1_offset += ADC1->DR;
@@ -57,6 +64,7 @@
 
 void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
        /// Commutation Loop ///
+       
        controller->loop_count ++;   
        if(PHASE_ORDER){                                                                          // Check current sensor ordering
            controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
@@ -66,19 +74,23 @@
             controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
            controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
            }
-       controller->i_a = -controller->i_b - controller->i_c;                                   
-       dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
+       controller->i_a = -controller->i_b - controller->i_c;       
        
+       float s = FastSin(theta); 
+       float c = FastCos(theta);                            
+       //dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
+       controller->i_d = 0.6666667f*(c*controller->i_a + (0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c);   ///Fast DQ0 Transform
+       controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - (0.86602540378f*c-.5f*s)*controller->i_c);
        ///Cogging compensation lookup, doesn't actually work yet///
        //int ind = theta * (128.0f/(2.0f*PI));
        //float cogging_current = controller->cogging[ind];
-       //float cogging_current = 1.0f*cos(6*theta);
+       float cogging_current = 0.05f*s*controller->i_q_ref;
        
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
        float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current;
        float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE);   //feed-forward voltage
-       float v_q_ff = 2.0f*(2*controller->i_q_ref*R_PHASE + controller->dtheta_elec*WB*0.8165f);
+       float v_q_ff =  controller->dtheta_elec*WB*1.73205081;
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
        
@@ -86,10 +98,6 @@
        //v_q_ff = 0;
        
        limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_Q*KI_Q));        // Limit integrators to prevent windup
-       //controller->d_int = fminf(fmaxf(controller->d_int, -D_INT_LIM), D_INT_LIM);
-       //controller->q_int = fminf(fmaxf(controller->q_int, -Q_INT_LIM), Q_INT_LIM);
-       
-       
        controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*controller->d_int;// + v_d_ff;  
        controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*controller->q_int;// + v_q_ff; 
        
@@ -97,7 +105,11 @@
        //controller->v_q = v_q_ff; 
        
        limit_norm(&controller->v_d, &controller->v_q, controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
-       abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
+       //abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
+    
+        controller->v_u = c*controller->v_d - s*controller->v_q;
+        controller->v_v = (0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
+        controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
 
        
@@ -115,17 +127,27 @@
        controller->theta_elec = theta;                                          //For some reason putting this at the front breaks thins
        
 
-       //if(controller->loop_count >400){
+       if(controller->loop_count >400){
            //controller->i_q_ref = -controller->i_q_ref;
-          // controller->loop_count  = 0;
+          controller->loop_count  = 0;
            
-           //printf("%d   %f\n\r", ind, cogging_current);
+           //printf("%.1f  %.1f  %.1f  %.1f  %.1f\n\r", controller->i_d, controller->i_q, controller->v_d, controller->v_q, controller->dtheta_mech);
            //printf("%f\n\r", controller->theta_elec);
            //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
            //pc.printf("%f    %f\n\r", controller->i_d, controller->i_q);
            //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
-        //    }
+            }
     }
+    
+    
+void torque_control(ControllerStruct *controller){
+    float torque_ref = -controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff;// + controller->kd*(controller->v_des - GR*controller->dtheta_mech);
+    //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
+    controller->i_q_ref = torque_ref/KT_OUT;    
+    controller->i_d_ref = 0;
+    }
+
+
 /*    
 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
     

--- a/FOC/foc.h	Sun Apr 09 03:05:52 2017 +0000
+++ b/FOC/foc.h	Mon May 01 15:22:58 2017 +0000
@@ -16,4 +16,5 @@
 void zero_current(int *offset_1, int *offset_2);
 void reset_foc(ControllerStruct *controller);
 void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta);
+void torque_control(ControllerStruct *controller);
 #endif

--- a/PositionSensor/PositionSensor.cpp	Sun Apr 09 03:05:52 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Mon May 01 15:22:58 2017 +0000
@@ -11,8 +11,8 @@
     ElecOffset = offset;
     rotations = 0;
     spi = new SPI(PC_12, PC_11, PC_10);
-    spi->format(16, 1);
-    spi->frequency(5000000);
+    spi->format(8, 1);                                                          // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
+    spi->frequency(25000000);
     cs = new DigitalOut(PA_15);
     cs->write(1);
     readAngleCmd = 0xffff;   
@@ -25,7 +25,10 @@
     
 void PositionSensorAM5147::Sample(){
     cs->write(0);
-    raw = spi->write(readAngleCmd);
+    int raw1 = spi->write(0xFF);
+    int raw2 = spi->write(0xFF);
+    raw = (raw1<<8)|raw2;
+    //raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;                                                              //Extract last 14 bits
     cs->write(1);
     int off_1 = offset_lut[raw>>7];
@@ -44,7 +47,7 @@
     modPosition = ((6.28318530718f * ((float) angle))/ (float)_CPR);
     position = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
     MechPosition = position - MechOffset;
-    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) - ElecOffset;
+    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
     if(elec < 0) elec += 6.28318530718f;
     else if(elec > 6.28318530718f) elec -= 6.28318530718f ; 
     ElecPosition = elec;

--- a/main.cpp	Sun Apr 09 03:05:52 2017 +0000
+++ b/main.cpp	Mon May 01 15:22:58 2017 +0000
@@ -6,21 +6,28 @@
 
 #define REST_MODE 0
 #define CALIBRATION_MODE 1
-#define TORQUE_MODE 2
-#define PD_MODE 3
+#define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
+#define P_MASK
+#define D_MASK
+#define KP_MASK
+#define KD_MASK
+#define TFF_MASK
 
+/*
 const unsigned int BOARDNUM = 0x2;
 //const unsigned int a_id =                            
-const unsigned int TX_ID = 0x0100;
 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
+*/
+const unsigned int TX_ID = 0x01;
+
 
-float __float_reg[64];
-int __int_reg[256];
+float __float_reg[64];                                                          // Floats stored in flash
+int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
-#include "CANnucleo.h"
+
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -36,6 +43,7 @@
 #include "user_config.h"
 #include "PreferenceWriter.h"
 
+ 
 PreferenceWriter prefs(6);
 
 GPIOStruct gpio;
@@ -44,70 +52,112 @@
 VelocityEstimatorStruct velocity;
 
 
-CANnucleo::CAN          can(PB_8, PB_9);                                        // CAN Rx pin name, CAN Tx pin name
-CANnucleo::CANMessage   rxMsg;
-CANnucleo::CANMessage   txMsg;
-int                     ledState;
-int                     counter = 0;
-int canCmd = 1000;
-volatile bool           msgAvailable = false;
-
-DigitalOut toggle(PA_0);
-Ticker  loop;
-/**
- * @brief   'CAN receive-complete' interrup handler.
- * @note    Called on arrival of new CAN message.
- *          Keep it as short as possible.
- * @param   
- * @retval  
- */
-void onMsgReceived() {
-    msgAvailable = true;
-    //printf("ping\n\r");
-}
+//using namespace CANnucleo;
 
-void sendCMD(int TX_addr, int val){
-    txMsg.clear();      //clear Tx message storage
-    txMsg.id = TX_addr;
-    txMsg << val;
-    can.write(txMsg);
-    //wait(.1);
-    
-    }
-    
-void readCAN(void){
-    if(msgAvailable) { 
-    msgAvailable = false;                                                       // reset flag for next use
-    can.read(rxMsg);                                                            // read message into Rx message storage
-    // Filtering performed by software:           
-    if(rxMsg.id == cmd_ID) {                                                    // See comments in CAN.cpp for filtering performed by hardware
-            rxMsg >> canCmd;                                                    // extract first data item
-            }                                                                   
-        }
-    }
-    
-void cancontroller(void){
-    //printf("%d\n\r", canCmd);
-    readCAN();
-    //sendCMD(TX_ID, canCmd);
+CAN          can(PB_8, PB_9);                                        // CAN Rx pin name, CAN Tx pin name
+CANMessage   rxMsg;
+CANMessage   txMsg;
 
-    }
-    
 
 Serial pc(PA_2, PA_3);
 
-PositionSensorAM5147 spi(16384, 0.0, NPP);   
-PositionSensorEncoder encoder(4096, 0, 21); 
+PositionSensorAM5147 spi(16384, 0.0, NPP);  
+PositionSensorEncoder encoder(4096, 0, NPP); 
+
+
+DigitalOut toggle(PA_0);
 
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
 
+ #define P_MIN -12.5f
+ #define P_MAX 12.5f
+ #define V_MIN -30.0f
+ #define V_MAX 30.0f
+ #define KP_MIN 0.0f
+ #define KP_MAX 500.0f
+ #define KD_MIN 0.0f
+ #define KD_MAX 100.0f
+ #define T_MIN -18.0f
+ #define T_MAX 18.0f
+ 
+
+/// CAN Reply Packet Structure ///
+/// 16 bit position, between -4*pi and 4*pi
+/// 12 bit velocity, between -30 and + 30 rad/s
+/// 12 bit current, between -40 and 40;
+/// CAN Packet is 5 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], current[11-8]]
+/// 4: [current[7-0]]
+void pack_reply(CANMessage *msg, float p, float v, float i){
+    int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
+    int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
+    int i_int = float_to_uint(i, -I_MAX, I_MAX, 12);
+    msg->data[0] = p_int>>8;
+    msg->data[1] = p_int&0xFF;
+    msg->data[2] = v_int>>4;
+    msg->data[3] = ((v_int&0xF)<<4) + (i_int>>8);
+    msg->data[4] = i_int&0xFF;
+    }
+    
+/// CAN Command Packet Structure ///
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+void unpack_cmd(CANMessage msg, ControllerStruct * controller){
+    int p_int = (msg.data[0]<<8)|msg.data[1];
+    int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
+    int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
+    int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
+    int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
+    
+    controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
+    controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
+    controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
+    controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
+    controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
+    
+    /*
+    printf("Received   ");
+    printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
+    printf("\n\r");
+    */
+    
+    }
+
+void onMsgReceived() {
+    //msgAvailable = true;
+    //printf("%.3f   %.3f   %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q);
+    can.read(rxMsg);  
+    if((rxMsg.id == CAN_ID) && (state == MOTOR_MODE)){
+        unpack_cmd(rxMsg, &controller);
+        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q);
+        can.write(txMsg);
+    }
+    
+}
+
 void enter_menu_state(void){
     printf("\n\r\n\r\n\r");
     printf(" Commands:\n\r");
-    printf(" t - Torque Mode\n\r");
-    printf(" p - PD Mode\n\r");
+    printf(" m - Motor Mode\n\r");
     printf(" c - Calibrate Encoder\n\r");
     printf(" s - Setup\n\r");
     printf(" e - Display Encoder\n\r");
@@ -121,6 +171,7 @@
     printf(" %-7s %-25s %-5s %-5s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
     printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
     printf(" %-7s %-25s %-5s %-5s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+    printf(" %-7s %-25s %-5s %-5s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
     printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
     printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
     state_change = 0;
@@ -128,19 +179,19 @@
     
 void enter_torque_mode(void){
     controller.i_d_ref = 0;
-    controller.i_q_ref = 1;                                                     // Current Setpoints
+    controller.i_q_ref = 6;                                                     // Current Setpoints
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     gpio.enable->write(1);                                                      // Enable gate drive
-    GPIOC->ODR ^= (1 << 5);                                                     // Turn on status LED
+    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
     state_change = 0;
     }
     
 void calibrate(void){
     gpio.enable->write(1);                                                      // Enable gate drive
-    GPIOC->ODR ^= (1 << 5);                                                     // Turn on status LED
+    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
-    GPIOC->ODR ^= (1 << 5);                                                     // Turn off status LED
+    GPIOC->ODR &= !(1 << 5);                                                     // Turn off status LED
     wait(.2);
     gpio.enable->write(0);                                                      // Turn off gate drive
     printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
@@ -184,24 +235,31 @@
                     }
                 break;
              
-            case TORQUE_MODE:                                                   // Run torque control
+            case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
                     enter_torque_mode();
                     }
                 count++;
-                controller.theta_elec = spi.GetElecPosition();                  
+                toggle.write(1);
+                controller.theta_elec = spi.GetElecPosition();
+                controller.theta_mech = spi.GetMechPosition();
+                controller.dtheta_mech = spi.GetMechVelocity();  
+                //TIM1->CCR3 = 0x708*(1.0f);
+                //TIM1->CCR1 = 0x708*(1.0f);
+                //TIM1->CCR2 = 0x708*(1.0f);     
+                
+                //controller.i_q_ref = controller.t_ff/KT_OUT;   
+                //torque_control(&controller);      
+                controller.i_q_ref = 1; 
                 commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
                 spi.Sample();                                                   // Sample position sensor
+                toggle.write(0);
+                
                 if(count > 100){
                      count = 0;
-                     //readCAN();
-                     //controller.i_q_ref = ((float)(canCmd-1000))/100;
-                    //pc.printf("%f\n\r ", controller.theta_elec);
+                     //printf("%d  %d\n\r", controller.adc1_raw, controller.adc2_raw);
                      }
                 break;
-            
-            case PD_MODE:
-                break;
             case SETUP_MODE:
                 if(state_change){
                     enter_setup_state();
@@ -211,9 +269,7 @@
                 print_encoder();
                 break;
                 }
-                
-   
-            
+                 
       }
   TIM1->SR = 0x0;                                                               // reset the status register
 }
@@ -233,6 +289,7 @@
                 state_change = 1;
                 char_count = 0;
                 cmd_id = 0;
+                GPIOC->ODR &= !(1 << 5); 
                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                 }
         if(state == REST_MODE){
@@ -241,8 +298,8 @@
                     state = CALIBRATION_MODE;
                     state_change = 1;
                     break;
-                case 't':
-                    state = TORQUE_MODE;
+                case 'm':
+                    state = MOTOR_MODE;
                     state_change = 1;
                     break;
                 case 'e':
@@ -264,6 +321,9 @@
                     case 'i':
                         CAN_ID = atoi(cmd_val);
                         break;
+                    case 'm':
+                        CAN_MASTER = atoi(cmd_val);
+                        break;
                     case 'l':
                         TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
                         break;
@@ -310,24 +370,27 @@
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
 
     wait(.1);
-    //TIM1->CR1 |= TIM_CR1_UDIS;
-    gpio.enable->write(1);                                                      // Enable gate drive
-    gpio.pwm_u->write(1.0f);                                                    // Write duty cycles
-    gpio.pwm_v->write(1.0f);
-    gpio.pwm_w->write(1.0f);
+    gpio.enable->write(1);
+    TIM1->CCR3 = 0x708*(1.0f);                        // Write duty cycles
+    TIM1->CCR2 = 0x708*(1.0f);
+    TIM1->CCR1 = 0x708*(1.0f);
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
-    //gpio.enable->write(0);
+    gpio.enable->write(0);
     reset_foc(&controller);                                                     // Reset current controller
-    
-    TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
+    TIM1->CR1 ^= TIM_CR1_UDIS;
+    //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
     wait(.1);
     NVIC_SetPriority(TIM5_IRQn, 2);                                             // set interrupt priority
 
+
     can.frequency(1000000);                                                     // set bit rate to 1Mbps
-    can.attach(&onMsgReceived);                                                 // attach 'CAN receive-complete' interrupt handler
-    can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
-    
+    can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    //can.filter(CAN_ID, 0xF, CANStandard, 0);
+    can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler                                                                    
+    txMsg.id = TX_ID;
+    txMsg.len = 5;
+    rxMsg.len = 8;
     
     prefs.load();                                                               // Read flash
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
@@ -335,7 +398,7 @@
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     
-    pc.baud(115200);                                                            // set serial baud rate
+    pc.baud(921600);                                                            // set serial baud rate
     wait(.01);
     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
     wait(.01);

--- a/math_ops.cpp	Sun Apr 09 03:05:52 2017 +0000
+++ b/math_ops.cpp	Mon May 01 15:22:58 2017 +0000
@@ -30,3 +30,15 @@
         *y = *y * limit/norm;
         }
     }
+
+int float_to_uint(float x, float x_min, float x_max, int bits){
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (int) ((x+offset)*((float)((1<<bits)-1))/span);
+    }
+    
+float uint_to_float(int x_int, float x_min, float x_max, int bits){
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+    }

--- a/math_ops.h	Sun Apr 09 03:05:52 2017 +0000
+++ b/math_ops.h	Mon May 01 15:22:58 2017 +0000
@@ -10,5 +10,7 @@
 float fmaxf3(float x, float y, float z);
 float fminf3(float x, float y, float z);
 void limit_norm(float *x, float *y, float limit);
+int float_to_uint(float x, float x_min, float x_max, int bits);
+float uint_to_float(int x_int, float x_min, float x_max, int bits);
 
 #endif

--- a/mbed.bld	Sun Apr 09 03:05:52 2017 +0000
+++ b/mbed.bld	Mon May 01 15:22:58 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file
+https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10
\ No newline at end of file

--- a/structs.h	Sun Apr 09 03:05:52 2017 +0000
+++ b/structs.h	Mon May 01 15:22:58 2017 +0000
@@ -30,6 +30,7 @@
     float i_d_ref, i_q_ref;
     int loop_count;
     int mode;
+    float p_des, v_des, kp, kd, t_ff;
     float cogging[128];
     } ControllerStruct;