Revision 1:94193b31f0ee, committed 2019-04-06

Comitter:

austinbrown124

Date:

Sat Apr 06 02:16:12 2019 +0000

Parent:

0:9edd6ec0f56a

Commit message:

first commit

Changed in this revision

diff -r 9edd6ec0f56a -r 94193b31f0ee FOC/foc.cpp
--- a/FOC/foc.cpp	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,161 +0,0 @@
-
-#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);
-    
-    }
-
-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_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
-       
-       ///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;
-       
-       //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);  
-       
-       //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);
-    }
-       
-       
-       
-       
-       
-    
-    
\ No newline at end of file

diff -r 9edd6ec0f56a -r 94193b31f0ee FOC/foc.h
--- a/FOC/foc.h	Sat May 20 21:42:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#ifndef FOC_H
-#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"
-
-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);
-#endif
\ No newline at end of file

diff -r 9edd6ec0f56a -r 94193b31f0ee FastPWM.lib
--- 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

diff -r 9edd6ec0f56a -r 94193b31f0ee Inverter/Inverter.cpp
--- a/Inverter/Inverter.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/Inverter/Inverter.cpp	Sat Apr 06 02:16:12 2019 +0000
@@ -1,96 +1,111 @@
+#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->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;
     
-    
-     //ISR Setup     
-    
-    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);                         //Enable TIM1 IRQ
+    //PWM Setup
+    TIM1->CCMR1 |= 0x0070;                             // Enable output compare 1 PWM mode 2 (inverted for low side shunts)
+    TIM1->CCER |= TIM_CCER_CC1E;                        // enable outputs 1
+    //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
     
-    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 
+    //hardware pin setup
+    GPIOA->MODER   |= GPIO_MODER_MODER8_1 ;
+    GPIOA->AFR[1]    |= 0x00000006;                    // PA8 to alternate function 6
+    
+    //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;
     
-    
-        //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;   
-    
+    TIM1->CR1 |= TIM_CR1_CEN;   //go!
 
     }
 
-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 |= 0x20001;                 // Regular simultaneous mode plus injected conversions
+    
+
+    //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 
 
-    }
+    ADC1->SMPR1 = 32;     //19.5 adc cock cycles for sample
+    ADC2->SMPR1 = 32;     //19.5 adc cock cycles for sample. 
+    
+    ADC2->CR |= ADC_CR_ADEN;
+    ADC1->CR |= ADC_CR_ADEN;
+    
+    wait_ms(10);
 
-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  
-    }
+}
+
 
-void Init_All_HW(GPIOStruct *gpio){
+
+
+void Inverter::Init(){
     wait_ms(100);
     Init_ADC();
     wait(0.1);         
+    Init_PWM();
     
-    //Init_DAC();
-    wait(0.1);
+    }
     
-    Init_PWM(gpio);
+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;
+
+    }
     
-    }
\ No newline at end of file
+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; 
+}
+
+
+    
\ No newline at end of file

diff -r 9edd6ec0f56a -r 94193b31f0ee Inverter/Inverter.h
--- a/Inverter/Inverter.h	Sat May 20 21:42:20 2017 +0000
+++ b/Inverter/Inverter.h	Sat Apr 06 02:16:12 2019 +0000
@@ -1,12 +1,31 @@
-#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();
+    
+    int adc1_raw, adc2_raw;
+    int adc1_offset, adc2_offset;
+
+private:
+
+    
+
+};
 
 #endif
\ No newline at end of file

diff -r 9edd6ec0f56a -r 94193b31f0ee Inverter/inv_config.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Inverter/inv_config.h	Sat Apr 06 02:16:12 2019 +0000
@@ -0,0 +1,13 @@
+#ifndef INV_CONFIG_H
+#define INV_CONFIG_H
+
+
+
+#define I_SCALE 0.02014160156f  // Amps per A/D Count
+#define PWM_ARR 6400
+//#define PWM_ARR 2560
+//    1/(.002*1*20*4096/3.3) 
+
+
+
+#endif
\ No newline at end of file

diff -r 9edd6ec0f56a -r 94193b31f0ee PositionSensor/PositionSensor.cpp
--- 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));
-    }

diff -r 9edd6ec0f56a -r 94193b31f0ee PositionSensor/PositionSensor.h
--- 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

diff -r 9edd6ec0f56a -r 94193b31f0ee ServoInput/ServoInput.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ServoInput/ServoInput.cpp	Sat Apr 06 02:16:12 2019 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+#include "ServoInput.h"
+
+//Takes servo inputs on PA2
+
+ServoTimer::ServoTimer() {
+    
+    RCC->APB2ENR |= RCC_APB2ENR_TIM15EN;                                 // enable TIM2 clock
+    
+    __GPIOA_CLK_ENABLE(); //PA2 TIM15 Servo Input
+     
+    GPIOA->MODER   |= GPIO_MODER_MODER2_1;                                 //PA2 as Alternate Function 
+    GPIOA->OTYPER  |= GPIO_OTYPER_OT_2;                                    //PA2  as Inputs               
+    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR2;                              //Low speed                         
+    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR2_1;                                 //Pull Down                     
+    //GPIOA->AFR[0]  |= 0x00000900 ;                                          //AF0          
+    GPIOA->AFR[0]  |= 0x00000900 ;                                          //AF0  but Mbed is messed up. 
+    GPIOA->AFR[0]  &= ~0x00000600 ;                                          //AF0  but Mbed is messed up.     
+    GPIOA->AFR[1]  |= 0x00000000 ;                                          //AF1
+    
+    __TIM15_CLK_ENABLE();
+    
+    TIM15->PSC = 71;                   //timer counts a 1Mhz
+    
+    
+    //TIM15->SMCR = 0b0000000001000100;   //TS = 100 (edge detection), SMS = 100 (trigger on counter reset)
+    //TIM15->OR |= 0x2;
+    
+    //setting it up in PWM input capture mode
+    //set CR1 to input mode?
+    
+    TIM15->SMCR = 0b0000000001010100;   //TS = 101 (input 1 detection), SMS = 100 reset mode
+    
+    
+    TIM15->CCMR1 |= 0x200;                            // CC2S = 10, IC2 mapped to TI1
+    TIM15->CCMR1 |= 0x1;                              // CC1S = 01, IC1 mapped to TI1
+    TIM15->CCER |= TIM_CCER_CC1E | TIM_CCER_CC2E;     // Enable OC1 and OC2
+    TIM15->CCER |= TIM_CCER_CC2P | TIM_CCER_CC2NP;    // set TI1FP2 to active on falling edge
+    
+    TIM15->CCMR1 |= TIM_CCMR1_IC1F_3;  //enable some filtering
+
+    TIM15->CR1 = 0x01;
+    
+    trash_input_count = 2;
+    oldCCR1 = 0;
+    servo_sig = 0;
+    
+    
+   
+}
+
+void ServoTimer::update_servo_input() {
+    
+    if (TIM15->CNT > 40000) {TIM15->CNT = 40000; trash_input_count=30;}  //pin stuck in some state
+    
+    
+    if (((TIM15->SR)>>1) & (0x01)) {                  //if an edge has recently occurred
+        
+        if ((TIM15->CCR2 < 2000)&(TIM15->CCR2 > 800)) {
+        //servo_sig = 0.95f*servo_sig + 0.05f*(float)TIM15->CCR2;
+        
+        trash_input_count = trash_input_count - 2; 
+        
+            if (trash_input_count < 20) {
+                servo_sig = TIM15->CCR2;
+            }
+        }
+        else {
+            trash_input_count++; 
+        }
+    }
+    
+    
+    
+
+    
+    if (trash_input_count>=20) { servo_sig = 800; }
+    
+    if (trash_input_count>=30) { trash_input_count=20; }
+    if (trash_input_count<=0) { trash_input_count=0; }
+    
+}
+
+int ServoTimer::get_servo_input() {
+    return servo_sig;
+}
+
+int ServoTimer::get_trash_input() {
+    return trash_input_count;
+}
+

diff -r 9edd6ec0f56a -r 94193b31f0ee ServoInput/ServoInput.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ServoInput/ServoInput.h	Sat Apr 06 02:16:12 2019 +0000
@@ -0,0 +1,27 @@
+#ifndef SERVOINPUT_H
+#define SERVOINPUT_H
+
+#include "mbed.h"
+
+
+class ServoTimer {
+public:
+    ServoTimer();
+    void update_servo_input();
+    int get_trash_input();
+    int get_servo_input();
+
+private:
+
+    int trash_input_count;
+    int oldCCR1;
+    int servo_sig;
+    
+};
+
+
+#endif
+
+
+
+

diff -r 9edd6ec0f56a -r 94193b31f0ee fw_config.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fw_config.h	Sat Apr 06 02:16:12 2019 +0000
@@ -0,0 +1,32 @@
+#ifndef FW_CONFIG_H
+#define FW_CONFIG_H
+
+//hardware stuff
+#define R_S 0.027f            //Ohms
+#define L_S 0.00004f            //Henries
+
+
+
+//current controler settings
+#define KP 0.20f                     // Volts/Amp
+#define KI 0.03f                  // 1/samples
+
+
+#define V_BUS 24.0f                // Volts, actual bus voltage
+#define MAX_AMPS 100.0f
+
+#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 DT 0.00005f
+#define F_SW 20000
+
+
+#define DTC_MAX 0.95f          // Max phase duty cycle, determines current sensor read time
+#define DTC_MIN 0.02f          // Min phase duty cycle
+
+
+
+#endif
+

diff -r 9edd6ec0f56a -r 94193b31f0ee hw_setup/current_controller_config.h
--- 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

diff -r 9edd6ec0f56a -r 94193b31f0ee hw_setup/hw_pins.h
--- 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

diff -r 9edd6ec0f56a -r 94193b31f0ee hw_setup/motor_config.h
--- 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

diff -r 9edd6ec0f56a -r 94193b31f0ee main.cpp
--- a/main.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/main.cpp	Sat Apr 06 02:16:12 2019 +0000
@@ -1,241 +1,236 @@
 #include "mbed.h"
-#include "PositionSensor.h"
-#include "structs.h"
+
+#include "fw_config.h"
+#include "math_ops.h"
+#include "Inverter.h"
+#include "ServoInput.h"
+
+
+/*
+
+
+PA2 goes to servo input
+
+PA3-7, PA0-1 goes to  analog input  (on of those)
 
-#include "Inverter.h"
-//#define PI 3.14159f
+use two of either PA8, 9,   10, 
+slaved with       PA7, PB0, PB1
+
+PA7 and PA8 for high and low sides
+
+PB6, 7 used for serial
+PA13, 14, reset used for programming
 
-#include "foc.h"
+some other random pin used for LED
+some other random pin used for clockpin, for debugging
+
+
+======
+pins finally used by mike:
+PA2: RC PWM
+PA5: clockpin
+PA8: gate drive
 
 
 
+*/
 
-DigitalOut clockpin(PC_12);
-//AnalogIn   pot_in(PC_3);
+
+
+DigitalOut clockpin(PA_5); 
+DigitalOut led1(PB_1);  
+DigitalIn gpio1(PA_6);  
+
+//DigitalIn gpio2(PA_2); 
 
 
 // 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
- 
+#define VOLTAGE_MODE 1
+#define CURRENT_MODE 2
+
 
-GPIOStruct gpio;
-ControllerStruct controller;
+volatile int count = 0;
+volatile int main_int_count = 0;
+volatile unsigned int main_int_clock = 0;
+
+int controller_state = 0;
 
 
-//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;
+Serial pc(PB_6, PB_7);
+Inverter inverter;
+ServoTimer servoinp;
 
-int bing1 = 0;
-int bing2 = 0;
-int bing3 = 0;
+
+float a1 = 0.0f;
+float a2 = 0.0f;
+
 
-int var1 = 0;
-int var2 = 0;
+float current = 0.0f;
+float current_raw = 0.0f;
+float servo_cmd = 0.0f;
 
-float a = 0.0f;
-float b = 0.0f;
-float c = 0.0f;
 
-volatile int count = 0;
-
-int controller_state = ONEUPTWODOWN;//VMODESINE;
+int servo_low = 890;
+int servo_high = 1800;
+int servo_range = servo_high - servo_low;
 
-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;
+
+extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
   
   if (TIM1->SR & TIM_SR_UIF ) {
+
+        main_int_count++;
+        main_int_clock++;
+
+        //
+        //current_raw = float(ADC1->DR) - float(inverter.adc1_offset);
+        current_raw = float(ADC1->DR) - 2000;
+        //current_raw =  float(ADC1->DR));
+        //current = current_raw*0.1f; //depends on shunts
+        
+        inverter.adc2_raw = ADC2->DR;
+        inverter.adc1_raw = ADC1->DR;
+        clockpin = 1;
+        servoinp.update_servo_input();
         
         
-        ///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){
+                TIM1->CCR1 = 6400;  
+                count++;
+                if(count > 20000){
                     count = 0;                                                    
-                    printf("Rest Mode");
-                    printf("\n\r");
+                    led1 = !led1;
                 }
-                break;
+                servo_cmd = servoinp.get_servo_input();
+                
+            break;
+                
             
-            case OLSINE:        // open loop sines
+            case VOLTAGE_MODE: 
+                
+                servo_cmd = servoinp.get_servo_input();
 
-                //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;}
+                TIM1->CCR1 = 6400 - constrain((servo_cmd-880)*8.0f , 0, 6150);  //0 to 96% duty cycle
 
-                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;
-
-                TIM1->CCR1 = 0x1194*(a);
-                TIM1->CCR2 = 0x1194*(b);
-                TIM1->CCR3 = 0x1194*(c);
-                break;
+                if(count > 10000){ count = 0; }
+                
+            break;
+            
+            case CURRENT_MODE: 
                 
-            case ONEUPTWODOWN:        // one up, two down
-        
-                a = 0.8f;
-                b = 0.2f;
-                c = 0.2f;
+                servo_cmd = servoinp.get_servo_input();
+                /*
+                float current_setpoint = (servo_cmd-880)/100;  //0 to 10 amps
+                if (current > current_setpoint) {
+                    //set bridge state to LOW
+                    TIM1->CCR1 = 6100;
+                }
+                else {
+                    TIM1->CCR1 = 300;
+                }*/
+                
                 
-                count++;                       
-                if(count > 500){
-                    count = 0;
-                    //printf("%f  ",encoder.GetElecPosition());
+                // servo inputs go from 880 to 1700
+                
+                // the constant in the next line is set 1800/servo_range
+                //float current_setpoint_raw = (servo_cmd-servo_low)*2.25f;  //scales 800 range to 1800
+                float current_setpoint_raw = constrain((servo_cmd-servo_low)*2.0f , 0, 800);  //scales 800 range to 1800
+                if (current_raw > current_setpoint_raw) {
+                    //set bridge state to LOW
+                    TIM1->CCR1 = 6400;
+                }
+                else {
+                    TIM1->CCR1 = 250;
                 }
                 
-                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);
+                if ((servo_cmd < servo_low) & (current_raw < 200) & (current_raw > -200)) {
+                    TIM1->CCR1 = 6400;
                 }
-    
-                //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;
-
-                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");
-                    }
+                if(count > 10000){ count = 0; }
                 
-                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);
+            break;
             
-                    //printf("%i  ",controller.adc1_raw);
-                    //printf("%i \n\r ",controller.adc2_raw);
-        }
-                }
-                
-    
-        ///*
-        
-        controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
-        commutate(&controller, &gpio, controller.theta_elec); 
-        
-        
-        
-        //*/
+
+        }  
             
       }
+            //b = TIM1->CNT;
   TIM1->SR = 0x0;  
-  //clockpin = 0;                                                             // reset the status register
+  clockpin = 0;                                                             // reset the status register
 }
 
 int main() {
-    //float meas;
-    pc.baud(115200);
-    printf("\nStarting Hardware\n");
-    Init_All_HW(&gpio);
+    wait_ms(200);
+    pc.baud(256000);
+    printf("\rStarting Hardware\n");
+    gpio1.mode(PullUp);
+
     
-    zero_current(&controller.adc1_offset, &controller.adc2_offset);                                                        // Setup PWM, ADC, GPIO
+    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();
+
     
+
+    //controller_state = VOLTAGE_MODE;
+    controller_state = CURRENT_MODE;
+
     
     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);
+
+        wait_us(2);
         
-        printf("%i  ",controller.adc1_raw);
-        printf("%i  ",controller.adc2_raw);
-        
-        printf("%i  ",controller.adc1_offset);
-        printf("%i  ",controller.adc2_offset);
+        while (1==1) {
+            
+        wait_us(30);
+
+        pc.printf("%f,  ", servo_cmd); 
         
-        printf("%f  ",controller.i_q);
-        printf("%f  ",controller.i_d);
+        pc.printf("%f,  ", current_raw); 
         
-        printf("%f  ",controller.i_a);
-        printf("%f  ",controller.i_b);
-        
-        //printf("%f  ",controller.theta_elec);
+        pc.printf("%f,  ", float(inverter.adc1_offset));
+        pc.printf("%f,  ", a1); 
+        pc.printf("%f,  ", a2); 
         
         
-        
-        printf("\n\r");
-        
+        //printf("%i,  ", b3); 
+        //printf("%i,  ", b4);
+
+        //pc.printf("%i, ", TIM15->CNT);  
+        //pc.printf("%x  ", GPIOA->AFR[0]); 
+
+        pc.printf("%i,  ", ADC1->DR);  
+        pc.printf("%i,  ", ADC2->DR);
+        //printf("    %i, %i %i", a1, a2, a3);
+        //if (gpio2) {pc.printf("low ");}
+        pc.printf("\r");
+
+
         }
-        
-        
-        
-        
+
     }
 }
+
+
+
+
+
+
+

diff -r 9edd6ec0f56a -r 94193b31f0ee math_ops/math_ops.cpp
--- a/math_ops/math_ops.cpp	Sat May 20 21:42:20 2017 +0000
+++ b/math_ops/math_ops.cpp	Sat Apr 06 02:16:12 2019 +0000
@@ -25,3 +25,20 @@
         *y = *y * limit/norm;
         }
     }
+    
+void limit_abs(float *x, float limit){
+    limit = abs(limit);
+    if(*x > limit){
+        *x = limit;
+        }
+    if(*x < -limit){
+        *x = -limit;
+        }
+    }
+
+float constrain(float x, float bot, float top){
+    x = (((x)>(top))?(top):(x));
+    x = (((x)<(bot))?(bot):(x));
+    return x;
+}
+    
\ No newline at end of file

diff -r 9edd6ec0f56a -r 94193b31f0ee math_ops/math_ops.h
--- a/math_ops/math_ops.h	Sat May 20 21:42:20 2017 +0000
+++ b/math_ops/math_ops.h	Sat Apr 06 02:16:12 2019 +0000
@@ -10,5 +10,7 @@
 float fmaxf3(float x, float y, float z);
 float fminf3(float x, float y, float z);
 void limit_norm(float *x, float *y, float limit);
+void limit_abs(float *x, float limit);
+float constrain(float x, float bot, float top);
 
 #endif

diff -r 9edd6ec0f56a -r 94193b31f0ee mbed.bld
--- a/mbed.bld	Sat May 20 21:42:20 2017 +0000
+++ b/mbed.bld	Sat Apr 06 02:16:12 2019 +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

diff -r 9edd6ec0f56a -r 94193b31f0ee structs.h
--- a/structs.h	Sat May 20 21:42:20 2017 +0000
+++ b/structs.h	Sat Apr 06 02:16:12 2019 +0000
@@ -1,48 +1,28 @@
 #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 i_motor;
     float v_bus;
-    float theta_mech, theta_elec;
-    float dtheta_mech, dtheta_elec;
-    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 d_int, q_int;
-    int adc1_offset, adc2_offset;
-    float i_d_ref, i_q_ref;
+    float v_output;
     int loop_count;
     int mode;
-    float cogging[128];
-    } ControllerStruct;
+    } HardwareStruct;
+    
+
+    
+    
 
 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