Revision 1:64b881306f6f, committed 2018-10-11

Comitter:

austinbrown124

Date:

Thu Oct 11 04:13:45 2018 +0000

Parent:

0:9edd6ec0f56a

Commit message:

DINGBAT

Changed in this revision

--- a/FOC/foc.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/FOC/foc.cpp	Thu Oct 11 04:13:45 2018 +0000
@@ -1,158 +1,118 @@
 
 #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 + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
-    *dtc_v = fminf(fmaxf(((v - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
-    *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+    *dtc_u = fminf(fmaxf(((u - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+    *dtc_v = fminf(fmaxf(((v - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+    *dtc_w = fminf(fmaxf(((w - v_offset)*MODULATION_FACTOR/v_bus + ((DTC_MAX+DTC_MIN)/2)), 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){
+void reset_foc(FocStruct *controller){
     controller->q_int = 0;
     controller->d_int = 0;
     }
 
 
-void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
+void commutate(FocStruct *controller){
        
-       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_a = 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_a = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
-           }
-       controller->i_c = -controller->i_b - controller->i_a;
-       
-       
-       dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
+    controller->loop_count ++;
+//    controller->i_a = inverter->i_a; 
+//    controller->i_b = inverter->i_b; 
+//    controller->i_c = inverter->i_c;
+    
+    
+    float s = sinf(controller->theta_elec); 
+    float c = cosf(controller->theta_elec);
+    //float s = FastSin(controller->theta_elec); 
+    //float c = FastCos(controller->theta_elec);
+                          
+    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);
        
-       ///Cogging Compensation Lookup///
-       //int ind = theta * (128.0f/(2.0f*PI));
-       //float cogging_current = controller->cogging[ind];
-       //float cogging_current = 1.0f*cos(6*theta);
-       ///Controller///
-       float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current;
-       //float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE);   //feed-forward voltage
-       //float v_q_ff = 2.0f*(2*controller->i_q_ref*R_PHASE + controller->dtheta_elec*WB*0.8165f);
-       controller->d_int += i_d_error;   
-       controller->q_int += i_q_error;
+    float i_d_error = controller->i_d_ref - controller->i_d;
+    float i_q_error = controller->i_q_ref - controller->i_q;
+    
+    controller->d_int += i_d_error;   
+    controller->q_int += i_q_error;
+    limit_norm(&controller->d_int, &controller->q_int, V_CLIP/(K_Q*KI_Q));
+        
+    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; 
+    
+    limit_norm(&controller->v_d, &controller->v_q, V_CLIP);
+    
+    if (USE_THETA_ADV) {
+        s = sinf(controller->theta_elec_adv); 
+        c = cosf(controller->theta_elec_adv);
+    }
+    
+    //s = FastSin(controller->theta_elec_adv); 
+    //c = FastCos(controller->theta_elec_adv);
+    
+    controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
+    controller->v_v = ( 0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
+    controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - ( 0.86602540378f*c-.5f*s)*controller->v_q;
+
+    svm(V_BUS, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
+    
+    //controller->dtc_u = 0.2;
+    //controller->dtc_v = 0.2;
+    //controller->dtc_w = 0.8;
+    
+//    focc->dtc_u = controller->dtc_u; 
+//    inverter->dtc_v = controller->dtc_v; 
+//    inverter->dtc_w = controller->dtc_w; 
+    
+
+    if(controller->loop_count >10000){
+        controller->loop_count  = 0;
+    }
+}
+    
+
+
        
-       //v_d_ff = 0;
-       //v_q_ff = 0;
+void voltage_mode_commutate(FocStruct *controller){
+       
+    controller->loop_count ++;
+    
+    float s = sinf(controller->theta_elec); 
+    float c = cosf(controller->theta_elec);
+                           
+    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);
        
-       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);
+    
+    limit_norm(&controller->v_d, &controller->v_q, V_CLIP);
+    
+    if (USE_THETA_ADV) {
+        s = sinf(controller->theta_elec_adv); 
+        c = cosf(controller->theta_elec_adv);
+    }
+    
+    controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
+    controller->v_v = ( 0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
+    controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - ( 0.86602540378f*c-.5f*s)*controller->v_q;
+
+    svm(V_BUS, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
+            
+
+    if(controller->loop_count >10000){
+        controller->loop_count  = 0;
+    }
+}   
        
        
-       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);  
-       
-       //bing1 = (controller->dtc_u);
-       //bing2 = (controller->dtc_v);
-       //bing3 = (controller->dtc_w); 
-       /*
-       //if(gpio->phasing){
-            TIM1->CCR1 = 0x1194*(1.0f-controller->dtc_u);
-            TIM1->CCR2 = 0x1194*(1.0f-controller->dtc_v);
-            TIM1->CCR3 = 0x1194*(1.0f-controller->dtc_w); 
-        }
-        else{
-            TIM1->CCR3 = 0x1194*(1.0f-controller->dtc_u);
-            TIM1->CCR1 = 0x1194*(1.0f-controller->dtc_v);
-            TIM1->CCR2 = 0x1194*(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 >400){
-           //controller->i_q_ref = -controller->i_q_ref;
-           controller->loop_count  = 0;
-           
-           //printf("%d   %f\n\r", ind, cogging_current);
-           //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, ){
     
-    }
-*/    
-
-void voltageabc( 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 = sinf(theta);
-    *b = sinf((2.0f*PI/3.0f)+theta);
-    *c = sinf((-2.0f*PI/3.0f)+theta);
-    }
-       
        
        
        

--- a/FOC/foc.h	Sat May 20 21:42:20 2017 +0000
+++ b/FOC/foc.h	Thu Oct 11 04:13:45 2018 +0000
@@ -2,18 +2,16 @@
 #define FOC_H
 
 #include "structs.h"
-#include "PositionSensor.h"
 #include "mbed.h"
 #include "math.h"
 #include "math_ops.h"
-#include "motor_config.h"
-#include "current_controller_config.h"
+#include "inv_config.h"
+#include "fw_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 voltageabc(float theta, float d, float q, float *a, float *b, float *c);
+void reset_foc(FocStruct *controller);
+void commutate(FocStruct *controller);
+void voltage_mode_commutate(FocStruct *controller);
+
 #endif
\ No newline at end of file

--- a/FastPWM.lib	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/benkatz/code/FastPWM3/#51c979bca21e

--- a/Inverter/Inverter.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/Inverter/Inverter.cpp	Thu Oct 11 04:13:45 2018 +0000
@@ -1,96 +1,218 @@
+#include "Inverter.h"
 
-#include "mbed.h"
-#include "hw_pins.h"
-//#include "hw_config.h"
-#include "structs.h"
-#include "FastPWM.h"
+
+
+//Condensing all hardware related variables into just this file.
+//This includes all the register diddling, assigning duty cycles, etc. 
+//should probably have a class called "inverter" but thats a lot of effort
+
+Inverter::Inverter(){
 
-void Init_PWM(GPIOStruct *gpio){
     
-    printf("\nStarting Hardware Function\n\r");
+    }
+
+void Inverter::Init_PWM(void){
     
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // enable the clock to GPIOC
-    RCC->APB1ENR |= 0x00000001;                                 // enable TIM2 clock
-    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                         // enable TIM1 clock
+    printf("\nStarting Hardware PWM\n\r");
     
-    GPIOC->MODER |= (1 << 10);                                  // set pin 5 to be general purpose output for LED
+    RCC->AHBENR |= RCC_AHBENR_GPIOAEN;                 // enable the clock to GPIOA
+    //RCC->AHBENR |= RCC_AHBENR_IOPAEN;                // the above line is supposed to be this but mbed is borked
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                // enable TIM1 clock
 
-    //gpio->enable = new DigitalOut(ENABLE_PIN);
-    gpio->pwm_ul = new FastPWM(PIN_AL);
-    gpio->pwm_vl = new FastPWM(PIN_BL);
-    gpio->pwm_wl = new FastPWM(PIN_CL);
-    gpio->pwm_uh = new FastPWM(PIN_AH);
-    gpio->pwm_vh = new FastPWM(PIN_BH);
-    gpio->pwm_wh = new FastPWM(PIN_CH);
-
-    gpio->phasing = 1;
     
+    //PWM Setup
+    TIM1->CCMR1 |= 0x7070;                             // Enable output compare 1 and 2 in PWM mode 2 (inverted for low side shunts)
+    TIM1->CCMR2 |= 0x70;                               // Enable output compare 3 in PWM mode 2 (inverted for low side shunts)
+    TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E;  // enable outputs 1, 2, 3
+    //no inverting outputs. PWM mode opposite of Jaredtroller because CH1N is on high side, effectively inverts input
+    //no dead time needed!
+    TIM1->BDTR |= TIM_BDTR_MOE;                         //main output enable = 1
+    TIM1->PSC = 0x0;                                   // no prescaler
+    TIM1->ARR = PWM_ARR;                               // set auto reload, 20 khz
+    TIM1->CR1 |= TIM_CR1_ARPE;                         // autoreload on, 
+    RCC->CFGR3 |= RCC_CFGR3_TIM1SW;                    // bump tim1 up to 144MHz
     
-     //ISR Setup     
-    
-    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);                         //Enable TIM1 IRQ
+    //hardware pin setup
+    GPIOA->MODER   |= GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1 | GPIO_MODER_MODER10_1;
+    GPIOA->AFR[1]    |= 0x00000666;                    // PA8,9,10 to alternate function 6
     
-    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 
+    //interrupt generation
+    NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);   //Enable TIM1 IRQ                     //dafuq is this TIM16 BS?
+    TIM1->DIER |= TIM_DIER_UIE;           // enable update interrupt
+    TIM1->CR1 |= 0x40;                    //CMS = 10, interrupt only when counting up
+    TIM1->RCR |= 0x001;                   // update event once per up/down count of tim1 
     TIM1->EGR |= TIM_EGR_UG;
     
+    TIM1->CR1 |= TIM_CR1_CEN;   //go!
+    
+    //sync with ADCs
+    
+    //nvm changed to Update as trgo2
+    //TIM1->CR2 |= 0x200000;  
+    
     
-        //PWM Setup
-    TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
-    TIM1->CR1 = 0x40;//CMS = 10, interrupt only when counting up
-    
-    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->ARR = 0x1194; // 20 Khz
-    //TIM1->BDTR |= TIM_BDTR_MOE;
-    //TIM1->BDTR |= TIM_BDTR_OSSI;
-    //TIM1->BDTR |= TIM_BDTR_OSSR;
-    TIM1->BDTR |= 0xBF;
-    TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2E | TIM_CCER_CC2NE | TIM_CCER_CC3E | TIM_CCER_CC3NE;
-    //TIM1->CCER |= ~TIM_CCER_CC1NP; //Interupt when low side is on.
-    //TIM1->CCER |= TIM_CCER_CC1NP;
-    TIM1->CR1 |= TIM_CR1_CEN;   
-    
-
     }
 
-void Init_ADC(void){
+void Inverter::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;                        // 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  this is the V phase
-     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-     ADC2->SQR3 = 0x0000000B;                                   // use PC_1 as input. This is the U phase
-     GPIOC->MODER |= 0x0000000f;                                // Alternate function, PC_0, PC_1 are analog inputs 
+    RCC->AHBENR |= RCC_AHBENR_GPIOAEN; 
+    RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
+    RCC->AHBENR |= RCC_AHBENR_ADC12EN;                        // clock for ADC1 and 2 enable 
+    //RCC->AHBENR |= RCC_AHBENR_GPIOAEN;     //page 149 on the ref manual
+    
+    ADC12_COMMON->CCR |= 0x20006;                 // Regular simultaneous mode only, sync clock?
+    
+    
+    /*
+    //for board 1
+    //PA_0 is horrendously noisy!
+    ADC1->SQR1 = 0x80;                            // use PA_1 as input, ADC1 in2
+    ADC2->SQR1 = 0x40;                            // use PA_4 as input, ADC2 in1
+    GPIOA->MODER |= 0x0000030C;                   // Alternate function, PA_1, PA_4 are analog inputs 
 
-    }
+    ADC1->SMPR1 = 256;     //19.5 adc cock cycles for sample
+    ADC2->SMPR1 = 32;     //19.5 adc cock cycles for sample. Found it works better without this
+    
+    ADC2->CR |= ADC_CR_ADEN;
+    ADC1->CR |= ADC_CR_ADEN;
+    */
+
+    //for board 3
+    ADC1->SQR1 = 0x40;                            // use PA_0 as input, ADC1 in1
+    ADC2->SQR1 = 0x40;                            // use PA_4 as input, ADC2 in1
+    GPIOA->MODER |= 0x00000303;                   // Alternate function, PA_0, PA_4 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  
-    }
+    ADC1->SMPR1 = 32;     //19.5 adc cock cycles for sample
+    ADC2->SMPR1 = 32;     //19.5 adc cock cycles for sample. Found it works better without this
+    
+    ADC2->CR |= ADC_CR_ADEN;
+    ADC1->CR |= ADC_CR_ADEN;
+    
+    //may try the sync clock later
 
-void Init_All_HW(GPIOStruct *gpio){
+}
+
+
+
+
+void Inverter::Init(){
     wait_ms(100);
     Init_ADC();
     wait(0.1);         
+    Init_PWM();
     
-    //Init_DAC();
-    wait(0.1);
+    }
+    
+void Inverter::zero_current(){
+    int adc1_offset_s = 0;
+    int adc2_offset_s = 0;
+    int n = 1024;
+    for (int i = 0; i<n; i++){
+        ADC1->CR  |= ADC_CR_ADSTART;  
+        //wait_us(5);
+        for (volatile int t = 0; t < 16; t++) {}
+        adc2_offset_s += ADC2->DR;
+        adc1_offset_s += ADC1->DR;
+        }
+    adc1_offset = adc1_offset_s/n;
+    adc2_offset = adc2_offset_s/n;
+
+    }
+    
+void Inverter::ADCsync() {
+    //EXTEN[1:0] = 01
+    //EXTSEL[3:0] = 1010
+    
+    //this code works to slave to center of TIM1 update
+    ADC1->CFGR |= ADC_CFGR_EXTEN_0 | ADC_CFGR_EXTEN_1;
+    ADC1->CFGR |= ADC_CFGR_EXTSEL_3 | ADC_CFGR_EXTSEL_1;
+    TIM1->CR2 |= TIM_CR2_MMS2_1;
+    ADC1->CR  |= ADC_CR_ADSTART; 
+}
+
+//below is inverter- specific. May have to rearrange i_a and stuff depending on board
+/*
+void Inverter::GetCurrents(FocStruct *focc){
+
+    if (!INVERT_MOTOR_DIR) {
+        focc->i_b = I_SCALE*(float)(adc1_raw - adc1_offset);
+        focc->i_a = I_SCALE*(float)(adc2_raw - adc2_offset);
+        focc->i_c = -focc->i_a - focc->i_b;
+    }
+    else {
+        focc->i_c = I_SCALE*(float)(adc1_raw - adc1_offset);
+        focc->i_a = I_SCALE*(float)(adc2_raw - adc2_offset);
+        focc->i_b = -focc->i_a - focc->i_c;
+    }
+    
+}
+
+void Inverter::SetDutyCycles (FocStruct *focc) {
     
-    Init_PWM(gpio);
+    if (!INVERT_MOTOR_DIR) {
+        TIM1->CCR1 = PWM_ARR*(1.0f - focc->dtc_u); 
+        TIM1->CCR2 = PWM_ARR*(1.0f - focc->dtc_v);
+        TIM1->CCR3 = PWM_ARR*(1.0f - focc->dtc_w);  
+    }
+    else {
+        TIM1->CCR3 = PWM_ARR*(1.0f - focc->dtc_u); 
+        TIM1->CCR2 = PWM_ARR*(1.0f - focc->dtc_v);
+        TIM1->CCR1 = PWM_ARR*(1.0f - focc->dtc_w);  
+    }
+    
+    
+}*/
+
+
+
+void Inverter::GetCurrents(FocStruct *focc){
+
+    if (!INVERT_MOTOR_DIR) {
+        focc->i_a = I_SCALE*(float)(adc1_raw - adc1_offset);
+        focc->i_b = I_SCALE*(float)(adc2_raw - adc2_offset);
+        focc->i_c = -focc->i_a - focc->i_b;
+    }
+    else {
+        focc->i_c = I_SCALE*(float)(adc1_raw - adc1_offset);
+        focc->i_b = I_SCALE*(float)(adc2_raw - adc2_offset);
+        focc->i_a = -focc->i_b - focc->i_c;
+    }
+    
+}
+
+void Inverter::SetDutyCycles (FocStruct *focc) {
     
-    }
\ No newline at end of file
+    if (!INVERT_MOTOR_DIR) {
+        TIM1->CCR3 = PWM_ARR*(1.0f - focc->dtc_u); 
+        TIM1->CCR2 = PWM_ARR*(1.0f - focc->dtc_v);
+        TIM1->CCR1 = PWM_ARR*(1.0f - focc->dtc_w);  
+    }
+    else {
+        TIM1->CCR1 = PWM_ARR*(1.0f - focc->dtc_u); 
+        TIM1->CCR2 = PWM_ARR*(1.0f - focc->dtc_v);
+        TIM1->CCR3 = PWM_ARR*(1.0f - focc->dtc_w);  
+    }
+    
+    
+}
+    
+
+/*for board 3:
+
+CCR3 -> phase C
+so if non inverted, U in FW is A on gd.
+V is CCR2
+so A is ADC1
+
+
+
+
+
+*/  
+    
+    
+    
+    
+    
+      
+    
\ No newline at end of file

--- a/Inverter/Inverter.h	Sat May 20 21:42:20 2017 +0000
+++ b/Inverter/Inverter.h	Thu Oct 11 04:13:45 2018 +0000
@@ -1,12 +1,33 @@
-#ifndef HW_PINS_H
-#define HW_PINS_H
+#ifndef INVERTER_H
+#define INVERTER_H
 
 #include "mbed.h"
+#include "inv_config.h"
 #include "structs.h"
 
-void Init_PWM(GPIOStruct *gpio);
-void Init_ADC(void);
-void Init_DAC(void);
-void Init_All_HW(GPIOStruct *gpio);
+
+
+
+class Inverter {
+public:
+    Inverter();
+    
+    void Init_PWM(void);
+    void Init_ADC(void);
+    void ADCsync(void);
+    void Init(void);
+
+    void zero_current();
+    void GetCurrents(FocStruct *focc);
+    void SetDutyCycles (FocStruct *focc);
+    
+    int adc1_raw, adc2_raw;
+    int adc1_offset, adc2_offset;
+
+private:
+
+    
+
+};
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Inverter/inv_config.h	Thu Oct 11 04:13:45 2018 +0000
@@ -0,0 +1,19 @@
+#ifndef INV_CONFIG_H
+#define INV_CONFIG_H
+
+
+
+
+
+
+#define I_SCALE -0.02014160156f  // Amps per A/D Count
+#define PWM_ARR 3200
+//#define PWM_ARR 2560
+//    1/(.002*1*20*4096/3.3) 
+
+#define INVERT_MOTOR_DIR false
+
+#define BRD_MODEL 3
+
+
+#endif
\ No newline at end of file

--- a/PositionSensor/PositionSensor.cpp	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,272 +0,0 @@
-
-#include "mbed.h"
-#include "PositionSensor.h"
-//#include "offset_lut.h"
-//#include <math.h>
-/*
-PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
-    //_CPR = CPR;
-    _CPR = CPR;
-    _ppairs = ppairs;
-    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;   
-    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;
-    else if(elec > 6.28318530718f) 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(){
-    return raw;
-    }
-    
-float PositionSensorAM5147::GetMechPosition(){
-    return MechPosition;
-    }
-
-float PositionSensorAM5147::GetElecPosition(){
-    return ElecPosition;
-    }
-
-float PositionSensorAM5147::GetMechVelocity(){
-    return MechVelocity;
-    }
-
-void PositionSensorAM5147::ZeroPosition(){
-    rotations = 0;
-    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
- 
-    GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
-    GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
-    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         /*!< GPIO port output speed register,       Address offset: 0x08      */
-    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                         /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
-    GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-    GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15   /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-   
-    // configure TIM3 as Encoder input
-    // 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 = 0x0101;                                                   // 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
-    
-    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->CR2 |= TIM_CR2_TI1S;
-    TIM2->CCER |= TIM_CCER_CC1P;
-    //TIM2->CCER |= TIM_CCER_CC1NP;
-    TIM2->CCER |= TIM_CCER_CC1E;
-    
-    
-    TIM2->CR1 = 0x01;                                                       //CEN,  enable timer
-    
-    TIM3->CR1   = 0x01;                                                     // CEN
-    ZPulse = new InterruptIn(PA_5);
-    ZSense = new DigitalIn(PA_5);
-    //ZPulse = new InterruptIn(PB_0);
-    //ZSense = new DigitalIn(PB_0);
-    ZPulse->enable_irq();
-    ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
-    //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
-    ZPulse->mode(PullDown);
-    flag = 0;
-
-    
-    //ZTest = new DigitalOut(PC_2);
-    //ZTest->write(1);
-    }
-    
-void PositionSensorEncoder::Sample(){
-    
-    }
-
- 
-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.
-    int raw = TIM3->CNT;
-    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
-    if(elec < 0) elec += 6.28318530718f;
-    return elec;
-}
-
-
-    
-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*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
-    if(isinf(meas)){ meas = 1;}
-    out = meas;
-    //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];
-        }
-    velVec[0] = out;
-    return sum/(float)n;
-    }
-    
-float PositionSensorEncoder::GetElecVelocity(){
-    return _ppairs*GetMechVelocity();
-    }
-    
-void PositionSensorEncoder::ZeroEncoderCount(void){
-    if (ZSense->read() == 1 & flag == 0){
-        if (ZSense->read() == 1){
-            GPIOC->ODR ^= (1 << 4);   
-            TIM3->CNT = 0x000;
-            //state = !state;
-            //ZTest->write(state);
-            GPIOC->ODR ^= (1 << 4);
-            //flag = 1;
-        }
-        }
-    }
-
-void PositionSensorEncoder::ZeroPosition(void){
-    
-    }
-    
-void PositionSensorEncoder::ZeroEncoderCountDown(void){
-    if (ZSense->read() == 0){
-        if (ZSense->read() == 0){
-            GPIOC->ODR ^= (1 << 4);
-            flag = 0;
-            float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
-            if(dir != dir){
-                dir = dir;
-                rotations +=  dir;
-                }
-
-            GPIOC->ODR ^= (1 << 4);
-
-        }
-        }
-    }
-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	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,65 +0,0 @@
-#ifndef POSITIONSENSOR_H
-#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, raw;
-    //int state;
-    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 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];
-
-};*/
-
-#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fw_config.h	Thu Oct 11 04:13:45 2018 +0000
@@ -0,0 +1,58 @@
+#ifndef FW_CONFIG_H
+#define FW_CONFIG_H
+
+//hardware stuff
+#define R_S 0.027f            //Ohms
+#define L_S 0.00004f            //Henries
+
+//#define KT .07f                 //N-m per peak phase amp (= RMS amps/1.5)
+#define POLE_PAIRS 7                 //Number of pole pairs
+#define ENC_TICKS_PER_REV 8191 //wank
+//#define WB KT/NPP               //Webers.  
+
+
+
+//current controler settings
+#define K_D 0.25f                     // Volts/Amp
+#define K_Q 0.25f                     // Volts/Amp
+
+#define KI_D 0.03f                  // 1/samples
+#define KI_Q 0.03f                  // 1/samples
+
+/*
+#define K_D 8.1f                     // Volts/Amp
+#define K_Q 8.1f                     // Volts/Amp
+#define KI_D 0.02f                  // 1/samples
+#define KI_Q 0.02f                  // 1/samples
+*/
+#define V_BUS 14.0f                // Volts, actual bus voltage
+#define V_CLIP 14.0f        // Volts, This is when the controller will nope out- not actually the real bus voltage
+//#define V_BUS 6.0f                // Volts, actual bus voltage
+//#define V_CLIP 6.0f        // Volts, This is when the controller will nope out- not actually the real bus voltage
+
+#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 MODULATION_FACTOR 0.574f // //perfect with zero clipping if v = v_bus, actually equal to 0.57735f * (DTC_MAX-DTC_MIN)
+#define MODULATION_FACTOR 0.57f // //perfect with zero clipping if v = v_bus, actually equal to 0.57735f * (DTC_MAX-DTC_MIN)
+//#define MODULATION_FACTOR 0.45f // //perfect with zero clipping if v = v_bus, actually equal to 0.57735f * (DTC_MAX-DTC_MIN)
+#define USE_THETA_ADV false
+#define MAX_AMPS 20.0f
+#define DT 0.00005f
+#define F_SW 20000
+
+
+#define DTC_MAX 0.93f          // Max phase duty cycle
+#define DTC_MIN 0.07f          // Min phase duty cycle
+
+
+#define FL_OBS_LOWSPEED_MULT 0.995 //0.9992^2500 = 0.13 so lowpass will go to 0.18 every 8hz
+#define FL_OBS_HIGHSPEED_MULT 0.95 //a lot faster
+
+
+
+
+
+
+#endif
+

--- a/hw_setup/current_controller_config.h	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,21 +0,0 @@
-#ifndef CURRENT_CONTROLLER_CONFIG_H
-#define CURRENT_CONTROLLER_CONFIG_H
-
-#define K_D .05f                     // Volts/Amp
-#define K_Q .05f                     // Volts/Amp
-#define KI_D 0.04f                  // 1/samples
-#define KI_Q 0.04f                  // 1/samples
-#define V_BUS 24.0f                 // Volts
-
-#define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples
-#define Q_INT_LIM V_BUS/(K_Q*KI_Q)  // Amps*samples
-
-#define DTC_MAX 0.95f          // Max phase duty cycle
-#define DTC_MIN 0.05f          // Min phase duty cycle
-
-#define I_SCALE 0.02014160156f  // Amps per A/D Count
-//    1/(.002*1*20*4096/3.3) 
-
-
-
-#endif

--- a/hw_setup/hw_pins.h	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#ifndef HW_PINS_H
-#define HW_PINS_H
-
-#define PIN_AL PA_8
-#define PIN_BL PA_9
-#define PIN_CL PA_10
-#define PIN_AH PB_13    //U, the top fet
-#define PIN_BH PB_14
-#define PIN_CH PB_15    //W, the bottom fet with no shunt amp
-
-
-
-#endif
\ No newline at end of file

--- a/hw_setup/motor_config.h	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,16 +0,0 @@
-#ifndef MOTOR_CONFIG_H
-#define MOTOR_CONFIG_H
-
-//#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 POLE_PAIRS 7                 //Number of pole pairs
-
-//#define WB KT/NPP               //Webers.  
-
-#define ENC_TICKS_PER_REV 8192
-
-
-
-#endif

--- a/main.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/main.cpp	Thu Oct 11 04:13:45 2018 +0000
@@ -1,241 +1,435 @@
 #include "mbed.h"
-#include "PositionSensor.h"
 #include "structs.h"
+#include "fw_config.h"
+#include "Inverter.h"
+#include "foc.h"
+
+
+
+//changes to make:
+//enable pin, LED
+//reshuffle currents and phases
+//clockpin dang should have put on a test pad
+
+
+/*if (BRD_MODEL == 1) {
+    DigitalOut clockpin(PA_11); //correct for ESCMK2
+    DigitalOut led1(PB_0);      //
+    DigitalIn gpio1(PB_1);      // switch
+    DigitalOut en_gate(PB_7);  
+}*/
+
+//if (BRD_MODEL == 3) {
+    DigitalOut clockpin(PB_1); //correct for ESCMK2
+    DigitalOut led1(PB_7);      //
+    //DigitalIn gpio1(PA_1);      // switch
+    DigitalIn gpio1(PB_0);      // switch
+    DigitalOut en_gate(PA_12); 
+//}
+
+SPI spi(PB_5, PB_4, PB_3); // mosi, miso, sclk
+DigitalOut drv_cs(PA_15);  // all correct for ESCMK2
+DigitalIn drv_fault(PB_6);  //correct
+AnalogOut dout(PA_5);   //   DAC
+
+DigitalIn forward_button(PA_6);      // forward
+DigitalIn backward_button(PA_7);      // backward
+
+// controller modes
+#define REST_MODE 0
+#define TEST_MODE 4
+
+
+FocStruct focc;
+//InverterStruct inverter;
+
+float theta_offset = 5.424f;
+
+
+float theta = 0.0f;
+float offset_est = 0.0f;
+float offset_est_agg = 0.0f;
+float fake_theta = 0.0f;
+
+float elec_pos = 0;
+float e_rad_sec = 0;
+float delta_epos_rev = 0;
+float delta_epos_avg = 0;
+float elec_pos_smooth = 0;
+float smooth_pos_weight = 0.85;
+float adv = 0.45;
+
+volatile int count = 0;
+volatile int main_int_count = 0;
+volatile unsigned int main_int_clock = 0;
+
+float current_setpoint = 3.0f;
+float full_amp = 20.0f;
+
+int controller_state = 0;
+
+int a = 0;
+int b1 = 0;
+int b2 = 0;
+int b3 = 0;
+int b4 = 0;
+float a1 = 0;
+float last_pos = 0;
+float a2 = 0;
+
+
+Serial pc(PA_2, PA_3);
+Inverter inverter;
+
+
+void Init_DRV8323();
+void get_DRV8323_status();
+
+
+//data logging crap
+int data_count = 0;
+#define DATA_LEN 500
+short data_array1[DATA_LEN];
+short data_array2[DATA_LEN];
+
+void print_data_array_short();
+void log_data_short( short input1, short input2 );
+
+
+// Main 20khz loop Interrupt ///
+
+extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
+  
+  if (TIM1->SR & TIM_SR_UIF ) {
+
+        
+        //ADC1->CR  |= ADC_CR_ADSTART;  //Begin sample and conversion
+        main_int_count++;
+        main_int_clock++;
+
+        for (volatile int t = 0; t < 10; t++) {}
+        
+        
+        
+        inverter.adc2_raw = ADC2->DR;
+        inverter.adc1_raw = ADC1->DR;
+        clockpin = 1;
+        
+        
+        /// Check state machine state, and run the appropriate function ///
+        switch(controller_state){
+            case REST_MODE:   //nothing 
+                TIM1->CCR1 = 3000*(0.5f);  
+                TIM1->CCR2 = 3000*(0.5f);
+                TIM1->CCR3 = 3000*(0.5f);  
+                count++;
+                if(count > 20000){
+                    count = 0;                                                    
+                    led1 = !led1;
+                }
+                //focc.theta_elec = 0;             
+                //commutate(&focc, focc.theta_elec); 
+                
+            break;
+                
+            
+            case TEST_MODE: 
+                //inverter.GetCurrents(&focc);
+                
+                float i_b_f = I_SCALE*(float)(inverter.adc2_raw - inverter.adc2_offset);
+                int   i_b_i = (inverter.adc2_raw - inverter.adc2_offset);
+                TIM1->CCR2 = 3200;
+                TIM1->CCR1 = 3200;
+                
+                
+                
+                /*
+                structure of each test:
+                1. make current rise to setpoint, hyst. oscillation range to +1a -a1
+                2. begin data logger
+                3. log til end of log
+                4. when log is done, enter rest mode
+                */
+                
+                //TIM1->CCR1 = 3000*(0.0f);  //unused phase- the one with no shunt on it
+                
+                //TIM1->CCR2 = 3000*(0.5f);  //phase upon which current will be sensed
+                //TIM1->CCR3 = 3000*(0.5f);  //phse which will be brought high
+                
 
-#include "Inverter.h"
-//#define PI 3.14159f
+                if ( ( !backward_button) && (current_setpoint <= full_amp)) {
+                    
+                    log_data_short( TIM1->CCR3, i_b_i);
+                    
+                    if ((data_count >= DATA_LEN) | (data_count <= 40))  {  //no logging happening atm, or beginning of log
+                        
+                        if (i_b_f < current_setpoint) {
+                            TIM1->CCR3 = 0; //pull phase high
+                        }
+                        else {
+                            TIM1->CCR3 = 3200; //pull phase low
+                        }
+                    }
+                    else {                                                //its logging time
+                        if (i_b_f > (current_setpoint + 1.5f)) {a = 0;}   //current is too high
+                        if (i_b_f < (current_setpoint - 1.5f)) {a = 1;}
+                    
+                        if (a == 1) {
+                            TIM1->CCR3 = 0;    //pull phase high
+                        }
+                        else {  //a = 0;
+                            TIM1->CCR3 = 3200; //pull phase low
+                        }
+                        
+                        
+                    }
+                    
+                
+                }
+                else {
+                    TIM1->CCR3 = 3200;
+                    a = 1;
+                }
+                
+                
+                
+                
+                inverter.GetCurrents(&focc);
+                
+            break;
+            
+
+
+        }  
+            
+      }
+            //b = TIM1->CNT;
+  TIM1->SR = 0x0;  
+  clockpin = 0;                                                             // reset the status register
+}
+
+int main() {
+    wait_ms(200);
+    pc.baud(256000);
+    printf("\rStarting Hardware\n");
+    gpio1.mode(PullUp);
+    forward_button.mode(PullUp);
+    backward_button.mode(PullUp);
+
+    Init_DRV8323();
+    
+    RCC->APB2ENR |= RCC_APB2ENR_TIM17EN;
+    TIM17->PSC = 71;
+    wait_ms(100);
+    TIM17->CR1 |= TIM_CR1_CEN;
+    
+    inverter.Init();                     // Setup PWM, ADC
+    wait(0.1);
+    controller_state = REST_MODE;
+    wait(0.1);
+    inverter.zero_current();
+    wait(0.1);
+    pc.printf("ADCs zeroed at ");
+    pc.printf("%i, %i \n",inverter.adc1_offset,inverter.adc2_offset);                                                 
+    wait(0.1);
+    inverter.ADCsync();
+    focc.i_d_ref = 0.0f;
+    //en_gate = 0;
+    
+    
+    //disable_inverter();
+    //DigitalIn ranan(PA_0);
+    //controller_state = POS_MODE;
+    //controller_state = TORQUE_MODE;
+    controller_state = TEST_MODE;
+    //controller_state = OFFSET_LEARN_MODE;
+    
+    while(1) {
 
-#include "foc.h"
+        wait_us(2);
+        
+        while (1==1) {
+            
+        wait_us(30);
+        
+        //en_gate = 0;
+        //wait(3);
+        //en_gate = 1;
+        //drv_cs = 1; wait_us(1); drv_cs = 0;
+        //spi.write((0x00<<15) + (0x02<<11) + (0x01<<5));  //set control register for 3x PWM
+         //drv_cs = 1;
+         //Init_DRV8323();
+        //wait(3);
+        
+        //bayley_printf();
+        //while (1==1) {}
+        while (1==1) {
+            //if ((data_count == DATA_LEN) && (e_rad_sec > 40)) {
+            if ((data_count == DATA_LEN) ) {
+                pc.printf(" 0,0 \n");
+                print_data_array_short();
+                current_setpoint += 0.33333333333f;
+                data_count = 0;
+                
+                if (current_setpoint > full_amp){
+                    pc.printf(" 4000,4000 \n");
+                    current_setpoint = 3.0f;
+                    led1 = 1;
+                    wait(5);
+                    led1 = 0;
+                    
+                }
+                
+                //wait_ms(50);
+            }
+            //if (drv_fault) {
+            //    pc.printf(" 0,0,0,0,0,0,0 \n");
+            //}
+        
+        }
+       
+        //printf("%i,  ", TIM3->CNT);
+        //printf("%i,  ", TIM1->CCR1);
+        //printf("%i, ", TIM3->CNT);
+        //printf("%X, ", GPIOA->MODER);
+        //if (!gpio1) {
+        
+
+        //printf("%f,  ", offset_est); 
+        //printf("%f,  ", focc.theta_elec);
+        //printf("%i,  ", TIM1->CCR1);
+        //printf("%i,  ", count);
+        
+        printf("%f, ",focc.i_b);
+        //printf("%f, ",focc.i_b*100);
+        //printf("%f, ",focc.theta_elec*1000);
+
+        //printf("%f, ",focc.v_q);
+        //printf("%f, ",focc.v_d);
+
+        //printf("%f, ",focc.v_q);
+        //printf("%f, ",focc.v_d);//*/
+        printf("%f, ",focc.i_d);
+        //printf("%f, ",focc.dtc_u);
+        //printf("%f,  ", encoder.GetElecVel()/7/6.28 );
+        //printf("%f,  ", a1 );
+        printf("%i,  ", ADC1->DR);  
+        printf("%i,  ", ADC2->DR);
+        //printf("    %i, %i %i", a1, a2, a3);
+
+        printf("\r");
+        /*
+        printf("%x,  ", ADC12_COMMON->CCR);
+        printf("%x,  ", ADC1->SQR1);
+        printf("%x,  ", ADC2->SQR1);
+        printf("%x,  ", GPIOA->MODER);
+        printf("%i,  ", ADC1->SMPR1);
+        printf("%i,  ", ADC2->SMPR1);
+        printf("\r");
+        printf("\r");
+        printf("%f, ",focc.i_b);
+        printf("%f, ",focc.dtc_u);
+        while (1==1) {}*/
+
+        //}
+        //if (!drv_fault) {printf(" DRV fault "); clockpin = 1; get_DRV8323_status();while (1==1) {}}
+
+
+
+        }
+
+    }
+}
 
 
 
 
-DigitalOut clockpin(PC_12);
-//AnalogIn   pot_in(PC_3);
-
-
-// controller modes
-#define REST_MODE 0
-#define OLSINE 1
-#define ONEUPTWODOWN 2
-#define VMODESINE 3
-//#define BEN_CALIBRATION_MODE 1
-#define TORQUE_MODE 4
-//#define PD_MODE 5
-//#define SETUP_MODE 6
-//#define ENCODER_MODE 7
- 
-
-GPIOStruct gpio;
-ControllerStruct controller;
 
 
-//float fake_theta = 0.0;
-float theta1 = 0.0f;
-//float theta_offset = 4.708f;
-//float theta_offset = 3.38f;
-//float theta_offset = 4.97f;
-float theta_offset = 4.8f;
-
-int bing1 = 0;
-int bing2 = 0;
-int bing3 = 0;
-
-int var1 = 0;
-int var2 = 0;
-
-float a = 0.0f;
-float b = 0.0f;
-float c = 0.0f;
-
-volatile int count = 0;
-
-int controller_state = ONEUPTWODOWN;//VMODESINE;
-
-Serial pc(PA_2, PA_3);
-
-PositionSensorEncoder encoder(ENC_TICKS_PER_REV, 0, POLE_PAIRS); 
-
-
-// Main 20khz loop Interrupt ///
-/// This runs at 20 kHz, regardless of of the mode the controller is in, because it is triggered by hw timers ///
-extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
-  //clockpin = 1;
-  
-  if (TIM1->SR & TIM_SR_UIF ) {
-        
-        
-        ///Sample current always ///
-        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;
 
 
-        
-        /// Check state machine state, and run the appropriate function ///
-        //printf("%d\n\r", state);
-        switch(controller_state){
-            case REST_MODE:   //nothing 
-                if(count > 1000){
-                    count = 0;                                                    
-                    printf("Rest Mode");
-                    printf("\n\r");
-                }
-                break;
-            
-            case OLSINE:        // open loop sines
+void Init_DRV8323() {
 
-                //theta1+= 0.1f*pot_in;
-                theta1+= 0.001f;
-                if (theta1 > 2*PI) { theta1-=(2*PI); }
-    
-                //trigger clock pin sychronous with electrical revolutions
-                if (theta1 > PI) { clockpin = 1;  }
-                else {clockpin = 0;}
-
-                a = cosf(theta1)/2 + 0.5f;
-                b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
-                c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
+    drv_cs = 1;
 
-                TIM1->CCR1 = 0x1194*(a);
-                TIM1->CCR2 = 0x1194*(b);
-                TIM1->CCR3 = 0x1194*(c);
-                break;
-                
-            case ONEUPTWODOWN:        // one up, two down
-        
-                a = 0.8f;
-                b = 0.2f;
-                c = 0.2f;
-                
-                count++;                       
-                if(count > 500){
-                    count = 0;
-                    //printf("%f  ",encoder.GetElecPosition());
-                }
-                
-                TIM1->CCR1 = 0x1194*(a);
-                TIM1->CCR2 = 0x1194*(b);
-                TIM1->CCR3 = 0x1194*(c);
-                
-                break;
-                
-            case VMODESINE:        // position mode sines
-                theta1 = encoder.GetElecPosition() + theta_offset;
-
-                if (theta1 > 2*PI) {
-                    theta1-=(2*PI);
-                }
+    spi.format(16, 1);
+    spi.frequency(1000000);
+    en_gate = 1;
+    wait_ms(20);
+    en_gate = 0;  //clear residual faults
+    wait_us(20);
+    en_gate = 1;
     
-                //trigger clock pin sychronous with electrical revolutions
-                if (encoder.GetElecPosition() > PI) { clockpin = 1;  }
-                else {clockpin = 0;}
-
-                a = cosf(theta1)/2 + 0.5f;
-                b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
-                c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
+    wait_ms(20);
+    drv_cs = 1; wait_us(1); drv_cs = 0;
+    int fault1 = 0;
+    fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
+    drv_cs = 1; wait_us(1); drv_cs = 0;
 
-                TIM1->CCR1 = 0x1194*(a);
-                TIM1->CCR2 = 0x1194*(b);
-                TIM1->CCR3 = 0x1194*(c);
-                
-                
-                //remove this code later
-                //print Q and D values
-                count++;
-                          // Run current loop
-                //spi.Sample();                                                   // Sample position sensor
-                if(count > 1000){
-                    count = 0;
-                    //printf("%f  ",controller.i_q);
-                    //printf("%f  ",controller.i_d);
-                    //printf("\n\r");
-                    }
-                
-                break;
-             
-            case TORQUE_MODE: 
-             
-                // Run torque control
-                count++;
-                controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
-                commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
-                //spi.Sample();                                                   // Sample position sensor
-                if(count > 1000){
-                    count = 0;
-                    //readCAN();
-                    //controller.i_q_ref = ((float)(canCmd-1000))/100;
-                    controller.i_q_ref = 2;
-                    //pc.printf("%f\n\r ", controller.theta_elec);
-            
-                    //printf("%i  ",controller.adc1_raw);
-                    //printf("%i \n\r ",controller.adc2_raw);
+    while ((fault1>>10) > 0) {
+        fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
+        pc.printf("Fault Detected!! 0x00=0x%X ", fault1);wait(2); 
+        drv_cs = 1; wait_us(1); drv_cs = 0;
         }
-                }
-                
+    drv_cs = 1; wait_us(1); drv_cs = 0;
+    spi.write((0x00<<15) + (0x02<<11) + (0x01<<5));  //set control register for 3x PWM
+    //drv_cs = 1; wait_us(1); drv_cs = 0;
+    //fault1 = spi.write((0x01<<15) + (0x02<<11));
+    //pc.printf("DRV Setup 0x00=0x%X ",fault1);
+    drv_cs = 1; 
+    //spi.write((0x00<<15) + (0x05<<11) + (0x03));  //set OCP register for 0.26v OCP
+    //drv_cs = 1; wait_us(1); drv_cs = 0;    //enable once bridge is working. 
+    
     
-        ///*
-        
-        controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
-        commutate(&controller, &gpio, controller.theta_elec); 
-        
-        
-        
-        //*/
-            
-      }
-  TIM1->SR = 0x0;  
-  //clockpin = 0;                                                             // reset the status register
+    }
+
+void get_DRV8323_status() { 
+    wait_ms(20);
+    drv_cs = 1; wait_us(1); drv_cs = 0;
+    int fault1 = 0;
+    fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
+    drv_cs = 1; wait_us(50);   
+    
+    pc.printf("Fault1 0x00=0x%X \n\r",fault1);
+    
+    fault1 = 0;
+    wait_us(1); drv_cs = 0;
+    fault1 = spi.write((0x01<<15) + (0x01<<11));  //status register 1
+    drv_cs = 1; wait_us(50);   
+    
+    pc.printf("Fault2 0x00=0x%X \n\r",fault1);
+    
 }
 
-int main() {
-    //float meas;
-    pc.baud(115200);
-    printf("\nStarting Hardware\n");
-    Init_All_HW(&gpio);
-    
-    zero_current(&controller.adc1_offset, &controller.adc2_offset);                                                        // Setup PWM, ADC, GPIO
-    wait(0.1);
+ 
     
     
-    while(1) {
-        //printf("AnalogIn example\n");
-        //printf("%f  ",a);
-        //printf("\n");
-        wait(0.1);
-        
-        if (0==0) {
-        //printf("%i  ", TIM3->CNT);
-        
-        printf("%f  ",encoder.GetElecPosition());
-        //printf("%f  ",encoder.GetMechPosition());
-        
-        printf("%f  ",theta_offset);
-        
-        printf("%i  ",controller.adc1_raw);
-        printf("%i  ",controller.adc2_raw);
-        
-        printf("%i  ",controller.adc1_offset);
-        printf("%i  ",controller.adc2_offset);
-        
-        printf("%f  ",controller.i_q);
-        printf("%f  ",controller.i_d);
-        
-        printf("%f  ",controller.i_a);
-        printf("%f  ",controller.i_b);
-        
-        //printf("%f  ",controller.theta_elec);
-        
-        
-        
-        printf("\n\r");
-        
+//hyper speed logger
+void log_data_short( short input1, short input2 )  {
+
+        if (data_count < DATA_LEN) {
+            data_array1[data_count] = input1;
+            data_array2[data_count] = input2;
+            data_count++;
         }
-        
-        
-        
-        
+}
+
+
+void print_data_array_short() {
+
+    for( int a = 0; a < DATA_LEN; a++ ) {
+        pc.printf("%i, %i",data_array1[a],data_array2[a]);
+        pc.printf("\n");
     }
+    //pc.printf("\n");
 }
+
+
+void enable_inverter() {
+    en_gate = 1;
+    }
+    
+void disable_inverter() {
+    en_gate = 1;
+    }
+    

--- a/math_ops/math_ops.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/math_ops/math_ops.cpp	Thu Oct 11 04:13:45 2018 +0000
@@ -25,3 +25,13 @@
         *y = *y * limit/norm;
         }
     }
+    
+void limit_abs(float *x, float limit){
+    limit = abs(limit);
+    if(*x > limit){
+        *x = limit;
+        }
+    if(*x < -limit){
+        *x = -limit;
+        }
+    }

--- a/math_ops/math_ops.h	Sat May 20 21:42:20 2017 +0000
+++ b/math_ops/math_ops.h	Thu Oct 11 04:13:45 2018 +0000
@@ -10,5 +10,6 @@
 float fmaxf3(float x, float y, float z);
 float fminf3(float x, float y, float z);
 void limit_norm(float *x, float *y, float limit);
+void limit_abs(float *x, float limit);
 
 #endif

--- a/mbed.bld	Sat May 20 21:42:20 2017 +0000
+++ b/mbed.bld	Thu Oct 11 04:13:45 2018 +0000
@@ -1,1 +1,1 @@
-https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb
\ No newline at end of file

--- a/structs.h	Sat May 20 21:42:20 2017 +0000
+++ b/structs.h	Thu Oct 11 04:13:45 2018 +0000
@@ -1,48 +1,35 @@
 #ifndef STRUCTS_H
 #define STRUCTS_H
 
-//#include "CANnucleo.h"
 #include "mbed.h"
-#include "FastPWM.h"
-
 
-typedef struct{
-    DigitalOut *enable;
-    FastPWM *pwm_ul, *pwm_vl, *pwm_wl, *pwm_uh, *pwm_vh, *pwm_wh;
-    int phasing;
-    } GPIOStruct;
     
 typedef struct{
-    
-    }COMStruct;
-    
-typedef struct{
-    int adc1_raw, adc2_raw;
+
     float i_a, i_b, i_c;
     float v_bus;
-    float theta_mech, theta_elec;
-    float dtheta_mech, dtheta_elec;
+    float theta_mech, theta_elec, theta_elec_adv;
     float i_d, i_q;
     float v_d, v_q;
     float dtc_u, dtc_v, dtc_w;
     float v_u, v_v, v_w;
+    float test_d, test_q;
     float d_int, q_int;
-    int adc1_offset, adc2_offset;
     float i_d_ref, i_q_ref;
     int loop_count;
     int mode;
-    float cogging[128];
-    } ControllerStruct;
+    } FocStruct;
+    
+
+    
+    
 
 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;
+    int can_dead_count;
+    float throttle_frac;
+    int status;
+    } CommandStruct;
+
+
     
 #endif