Revision 20:bf9ea5125d52, committed 2017-03-02

Comitter:

benkatz

Date:

Thu Mar 02 15:31:23 2017 +0000

Parent:

19:bd10a04eedc2

Child:

21:7d1f0a206668

Commit message:

Compact version works.;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Config/current_controller_config.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,15 @@
+#ifndef CURRENT_CONTROLLER_CONFIG_H
+#define CURRENT_CONTROLLER_CONFIG_H
+
+#define K_D .5f   //V/A
+#define K_Q .5f   //V/A
+#define KI_D 0.04f  //1/samples
+#define KI_Q 0.04f  //1/samples
+#define V_BUS 14.0f
+
+#define D_INT_LIM V_BUS/(K_D*KI_D)  //A*samples
+#define Q_INT_LIM V_BUS/(K_Q*KI_Q)  //A*samples
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Config/hw_config.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,13 @@
+#ifndef HW_CONFIG_H
+#define HW_CONFIG_H
+
+#define PIN_U PA_10
+#define PIN_V PA_9
+#define PIN_W PA_8
+#define ENABLE_PIN PA_11
+#define I_SCALE 0.02014160156f // Amps per A/D Count
+#define DTC_MAX 0.935f
+#define DTC_MIN 0.065f
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Config/motor_config.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,12 @@
+#ifndef MOTOR_CONFIG_H
+#define MOTOR_CONFIG_H
+
+#define R_PHASE 0.1f
+#define L_D 0.00003f
+#define L_Q 0.00003f
+
+#define R_TOTAL 1.5f*R_PHASE
+#define NPP 21
+
+
+#endif

--- a/CurrentRegulator/CurrentRegulator.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,169 +0,0 @@
-#include "Transforms.h"
-#include "CurrentRegulator.h"
-#include "Inverter.h"
-#include "SVM.h"
-#include "Transforms.h"
-#include "PositionSensor.h"
-
-using namespace Transforms;
-
-CurrentRegulator::CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, PositionSensor *velocity_sensor, float L, float Kp, float Ki){
-    _Inverter = inverter;
-    PWM = new SVPWM(inverter, 2.0);
-    _PositionSensor = position_sensor;
-    _VelocitySensor = velocity_sensor;
-    IQ_Ref = 0;
-    Q_Max = 40.0f;
-    ID_Ref = 0;
-    V_Q = 0;
-    V_D = 0;
-    V_Alpha = 0;
-    V_Beta = 0;
-    V_A = 0;
-    V_B = 0;
-    V_C = 0;
-    I_Q = 0;
-    I_D = 0;
-    I_A = 0;
-    I_B = 0;
-    I_C = 0;
-    I_Alpha = 0;
-    I_Beta = 0;
-    IQ_Old = 0;
-    ID_Old = 0;
-    count = 0;
-    _L = L;
-    _Kp = Kp;
-    _Ki = Ki;
-    Q_Integral = 0;
-    D_Integral = 0;
-    Int_Max = .9;
-    DTC_Max = .97;
-    //theta_elec = _PositionSensor->GetElecPosition();
-
-    
-    }
-
-void CurrentRegulator::SendSPI(){
-    
-    
-    }
-    
-float CurrentRegulator::GetQ(){
-    return I_Q;
-    }
-    
-void CurrentRegulator::Reset(void){
-    IQ_Ref = 0;
-    ID_Ref = 0;
-    V_Q = 0;
-    V_D = 0;
-    V_Alpha = 0;
-    V_Beta = 0;
-    V_A = 0;
-    V_B = 0;
-    V_C = 0;
-    I_Q = 0;
-    I_D = 0;
-    I_A = 0;
-    I_B = 0;
-    I_C = 0;
-    I_Alpha = 0;
-    I_Beta = 0;
-    //pc->printf("%f, %f\n\r", Q_Integral, D_Integral);
-    wait(.03);
-    Q_Integral = 0;
-    D_Integral = 0;    
-    }
-    
-void CurrentRegulator::UpdateRef(float D, float Q){
-    if(Q>Q_Max){
-        Q = Q_Max;
-        }
-    else if(Q<-Q_Max){
-        Q = -Q_Max;
-        }
-    if(D>Q_Max){
-        D = Q_Max;
-        }
-    else if(D<-Q_Max){
-        D = -Q_Max;
-        }
-    IQ_Ref = Q;
-    ID_Ref = D;
-    }
-
-void CurrentRegulator::SampleCurrent(){
-    _Inverter->GetCurrent(&I_A, &I_B, &I_C);
-    Clarke(I_A, I_B, &I_Alpha, &I_Beta);
-    float ID_Sample, IQ_Sample;
-    //Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
-    
-    Park(I_Alpha, I_Beta, theta_elec, &ID_Sample, &IQ_Sample);    
-    I_D = 1.0f*ID_Sample + 0.0f*ID_Old;
-    I_Q = 1.0f*IQ_Sample + 0.0f*IQ_Old;
-    ID_Old = I_D;
-    IQ_Old = I_Q;
-    //count += 1;
-    //if(count > 10000) {
-    //    count=0;
-    //    printf("I_A:  %f        I_C:  %f       I_C:  %f\n\r", I_A, I_B, I_C);
-    //IQ_Ref = -IQ_Ref;
-    //    }
-
-    //DAC->DHR12R1 = (int) (I_Q*490.648f) + 2048;
-    //DAC->DHR12R1 = (int) (I_Alpha*4096.0f) + 2048;
-    }
-    
-void CurrentRegulator::Update(){
-        float Q_Error = IQ_Ref - I_Q;
-        float D_Error = ID_Ref - I_D;
-        float w_elec = _VelocitySensor->GetElecVelocity();
-        
-        Q_Integral += Q_Error*_Ki*_Kp;
-        D_Integral += D_Error*_Ki*_Kp;
-        
-        if (Q_Integral > Int_Max) Q_Integral = Int_Max;
-        else if(Q_Integral < -Int_Max) Q_Integral = -Int_Max;
-        if (D_Integral > Int_Max) D_Integral = Int_Max;
-        else if(D_Integral < -Int_Max) D_Integral = -Int_Max;       
-         
-        V_Q = Q_Integral + _Kp*Q_Error;
-        //V_Q = V_Q - w_elec*I_D;
-        V_D = D_Integral + _Kp*D_Error;     
-        //V_D = V_D + w_elec*I_Q;   //decoupling needs moar testing
-        float mag2 = (V_Q*V_Q + V_D*V_D);
-        if(mag2>1){
-            V_Q = V_Q/mag2;
-            V_D = V_D/mag2;
-            }
-        
-    }
-        
-void CurrentRegulator::SetVoltage(){
-    InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
-    InvClarke(V_Alpha, V_Beta, &V_A, &V_B, &V_C);
-    PWM->Update_DTC(V_A, V_B, V_C);
-    }
-    
-    
-void CurrentRegulator::Commutate(){
-    count += 1;
-    //GPIOC->ODR = (1 << 4); //Toggle pin for debugging
-    theta_elec = _PositionSensor->GetElecPosition();
-    _PositionSensor->GetMechPosition();
-    SampleCurrent(); //Grab most recent current sample
-    Update();   //Run control loop
-    SetVoltage();   //Set inverter duty cycles
-    //GPIOC->ODR = (0 << 4); //Toggle pin for debugging
-
-    
-    if (count==1000){
-        //printf("%f\n\r", V_A);
-        count = 0;
-        }
-
-    
-    
-      
-      }

--- a/CurrentRegulator/CurrentRegulator.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef CURRENTREGULATOR_H
-#define CURRENTREGULATOR_H
-
-#include "Inverter.h"
-#include "SVM.h"
-#include "PositionSensor.h"
-
-class CurrentRegulator{
-    public:
-        CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, PositionSensor *velocity_sensor, float L, float Kp, float Ki);
-        void UpdateRef(float D, float Q);
-        void Commutate();
-        void Reset();
-        virtual float GetQ();
-    private:
-        float IQ_Ref, ID_Ref, V_Q, V_D, V_Alpha, V_Beta, V_A, V_B, V_C, I_Q, I_D, IQ_Old,ID_Old,I_A, I_B, I_C, I_Alpha, I_Beta, theta_elec, _Kp, _Ki, _L;
-        float Q_Integral, D_Integral, Q_Error, D_Error, Int_Max, DTC_Max, Q_Max;
-        void SampleCurrent();
-        void SetVoltage();
-        void Update();
-        void SendSPI();
-        Inverter* _Inverter;
-        PositionSensor* _PositionSensor;
-        PositionSensor* _VelocitySensor;
-        SVM* PWM;
-        //Serial* pc;
-        int count;
-            
-    
-    
-    };
-    
-    
-#endif
\ No newline at end of file

--- a/FastMath/FastMath.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ b/FastMath/FastMath.cpp	Thu Mar 02 15:31:23 2017 +0000
@@ -4,8 +4,8 @@
 const float Multiplier = 81.4873308631f;
 
 float FastMath::FastSin(float theta){
-    if (theta < 0.0f) theta += 6.28318530718f;
-    if (theta >= 6.28318530718f) theta -= 6.28318530718f;    
+    while (theta < 0.0f) theta += 6.28318530718f;
+    while (theta >= 6.28318530718f) theta -= 6.28318530718f;    
     return SinTable[(int) (Multiplier*theta)] ;
     }
     

--- a/FastMath/FastMath.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,11 +0,0 @@
-#ifndef __FASTMATH_H
-#define __FASTMATH_H
-
-#include "LUT.h"
-
-namespace FastMath{
-    float FastSin(float theta);
-    float FastCos(float theta);
-    };
-
-#endif

--- a/FastMath/LUT.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,8 +0,0 @@
-#ifndef __LUT_H
-#define __LUT_H
-
-const float SinTable[] = {
-    0,0.012296,0.024589,0.036879,0.049164,0.061441,0.073708,0.085965,0.098208,0.11044,0.12265,0.13484,0.14702,0.15917,0.17129,0.18339,0.19547,0.20751,0.21952,0.2315,0.24345,0.25535,0.26722,0.27905,0.29084,0.30258,0.31427,0.32592,0.33752,0.34907,0.36057,0.37201,0.38339,0.39472,0.40599,0.41719,0.42834,0.43941,0.45043,0.46137,0.47224,0.48305,0.49378,0.50443,0.51501,0.52551,0.53593,0.54627,0.55653,0.5667,0.57679,0.58679,0.5967,0.60652,0.61625,0.62589,0.63543,0.64488,0.65423,0.66348,0.67263,0.68167,0.69062,0.69946,0.70819,0.71682,0.72534,0.73375,0.74205,0.75023,0.75831,0.76626,0.77411,0.78183,0.78944,0.79693,0.80429,0.81154,0.81866,0.82566,0.83254,0.83928,0.84591,0.8524,0.85876,0.865,0.8711,0.87708,0.88292,0.88862,0.89419,0.89963,0.90493,0.9101,0.91512,0.92001,0.92476,0.92937,0.93384,0.93816,0.94235,0.94639,0.95029,0.95405,0.95766,0.96113,0.96445,0.96763,0.97066,0.97354,0.97628,0.97887,0.98131,0.9836,0.98574,0.98774,0.98958,0.99128,0.99282,0.99422,0.99546,0.99656,0.9975,0.99829,0.99894,0.99943,0.99977,0.99996,1,0.99988,0.99962,0.9992,0.99863,0.99792,0.99705,0.99603,0.99486,0.99354,0.99207,0.99045,0.98868,0.98676,0.98469,0.98247,0.9801,0.97759,0.97493,0.97212,0.96916,0.96606,0.96281,0.95941,0.95587,0.95219,0.94836,0.94439,0.94028,0.93602,0.93162,0.92708,0.9224,0.91758,0.91263,0.90753,0.9023,0.89693,0.89142,0.88579,0.88001,0.87411,0.86807,0.8619,0.8556,0.84917,0.84261,0.83593,0.82911,0.82218,0.81512,0.80793,0.80062,0.7932,0.78565,0.77798,0.7702,0.7623,0.75428,0.74615,0.73791,0.72956,0.72109,0.71252,0.70384,0.69505,0.68616,0.67716,0.66806,0.65886,0.64956,0.64017,0.63067,0.62108,0.6114,0.60162,0.59176,0.5818,0.57176,0.56163,0.55141,0.54111,0.53073,0.52027,0.50973,0.49911,0.48842,0.47765,0.46682,0.45591,0.44493,0.43388,0.42277,0.4116,0.40036,0.38906,0.37771,0.36629,0.35483,0.3433,0.33173,0.32011,0.30843,0.29671,0.28495,0.27314,0.26129,0.2494,0.23748,0.22552,0.21352,0.20149,0.18943,0.17735,0.16523,0.15309,0.14093,0.12875,0.11655,0.10432,0.092088,0.079838,0.067576,0.055303,0.043022,0.030735,0.018443,0.0061479,-0.0061479,-0.018443,-0.030735,-0.043022,-0.055303,-0.067576,-0.079838,-0.092088,-0.10432,-0.11655,-0.12875,-0.14093,-0.15309,-0.16523,-0.17735,-0.18943,-0.20149,-0.21352,-0.22552,-0.23748,-0.2494,-0.26129,-0.27314,-0.28495,-0.29671,-0.30843,-0.32011,-0.33173,-0.3433,-0.35483,-0.36629,-0.37771,-0.38906,-0.40036,-0.4116,-0.42277,-0.43388,-0.44493,-0.45591,-0.46682,-0.47765,-0.48842,-0.49911,-0.50973,-0.52027,-0.53073,-0.54111,-0.55141,-0.56163,-0.57176,-0.5818,-0.59176,-0.60162,-0.6114,-0.62108,-0.63067,-0.64017,-0.64956,-0.65886,-0.66806,-0.67716,-0.68616,-0.69505,-0.70384,-0.71252,-0.72109,-0.72956,-0.73791,-0.74615,-0.75428,-0.7623,-0.7702,-0.77798,-0.78565,-0.7932,-0.80062,-0.80793,-0.81512,-0.82218,-0.82911,-0.83593,-0.84261,-0.84917,-0.8556,-0.8619,-0.86807,-0.87411,-0.88001,-0.88579,-0.89142,-0.89693,-0.9023,-0.90753,-0.91263,-0.91758,-0.9224,-0.92708,-0.93162,-0.93602,-0.94028,-0.94439,-0.94836,-0.95219,-0.95587,-0.95941,-0.96281,-0.96606,-0.96916,-0.97212,-0.97493,-0.97759,-0.9801,-0.98247,-0.98469,-0.98676,-0.98868,-0.99045,-0.99207,-0.99354,-0.99486,-0.99603,-0.99705,-0.99792,-0.99863,-0.9992,-0.99962,-0.99988,-1,-0.99996,-0.99977,-0.99943,-0.99894,-0.99829,-0.9975,-0.99656,-0.99546,-0.99422,-0.99282,-0.99128,-0.98958,-0.98774,-0.98574,-0.9836,-0.98131,-0.97887,-0.97628,-0.97354,-0.97066,-0.96763,-0.96445,-0.96113,-0.95766,-0.95405,-0.95029,-0.94639,-0.94235,-0.93816,-0.93384,-0.92937,-0.92476,-0.92001,-0.91512,-0.9101,-0.90493,-0.89963,-0.89419,-0.88862,-0.88292,-0.87708,-0.8711,-0.865,-0.85876,-0.8524,-0.84591,-0.83928,-0.83254,-0.82566,-0.81866,-0.81154,-0.80429,-0.79693,-0.78944,-0.78183,-0.77411,-0.76626,-0.75831,-0.75023,-0.74205,-0.73375,-0.72534,-0.71682,-0.70819,-0.69946,-0.69062,-0.68167,-0.67263,-0.66348,-0.65423,-0.64488,-0.63543,-0.62589,-0.61625,-0.60652,-0.5967,-0.58679,-0.57679,-0.5667,-0.55653,-0.54627,-0.53593,-0.52551,-0.51501,-0.50443,-0.49378,-0.48305,-0.47224,-0.46137,-0.45043,-0.43941,-0.42834,-0.41719,-0.40599,-0.39472,-0.38339,-0.37201,-0.36057,-0.34907,-0.33752,-0.32592,-0.31427,-0.30258,-0.29084,-0.27905,-0.26722,-0.25535,-0.24345,-0.2315,-0.21952,-0.20751,-0.19547,-0.18339,-0.17129,-0.15917,-0.14702,-0.13484,-0.12265,-0.11044,-0.098208,-0.085965,-0.073708,-0.061441,-0.049164,-0.036879,-0.024589,-0.012296,0
-};
-
-#endif

--- a/ImpedanceController/ImpedanceController.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-#include "TorqueController.h"
-#include "ImpedanceController.h"
-#include "CurrentRegulator.h"
-#include "PositionSensor.h"
-
-
-ImpedanceController::ImpedanceController(TorqueController *torqueController, PositionSensor *sensor_pos, PositionSensor *sensor_vel){
-    _torqueController = torqueController;
-    _sensor_pos = sensor_pos;
-    _sensor_vel = sensor_vel;
-    }
-
- void ImpedanceController::SetImpedance(float K, float B, float ref){
-    float position = _sensor_pos->GetMechPosition();
-    float velocity = _sensor_vel->GetMechVelocity();
-    float error = ref-position;
-    float output = K*error + B*velocity;
-    
-    _torqueController->SetTorque(output);
-    
-    }
-    

--- a/ImpedanceController/ImpedanceController.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#ifndef IMPEDANCECONTROLLER_H
-#define IMPEDANCECONTROLLER_H
-#include "TorqueController.h"
-#include "PositionSensor.h"
-
-class ImpedanceController{
-    public:
-        //CurrentRegulator();
-        ImpedanceController(TorqueController *torqueController, PositionSensor *sensor_pos, PositionSensor *sensor_vel);
-        virtual void SetImpedance(float K, float B, float ref);
-    
-    private:
-        float reference, _K, _B, output;   //Referene position (rad), motor stiffness (N-m/rad), motor damping (N-m*s/rad), output(N-m)
-        TorqueController* _torqueController;
-        PositionSensor* _sensor_pos;
-        PositionSensor* _sensor_vel;
-    };
-    
-#endif
\ No newline at end of file

--- a/Inverter/Inverter.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,118 +0,0 @@
-#include "mbed.h"
-#include "FastPWM.h"
-#include "Inverter.h"
-
-Inverter::Inverter(PinName PinA, PinName PinB, PinName PinC, PinName PinEnable, float I_Scale, float Period){
-    
-    _I_Scale = I_Scale;
-
-    
-    Enable = new DigitalOut(PinEnable);
-    
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable the clock to GPIOC
-    //RCC->APB1ENR |= 0x00000001; // enable TIM2 clock
-    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
-
-    GPIOC->MODER = (1 << 8); // set pin 4 to be general purpose output
-
-    PWM_A = new FastPWM(PinA);
-    PWM_B = new FastPWM(PinB);
-    PWM_C = new FastPWM(PinC);
-
-    //TIM2->CR1 &= ~(TIM_CR1_CEN);
-    //TIM2->CR1 |= TIM_CR1_CMS;
-    //TIM2->CR1 |= TIM_CR1_CEN;
-
-    //PWM_A->period(Period);
-    
-     //ISR Setup     
-    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);   //Enable TIM1 IRQ
-
-    TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
-    TIM1->CR1 = 0x40;//CMS = 10, interrupt only when counting up
-    TIM1->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->ARR = 0x8CA;
-    TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
-    TIM1->CR1 |= TIM_CR1_CEN;     
-    
-    // ADC Setup
-     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
-     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
-     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;//0x0000002; // Enable clock for GPIOC
-     
-     ADC->CCR = 0x00000006; // Regular simultaneous mode only
-     ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
-     ADC1->SQR3 = 0x000000A; // use PC_0 as input
-     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
-     ADC2->SQR3 = 0x0000000B; // use PC_1 as input
-     GPIOC->MODER |= 0x0000000f; // PC_0, PC_1 are analog inputs 
-     
-    // DAC set-up
-     RCC->APB1ENR |= 0x20000000; // Enable clock for DAC
-     DAC->CR |= 0x00000001; // DAC control reg, both channels ON
-     GPIOA->MODER |= 0x00000300; // PA04 as analog output   
-     
-     
-     //Enabled pin must be on for current sensors to turn on
-     EnableInverter();
-     SetDTC(0.0f, 0.0f, 0.0f);
-     wait(.2);
-     ZeroCurrent();
-     wait(.1);
-     DisableInverter();
-    }
-
-void Inverter::SetDTC(float DTC_A, float DTC_B, float DTC_C){
-        PWM_A->write(1.0f-DTC_A);
-        PWM_B->write(1.0f-DTC_B);
-        PWM_C->write(1.0f-DTC_C);
-    }
-
-void Inverter::EnableInverter(){
-    Enable->write(1);
-    }
-
-void Inverter::DisableInverter(){
-    Enable->write(0);
-    }
-
-void Inverter::ZeroCurrent(){
-    I_B_Offset = 0;
-    I_C_Offset = 0;
-    for (int i=0; i < 1000; i++){
-        I_B_Offset += ADC2->DR;
-        I_C_Offset += ADC1->DR;
-        ADC1->CR2  |= 0x40000000; 
-        wait(.0001);
-        }
-    I_B_Offset = I_B_Offset/1000.0f;
-    I_C_Offset = I_C_Offset/1000.0f;
-    //printf("B_Offset:  %f     C_Offset:  %f\n\r", I_B_Offset, I_C_Offset);
-    }
-
-void Inverter::GetCurrent(float *A, float *B, float *C){
-    *A = I_A;
-    *B = I_B;
-    *C = I_C;
-    //printf("I_A: %f    I_B: %f   I_C: %f\n\r", I_A, I_B, I_C);
-    }
-
-void Inverter::SampleCurrent(void){
- //   Dbg->write(1);
-    GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
-    I_B = _I_Scale*((float) (ADC2->DR) -  I_B_Offset);
-    I_C = _I_Scale*((float) (ADC1->DR)-  I_C_Offset);
-    I_A = -I_B - I_C;
-    //DAC->DHR12R1 = ADC2->DR; 
-    //DAC->DHR12R1 = TIM3->CNT>>2;//ADC2->DR; // pass ADC -> DAC, also clears EOC flag
-    ADC1->CR2  |= 0x40000000; 
-    GPIOC->ODR ^= (1 << 4); //toggle pin for debugging
-    }
-    

--- a/Inverter/Inverter.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,26 +0,0 @@
-#ifndef INVERTER_H
-#define INVERTER_H
-#include "FastPWM.h"
-#include "mbed.h"
-
-
-class Inverter{
-    public:
-        Inverter(PinName PinA, PinName PinB, PinName PinC, PinName PinEnable, float I_Scale, float Period);
-        void SetDTC(float DTC_A, float DTC_B, float DTC_C);
-        void EnableInverter();
-        void DisableInverter();
-        void InitCurrentSense();
-        void SampleCurrent(); 
-        void GetCurrent(float *A, float *B, float *C);
-        float I_A, I_B, I_C, _I_Scale;
-
-    private:
-        float I_B_Offset, I_C_Offset;
-        void ZeroCurrent(void);
-        FastPWM *PWM_A, *PWM_B, *PWM_C;  
-        DigitalOut *Enable, *Dbg; 
-        AnalogIn *Current_B, *Current_C;
-    };
-
-#endif
\ No newline at end of file

--- a/PositionSensor/PositionSensor.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Thu Mar 02 15:31:23 2017 +0000
@@ -66,7 +66,7 @@
     rotations = 0;
     spi = new SPI(PC_12, PC_11, PC_10);
     spi->format(16, 1);
-    spi->frequency(10000000);
+    spi->frequency(5000000);
     cs = new DigitalOut(PA_15);
     cs->write(1);
     readAngleCmd = 0xffff;
@@ -118,6 +118,8 @@
     MechOffset = GetMechPosition();
     }
     
+    
+    
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;
     _CPR = CPR;

--- a/PositionSensor/PositionSensor.h	Tue Feb 14 03:28:16 2017 +0000
+++ b/PositionSensor/PositionSensor.h	Thu Mar 02 15:31:23 2017 +0000
@@ -58,4 +58,19 @@
     int readAngleCmd;
 
 };
+
+class PositionSensorSineGen: public PositionSensor{
+public:
+    PositionSensorSineGen(int CPR, float offset, int ppairs);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual int GetRawPosition();
+    virtual void ZeroPosition();
+private:
+    float _offset, MechPosition, MechOffset;
+    int _CPR, rotations, old_counts, _ppairs;
+    int readAngleCmd;
+
+};
 #endif
\ No newline at end of file

--- a/SVM/SVM.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-
-#include "mbed.h"
-#include "SVM.h"
-#include "Inverter.h"
-#define min(x,y,z) (x < y ? (x < z ? x : z) : (y < z ? y : z))
-#define max(x,y,z) (x > y ? (x > z ? x : z) : (y > z ? y : z))
-
-SPWM::SPWM(Inverter *inverter, float V_Bus){
-    _inverter = inverter;
-    _V_Bus = V_Bus;
-    }
-
-//sinusoidal PWM
-void SPWM::Update_DTC(float V_A, float V_B, float V_C){
-    float DTC_A = V_A/_V_Bus + .5f;
-    float DTC_B = V_B/_V_Bus + .5f;
-    float DTC_C = V_C/_V_Bus + .5f;
-    
-    if(DTC_A > .95f) DTC_A = .95f;
-    else if(DTC_A < .05f) DTC_A = .05f;
-    if(DTC_B > .95f) DTC_B = .95f;
-    else if(DTC_B < .05f) DTC_B = .05f;
-    if(DTC_C > .95f) DTC_C = .95f;
-    else if(DTC_C < .05f) DTC_C = .05f;
-    _inverter->SetDTC(DTC_A, DTC_B, DTC_C);
-    }
-
-SVPWM::SVPWM(Inverter *inverter, float V_Bus){
-    _inverter = inverter;
-    _V_Bus = V_Bus;
-    }
-    
-//space vector pwm (better bus utilization)
-void SVPWM::Update_DTC(float V_A, float V_B, float V_C){
-    
-    float Voff = (min(V_A, V_B, V_C) + max(V_A, V_B, V_C))/2.0f;
-    
-    V_A = V_A - Voff;
-    V_B = V_B - Voff;
-    V_C = V_C - Voff;
-    
-    float DTC_A = V_A/_V_Bus + .5f;
-    float DTC_B = V_B/_V_Bus + .5f;
-    float DTC_C = V_C/_V_Bus + .5f;
-    
-    
-    
-    if(DTC_A > .95f) DTC_A = .95f;
-    else if(DTC_A < .05f) DTC_A = .05f;
-    if(DTC_B > .95f) DTC_B = .95f;
-    else if(DTC_B < .05f) DTC_B = .05f;
-    if(DTC_C > .95f) DTC_C = .95f;
-    else if(DTC_C < .05f) DTC_C = .05f;
-    _inverter->SetDTC(DTC_A, DTC_B, DTC_C);
-    }
-    
\ No newline at end of file

--- a/SVM/SVM.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,41 +0,0 @@
-#ifndef SVM_H
-#define SVM_H
-#include "Inverter.h"
-
-class SVM{
-public:
-    //virtual void Update_DTC(float V_Alpha, float V_Beta) = 0;
-    virtual void Update_DTC(float V_A, float V_B, float V_C) = 0;
-private:
-    float _V_Bus;
-protected:
-    Inverter* _inverter;
-    };
-    
-    
-class SPWM: public SVM{             //Sinusoidal PWM
-public: 
-    SPWM(Inverter *inverter, float V_Bus);
-    //virtual void Update_DTC(float V_Alpha, float V_Beta);
-    virtual void Update_DTC(float V_A, float V_B, float V_C);
-private:
-    float _V_Bus;
-protected:
-    Inverter* _inverter;
-    };
-    
-    
-class SVPWM: public SVM{            //SVM
-public: 
-     SVPWM(Inverter *inverter, float V_Bus);
-     //virtual void Update_DTC(float V_Alpha, float V_Beta);
-    virtual void Update_DTC(float V_A, float V_B, float V_C);
-
-private:
-    float _V_Bus;
-protected:
-    Inverter* _inverter;
-    };
-    
-    
-#endif
\ No newline at end of file

--- a/TorqueController/TorqueController.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,24 +0,0 @@
-
-//cogging torque and torque ripple compensation can go here later
-
-#include "CurrentRegulator.h"
-#include "TorqueController.h"
-
-TorqueController::TorqueController(float Kt, CurrentRegulator *regulator)
-    {
-    _CurrentRegulator = regulator;
-    _Kt = Kt;
-    
-    }
-
-void TorqueController::SetTorque(float torque)
-    {
-        SetCurrent(0, torque/_Kt);
-    }
-
-void TorqueController::SetCurrent(float Id, float Iq)
-    {
-        _CurrentRegulator->UpdateRef(Id, Iq);
-        _CurrentRegulator->Commutate();
-        
-    }
\ No newline at end of file

--- a/TorqueController/TorqueController.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#ifndef TORQUECONTROLLER_H
-#define TORQUECONTROLLER_H
-
-#include "CurrentRegulator.h"
-
-class TorqueController{
-public:
-    TorqueController(float Kt, CurrentRegulator *regulator);
-    virtual void SetTorque(float torque);
-
-private:    
-    virtual void SetCurrent(float Id, float Iq);
-    CurrentRegulator* _CurrentRegulator;
-    float _Kt;
-    };
-    
-    
-
-#endif
\ No newline at end of file

--- a/Transforms/Transforms.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,39 +0,0 @@
-#include "mbed.h"
-#include "Transforms.h"
-#include "FastMath.h"
-
-using namespace FastMath;
-
-void Transforms::Park(float alpha, float beta, float theta, float *d, float *q){
-    //float cosine = cos(theta);
-    //float sine = sin(theta);
-    float cosine = FastCos(theta);
-    float sine = FastSin(theta);
-    //*d = alpha*cosine - beta*sine;      //This is a hack - effectively using -beta instead of beta
-    //*q = -beta*cosine - alpha*sine;     //I think because I'm using pi as the d axis offset instead of zero, but I need to investigate more.
-    *d = alpha*cosine + beta*sine;
-    *q = beta*cosine - alpha*sine;
-    //DAC->DHR12R1 = (int) (*q*49.648f) + 2048;
-    //DAC->DHR12R1 = (int) (*q*2048.0f) + 2048;
-    }
-
-void Transforms::InvPark(float d, float q, float theta, float *alpha, float *beta){
-    //float cosine = cos(theta);
-    //float sine = sin(theta);
-    float cosine = FastCos(theta);
-    float sine = FastSin(theta);
-    *alpha = d*cosine - q*sine;
-    *beta = q*cosine + d*sine;
-    }
-
-void Transforms::Clarke(float a, float b, float *alpha, float *beta){
-    *alpha = a;
-    *beta = 0.57735026919f*(a + 2.0f*b);
-    }
-
-void Transforms::InvClarke(float alpha, float beta, float *a, float *b, float *c){
-    *a = alpha;
-    *b = 0.5f*(-alpha + 1.73205080757f*beta);
-    *c = 0.5f*(-alpha - 1.73205080757f*beta);
-    } 
-

--- a/Transforms/Transforms.h	Tue Feb 14 03:28:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,11 +0,0 @@
-
-#include "FastMath.h"
-
-namespace Transforms{
-    void Park(float alpha, float beta, float theta, float *d, float *q);
-    void InvPark(float d, float q, float theta, float *alpha, float *beta);
-    void Clarke(float a, float b, float *alpha, float *beta);
-    void InvClarke(float alpha, float beta, float *a, float *b, float *c);
-    
-    };
-    

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/foc.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,12 @@
+#ifndef FOC_H
+#define FOC_H
+
+#include "structs.h"
+
+
+void abc(float theta, float d, float q, float *a, float *b, float *c);
+void dq0(float theta, float a, float b, float c, float *d, float *q);
+void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w);
+void zero_current(int *offset_1, int *offset_2);
+void reset_foc(ControllerStruct *controller);
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hw_setup.cpp	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,67 @@
+
+#include "mbed.h"
+#include "hw_setup.h"
+#include "hw_config.h"
+#include "structs.h"
+#include "FastPWM.h"
+
+void Init_PWM(GPIOStruct *gpio){
+    
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable the clock to GPIOC
+    RCC->APB1ENR |= 0x00000001; // enable TIM2 clock
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
+
+    //GPIOC->MODER = (1 << 8); // set pin 4 to be general purpose output
+    gpio->enable = new DigitalOut(ENABLE_PIN);
+    gpio->pwm_u = new FastPWM(PIN_U);
+    gpio->pwm_v = new FastPWM(PIN_V);
+    gpio->pwm_w = new FastPWM(PIN_W);
+    
+     //ISR Setup     
+    
+    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);   //Enable TIM1 IRQ
+
+    TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
+    TIM1->CR1 = 0x40;//CMS = 10, interrupt only when counting up
+    TIM1->CR1 |= TIM_CR1_UDIS;
+    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->ARR = 0x8CA;    //40 khz
+    TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
+    TIM1->CR1 |= TIM_CR1_CEN;    
+    
+    }
+
+void Init_ADC(void){
+        // ADC Setup
+     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
+     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;//0x0000002; // Enable clock for GPIOC
+     
+     ADC->CCR = 0x00000006; // Regular simultaneous mode only
+     ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
+     ADC1->SQR3 = 0x000000A; // use PC_0 as input
+     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
+     ADC2->SQR3 = 0x0000000B; // use PC_1 as input
+     GPIOC->MODER |= 0x0000000f; // PC_0, PC_1 are analog inputs 
+
+    }
+
+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){
+    Init_PWM(gpio);
+    Init_ADC();
+    //Init_DAC();
+    
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hw_setup.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,12 @@
+#ifndef HW_SETUP_H
+#define HW_SETUP_H
+
+#include "mbed.h"
+#include "structs.h"
+
+void Init_PWM(GPIOStruct *gpio);
+void Init_ADC(void);
+void Init_DAC(void);
+void Init_All_HW(GPIOStruct *gpio);
+
+#endif
\ No newline at end of file

--- a/main.cpp	Tue Feb 14 03:28:16 2017 +0000
+++ b/main.cpp	Thu Mar 02 15:31:23 2017 +0000
@@ -11,17 +11,18 @@
 #include "CANnucleo.h"
 #include "mbed.h"
 #include "PositionSensor.h"
-#include "Inverter.h"
-#include "SVM.h"
-#include "FastMath.h"
-#include "Transforms.h"
-#include "CurrentRegulator.h"
-#include "TorqueController.h"
-#include "ImpedanceController.h"
+#include "structs.h"
+#include "foc.h"
+#include "hw_setup.h"
+#include "math_ops.h"
+#include "current_controller_config.h"
+#include "hw_config.h"
+#include "motor_config.h"
 
+GPIOStruct gpio;
+ControllerStruct controller;
+COMStruct com;
 
-using namespace FastMath;
-using namespace Transforms;
 
 
 CANnucleo::CAN          can(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name
@@ -32,6 +33,8 @@
 int canCmd = 1000;
 volatile bool           msgAvailable = false;
 
+DigitalOut toggle(PA_0);
+Ticker  loop;
 /**
  * @brief   'CAN receive-complete' interrup handler.
  * @note    Called on arrival of new CAN message.
@@ -64,7 +67,7 @@
         }
         }
     
-void canLoop(void){
+void cancontroller(void){
     //printf("%d\n\r", canCmd);
     readCAN();
     //sendCMD(TX_ID, canCmd);
@@ -73,108 +76,129 @@
     //sendCMD(TX_ID+c_ID, c1);
     }
     
-int id[3] = {0};
-float cmd_float[3] = {0.0f};
-int raw[3] = {0};
-float val_max[3] = {18.0f, 1.0f, 0.1f};    //max angle in radians, stiffness in N-m/rad, damping in N-m*s/rad
-int buff[8];
+
 Serial pc(PA_2, PA_3);
 
-Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005);  //hall motor
-PositionSensorAM5147 spi(16384, 4.7, 21);   ///1  I really need an eeprom or something to store this....
-//PositionSensorSPI spi(2048, 1.34f, 7); ///2
+PositionSensorAM5147 spi(16384, 4.7, NPP);   ///1  I really need an eeprom or something to store this....
+//PositionSensorEncoder encoder(4096, 0, 21); 
+
+int count = 0;
+void commutate(void){
+       
+       count ++;
+
+       //pc.printf("%f\n\r", controller.theta_elec);
+        //Get rotor angle
+       //spi.GetMechPosition();
+       controller.i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset);    //Calculate phase currents from ADC readings
+       controller.i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset);
+       controller.i_a = -controller.i_b - controller.i_c;
+       
+       
+       dq0(controller.theta_elec, controller.i_a, controller.i_b, controller.i_c, &controller.i_d, &controller.i_q);    //dq0 transform on currents
+       
+       ///Control Law///
+       float i_d_error = controller.i_d_ref - controller.i_d;
+       float i_q_error = controller.i_q_ref - controller.i_q;
+       float v_d_ff = controller.i_d_ref*R_TOTAL;   //feed-forward voltage
+       float v_q_ff = controller.i_q_ref*R_TOTAL;
+       controller.d_int += i_d_error;   
+       controller.q_int += i_q_error;
+       
+       limit_norm(&controller.d_int, &controller.q_int, V_BUS/(K_Q*KI_Q));
+       //controller.d_int = fminf(fmaxf(controller.d_int, -D_INT_LIM), D_INT_LIM);
+       //controller.q_int = fminf(fmaxf(controller.q_int, -Q_INT_LIM), Q_INT_LIM);
+       
+       
+       controller.v_d = K_D*i_d_error + K_D*KI_D*controller.d_int;// + v_d_ff;  
+       controller.v_q = K_Q*i_q_error + K_Q*KI_Q*controller.q_int;// + v_q_ff; 
+       //controller.v_d = 10*v_d_ff;
+       //controller.v_q = 10*v_q_ff; 
+       limit_norm(&controller.v_d, &controller.v_q, controller.v_bus);
+       
+       abc(controller.theta_elec, controller.v_d, controller.v_q, &controller.v_u, &controller.v_v, &controller.v_w); //inverse dq0 transform on voltages
+       svm(controller.v_bus, controller.v_u, controller.v_v, controller.v_w, &controller.dtc_u, &controller.dtc_v, &controller.dtc_w); //space vector modulation
+
+       gpio.pwm_u->write(1.0f-controller.dtc_u);  //write duty cycles
+       gpio.pwm_v->write(1.0f-controller.dtc_v);
+       gpio.pwm_w->write(1.0f-controller.dtc_w);  
+       
+       controller.theta_elec = spi.GetElecPosition();   
+       //TIM1->CCR1 = (int)(controller.dtc_u * 0x8CA);//gpio.pwm_u->write(1.0f-controller.dtc_u);  //write duty cycles
+       //TIM1->CCR2 = (int)(controller.dtc_v * 0x8CA);//gpio.pwm_v->write(1.0f-controller.dtc_v);
+       //TIM1->CCR3 = (int)(controller.dtc_w * 0x8CA);//gpio.pwm_w->write(1.0f-controller.dtc_w);
+
+       //gpio.pwm_u->write(1.0f - .1f);  //write duty cycles
+       //gpio.pwm_v->write(1.0f - .1f);
+       //gpio.pwm_w->write(1.0f - .15f);
 
 
-PositionSensorEncoder encoder(4096, 0, 21); 
-
-
-
-CurrentRegulator foc(&inverter, &spi, &encoder, 0.000033, .005, .55);    
-TorqueController torqueController(.082f, &foc);
-ImpedanceController impedanceController(&torqueController, &spi, &encoder);
-
-Ticker  testing;
-
-
-
+       if(count >1000){
+           controller.i_q_ref = -controller.i_q_ref;
+           count  = 0;
+           //pc.printf("%f\n\r", controller.theta_elec);
+           //pc.printf("%f    %f    %f\n\r", controller.i_a, controller.i_b, controller.i_c);
+           //pc.printf("%f    %f\n\r", controller.i_d, controller.i_q);
+           //pc.printf("%d    %d\n\r", controller.adc1_raw, controller.adc2_raw);
+            }
+       }
+       
 
 // Current Sampling IRQ
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
   if (TIM1->SR & TIM_SR_UIF ) {
-      inverter.SampleCurrent();
-      //foc.Commutate(); ///Putting the loop here doesn't work for some reason.  Need to figure out why
+        //toggle = 1;
+        ADC1->CR2  |= 0x40000000;  
+        //volatile int delay;
+        //for (delay = 0; delay < 55; delay++);
+        
+        controller.adc2_raw = ADC2->DR;
+        controller.adc1_raw = ADC1->DR;
+        //toggle = 0;
+        commutate();        
+         
       }
   TIM1->SR = 0x0; // reset the status register
 }
 
-int count = 0;
-void Loop(void){
-    count++;
-    //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
-    //impedanceController.SetImpedance(.1, -0.01, 0);
-    float torqueCmd = ((float)(canCmd-1000))/100;
-    torqueController.SetTorque(torqueCmd);
-    if(count>100){
-        canLoop();
-        //float e = spi.GetElecPosition();
-        //float v = encoder.GetMechVelocity();
-        //printf("%f\n\r", torqueCmd);
-        //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
-        count = 0;
-        }
-
-    }
-
-void PrintStuff(void){
-    //inverter.SetDTC(0.03, 0.0, 0.0);
-
-    //float v = encoder.GetMechVelocity();
-    //float position = encoder.GetElecPosition();
-    int position = spi.GetRawPosition();
-    //float m = spi.GetMechPosition();
-    float e = spi.GetElecPosition();
-    printf("%f\n\r", e);
-    //foc.Commutate();
-    //float q = foc.GetQ();
-    //printf("position: %d   angle: %f    q current: %f\n\r", position, e, q);
-    //inverter.getCurrent()
-    //printf("%f   %f   %f   %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
-    //printf("%d   %d   %d\n\r", raw[0], raw[1], raw[2]);
-    //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
-    }
-
- /*
- ////Throw some sines on the phases.  useful to make sure the hardware works.
- void gen_sine(void){
-     float f = 1.0f;
-     float time = t.read();
-     float a = .45f*sin(6.28318530718f*f*time) + .5f;
-     float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
-     float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
-     inverter.SetDTC(a, b, c);
-     }
-*/
+       
        
 int main() {
-    inverter.DisableInverter();
+
+    controller.v_bus = V_BUS;
     spi.ZeroPosition();
+    Init_All_HW(&gpio);
+
     wait(.1);
-    inverter.SetDTC(0.0, 0.0, 0.0);
-    inverter.EnableInverter();
-    foc.Reset();
-    testing.attach(&Loop, .000025);
-    //canTick.attach(&canLoop, .01);
-    //testing.attach(&PrintStuff, .05);
+    //TIM1->CR1 |= TIM_CR1_UDIS;
+    gpio.enable->write(1);
+    gpio.pwm_u->write(1.0f);  //write duty cycles
+    gpio.pwm_v->write(1.0f);
+    gpio.pwm_w->write(1.0f);
+    zero_current(&controller.adc1_offset, &controller.adc2_offset);
+    reset_foc(&controller);
+    TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
+    gpio.enable->write(1);
+       //gpio.pwm_u->write(1.0f - .05f);  //write duty cycles
+       //gpio.pwm_v->write(1.0f - .05f);
+       //gpio.pwm_w->write(1.0f - .1f);
+    
+    wait(.1);
     NVIC_SetPriority(TIM5_IRQn, 2);
-    
+    //loop.attach(&commutate, .000025);
     can.frequency(1000000);                     // set bit rate to 1Mbps
     can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
     can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
     
     pc.baud(921600);
-    wait(.1);
-    pc.printf("HobbyKing Cheeta v1.1\n\r");
-    wait(.1);
+    wait(.01);
+    pc.printf("HobbyKing Cheetah v1.1\n\r");
+    pc.printf("ADC1 Offset: %d    ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
+    wait(.01);
+    
+
+       controller.i_d_ref = 0;
+       controller.i_q_ref = 0;
     while(1) {
 
     }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops.cpp	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,27 @@
+
+#include "math_ops.h"
+
+
+float fmaxf(float x, float y){
+    return (((x)>(y))?(x):(y));
+    }
+
+float fminf(float x, float y){
+    return (((x)<(y))?(x):(y));
+    }
+
+float fmaxf3(float x, float y, float z){
+    return (x > y ? (x > z ? x : z) : (y > z ? y : z));
+    }
+
+float fminf3(float x, float y, float z){
+    return (x < y ? (x < z ? x : z) : (y < z ? y : z));
+    }
+    
+void limit_norm(float *x, float *y, float limit){
+    float norm = sqrt(*x * *x + *y * *y);
+    if(norm > limit){
+        *x = *x * limit/norm;
+        *y = *y * limit/norm;
+        }
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,14 @@
+#ifndef MATH_OPS_H
+#define MATH_OPS_H
+
+#define PI 3.14159265359f
+
+#include "math.h"
+
+float fmaxf(float x, float y);
+float fminf(float x, float y);
+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);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/structs.h	Thu Mar 02 15:31:23 2017 +0000
@@ -0,0 +1,34 @@
+#ifndef STRUCTS_H
+#define STRUCTS_H
+
+//#include "CANnucleo.h"
+#include "mbed.h"
+#include "FastPWM.h"
+
+
+typedef struct{
+    DigitalOut *enable;
+    FastPWM *pwm_u, *pwm_v, *pwm_w;
+    } 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 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;
+    } ControllerStruct;
+
+
+#endif