Revision 47:e1196a851f76, committed 2018-12-05

Comitter:

benkatz

Date:

Wed Dec 05 04:07:46 2018 +0000

Parent:

46:2d4b1dafcfe3

Child:

48:74a40481740c

Commit message:

albert revision;

Changed in this revision

--- a/CAN/CAN_com.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/CAN/CAN_com.h	Wed Dec 05 04:07:46 2018 +0000
@@ -1,10 +1,10 @@
 #ifndef CAN_COM_H
 #define CAN_COM_H
 
-#include "structs.h"
+#include "../structs.h"
 #include "user_config.h"
 #include "mbed.h"
-#include "math_ops.h"
+#include "../math_ops.h"
 
 void pack_reply(CANMessage *msg, float p, float v, float t);
 void unpack_cmd(CANMessage msg, ControllerStruct * controller);

--- a/Calibration/calibration.cpp	Thu Jul 12 02:50:34 2018 +0000
+++ b/Calibration/calibration.cpp	Wed Dec 05 04:07:46 2018 +0000
@@ -16,7 +16,7 @@
     printf("\n\r Checking phase ordering\n\r");
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .05f;                                                             //Put all volts on the D-Axis
+    float v_d = V_CAL;                                                             //Put all volts on the D-Axis
     float v_q = 0.0f;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5f;
@@ -32,7 +32,7 @@
         wait_us(100);
         }
     //ps->ZeroPosition();
-    ps->Sample(); 
+    ps->Sample(DT); 
     wait_us(1000);
     //float theta_start = ps->GetMechPositionFixed();                                  //get initial rotor position
     float theta_start;
@@ -51,7 +51,7 @@
         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        //Set duty cycles
         TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
         TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
-       ps->Sample();                                                            //sample position sensor
+       ps->Sample(DT);                                                            //sample position sensor
        theta_actual = ps->GetMechPositionFixed();
        if(theta_ref==0){theta_start = theta_actual;}
        if(sample_counter > 200){
@@ -84,7 +84,7 @@
     float * error_filt;
     
     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 = 50;                                                          // increments between saved samples (for smoothing motion)
+    const int n2 = 40;                                                          // increments between saved samples (for smoothing motion)
     float delta = 2*PI*NPP/(n*n2);                                              // change in angle between samples
     error_f = new float[n]();                                                     // error vector rotating forwards
     error_b = new float[n]();                                                     // error vector rotating backwards
@@ -101,7 +101,7 @@
     raw_b = new int[n]();
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .05f;                                                             // Put volts on the D-Axis
+    float v_d = V_CAL;                                                             // Put volts on the D-Axis
     float v_q = 0.0f;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5f;
@@ -122,7 +122,7 @@
             }
         wait_us(100);
         }
-    ps->Sample();   
+    ps->Sample(DT);   
     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;
@@ -144,9 +144,9 @@
             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
             }
             wait_us(100);
-            ps->Sample();
+            ps->Sample(DT);
         }
-       ps->Sample();
+       ps->Sample(DT);
        theta_actual = ps->GetMechPositionFixed();
        error_f[i] = theta_ref/NPP - theta_actual;
        raw_f[i] = ps->GetRawPosition();
@@ -169,9 +169,9 @@
             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
             }
             wait_us(100);
-            ps->Sample();
+            ps->Sample(DT);
         }
-       ps->Sample();                                                            // sample position sensor
+       ps->Sample(DT);                                                            // sample position sensor
        theta_actual = ps->GetMechPositionFixed();                                    // get mechanical position
        error_b[i] = theta_ref/NPP - theta_actual;
        raw_b[i] = ps->GetRawPosition();

--- a/Calibration/calibration.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/Calibration/calibration.h	Wed Dec 05 04:07:46 2018 +0000
@@ -7,6 +7,8 @@
 #include "PreferenceWriter.h"
 #include "user_config.h"
 
+#define V_CAL 0.15f;
+
 
 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
 void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);

--- a/Config/current_controller_config.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/Config/current_controller_config.h	Wed Dec 05 04:07:46 2018 +0000
@@ -7,13 +7,14 @@
 #define K_SCALE 0.0001f             // K_loop/Loop BW (Hz) 0.0042
 #define KI_D 0.0255f                // PI zero, in radians per sample
 #define KI_Q 0.0255f                // PI zero, in radians per sample
-#define V_BUS 24.0f                 // Volts
-#define OVERMODULATION 1.4f         // 1.0 = no overmodulation
+#define V_BUS 35.0f                 // Volts
+#define OVERMODULATION 1.15f         // 1.0 = no overmodulation
 
 #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
+#define I_MAX_FW 0.0f
 
 //Observer//
 #define DT 0.000025f

--- a/Config/hw_config.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/Config/hw_config.h	Wed Dec 05 04:07:46 2018 +0000
@@ -7,9 +7,9 @@
 #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 V_SCALE 0.00886f        // Bus volts per A/D Count
+#define DTC_MAX 0.94f          // Max phase duty cycle
+#define DTC_MIN 0.0f          // Min phase duty cycle
 #define PWM_ARR 0x8CA           /// timer autoreload value
 
 

--- a/Config/motor_config.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/Config/motor_config.h	Wed Dec 05 04:07:46 2018 +0000
@@ -1,12 +1,12 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.13f          //Ohms
-#define L_D 0.00003f            //Henries
-#define L_Q 0.00003f            //Henries
-#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
-#define NPP 20                  //Number of pole pairs
-#define GR 6.0f                 //Gear ratio
+#define R_PHASE 0.02f          //Ohms
+#define L_D 0.00002f            //Henries
+#define L_Q 0.00002f            //Henries
+#define KT .08f                 //N-m per peak phase amp, = WB*NPP*3/2
+#define NPP 21                  //Number of pole pairs
+#define GR 1.0f                 //Gear ratio
 #define KT_OUT 0.45f            //KT*GR
 #define WB 0.0024f               //Flux linkage, Webers.  
 

--- a/FOC/foc.cpp	Thu Jul 12 02:50:34 2018 +0000
+++ b/FOC/foc.cpp	Wed Dec 05 04:07:46 2018 +0000
@@ -34,12 +34,22 @@
     /// 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)/v_bus + .5f), DTC_MIN), DTC_MAX);
     *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
-    *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);  
+    *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 
+    
+    /*
+    sinusoidal pwm
+    *dtc_u = fminf(fmaxf((u/v_bus + .5f), DTC_MIN), DTC_MAX);
+    *dtc_v = fminf(fmaxf((v/v_bus + .5f), DTC_MIN), DTC_MAX);
+    *dtc_w = fminf(fmaxf((w/v_bus + .5f), DTC_MIN), DTC_MAX);
+    */
+     
     
     }
 
+
 void zero_current(int *offset_1, int *offset_2){                                // Measure zero-offset of the current sensors
     int adc1_offset = 0;
     int adc2_offset = 0;
@@ -79,6 +89,7 @@
     controller->d_int = 0;
     controller->v_q = 0;
     controller->v_d = 0;
+
     }
     
 void limit_current_ref (ControllerStruct *controller){
@@ -88,11 +99,8 @@
     }
 
 
-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);
-       
+void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
+
        /// Commutation Loop ///
        controller->loop_count ++;   
        if(PHASE_ORDER){                                                                          // Check current sensor ordering
@@ -112,21 +120,8 @@
        //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 = 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;
+        controller->i_d_filt = 0.95f*controller->i_d_filt + 0.05f*controller->i_d;
         
-        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 scog12 = FastSin(12.0f*theta);
-        //float scog1 = s;
-        //float cogging_current = 0.25f*scog1 - 0.3f*scog12;
         
         // Filter the current references to the desired closed-loopbandwidth
         // Allows calculation of desired di/dt for inductance, etc
@@ -137,32 +132,42 @@
         controller->did_dt = (controller->i_d_ref_filt - controller->did_dt)/DT;
         controller->diq_dt = (controller->i_q_ref_filt - controller->diq_dt)/DT;
        
+       
+       /// Field Weakening ///
+       /*
+       controller->fw_int += .001*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
+       controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_MAX_FW);
+       controller->i_d_ref = controller->fw_int;
+       float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
+       limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);
+       
+       */
+       
        /// 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;
        
        // Calculate feed-forward voltages //
-       float v_d_ff = 2.0f*(0.0f*controller->i_d_ref*R_PHASE + 0.0f*L_D*controller->did_dt - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
-       float v_q_ff =  2.0f*(0.0f*controller->i_q_ref*R_PHASE + 0.0f*L_Q*controller->diq_dt + controller->dtheta_elec*(L_D*controller->i_d + 0.0f*WB));
+       float v_d_ff = 2.0f*(1.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
+       float v_q_ff =  2.0f*(1.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB));
        
        // Integrate Error //
-       controller->d_int += i_d_error;   
-       controller->q_int += i_q_error;
-
-       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(controller->k_q*controller->ki_q));        
-       controller->v_d = controller->k_d*(i_d_error + controller->ki_d*controller->d_int) + v_d_ff;  
-       controller->v_q = controller->k_q*(i_q_error + controller->ki_q*controller->q_int) + v_q_ff; 
+       controller->d_int += controller->k_d*controller->ki_d*i_d_error;   
+       controller->q_int += controller->k_q*controller->ki_q*i_q_error;
+       
+       controller->d_int = fmaxf(fminf(controller->d_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus);
+       controller->q_int = fmaxf(fminf(controller->q_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus); 
+       
+       //limit_norm(&controller->d_int, &controller->q_int, OVERMODULATION*controller->v_bus);     
+       controller->v_d = controller->k_d*i_d_error + controller->d_int ;//+ v_d_ff;  
+       controller->v_q = controller->k_q*i_q_error + controller->q_int ;//+ v_q_ff; 
+       
+       controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q);
        
        limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
-       abc(controller->theta_elec + 0.5f*DT*controller->dtheta_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 + 0.0f*DT*controller->dtheta_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
 
-       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;
-       
-       //controller->dtc_u = 0.5f;
-       //controller->dtc_v = 0.6f;
-       //controller->dtc_w = 0.5f;
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
             TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
@@ -176,17 +181,6 @@
 
        controller->theta_elec = theta;                                          //For some reason putting this at the front breaks thins
        
-
-       if(controller->loop_count >400){
-           //controller->i_q_ref = -controller->i_q_ref;
-          controller->loop_count  = 0;
-           
-           //printf("%.2f  %.2f  %.2f\n\r", controller->i_a, controller->i_b, controller->i_c);
-           //printf("%f\n\r", controller->dtheta_mech*GR);
-           //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
-           printf("%f %f\n\r", v_q_ff, v_d_ff);
-           //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
-            }
     }
     
     
@@ -196,10 +190,4 @@
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
     }
-
-
-/*    
-void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
-    
-    }
-*/    
\ No newline at end of file
+ 
\ No newline at end of file

--- a/FOC/foc.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/FOC/foc.h	Wed Dec 05 04:07:46 2018 +0000
@@ -1,12 +1,12 @@
 #ifndef FOC_H
 #define FOC_H
 
-#include "structs.h"
+#include "../structs.h"
 #include "PositionSensor.h"
 #include "mbed.h"
 #include "hw_config.h"
 #include "math.h"
-#include "math_ops.h"
+#include "../math_ops.h"
 #include "motor_config.h"
 #include "current_controller_config.h"
 #include "FastMath.h"
@@ -18,7 +18,7 @@
 void zero_current(int *offset_1, int *offset_2);
 void reset_foc(ControllerStruct *controller);
 void init_controller_params(ControllerStruct *controller);
-void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta);
+void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta);
 void torque_control(ControllerStruct *controller);
 void limit_current_ref (ControllerStruct *controller);
 #endif

--- a/PositionSensor/PositionSensor.cpp	Thu Jul 12 02:50:34 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Dec 05 04:07:46 2018 +0000
@@ -1,7 +1,7 @@
 
 #include "mbed.h"
 #include "PositionSensor.h"
-#include "math_ops.h"
+#include "../math_ops.h"
 //#include "offset_lut.h"
 //#include <math.h>
 
@@ -25,10 +25,12 @@
     raw = 0;
     }
     
-void PositionSensorAM5147::Sample(){
+void PositionSensorAM5147::Sample(float dt){
     GPIOA->ODR &= ~(1 << 15);
-    raw = spi->write(readAngleCmd);
-    raw &= 0x3FFF;                                                              //Extract last 14 bits
+    //raw = spi->write(readAngleCmd);
+    //raw &= 0x3FFF;   
+    raw = spi->write(0);
+    raw = raw>>1;                                                             //Extract last 14 bits
     GPIOA->ODR |= (1 << 15);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];
@@ -53,15 +55,16 @@
     
     float vel;
     //if(modPosition<.1f && oldModPosition>6.1f){
+
     if((modPosition-oldModPosition) < -3.0f){
-        vel = (modPosition - oldModPosition + 2.0f*PI)*40000.0f;
+        vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
         }
     //else if(modPosition>6.1f && oldModPosition<0.1f){
     else if((modPosition - oldModPosition) > 3.0f){
-        vel = (modPosition - oldModPosition - 2.0f*PI)*40000.0f;
+        vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
         }
     else{
-        vel = (modPosition-oldModPosition)*40000.0f;
+        vel = (modPosition-oldModPosition)/dt;
     }    
     
     int n = 40;
@@ -103,7 +106,7 @@
 void PositionSensorAM5147::ZeroPosition(){
     rotations = 0;
     MechOffset = 0;
-    Sample();
+    Sample(.00025f);
     MechOffset = GetMechPosition();
     }
     
@@ -192,7 +195,7 @@
     //ZTest->write(1);
     }
     
-void PositionSensorEncoder::Sample(){
+void PositionSensorEncoder::Sample(float dt){
     
     }
 

--- a/PositionSensor/PositionSensor.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/PositionSensor/PositionSensor.h	Wed Dec 05 04:07:46 2018 +0000
@@ -2,7 +2,7 @@
 #define POSITIONSENSOR_H
 class PositionSensor {
 public:
-    virtual void Sample(void) = 0;
+    virtual void Sample(float dt) = 0;
     virtual float GetMechPosition() {return 0.0f;}
     virtual float GetMechPositionFixed() {return 0.0f;}
     virtual float GetElecPosition() {return 0.0f;}
@@ -19,7 +19,7 @@
 class PositionSensorEncoder: public PositionSensor {
 public:
     PositionSensorEncoder(int CPR, float offset, int ppairs);
-    virtual void Sample();
+    virtual void Sample(float dt);
     virtual float GetMechPosition();
     virtual float GetElecPosition();
     virtual float GetMechVelocity();
@@ -44,7 +44,7 @@
 class PositionSensorAM5147: public PositionSensor{
 public:
     PositionSensorAM5147(int CPR, float offset, int ppairs);
-    virtual void Sample();
+    virtual void Sample(float dt);
     virtual float GetMechPosition();
     virtual float GetMechPositionFixed();
     virtual float GetElecPosition();

--- a/main.cpp	Thu Jul 12 02:50:34 2018 +0000
+++ b/main.cpp	Wed Dec 05 04:07:46 2018 +0000
@@ -2,6 +2,7 @@
 /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, 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
+/// Version for the TI DRV8323 Everything Chip
 
 #define REST_MODE 0
 #define CALIBRATION_MODE 1
@@ -37,7 +38,6 @@
 GPIOStruct gpio;
 ControllerStruct controller;
 COMStruct com;
-ObserverStruct observer;
 Serial pc(PA_2, PA_3);
 
 
@@ -59,7 +59,7 @@
 
 void onMsgReceived() {
     //msgAvailable = true;
-    //printf("%.3f   %.3f   %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q);
+    printf("%df\n\r", rxMsg.id);
     can.read(rxMsg);  
     if((rxMsg.id == CAN_ID)){
         controller.timeout = 0;
@@ -86,6 +86,7 @@
 
 void enter_menu_state(void){
     drv.disable_gd();
+    //gpio.enable->write(0);
     printf("\n\r\n\r\n\r");
     printf(" Commands:\n\r");
     wait_us(10);
@@ -102,7 +103,6 @@
     printf(" esc - Exit to Menu\n\r");
     wait_us(10);
     state_change = 0;
-    //gpio.enable->write(0);
     gpio.led->write(0);
     }
 
@@ -128,6 +128,7 @@
     
 void enter_torque_mode(void){
     drv.enable_gd();
+    //gpio.enable->write(1);
     controller.ovp_flag = 0;
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     wait(.001);
@@ -140,6 +141,7 @@
     
 void calibrate(void){
     drv.enable_gd();
+    //gpio.enable->write(1);
     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
@@ -147,12 +149,14 @@
     wait(.2);
     printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     drv.disable_gd();
+    //gpio.enable->write(0);
      state_change = 0;
     }
     
 void print_encoder(void){
-    printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
-    wait(.05);
+    //printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+    printf("%d\n\r", spi.GetRawPosition());
+    wait(.001);
     }
 
 /// Current Sampling Interrupt ///
@@ -165,7 +169,7 @@
         //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
 
-        spi.Sample();                                                           // sample position sensor
+        spi.Sample(DT);                                                           // sample position sensor
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
@@ -173,7 +177,7 @@
         controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
         controller.dtheta_elec = spi.GetElecVelocity();
-        controller.v_bus = 24.0f;//0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+        controller.v_bus = V_BUS;//b0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
         ///
         
         /// Check state machine state, and run the appropriate function ///
@@ -198,7 +202,8 @@
                 else{
                 /*
                 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);
+                    gpio.
+                    ->write(0);
                     controller.ovp_flag = 1;
                     state = REST_MODE;
                     state_change = 1;
@@ -206,7 +211,7 @@
                     }
                     */  
 
-                torque_control(&controller);    
+                //torque_control(&controller);    
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
@@ -214,24 +219,46 @@
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
+                //controller.i_q_ref = 0.0f;
+                //controller.i_d_ref = 0.0f;
+                
                 //controller.i_q_ref = 2.0f;
-                //controller.i_d_ref = 30.0f;
-                commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
+                //controller.i_d_ref = -30.0f;
+                //controller.kd = .1;
+                //controller.v_des = 25;
+                torque_control(&controller);
+                //controller.i_q_ref = 2.0f;
+                //controller.i_d_ref = -20.0f;
+                commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
                 //TIM1->CCR3 = (PWM_ARR)*(0.5f);                        // Write duty cycles
                 //TIM1->CCR2 = (PWM_ARR)*(0.5f);
                 //TIM1->CCR1 = (PWM_ARR)*(0.5f);
                 controller.timeout += 1;
+
+                //pc.putc(iq);
+                //pc.putc(id);
+                //pc.putc(ang);
+                //pc.printf("\n\r");
                 
                 
                 count++;
+                
                 /*
-                if(count == 4000){
+                if(count == 400){
                      //printf("%d  %d  %.4f  %.4f  %.4f\n\r", controller.adc1_raw, controller.adc2_raw, controller.i_a, controller.i_b, controller.i_c);
-                     printf("%.3f\n\r", controller.dtheta_mech);
+                     printf("%.2f\n\r", 1000.0f*controller.h1_iq_afc_int1);
+                     //pc.putc(id);
+                     //pc.putc(iq);
+                     //pc.putc(ang);
+                     //pc.putc('\n');
+                     //pc.putc('\r');
+                     //printf("%.1f\n", controller.k_q*controller.ki_q*controller.q_int);
                      count = 0;
                      }
                      */
                      
+                    
+                     
                    
                      
             
@@ -288,7 +315,7 @@
                     break;
                 case 'z':
                     spi.SetMechOffset(0);
-                    spi.Sample();
+                    spi.Sample(DT);
                     wait_us(20);
                     M_OFFSET = spi.GetMechPosition();
                     if (!prefs.ready()) prefs.open();
@@ -370,11 +397,11 @@
     wait_us(100);
     drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
     wait_us(100);
-    drv.write_OCPCR(TRETRY_4MS, DEADTIME_100NS, OCP_RETRY, OCP_DEG_4US, VDS_LVL_0_75);
+    drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_4US, VDS_LVL_1_5);
     
     //drv.enable_gd();
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
-    //drv.disable_gd();
+    drv.disable_gd();
 
 
     
@@ -426,12 +453,12 @@
 
 
 
-    printf(" %d\n\r", drv.read_register(DCR));
-    wait_us(100);
-    printf(" %d\n\r", drv.read_register(CSACR));
-    wait_us(100);
-    printf(" %d\n\r", drv.read_register(OCPCR));
-    drv.disable_gd();
+    //printf(" %d\n\r", drv.read_register(DCR));
+    //wait_us(100);
+    //printf(" %d\n\r", drv.read_register(CSACR));
+    //wait_us(100);
+    //printf(" %d\n\r", drv.read_register(OCPCR));
+    //drv.disable_gd();
     
     pc.attach(&serial_interrupt);                                               // attach serial interrupt
     
@@ -440,5 +467,13 @@
 
     int counter = 0;
     while(1) {
+        drv.print_faults();
+       wait(.1);
+        if(state == MOTOR_MODE)
+        {
+            //printf("%.3f\n\r", controller.dtheta_mech);
+            //wait(.001);
+        }
+
     }
 }

--- a/math_ops.cpp	Thu Jul 12 02:50:34 2018 +0000
+++ b/math_ops.cpp	Wed Dec 05 04:07:46 2018 +0000
@@ -1,5 +1,5 @@
 
-#include "math_ops.h"
+#include "../math_ops.h"
 
 
 float fmaxf(float x, float y){
@@ -36,14 +36,21 @@
         *y = *y * limit/norm;
         }
     }
+    
+void limit(float *x, float min, float max){
+    *x = fmaxf(fminf(*x, max), min);
+    }
 
 int float_to_uint(float x, float x_min, float x_max, int bits){
+    /// Converts a float to an unsigned int, given range and number of bits ///
     float span = x_max - x_min;
     float offset = x_min;
-    return (int) ((x+offset)*((float)((1<<bits)-1))/span);
+    return (int) ((x-offset)*((float)((1<<bits)-1))/span);
     }
     
+    
 float uint_to_float(int x_int, float x_min, float x_max, int bits){
+    /// converts unsigned int to float, given range and number of 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	Thu Jul 12 02:50:34 2018 +0000
+++ b/math_ops.h	Wed Dec 05 04:07:46 2018 +0000
@@ -11,6 +11,7 @@
 float fminf3(float x, float y, float z);
 float roundf(float x);
 void limit_norm(float *x, float *y, float limit);
+void limit(float *x, float min, float max);
 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);
 

--- a/structs.h	Thu Jul 12 02:50:34 2018 +0000
+++ b/structs.h	Wed Dec 05 04:07:46 2018 +0000
@@ -1,7 +1,6 @@
 #ifndef STRUCTS_H
 #define STRUCTS_H
 
-//#include "CANnucleo.h"
 #include "mbed.h"
 #include "FastPWM.h"
 
@@ -22,7 +21,7 @@
     float v_bus;                                            // DC link voltage
     float theta_mech, theta_elec;                           // Rotor mechanical and electrical angle
     float dtheta_mech, dtheta_elec, dtheta_elec_filt;       // Rotor mechanical and electrical angular velocit
-    float i_d, i_q, i_q_filt;                               // D/Q currents
+    float i_d, i_q, i_q_filt, i_d_filt;                               // D/Q currents
     float v_d, v_q;                                         // D/Q voltages
     float dtc_u, dtc_v, dtc_w;                              // Terminal duty cycles
     float v_u, v_v, v_w;                                    // Terminal voltages
@@ -36,17 +35,18 @@
     int mode;
     int ovp_flag;                                           // Over-voltage flag
     float p_des, v_des, kp, kd, t_ff;                       // Desired position, velocity, gians, torque
+    float v_ref, fw_int;                                     // output voltage magnitude, field-weakening integral
     float cogging[128];
     } ControllerStruct;
 
 typedef struct{
-    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;
+    float h;
+    float k;
+    float phi;
+    float cos_integral;
+    float sin_integral;
+    float afc_out;
+    } AFCStruct;
+
     
 #endif