Revision 22:60276ba87ac6, committed 2017-03-31

Comitter:

benkatz

Date:

Fri Mar 31 18:24:46 2017 +0000

Parent:

21:7d1f0a206668

Child:

23:2adf23ee0305

Commit message:

Encoder autocalibration for dc offset and harmonics

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/calibration.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,201 @@
+/// Calibration procedures for determining position sensor offset, 
+/// phase ordering, and position sensor linearization
+/// 
+
+#include "calibration.h"
+
+
+void order_phases(PositionSensor *ps, GPIOStruct *gpio){   
+    ///Checks phase order, to ensure that positive Q current produces
+    ///torque in the positive direction wrt the position sensor.
+    
+    printf("\n\r Checking phase ordering\n\r");
+    float theta_ref = 0;
+    float theta_actual = 0;
+    float v_d = .2;                                         //Put all volts on the D-Axis
+    float v_q = 0.0;
+    float v_u, v_v, v_w = 0;
+    float dtc_u, dtc_v, dtc_w = .5;
+    int sample_counter = 0;
+    
+    ///Set voltage angle to zero, wait for rotor position to settle
+    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);             //inverse dq0 transform on voltages
+    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);        //space vector modulation
+    for(int i = 0; i<20000; i++){
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                    // Set duty cycles
+        TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+        TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+        wait_us(100);
+        }
+    //ps->ZeroPosition();
+    ps->Sample(); 
+    float theta_start = ps->GetMechPosition();              //get initial rotor position
+    
+    /// Rotate voltage angle
+    while(theta_ref < 4*PI){                                //rotate for 2 electrical cycles
+        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);         //inverse dq0 transform on voltages
+        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);    //space vector modulation
+        wait_us(100);
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                    //Set duty cycles
+        TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+        TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+       ps->Sample();                                        //sample position sensor
+       theta_actual = ps->GetMechPosition();
+       if(sample_counter > 200){
+           sample_counter = 0 ;
+        printf("%.4f   %.4f\n\r", theta_ref/(NPP), theta_actual);
+        }
+        sample_counter++;
+       theta_ref += 0.001f;
+        }
+    float theta_end = ps->GetMechPosition();
+    int direction = (theta_end - theta_start)>0;
+    printf("Theta Start:   %f    Theta End:  %f\n\r", theta_start, theta_end);
+    printf("Direction:  %d\n\r", direction);
+    if(direction){printf("Phaseing correct\n\r");}
+    else if(!direction){printf("Phasing incorrect.  Swapping phases V and W\n\r");}
+    gpio->phasing = direction;
+    
+    }
+    
+    
+    
+void calibrate(PositionSensor *ps, GPIOStruct *gpio){
+    /// Measures the electrical angle offset of the position sensor
+    /// and (in the future) corrects nonlinearity due to position sensor eccentricity
+    
+    printf("Starting calibration procedure\n\r");
+    
+    const int n = 128*NPP;                                  // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
+    const int n2 = 10;                                     // increments between saved samples (for smoothing motion)
+    float delta = 2*PI*NPP/(n*n2);                               // change in angle between samples
+    float error_f[n] = {0};                                 // error vector rotating forwards
+    float error_b[n] = {0};                                 // error vector rotating backwards
+    int raw_f[n] = {0};
+    int raw_b[n] = {0};
+    float theta_ref = 0;
+    float theta_actual = 0;
+    float v_d = .2;                                         // Put volts on the D-Axis
+    float v_q = 0.0;
+    float v_u, v_v, v_w = 0;
+    float dtc_u, dtc_v, dtc_w = .5;
+    
+        
+    ///Set voltage angle to zero, wait for rotor position to settle
+    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);             // inverse dq0 transform on voltages
+    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);        // space vector modulation
+    for(int i = 0; i<40000; i++){
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                    // Set duty cycles
+        if(gpio->phasing){
+            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+            TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+            }
+        else{
+            TIM1->CCR1 = 0x708*(1.0f-dtc_v);
+            TIM1->CCR2 = 0x708*(1.0f-dtc_w);
+            }
+        wait_us(100);
+        }
+    ps->Sample();   
+    
+    for(int i = 0; i<n; i++){                               // rotate forwards
+       for(int j = 0; j<n2; j++){   
+        theta_ref += delta;
+       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);          // inverse dq0 transform on voltages
+       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);     // space vector modulation
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);
+        if(gpio->phasing){
+            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+            TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+            }
+        else{
+            TIM1->CCR1 = 0x708*(1.0f-dtc_v);
+            TIM1->CCR2 = 0x708*(1.0f-dtc_w);
+            }
+            wait_us(100);
+            ps->Sample();
+        }
+       ps->Sample();
+       theta_actual = ps->GetMechPosition();
+       error_f[i] = theta_ref/NPP - theta_actual;
+       raw_f[i] = ps->GetRawPosition();
+        printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
+       //theta_ref += delta;
+        }
+    for(int i = 0; i<n; i++){                               // rotate backwards
+       for(int j = 0; j<n2; j++){
+       theta_ref -= delta;
+       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);          // inverse dq0 transform on voltages
+       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);     // space vector modulation
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);
+        if(gpio->phasing){
+            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+            TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+            }
+        else{
+            TIM1->CCR1 = 0x708*(1.0f-dtc_v);
+            TIM1->CCR2 = 0x708*(1.0f-dtc_w);
+            }
+            wait_us(100);
+            ps->Sample();
+        }
+       ps->Sample();                                        // sample position sensor
+       theta_actual = ps->GetMechPosition();                // get mechanical position
+       error_b[i] = theta_ref/NPP - theta_actual;
+       raw_b[i] = ps->GetRawPosition();
+       printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
+       //theta_ref -= delta;
+        }    
+        
+        float offset = 0;                                  
+        for(int i = 0; i<n; i++){
+            offset += (error_f[i] + error_b[n-1-i])/(2.0f*n);   // calclate average position sensor offset
+            }
+        offset = fmod(offset*NPP, 2*PI);                    // convert mechanical angle to electrical angle
+        printf("Encoder Electrical Offset (rad) %f\n\r",  offset);
+            
+        ps->SetElecOffset(offset);                          // Set position sensor offset
+        
+        /// Perform filtering to linearize position sensor eccentricity
+        /// FIR n-sample average, where n = number of samples in one electrical cycle
+        /// This filter has zero gain at electrical frequency and all integer multiples
+        /// So cogging should also be completely filtered out.
+        
+        float error[n] = {0};
+        int window = 128;
+        float error_filt[n] = {0};
+        float mean = 0;
+        for (int i = 0; i<n; i++){                          //Average the forward and back directions
+            error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
+            }
+        for (int i = 0; i<n; i++){
+            for(int j = 0; j<window; j++){
+                int ind = -window/2 + j + i;                    // Indexes from -window/2 to + window/2
+                if(ind<0){
+                    ind += n;}                        // Moving average wraps around
+                else if(ind > n-1) {
+                    ind -= n;}
+                error_filt[i] += error[ind]/(float)window;
+                }
+            //printf("%.4f   %4f    %.4f   %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
+            mean += error_filt[i]/n;
+            }
+        int raw_offset = (raw_f[0] + raw_b[n-1])/2;     //Insensitive to errors in this direction, so 2 points is plenty
+        const int  n_lut = 128;
+        int lut[n_lut];
+        for (int i = 0; i<n_lut; i++){                      // build lookup table
+            int ind = (raw_offset>>7) + i;
+            if(ind > (n_lut-1)){ 
+                ind -= n_lut;
+                }
+            lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
+            printf("%d   %d   %d   %d\n\r", raw_offset>>7, i, ind, lut[ind]);
+            }
+            ps->WriteLUT(lut);                                            // write lookup table to position sensor object
+         
+        
+        
+        
+    
+    
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/calibration.h	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,12 @@
+#ifndef CALIBRATION_H
+#define CALIBRATION_H
+
+#include "foc.h"
+#include "mbed.h"
+#include "PositionSensor.h"
+
+
+
+void order_phases(PositionSensor *ps, GPIOStruct *gpio);
+void calibrate(PositionSensor *ps, GPIOStruct *gpio);
+#endif

--- a/Config/current_controller_config.h	Thu Mar 02 15:31:45 2017 +0000
+++ b/Config/current_controller_config.h	Fri Mar 31 18:24:46 2017 +0000
@@ -1,14 +1,14 @@
 #ifndef CURRENT_CONTROLLER_CONFIG_H
 #define CURRENT_CONTROLLER_CONFIG_H
 
-#define K_D .5f   //V/A
-#define K_Q .5f   //V/A
-#define KI_D 0.04f  //1/samples
-#define KI_Q 0.04f  //1/samples
-#define V_BUS 14.0f
+#define K_D .02f                     // Volts/Amp
+#define K_Q .02f                     // Volts/Amp
+#define KI_D 0.04f                  // 1/samples
+#define KI_Q 0.04f                  // 1/samples
+#define V_BUS 14.0f                 // Volts
 
-#define D_INT_LIM V_BUS/(K_D*KI_D)  //A*samples
-#define Q_INT_LIM V_BUS/(K_Q*KI_Q)  //A*samples
+#define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples
+#define Q_INT_LIM V_BUS/(K_Q*KI_Q)  // Amps*samples
 
 
 

--- a/Config/hw_config.h	Thu Mar 02 15:31:45 2017 +0000
+++ b/Config/hw_config.h	Fri Mar 31 18:24:46 2017 +0000
@@ -4,10 +4,10 @@
 #define PIN_U PA_10
 #define PIN_V PA_9
 #define PIN_W PA_8
-#define ENABLE_PIN PA_11
-#define I_SCALE 0.02014160156f // Amps per A/D Count
-#define DTC_MAX 0.935f
-#define DTC_MIN 0.065f
+#define ENABLE_PIN PA_11        // Enable gate drive pin
+#define I_SCALE 0.02014160156f  // Amps per A/D Count
+#define DTC_MAX 0.95f          // Max phase duty cycle
+#define DTC_MIN 0.05f          // Min phase duty cycle
 
 
 #endif

--- a/Config/motor_config.h	Thu Mar 02 15:31:45 2017 +0000
+++ b/Config/motor_config.h	Fri Mar 31 18:24:46 2017 +0000
@@ -1,12 +1,14 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.1f
-#define L_D 0.00003f
-#define L_Q 0.00003f
+#define R_PHASE 0.105f            //Ohms
+#define L_D 0.00003f            //Henries
+#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 R_TOTAL 1.5f*R_PHASE
-#define NPP 21
+#define WB KT/NPP               //Webers.  
+
 
 
 #endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FOC/foc.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,136 @@
+
+#include "foc.h"
+
+//#include "FastMath.h"
+//using namespace FastMath;
+
+
+void abc( float theta, float d, float q, float *a, float *b, float *c){
+    ///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);
+    }
+    
+void dq0(float theta, float a, float b, float c, float *d, float *q){
+    ///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));
+    }
+    
+void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
+    ///u,v,w amplitude = v_bus for full modulation depth///
+    
+    float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
+    *dtc_u = fminf(fmaxf(((u - v_offset)*0.5f/v_bus + 0.5f), DTC_MIN), DTC_MAX);
+    *dtc_v = fminf(fmaxf(((v - v_offset)*0.5f/v_bus + 0.5f), DTC_MIN), DTC_MAX);
+    *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + 0.5f), DTC_MIN), DTC_MAX);
+    
+    }
+
+void zero_current(int *offset_1, int *offset_2){
+    int adc1_offset = 0;
+    int adc2_offset = 0;
+    int n = 1024;
+    for (int i = 0; i<n; i++){
+        ADC1->CR2  |= 0x40000000; 
+        wait(.001);
+        adc2_offset += ADC2->DR;
+        adc1_offset += ADC1->DR;
+        }
+    *offset_1 = adc1_offset/n;
+    *offset_2 = adc2_offset/n;
+    }
+
+void reset_foc(ControllerStruct *controller){
+    controller->q_int = 0;
+    controller->d_int = 0;
+    }
+
+
+void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
+       
+       controller->loop_count ++;
+       if(gpio->phasing){
+           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);
+           }
+        else{
+            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    //Calculate phase currents from ADC readings
+           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///
+       float i_d_error = controller->i_d_ref - controller->i_d;
+       float i_q_error = controller->i_q_ref - controller->i_q;
+       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);
+       controller->d_int += i_d_error;   
+       controller->q_int += i_q_error;
+       
+       //v_d_ff = 0;
+       //v_q_ff = 0;
+       
+       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_Q*KI_Q));
+       //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_D*i_d_error + K_D*KI_D*controller->d_int;// + v_d_ff;  
+       controller->v_q = K_Q*i_q_error + K_Q*KI_Q*controller->q_int;// + v_q_ff; 
+       
+       //controller->v_d = v_d_ff;
+       //controller->v_q = v_q_ff; 
+       
+       limit_norm(&controller->v_d, &controller->v_q, controller->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
+       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
+
+       //gpio->pwm_u->write(1.0f-controller->dtc_u);  //write duty cycles
+       //gpio->pwm_v->write(1.0f-controller->dtc_v);
+       //gpio->pwm_w->write(1.0f-controller->dtc_w);  
+       
+       if(gpio->phasing){
+            TIM1->CCR3 = 0x708*(1.0f-controller->dtc_u);
+            TIM1->CCR2 = 0x708*(1.0f-controller->dtc_v);
+            TIM1->CCR1 = 0x708*(1.0f-controller->dtc_w);
+        }
+        else{
+            TIM1->CCR3 = 0x708*(1.0f-controller->dtc_u);
+            TIM1->CCR1 = 0x708*(1.0f-controller->dtc_v);
+            TIM1->CCR2 = 0x708*(1.0f-controller->dtc_w);
+        }
+       //gpio->pwm_u->write(1.0f - .05f);  //write duty cycles
+        //gpio->pwm_v->write(1.0f - .05f);
+        //gpio->pwm_w->write(1.0f - .1f);
+       //TIM1->CCR1 = 0x708*(1.0f-controller->dtc_u);
+        //TIM1->CCR2 = 0x708*(1.0f-controller->dtc_v);
+        //TIM1->CCR3 = 0x708*(1.0f-controller->dtc_w);
+       controller->theta_elec = theta;   //For some reason putting this at the front breaks thins
+       
+
+       if(controller->loop_count >1000){
+           //controller->i_q_ref = -controller->i_q_ref;
+           controller->loop_count  = 0;
+           
+           //printf("%f\n\r", controller->dtheta_elec);
+           //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 zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
+    
+    }
+*/    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FOC/foc.h	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,19 @@
+#ifndef FOC_H
+#define FOC_H
+
+#include "structs.h"
+#include "PositionSensor.h"
+#include "mbed.h"
+#include "hw_config.h"
+#include "math.h"
+#include "math_ops.h"
+#include "motor_config.h"
+#include "current_controller_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);
+#endif

--- a/PositionSensor/PositionSensor.cpp	Thu Mar 02 15:31:45 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -1,116 +1,86 @@
 
 #include "mbed.h"
 #include "PositionSensor.h"
+//#include "offset_lut.h"
 //#include <math.h>
 
-PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){
-    //_CPR = CPR;
-    _CPR = CPR;
-    _ppairs = ppairs;
-    _offset = offset;
-    rotations = 0;
-    spi = new SPI(PC_12, PC_11, PC_10);
-    spi->format(16, 0);
-    cs = new DigitalOut(PA_15);
-    cs->write(1);
-    MechOffset = 0;
-    }
-
-int PositionSensorMA700::GetRawPosition(){
-        cs->write(0);
-    int response = spi->write(0)>>4;
-    cs->write(1);
-    return response;
-    }
-    
-
-float PositionSensorMA700::GetMechPosition(){
-    cs->write(0);
-    int response = spi->write(0)>>4;
-    cs->write(1);
-    if(response - old_counts > _CPR/4){
-        rotations -= 1;
-        }
-    else if (response - old_counts < -_CPR/4){
-        rotations += 1;
-        }
-    old_counts = response;
-    MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
-    //return MechPosition - MechOffset;
-    return MechPosition;
-    }
-
-float PositionSensorMA700::GetElecPosition(){
-    cs->write(0);
-    int response = spi->write(0)>>4;
-    cs->write(1);
-    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*response)%_CPR)) - _offset;
-    if(elec < 0) elec += 6.28318530718f;
-    return elec;
-    }
-
-float PositionSensorMA700::GetMechVelocity(){
-    return 0;
-    }
-
-void PositionSensorMA700::ZeroPosition(){
-    rotations = 0;
-    MechOffset = GetMechPosition();
-    }
-    
 PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
     //_CPR = CPR;
     _CPR = CPR;
     _ppairs = ppairs;
-    _offset = offset;
+    ElecOffset = offset;
     rotations = 0;
     spi = new SPI(PC_12, PC_11, PC_10);
     spi->format(16, 1);
     spi->frequency(5000000);
     cs = new DigitalOut(PA_15);
     cs->write(1);
-    readAngleCmd = 0xffff;
+    readAngleCmd = 0xffff;   
     MechOffset = 0;
+    modPosition = 0;
+    oldModPosition = 0;
+    oldVel = 0;
+    raw = 0;
+    }
+    
+void PositionSensorAM5147::Sample(){
+    cs->write(0);
+    raw = spi->write(readAngleCmd);
+    raw &= 0x3FFF;    //Extract last 14 bits
+    cs->write(1);
+    int angle = raw + offset_lut[raw>>7];
+    if(angle - old_counts > _CPR/2){
+        rotations -= 1;
+        }
+    else if (angle - old_counts < -_CPR/2){
+        rotations += 1;
+        }
+    
+    old_counts = angle;
+    oldModPosition = modPosition;
+    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;
+    if(elec < 0) elec += 6.28318530718f;
+    ElecPosition = elec;
+    
+    float vel;
+    if(modPosition<.1f && oldModPosition>6.1f){
+        vel = (modPosition - oldModPosition + 6.28318530718f)*40000.0f;
+        }
+    else if(modPosition>6.1f && oldModPosition<0.1f){
+        vel = (modPosition - oldModPosition - 6.28318530718f)*40000.0f;
+        }
+    else{
+        vel = (modPosition-oldModPosition)*40000.0f;
+    }    
+    
+    int n = 16;
+    float sum = vel;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
+        }
+    velVec[0] = vel;
+    MechVelocity =  sum/(float)n;
+    ElecVelocity = MechVelocity*_ppairs;
     }
 
 int PositionSensorAM5147::GetRawPosition(){
-    cs->write(0);
-    int angle = spi->write(0xffff);
-    angle &= 0x3FFF;    //Extract last 14 bits
-    cs->write(1);    
-    return angle;
+    return raw;
     }
     
-
 float PositionSensorAM5147::GetMechPosition(){
-    cs->write(0);
-    int angle = spi->write(readAngleCmd);
-    angle &= 0x3FFF;    //Extract last 14 bits
-    cs->write(1);
-    if(angle - old_counts > _CPR/4){
-        rotations -= 1;
-        }
-    else if (angle - old_counts < -_CPR/4){
-        rotations += 1;
-        }
-    old_counts = angle;
-    MechPosition = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
-    //return MechPosition - MechOffset;
     return MechPosition;
     }
 
 float PositionSensorAM5147::GetElecPosition(){
-    cs->write(0);
-    int angle = spi->write(readAngleCmd);
-    angle &= 0x3FFF;    //Extract last 14 bits
-    cs->write(1);
-    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) - _offset;
-    if(elec < 0) elec += 6.28318530718f;
-    return elec;
+    return ElecPosition;
     }
 
 float PositionSensorAM5147::GetMechVelocity(){
-    return 0;
+    return MechVelocity;
     }
 
 void PositionSensorAM5147::ZeroPosition(){
@@ -118,13 +88,28 @@
     MechOffset = GetMechPosition();
     }
     
+void PositionSensorAM5147::SetElecOffset(float offset){
+    ElecOffset = offset;
+    }
+
+int PositionSensorAM5147::GetCPR(){
+    return _CPR;
+    }
+
+
+void PositionSensorAM5147::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }
     
-    
+
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;
     _CPR = CPR;
     _offset = offset;
     MechPosition = 0;
+    out_old = 0;
+    oldVel = 0;
+    raw = 0;
     
     // Enable clock for GPIOA
     __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
@@ -140,25 +125,25 @@
     // Enable clock for TIM3
     __TIM3_CLK_ENABLE();
  
-    TIM3->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1
-    TIM3->SMCR  = TIM_ENCODERMODE_TI12;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
-    TIM3->CCMR1 = 0xf1f1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1, maximum digital filtering
-    TIM3->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
-    TIM3->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
-    TIM3->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
-    TIM3->ARR   = CPR-1; // reload at 0xfffffff         < TIM auto-reload register
+    TIM3->CR1   = 0x0001;                                                   // CEN(Counter ENable)='1'     < TIM control register 1
+    TIM3->SMCR  = TIM_ENCODERMODE_TI12;                                     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
+    TIM3->CCMR1 = 0x1111;                                                   // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1, maximum digital filtering
+    TIM3->CCMR2 = 0x0000;                                                   //                             < TIM capture/compare mode register 2
+    TIM3->CCER  = 0x0011;                                                   // CC1P CC2P                   < TIM capture/compare enable register
+    TIM3->PSC   = 0x0000;                                                   // Prescaler = (0+1)           < TIM prescaler
+    TIM3->ARR   = CPR;                                                      // IM auto-reload register
   
     TIM3->CNT = 0x000;  //reset the counter before we use it  
     
     // Extra Timer for velocity measurement
     
     __TIM2_CLK_ENABLE();
-    TIM3->CR2 = 0x030;  //MMS = 101
+    TIM3->CR2 = 0x030;                                                      //MMS = 101
     
     TIM2->PSC = 0x03;
     //TIM2->CR2 |= TIM_CR2_TI1S;
-    TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
-    TIM2->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC
+    TIM2->SMCR = 0x24;                                                      //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
+    TIM2->CCMR1 = 0x3;                                                      // CC1S = 11, IC1 mapped on TRC
     
     //TIM2->CR2 |= TIM_CR2_TI1S;
     TIM2->CCER |= TIM_CCER_CC1P;
@@ -166,9 +151,9 @@
     TIM2->CCER |= TIM_CCER_CC1E;
     
     
-    TIM2->CR1 = 0x01;       //CEN
+    TIM2->CR1 = 0x01;                                                       //CEN,  enable timer
     
-    TIM3->CR1   = 0x01;     // CEN
+    TIM3->CR1   = 0x01;                                                     // CEN
     ZPulse = new InterruptIn(PC_4);
     ZSense = new DigitalIn(PC_4);
     //ZPulse = new InterruptIn(PB_0);
@@ -182,17 +167,20 @@
     
     //ZTest = new DigitalOut(PC_2);
     //ZTest->write(1);
+    }
     
+void PositionSensorEncoder::Sample(){
     
-}
+    }
+
  
-float PositionSensorEncoder::GetMechPosition() {        //returns rotor angle in radians.
+float PositionSensorEncoder::GetMechPosition() {                            //returns rotor angle in radians.
     int raw = TIM3->CNT;
     float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
     return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
 }
 
-float PositionSensorEncoder::GetElecPosition() {        //returns rotor electrical angle in radians.
+float PositionSensorEncoder::GetElecPosition() {                            //returns rotor electrical angle in radians.
     int raw = TIM3->CNT;
     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
     if(elec < 0) elec += 6.28318530718f;
@@ -202,22 +190,30 @@
 
     
 float PositionSensorEncoder::GetMechVelocity(){
+
     float out = 0;
     float rawPeriod = TIM2->CCR1; //Clock Ticks
-    
+    int currentTime = TIM2->CNT;
+    if(currentTime > 2000000){rawPeriod = currentTime;}
     float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
-    float meas = dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
+    float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
+    if(isinf(meas)){ meas = 1;}
     out = meas;
-    if(meas == vel_old){
-        out = .99f*out_old;
+    //if(meas == oldVel){
+     //   out = .9f*out_old;
+     //   }
+    
+ 
+    oldVel = meas;
+    out_old = out;
+    int n = 16;
+    float sum = out;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
         }
-    else{
-        out = meas;
-        }
-    
-    vel_old = meas;
-    out_old = out;
-    return out;
+    velVec[0] = out;
+    return sum/(float)n;
     }
     
 float PositionSensorEncoder::GetElecVelocity(){
@@ -236,6 +232,10 @@
         }
         }
     }
+
+void PositionSensorEncoder::ZeroPosition(void){
+    
+    }
     
 void PositionSensorEncoder::ZeroEncoderCountDown(void){
     if (ZSense->read() == 0){
@@ -252,4 +252,20 @@
 
         }
         }
-    }
\ No newline at end of file
+    }
+void PositionSensorEncoder::SetElecOffset(float offset){
+    
+    }
+    
+int PositionSensorEncoder::GetRawPosition(void){
+    return 0;
+    }
+    
+int PositionSensorEncoder::GetCPR(){
+    return _CPR;
+    }
+    
+
+void PositionSensorEncoder::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }

--- a/PositionSensor/PositionSensor.h	Thu Mar 02 15:31:45 2017 +0000
+++ b/PositionSensor/PositionSensor.h	Fri Mar 31 18:24:46 2017 +0000
@@ -2,75 +2,64 @@
 #define POSITIONSENSOR_H
 class PositionSensor {
 public:
+    virtual void Sample(void) = 0;
     virtual float GetMechPosition() {return 0.0f;}
     virtual float GetElecPosition() {return 0.0f;}
     virtual float GetMechVelocity() {return 0.0f;}
     virtual float GetElecVelocity() {return 0.0f;}
+    virtual void ZeroPosition(void) = 0;
+    virtual int GetRawPosition(void) = 0;
+    virtual void SetElecOffset(float offset) = 0;
+    virtual int GetCPR(void) = 0;
+    virtual  void WriteLUT(int new_lut[128]) = 0;
 };
   
   
 class PositionSensorEncoder: public PositionSensor {
 public:
     PositionSensorEncoder(int CPR, float offset, int ppairs);
+    virtual void Sample();
     virtual float GetMechPosition();
     virtual float GetElecPosition();
     virtual float GetMechVelocity();
     virtual float GetElecVelocity();
+    virtual void ZeroPosition(void);
+    virtual void SetElecOffset(float offset);
+    virtual int GetRawPosition(void);
+    virtual int GetCPR(void);
+    virtual  void WriteLUT(int new_lut[128]); 
 private:
     InterruptIn *ZPulse;
     DigitalIn *ZSense;
     //DigitalOut *ZTest;
     virtual void ZeroEncoderCount(void);
     virtual void ZeroEncoderCountDown(void);
-    int _CPR, flag, rotations, _ppairs;
+    int _CPR, flag, rotations, _ppairs, raw;
     //int state;
-    float _offset, MechPosition, dir, test_pos, vel_old, out_old;
-};
-
-class PositionSensorMA700: public PositionSensor{
-public:
-    PositionSensorMA700(int CPR, float offset, int ppairs);
-    virtual float GetMechPosition();
-    virtual float GetElecPosition();
-    virtual float GetMechVelocity();
-    virtual int GetRawPosition();
-    virtual void ZeroPosition();
-private:
-    float _offset, MechPosition, MechOffset;
-    int _CPR, rotations, old_counts, _ppairs;
-    SPI *spi;
-    DigitalOut *cs;
+    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[16];
+    int offset_lut[128];
 };
 
 class PositionSensorAM5147: public PositionSensor{
 public:
     PositionSensorAM5147(int CPR, float offset, int ppairs);
+    virtual void Sample();
     virtual float GetMechPosition();
     virtual float GetElecPosition();
     virtual float GetMechVelocity();
     virtual int GetRawPosition();
     virtual void ZeroPosition();
+    virtual void SetElecOffset(float offset);
+    virtual int GetCPR(void);
+    virtual void WriteLUT(int new_lut[128]);
 private:
-    float _offset, MechPosition, MechOffset;
-    int _CPR, rotations, old_counts, _ppairs;
+    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[16], MechVelocity, ElecVelocity;
+    int raw, _CPR, rotations, old_counts, _ppairs;
     SPI *spi;
     DigitalOut *cs;
     int readAngleCmd;
+    int offset_lut[128];
 
 };
 
-class PositionSensorSineGen: public PositionSensor{
-public:
-    PositionSensorSineGen(int CPR, float offset, int ppairs);
-    virtual float GetMechPosition();
-    virtual float GetElecPosition();
-    virtual float GetMechVelocity();
-    virtual int GetRawPosition();
-    virtual void ZeroPosition();
-private:
-    float _offset, MechPosition, MechOffset;
-    int _CPR, rotations, old_counts, _ppairs;
-    int readAngleCmd;
-
-};
 #endif
\ No newline at end of file

--- a/foc.cpp	Thu Mar 02 15:31:45 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,55 +0,0 @@
-
-#include "foc.h"
-#include "mbed.h"
-#include "hw_config.h"
-#include "math.h"
-#include "math_ops.h"
-//#include "FastMath.h"
-//using namespace FastMath;
-
-
-void abc( float theta, float d, float q, float *a, float *b, float *c){
-    ///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);
-    }
-    
-void dq0(float theta, float a, float b, float c, float *d, float *q){
-    ///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));
-    }
-    
-void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
-    ///u,v,w amplitude = v_bus for full modulation depth///
-    
-    float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
-    *dtc_u = fminf(fmaxf(((u - v_offset)*0.5f/v_bus + 0.5f), DTC_MIN), DTC_MAX);
-    *dtc_v = fminf(fmaxf(((v - v_offset)*0.5f/v_bus + 0.5f), DTC_MIN), DTC_MAX);
-    *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + 0.5f), DTC_MIN), DTC_MAX);
-    
-    }
-
-void zero_current(int *offset_1, int *offset_2){
-    int adc1_offset = 0;
-    int adc2_offset = 0;
-    int n = 1024;
-    for (int i = 0; i<n; i++){
-        ADC1->CR2  |= 0x40000000; 
-        wait(.001);
-        adc2_offset += ADC2->DR;
-        adc1_offset += ADC1->DR;
-        }
-    *offset_1 = adc1_offset/n;
-    *offset_2 = adc2_offset/n;
-    }
-
-void reset_foc(ControllerStruct *controller){
-    controller->q_int = 0;
-    controller->d_int = 0;
-    }

--- a/foc.h	Thu Mar 02 15:31:45 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,12 +0,0 @@
-#ifndef FOC_H
-#define FOC_H
-
-#include "structs.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);
-#endif

--- a/hw_setup.cpp	Thu Mar 02 15:31:45 2017 +0000
+++ b/hw_setup.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -7,56 +7,59 @@
 
 void Init_PWM(GPIOStruct *gpio){
     
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable the clock to GPIOC
-    RCC->APB1ENR |= 0x00000001; // enable TIM2 clock
-    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // enable the clock to GPIOC
+    RCC->APB1ENR |= 0x00000001;                                 // enable TIM2 clock
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                         // enable TIM1 clock
 
-    //GPIOC->MODER = (1 << 8); // set pin 4 to be general purpose output
+    GPIOC->MODER |= (1 << 10);                                  // set pin 5 to be general purpose output for LED
     gpio->enable = new DigitalOut(ENABLE_PIN);
     gpio->pwm_u = new FastPWM(PIN_U);
     gpio->pwm_v = new FastPWM(PIN_V);
     gpio->pwm_w = new FastPWM(PIN_W);
     
+    gpio->phasing = 1;
+    
+    
      //ISR Setup     
     
-    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);   //Enable TIM1 IRQ
+    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);                         //Enable TIM1 IRQ
 
-    TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
-    TIM1->CR1 = 0x40;//CMS = 10, interrupt only when counting up
+    TIM1->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
+    TIM1->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up
     TIM1->CR1 |= TIM_CR1_UDIS;
-    TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on, 
-    TIM1->RCR |= 0x001; // update event once per up/down count of tim1 
+    TIM1->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
+    TIM1->RCR |= 0x001;                                         // update event once per up/down count of tim1 
     TIM1->EGR |= TIM_EGR_UG;
  
     //PWM Setup
 
-    TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
+    TIM1->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
     //TIM1->ARR = 0x1194; // 20 khz
-    TIM1->ARR = 0x8CA;    //40 khz
-    TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
-    TIM1->CR1 |= TIM_CR1_CEN;    
+    TIM1->ARR = 0x8CA;                                          // set auto reload, 40 khz
+    TIM1->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
+    TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM1
     
     }
 
 void Init_ADC(void){
         // ADC Setup
-     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
-     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;//0x0000002; // Enable clock for GPIOC
+     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
-     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
-     GPIOC->MODER |= 0x0000000f; // PC_0, PC_1 are analog inputs 
+     ADC->CCR = 0x00000006;                                     // 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
+     GPIOC->MODER |= 0x0000000f;                                // Alternate function, PC_0, PC_1 are analog inputs 
 
     }
 
 void Init_DAC(void){
-     RCC->APB1ENR |= 0x20000000; // Enable clock for DAC
-     DAC->CR |= 0x00000001; // DAC control reg, both channels ON
-     GPIOA->MODER |= 0x00000300; // PA04 as analog output  
+     RCC->APB1ENR |= 0x20000000;                                // Enable clock for DAC
+     DAC->CR |= 0x00000001;                                     // DAC control reg, both channels ON
+     GPIOA->MODER |= 0x00000300;                                // PA04 as analog output  
     }
 
 void Init_All_HW(GPIOStruct *gpio){

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/loops.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,2 @@
+#include "loops.h"
+#include "foc.h"
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/loops.h	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,10 @@
+#ifndef LOOPS_H
+#define LOOPS_H
+
+#include "structs.h"
+#include "PositionSensor.h"
+
+
+void pd(float p_des, float v_des, float kp, float kd);
+
+#endif

--- a/main.cpp	Thu Mar 02 15:31:45 2017 +0000
+++ b/main.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -1,3 +1,11 @@
+/// high-bandwidth 3-phase motor control, for robots
+/// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others
+/// Hardware documentation can be found at build-its.blogspot.com
+
+/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
+
+
+
 const unsigned int BOARDNUM = 0x2;
 
 //const unsigned int a_id =                            
@@ -13,6 +21,7 @@
 #include "PositionSensor.h"
 #include "structs.h"
 #include "foc.h"
+#include "calibration.h"
 #include "hw_setup.h"
 #include "math_ops.h"
 #include "current_controller_config.h"
@@ -22,6 +31,7 @@
 GPIOStruct gpio;
 ControllerStruct controller;
 COMStruct com;
+VelocityEstimatorStruct velocity;
 
 
 
@@ -79,126 +89,141 @@
 
 Serial pc(PA_2, PA_3);
 
-PositionSensorAM5147 spi(16384, 4.7, NPP);   ///1  I really need an eeprom or something to store this....
-//PositionSensorEncoder encoder(4096, 0, 21); 
+PositionSensorAM5147 spi(16384, -0.4658, NPP);   ///1  I really need an eeprom or something to store this....
+PositionSensorEncoder encoder(4096, 0, 21); 
 
 int count = 0;
-void commutate(void){
-       
-       count ++;
+int mode = 0;
 
-       //pc.printf("%f\n\r", controller.theta_elec);
-        //Get rotor angle
-       //spi.GetMechPosition();
-       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;
-       
-       
-       dq0(controller.theta_elec, controller.i_a, controller.i_b, controller.i_c, &controller.i_d, &controller.i_q);    //dq0 transform on currents
-       
-       ///Control Law///
-       float i_d_error = controller.i_d_ref - controller.i_d;
-       float i_q_error = controller.i_q_ref - controller.i_q;
-       float v_d_ff = controller.i_d_ref*R_TOTAL;   //feed-forward voltage
-       float v_q_ff = controller.i_q_ref*R_TOTAL;
-       controller.d_int += i_d_error;   
-       controller.q_int += i_q_error;
-       
-       limit_norm(&controller.d_int, &controller.q_int, V_BUS/(K_Q*KI_Q));
-       //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_D*i_d_error + K_D*KI_D*controller.d_int;// + v_d_ff;  
-       controller.v_q = K_Q*i_q_error + K_Q*KI_Q*controller.q_int;// + v_q_ff; 
-       //controller.v_d = 10*v_d_ff;
-       //controller.v_q = 10*v_q_ff; 
-       limit_norm(&controller.v_d, &controller.v_q, controller.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
-       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
+    
+float velocity_estimate(void){
+    velocity.vel_2 = encoder.GetMechVelocity();
+    velocity.vel_1 = spi.GetMechVelocity();
+    
+    velocity.ts = .01f;
+    velocity.vel_1_est = velocity.ts*velocity.vel_1 + (1-velocity.ts)*velocity.vel_1_est;   //LPF
+    velocity.vel_2_est = (1-velocity.ts)*(velocity.vel_2_est + velocity.vel_2 - velocity.vel_2_old);    //HPF
+    velocity.est = velocity.vel_1_est + velocity.vel_2_est;
+    
+    velocity.vel_1_old = velocity.vel_1;
+    velocity.vel_2_old = velocity.vel_2;
+    return velocity.est;
+    }
 
-       gpio.pwm_u->write(1.0f-controller.dtc_u);  //write duty cycles
-       gpio.pwm_v->write(1.0f-controller.dtc_v);
-       gpio.pwm_w->write(1.0f-controller.dtc_w);  
-       
-       controller.theta_elec = spi.GetElecPosition();   
-       //TIM1->CCR1 = (int)(controller.dtc_u * 0x8CA);//gpio.pwm_u->write(1.0f-controller.dtc_u);  //write duty cycles
-       //TIM1->CCR2 = (int)(controller.dtc_v * 0x8CA);//gpio.pwm_v->write(1.0f-controller.dtc_v);
-       //TIM1->CCR3 = (int)(controller.dtc_w * 0x8CA);//gpio.pwm_w->write(1.0f-controller.dtc_w);
-
-       //gpio.pwm_u->write(1.0f - .1f);  //write duty cycles
-       //gpio.pwm_v->write(1.0f - .1f);
-       //gpio.pwm_w->write(1.0f - .15f);
-
-
-       if(count >1000){
-           controller.i_q_ref = -controller.i_q_ref;
-           count  = 0;
-           //pc.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);
-            }
-       }
-       
-
-// Current Sampling IRQ
+// Current Sampling Interrupt
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
   if (TIM1->SR & TIM_SR_UIF ) {
         //toggle = 1;
-        ADC1->CR2  |= 0x40000000;  
-        //volatile int delay;
+        count++;
+        ADC1->CR2  |= 0x40000000;                                       //Begin sample and conversion
+        //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
         
-        controller.adc2_raw = ADC2->DR;
-        controller.adc1_raw = ADC1->DR;
-        //toggle = 0;
-        commutate();        
+        if(controller.mode == 2){
+            controller.adc2_raw = ADC2->DR;
+            controller.adc1_raw = ADC1->DR;
+            
+            //toggle = 0;
+            
+            spi.Sample();
+            controller.theta_elec = spi.GetElecPosition();
+            commutate(&controller, &gpio, controller.theta_elec);        
+            }
+         
+         
+         //controller.dtheta_mech = spi.GetMechVelocity();
+         //controller.dtheta_elec = encoder.GetElecVelocity();
+         //ontroller.dtheta_mech = encoder.GetMechVelocity();
+         //controller.i_q_ref = 2.0f*controller.dtheta_mech;
          
+         
+         //float v1 = encoder.GetMechVelocity();
+         //float v2 = spi.GetMechVelocity();
+         
+         
+         if(count > 100){
+             count = 0;
+             //for (int i = 1; i<16; i++){
+                //pc.printf("%d\n\r ", spi.GetRawPosition());
+             //   }
+                //pc.printf("\n\r");
+                //pc.printf("%.4f %.4f  %.4f\n\r",velocity.vel_1 ,velocity.vel_2, velocity.est );
+
+             }
+         
+            
       }
   TIM1->SR = 0x0; // reset the status register
 }
 
-       
+
+void enter_torque_mode(void){
+    controller.mode = 2;
+    TIM1->CR1 ^= TIM_CR1_UDIS;                                          //enable interrupts
+    controller.i_d_ref = 0;
+    controller.i_q_ref = 1;
+    reset_foc(&controller);                                             //resets integrators, and other control loop parameters
+    gpio.enable->write(1);
+    GPIOC->ODR ^= (1 << 5);                                             //turn on LED
+    }
+    
+void enter_calibration_mode(void){
+    controller.mode = 1;
+    TIM1->CR1 ^= TIM_CR1_UDIS;
+    gpio.enable->write(1);
+    GPIOC->ODR ^= (1 << 5);     
+    //calibrate_encoder(&spi);
+    order_phases(&spi, &gpio);
+    calibrate(&spi, &gpio);
+    GPIOC->ODR ^= (1 << 5);
+    wait(.2);
+    gpio.enable->write(0);
+     TIM1->CR1 ^= TIM_CR1_UDIS;
+     controller.mode = 0;
+    }
+    
        
 int main() {
 
     controller.v_bus = V_BUS;
-    spi.ZeroPosition();
-    Init_All_HW(&gpio);
+    controller.mode = 0;
+    //spi.ZeroPosition(); 
+    Init_All_HW(&gpio);                                                 // Setup PWM, ADC, GPIO
 
     wait(.1);
     //TIM1->CR1 |= TIM_CR1_UDIS;
     gpio.enable->write(1);
-    gpio.pwm_u->write(1.0f);  //write duty cycles
+    gpio.pwm_u->write(1.0f);                                            //write duty cycles
     gpio.pwm_v->write(1.0f);
     gpio.pwm_w->write(1.0f);
-    zero_current(&controller.adc1_offset, &controller.adc2_offset);
+    zero_current(&controller.adc1_offset, &controller.adc2_offset);     //Measure current sensor zero-offset
+    //gpio.enable->write(0);
     reset_foc(&controller);
-    TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
-    gpio.enable->write(1);
-       //gpio.pwm_u->write(1.0f - .05f);  //write duty cycles
-       //gpio.pwm_v->write(1.0f - .05f);
-       //gpio.pwm_w->write(1.0f - .1f);
+    
+    //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
+    //gpio.enable->write(1);
+    //gpio.pwm_u->write(1.0f - .05f);  //write duty cycles
+    //gpio.pwm_v->write(1.0f - .05f);
+    //gpio.pwm_w->write(1.0f - .1f);
     
     wait(.1);
-    NVIC_SetPriority(TIM5_IRQn, 2);
-    //loop.attach(&commutate, .000025);
-    can.frequency(1000000);                     // set bit rate to 1Mbps
-    can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
+    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);
     
-    pc.baud(921600);
+    pc.baud(115200);
     wait(.01);
     pc.printf("HobbyKing Cheetah v1.1\n\r");
     pc.printf("ADC1 Offset: %d    ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
     wait(.01);
     
+    
+    enter_calibration_mode();
+    enter_torque_mode();
 
-       controller.i_d_ref = 0;
-       controller.i_q_ref = 0;
+    
     while(1) {
 
     }

--- a/structs.h	Thu Mar 02 15:31:45 2017 +0000
+++ b/structs.h	Fri Mar 31 18:24:46 2017 +0000
@@ -9,6 +9,7 @@
 typedef struct{
     DigitalOut *enable;
     FastPWM *pwm_u, *pwm_v, *pwm_w;
+    int phasing;
     } GPIOStruct;
     
 typedef struct{
@@ -28,7 +29,19 @@
     float d_int, q_int;
     int adc1_offset, adc2_offset;
     float i_d_ref, i_q_ref;
+    int loop_count;
+    int mode;
     } 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;
+    
 #endif