Revision 37:c0f352d6e8e3, committed 2018-04-13

Comitter:

benkatz

Date:

Fri Apr 13 13:50:54 2018 +0000

Parent:

36:d88fd41f60a6

Child:

38:67e4e1453a4b

Commit message:

always samples position sensor

Changed in this revision

--- a/Config/current_controller_config.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/Config/current_controller_config.h	Fri Apr 13 13:50:54 2018 +0000
@@ -1,6 +1,7 @@
 #ifndef CURRENT_CONTROLLER_CONFIG_H
 #define CURRENT_CONTROLLER_CONFIG_H
 
+// Current controller///
 #define K_D .05f                     // Volts/Amp
 #define K_Q .05f                     // Volts/Amp
 #define K_SCALE 0.0001f            // K_loop/Loop BW (Hz) 0.0042
@@ -13,6 +14,11 @@
 
 #define I_MAX 40.0f
 
+//Observer//
+#define DT 0.000025f
+#define K_O 0.02f
+
+
 
 
 #endif

--- a/Config/hw_config.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/Config/hw_config.h	Fri Apr 13 13:50:54 2018 +0000
@@ -5,10 +5,13 @@
 #define PIN_V PA_9
 #define PIN_W PA_8
 #define ENABLE_PIN PA_11        // Enable gate drive pin
+#define LED         PC_5        // LED Pin
 #define I_SCALE 0.02014160156f  // Amps per A/D Count
+#define V_SCALE 0.00884f        // Bus volts per A/D Count
 #define DTC_MAX 0.95f          // Max phase duty cycle
 #define DTC_MIN 0.05f          // Min phase duty cycle
 #define PWM_ARR 0x8CA           /// timer autoreload value
 
 
+
 #endif

--- a/Config/motor_config.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/Config/motor_config.h	Fri Apr 13 13:50:54 2018 +0000
@@ -4,12 +4,11 @@
 #define R_PHASE 0.105f          //Ohms
 #define L_D 0.00003f            //Henries
 #define L_Q 0.00003f            //Henries
-#define KT .075f                 //N-m per peak phase amp (= RMS amps/1.5)
+#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
 #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.  
+#define KT_OUT 0.45f            //KT*GR
+#define WB 0.0024f               //Webers.  
 
 
 

--- a/Config/user_config.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/Config/user_config.h	Fri Apr 13 13:50:54 2018 +0000
@@ -16,7 +16,8 @@
 #define CAN_ID                  __int_reg[1]                                    // CAN bus ID
 #define CAN_MASTER              __int_reg[2]                                    // CAN bus "master" ID
 #define CAN_TIMEOUT             __int_reg[3]                                    // CAN bus timeout period
-#define ENCODER_LUT             __int_reg[4]                                    // Encoder offset LUT - 128 elements long
+#define ENCODER_LUT             __int_reg[5]                                    // Encoder offset LUT - 128 elements long
+
 
 
 extern float __float_reg[];

--- a/FOC/foc.cpp	Fri Mar 02 15:24:00 2018 +0000
+++ b/FOC/foc.cpp	Fri Apr 13 13:50:54 2018 +0000
@@ -1,8 +1,5 @@
-#include "user_config.h"
-#include "hw_config.h"
+
 #include "foc.h"
-
-#include "FastMath.h"
 using namespace FastMath;
 
 
@@ -10,10 +7,12 @@
     /// Inverse DQ0 Transform ///
     ///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(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));
+    float cf = FastCos(theta);
+    float sf = FastSin(theta);
+    
+    *a = cf*d - sf*q;                // Faster Inverse DQ0 transform
+    *b = (0.86602540378f*sf-.5f*cf)*d - (-0.86602540378f*cf-.5f*sf)*q;
+    *c = (-0.86602540378f*sf-.5f*cf)*d - (0.86602540378f*cf-.5f*sf)*q;
     }
     
     
@@ -22,11 +21,12 @@
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
     
-    //float cos = cosf(theta);
-    //float sin = sinf(theta);
+    float cf = FastCos(theta);
+    float sf = FastSin(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)));
+    *d = 0.6666667f*(cf*a + (0.86602540378f*sf-.5f*cf)*b + (-0.86602540378f*sf-.5f*cf)*c);   ///Faster DQ0 Transform
+    *q = 0.6666667f*(-sf*a - (-0.86602540378f*cf-.5f*sf)*b - (0.86602540378f*cf-.5f*sf)*c);
+       
     }
     
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
@@ -65,6 +65,7 @@
     controller->i_q_ref = 0;
     controller->i_d = 0;
     controller->i_q = 0;
+    controller->i_q_filt = 0;
     controller->q_int = 0;
     controller->d_int = 0;
     controller->v_q = 0;
@@ -72,9 +73,12 @@
     }
 
 
-void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
+void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
+       /// Observer Prediction ///
+       observer->i_d_est += DT*(observer->i_d_dot);
+       observer->i_q_est += DT*(observer->i_q_dot);
+       
        /// 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
@@ -91,15 +95,31 @@
        //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);   ///Faster 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);
-        controller->i_q_filt = .95f*controller->i_q_filt + .05f*controller->i_q;
-        float s_cog = sinf(12.0f*theta);
-       float cogging_current =-0.33f*s_cog + .25f*s;
+        
+        controller->i_q_filt = 0.95f*controller->i_q_filt + 0.05f*controller->i_q;
+        observer->i_d_m = controller->i_d;
+        observer->i_q_m = controller->i_q;
+        
+        observer->e_d = observer->i_d_m - observer->i_d_est;
+        observer->e_q = observer->i_q_m - observer->i_q_est;
+        observer->e_d_int += observer->e_d;
+        observer->e_q_int += observer->e_q;
+        
+        observer->i_d_est +=  K_O*observer->e_d + .001f*observer->e_d_int;
+        observer->i_q_est += K_O*observer->e_q + .001f*observer->e_q_int;
+        
+        
+        //float s_cog = sinf(12.0f*theta);
+        
+        //float cogging_current =-0.33f*s_cog + .25f*s;
        
        /// 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 =  controller->dtheta_elec*WB*1.73205081f;
+       float i_q_error = controller->i_q_ref - controller->i_q ;// + cogging_current;
+       
+       float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
+       float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
+       
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
        
@@ -116,7 +136,7 @@
        //controller->v_d = v_d_ff;
        //controller->v_q = v_q_ff; 
        
-       limit_norm(&controller->v_d, &controller->v_q, 1.2f*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
+       limit_norm(&controller->v_d, &controller->v_q, 1.0f*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
     
         controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
@@ -124,6 +144,8 @@
         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
 
+       observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
+       observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
        
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
@@ -156,7 +178,7 @@
     float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - 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;
+    controller->i_d_ref = 0.0f;
     }
 
 

--- a/FOC/foc.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/FOC/foc.h	Fri Apr 13 13:50:54 2018 +0000
@@ -9,12 +9,14 @@
 #include "math_ops.h"
 #include "motor_config.h"
 #include "current_controller_config.h"
+#include "FastMath.h"
+#include "user_config.h"
 
 void abc(float theta, float d, float q, float *a, float *b, float *c);
 void dq0(float theta, float a, float b, float c, float *d, float *q);
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w);
 void zero_current(int *offset_1, int *offset_2);
 void reset_foc(ControllerStruct *controller);
-void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta);
+void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta);
 void torque_control(ControllerStruct *controller);
 #endif

--- a/PositionSensor/PositionSensor.cpp	Fri Mar 02 15:24:00 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Fri Apr 13 13:50:54 2018 +0000
@@ -13,10 +13,11 @@
     spi = new SPI(PC_12, PC_11, PC_10);
     spi->format(16, 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;   
-    MechOffset = 0;
+    MechOffset = offset;
     modPosition = 0;
     oldModPosition = 0;
     oldVel = 0;
@@ -24,10 +25,10 @@
     }
     
 void PositionSensorAM5147::Sample(){
-    cs->write(0);
+    GPIOA->ODR &= ~(1 << 15);
     raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;                                                              //Extract last 14 bits
-    cs->write(1);
+    GPIOA->ODR |= (1 << 15);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];
     int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries
@@ -84,6 +85,10 @@
     return ElecPosition;
     }
 
+float PositionSensorAM5147::GetElecVelocity(){
+    return ElecVelocity;
+    }
+
 float PositionSensorAM5147::GetMechVelocity(){
     return MechVelocity;
     }
@@ -98,6 +103,9 @@
 void PositionSensorAM5147::SetElecOffset(float offset){
     ElecOffset = offset;
     }
+void PositionSensorAM5147::SetMechOffset(float offset){
+    MechOffset = offset;
+    }
 
 int PositionSensorAM5147::GetCPR(){
     return _CPR;
@@ -109,6 +117,7 @@
     }
     
 
+
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;
     _CPR = CPR;

--- a/PositionSensor/PositionSensor.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/PositionSensor/PositionSensor.h	Fri Apr 13 13:50:54 2018 +0000
@@ -47,9 +47,11 @@
     virtual float GetMechPosition();
     virtual float GetElecPosition();
     virtual float GetMechVelocity();
+    virtual float GetElecVelocity();
     virtual int GetRawPosition();
     virtual void ZeroPosition();
     virtual void SetElecOffset(float offset);
+    virtual void SetMechOffset(float offset);
     virtual int GetCPR(void);
     virtual void WriteLUT(int new_lut[128]);
 private:

--- a/hw_setup.cpp	Fri Mar 02 15:24:00 2018 +0000
+++ b/hw_setup.cpp	Fri Apr 13 13:50:54 2018 +0000
@@ -33,7 +33,6 @@
     //PWM Setup
 
     TIM1->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    //TIM1->ARR = 0x1194; // 20 khz
     TIM1->ARR = PWM_ARR;                                          // set auto reload, 40 khz
     TIM1->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM1
@@ -41,17 +40,29 @@
     }
 
 void Init_ADC(void){
-        // ADC Setup
+    // ADC Setup
+     RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC3
      RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
      RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC1
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
      
-     ADC->CCR = 0x00000006;                                     // Regular simultaneous mode only
+     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
+     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;                        // Enable clock for GPIOA
+    
+     ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
      ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-     ADC1->SQR3 = 0x000000A;                                    // use PC_0 as input
-     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-     ADC2->SQR3 = 0x0000000B;                                   // use PC_1 as input
+     ADC1->SQR3 = 0x000000A;                                    // use PC_0 as input- ADC1_IN0
+     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
+     ADC2->SQR3 = 0x0000000B;                                   // use PC_1 as input - ADC2_IN11
+     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
+     ADC3->SQR3 = 0x00000000;                                   // use PA_0, - ADC3_IN0
      GPIOC->MODER |= 0x0000000f;                                // Alternate function, PC_0, PC_1 are analog inputs 
+     GPIOA->MODER |= 0x3;                                       // PA_0 as analog input
+     
+     ADC1->SMPR1 |= 0x1;                                        // 15 cycles on CH_10, 0b 001
+     ADC2->SMPR1 |= 0x8;                                        // 15 cycles on CH_11, 0b 0001 000
+     ADC3->SMPR2 |= 0x1;                                        // 15 cycles on CH_0, 0b 001;
+
+
 
     }
 
@@ -64,6 +75,7 @@
 void Init_All_HW(GPIOStruct *gpio){
     Init_PWM(gpio);
     Init_ADC();
+    gpio->led = new DigitalOut(LED);
     //Init_DAC();
     
     }
\ No newline at end of file

--- a/main.cpp	Fri Mar 02 15:24:00 2018 +0000
+++ b/main.cpp	Fri Apr 13 13:50:54 2018 +0000
@@ -10,7 +10,7 @@
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
-#define VERSION_NUM "1.2"
+#define VERSION_NUM "1.4"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
@@ -31,14 +31,14 @@
 #include "user_config.h"
 #include "PreferenceWriter.h"
 
+
  
 PreferenceWriter prefs(6);
 
 GPIOStruct gpio;
 ControllerStruct controller;
 COMStruct com;
-VelocityEstimatorStruct velocity;
-
+ObserverStruct observer;
 
 //using namespace CANnucleo;
 
@@ -50,10 +50,10 @@
 Serial pc(PA_2, PA_3);
 
 PositionSensorAM5147 spi(16384, 0.0, NPP);  
-PositionSensorEncoder encoder(4096, 0, NPP); 
+//PositionSensorEncoder encoder(4096, 0, NPP); 
 
 
-DigitalOut toggle(PA_0);
+DigitalOut toggle(PC_8);
 
 volatile int count = 0;
 volatile int state = REST_MODE;
@@ -61,8 +61,8 @@
 
  #define P_MIN -12.5f
  #define P_MAX 12.5f
- #define V_MIN -30.0f
- #define V_MAX 30.0f
+ #define V_MIN -45.0f
+ #define V_MAX 45.0f
  #define KP_MIN 0.0f
  #define KP_MAX 500.0f
  #define KD_MIN 0.0f
@@ -122,20 +122,15 @@
         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)){
         controller.timeout = 0;
         if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){
@@ -145,16 +140,16 @@
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
             state = REST_MODE;
             state_change = 1;
-            GPIOC->ODR &= !(1 << 5); 
+            gpio.led->write(0);; 
             }
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
             spi.ZeroPosition();
             }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
-            pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
-            can.write(txMsg);
             }
+        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+        can.write(txMsg);
         }
     
 }
@@ -166,9 +161,11 @@
     printf(" c - Calibrate Encoder\n\r");
     printf(" s - Setup\n\r");
     printf(" e - Display Encoder\n\r");
+    printf(" z - Set Zero Position\n\r");
     printf(" esc - Exit to Menu\n\r");
     state_change = 0;
     gpio.enable->write(0);
+    gpio.led->write(0);
     }
 
 void enter_setup_state(void){
@@ -184,32 +181,30 @@
     }
     
 void enter_torque_mode(void){
+    controller.ovp_flag = 0;
     gpio.enable->write(1);                                                      // Enable gate drive
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     wait(.001);
     controller.i_d_ref = 0;
     controller.i_q_ref = 0;                                                     // Current Setpoints
-    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
+    gpio.led->write(1);                                                     // Turn on status LED
     state_change = 0;
     printf("\n\r Entering Motor Mode \n\r");
     }
     
 void calibrate(void){
     gpio.enable->write(1);                                                      // Enable gate drive
-    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
+    gpio.led->write(1);                                                    // 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
+    gpio.led->write(0);;                                                     // 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");
      state_change = 0;
-     
     }
     
 void print_encoder(void){
-    spi.Sample();
-    wait(.001);
     printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
     wait(.05);
     }
@@ -218,20 +213,25 @@
 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
   if (TIM1->SR & TIM_SR_UIF ) {
-        //toggle = 1;
 
         ///Sample current always ///
         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
         //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
-        controller.adc2_raw = ADC2->DR;                                         // Read ADC1 and ADC2 Data Registers
+        controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
+        controller.adc3_raw = ADC3->DR;
+        spi.Sample();                                                           // sample position sensor
+        controller.theta_elec = spi.GetElecPosition();
+        controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
+        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
+        controller.dtheta_elec = spi.GetElecVelocity();
+        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
         ///
         
         /// Check state machine state, and run the appropriate function ///
-        //printf("%d\n\r", state);
         switch(state){
-            case REST_MODE:                                                     // Do nothing until
+            case REST_MODE:                                                     // Do nothing
                 if(state_change){
                     enter_menu_state();
                     }
@@ -249,35 +249,34 @@
                     count = 0;
                     }
                 else{
-                count++;
-                //toggle.write(1);
-                controller.theta_elec = spi.GetElecPosition();
-                controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
-                controller.dtheta_mech = (1.0f/GR)*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;   
-                
+                /*
+                if(controller.v_bus>28.0f){         //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen
+                    gpio.enable->write(0);
+                    controller.ovp_flag = 1;
+                    state = REST_MODE;
+                    state_change = 1;
+                    printf("OVP Triggered!\n\r");
+                    }
+                    */  
+
                 torque_control(&controller);     
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
+                    controller.kp = 0;
+                    controller.kd = 0;
+                    controller.t_ff = 0;
                     } 
-                //controller.i_q_ref = .4f;
-                commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
-                spi.Sample();                                                   // Sample position sensor
-                //toggle.write(0);
+                commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
                 controller.timeout += 1;
-                
+                /*
+                count++;
                 if(count == 1000){
                      count = 0;
-                     //wait(.001);
-                    //printf("%f\n\r", controller.theta_elec);
                      }
-                     }
-                     
+                     */
+            
+                }     
                 break;
             case SETUP_MODE:
                 if(state_change){
@@ -287,8 +286,7 @@
             case ENCODER_MODE:
                 print_encoder();
                 break;
-                }
-                 
+                }                 
       }
   TIM1->SR = 0x0;                                                               // reset the status register
 }
@@ -308,7 +306,7 @@
                 state_change = 1;
                 char_count = 0;
                 cmd_id = 0;
-                GPIOC->ODR &= !(1 << 5); 
+                gpio.led->write(0);; 
                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                 }
         if(state == REST_MODE){
@@ -329,7 +327,21 @@
                     state = SETUP_MODE;
                     state_change = 1;
                     break;
-                    }
+                case 'z':
+                    spi.SetMechOffset(0);
+                    spi.Sample();
+                    wait_us(20);
+                    M_OFFSET = spi.GetMechPosition();
+                    if (!prefs.ready()) prefs.open();
+                        prefs.flush();                                                  // Write new prefs to flash
+                        prefs.close();    
+                        prefs.load(); 
+                    spi.SetMechOffset(M_OFFSET);
+                    printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
+                    
+                    break;
+                }
+                
                 }
         else if(state == SETUP_MODE){
             if(c == 13){
@@ -403,7 +415,8 @@
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
     wait(.1);
-    NVIC_SetPriority(TIM5_IRQn, 2);                                             // set interrupt priority
+    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
+    NVIC_SetPriority(CAN1_RX0_IRQn, 3);
 
 
     can.frequency(1000000);                                                     // set bit rate to 1Mbps
@@ -415,7 +428,10 @@
     rxMsg.len = 8;
     
     prefs.load();                                                               // Read flash
+    if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
+    if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
+    spi.SetMechOffset(M_OFFSET);
     int lut[128] = {0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
@@ -428,6 +444,7 @@
     printf(" Firmware Version: %s\n\r", VERSION_NUM);
     printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
     printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
+    printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
     printf(" CAN ID:  %d\n\r", CAN_ID);
         
     pc.attach(&serial_interrupt);                                               // attach serial interrupt

--- a/mbed-dev.lib	Fri Mar 02 15:24:00 2018 +0000
+++ b/mbed-dev.lib	Fri Apr 13 13:50:54 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#9f17883fb9b6
+https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#902f8c6731d6

--- a/structs.h	Fri Mar 02 15:24:00 2018 +0000
+++ b/structs.h	Fri Apr 13 13:50:54 2018 +0000
@@ -8,6 +8,7 @@
 
 typedef struct{
     DigitalOut *enable;
+    DigitalOut *led;
     FastPWM *pwm_u, *pwm_v, *pwm_w;
     } GPIOStruct;
     
@@ -16,7 +17,7 @@
     }COMStruct;
     
 typedef struct{
-    int adc1_raw, adc2_raw;
+    int adc1_raw, adc2_raw, adc3_raw;
     float i_a, i_b, i_c;
     float v_bus;
     float theta_mech, theta_elec;
@@ -31,19 +32,19 @@
     int loop_count;
     int timeout;
     int mode;
+    int ovp_flag;
     float p_des, v_des, kp, kd, t_ff;
     float cogging[128];
     } ControllerStruct;
 
 typedef struct{
-    float vel_1;
-    float vel_1_old;
-    float vel_1_est;
-    float vel_2;
-    float vel_2_old;
-    float vel_2_est;
-    float ts;
-    float est;
-    } VelocityEstimatorStruct;
+    float theta_m, theta_est;
+    float thetadot_m, thetadot_est;
+    float i_d_m, i_d_est;
+    float i_q_m, i_q_est;
+    float i_d_dot, i_q_dot;
+    float e_d, e_q;
+    float e_d_int, e_q_int;
+    } ObserverStruct;
     
 #endif