Motor control for robots. More compact, less object-oriented revision.

Dependencies:   FastPWM3 mbed-dev

Fork of Hobbyking_Cheetah_V1 by Ben Katz

Files at this revision

API Documentation at this revision

Comitter:
benkatz
Date:
Fri Feb 05 00:52:53 2016 +0000
Child:
1:b8bceb4daed5
Commit message:
It works!

Changed in this revision

CurrentRegulator/CurrentRegulator.cpp Show annotated file Show diff for this revision Revisions of this file
CurrentRegulator/CurrentRegulator.h Show annotated file Show diff for this revision Revisions of this file
FOC/FOC.cpp Show annotated file Show diff for this revision Revisions of this file
FOC/FOC.h Show annotated file Show diff for this revision Revisions of this file
FastMath/FastMath.cpp Show annotated file Show diff for this revision Revisions of this file
FastMath/FastMath.h Show annotated file Show diff for this revision Revisions of this file
FastMath/LUT.h Show annotated file Show diff for this revision Revisions of this file
FastPWM.lib Show annotated file Show diff for this revision Revisions of this file
Inverter/Inverter.cpp Show annotated file Show diff for this revision Revisions of this file
Inverter/Inverter.h Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.cpp Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.h Show annotated file Show diff for this revision Revisions of this file
SVM/SVM.cpp Show annotated file Show diff for this revision Revisions of this file
SVM/SVM.h Show annotated file Show diff for this revision Revisions of this file
Transforms/Transforms.cpp Show annotated file Show diff for this revision Revisions of this file
Transforms/Transforms.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,86 @@
+#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, float Kp, float Ki){
+    _Inverter = inverter;
+    PWM = new SPWM(inverter, 2.0);
+    _PositionSensor = position_sensor;
+    IQ_Ref = .7;
+    ID_Ref = 0;
+    V_Q = 0;
+    V_D = 0;
+    V_Alpha = 0;
+    V_Beta = 0;
+    I_Q = 0;
+    I_D = 0;
+    I_A = 0;
+    I_B = 0;
+    I_C = 0;
+    I_Alpha = 0;
+    I_Beta = 0;
+    count = 0;
+    _Kp = Kp;
+    _Ki = Ki;
+    Q_Integral = 0;
+    D_Integral = 0;
+    Int_Max = .9;
+    DTC_Max = .95;
+    //theta_elec = _PositionSensor->GetElecPosition();
+
+    }
+
+void CurrentRegulator::UpdateRef(float D, float Q){
+    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);
+    Park(I_Alpha, I_Beta, theta_elec, &I_D, &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;
+        
+        Q_Integral += Q_Error*_Ki*_Kp;
+        D_Integral += D_Error*_Ki*_Kp;
+        
+        if (Q_Integral > Int_Max) Q_Integral = Int_Max;
+        else if(D_Integral < -Int_Max) D_Integral = Int_Max;
+        if (D_Integral > Int_Max) D_Integral = Int_Max;
+        else if(Q_Integral < -Int_Max) Q_Integral = Int_Max;       
+         
+        V_Q = Q_Integral + _Kp*Q_Error;
+        V_D = D_Integral + _Kp*D_Error;
+    }
+        
+void CurrentRegulator::SetVoltage(){
+    InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
+    PWM->Update_DTC(V_Alpha, V_Beta);
+    }
+    
+    
+void CurrentRegulator::Commutate(){
+    theta_elec = _PositionSensor->GetElecPosition();
+    SampleCurrent();
+    //Run control loop
+    Update();
+    SetVoltage();
+    }
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CurrentRegulator/CurrentRegulator.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,28 @@
+#ifndef CURRENTRETULATOR_H
+#define CURRENTREGULATOR_H
+#include "Inverter.h"
+#include "SVM.h"
+#include "PositionSensor.h"
+
+class CurrentRegulator{
+    public:
+        CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, float Kp, float Ki);
+        void UpdateRef(float D, float Q);
+        void Commutate();
+    private:
+        float IQ_Ref, ID_Ref, V_Q, V_D, V_Alpha, V_Beta, I_Q, I_D, I_A, I_B, I_C, I_Alpha, I_Beta, theta_elec, _Kp, _Ki;
+        float Q_Integral, D_Integral, Q_Error, D_Error, Int_Max, DTC_Max;
+        void SampleCurrent();
+        void SetVoltage();
+        void Update();
+        Inverter* _Inverter;
+        PositionSensor* _PositionSensor;
+        SPWM* PWM;
+        int count;
+            
+    
+    
+    };
+    
+    
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FOC/FOC.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,2 @@
+#include "FOC.h"
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastMath/FastMath.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,14 @@
+#include "FastMath.h"
+#include "LUT.h"
+
+const float Multiplier = 81.4873308631f;
+
+float FastMath::FastSin(float theta){
+    if (theta < 0.0f) theta += 6.28318530718f;
+    if (theta >= 6.28318530718f) theta -= 6.28318530718f;    
+    return SinTable[(int) (Multiplier*theta)] ;
+    }
+    
+float FastMath::FastCos(float theta){
+    return FastSin(1.57079632679f - theta);
+    }
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastMath/FastMath.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,11 @@
+#ifndef __FASTMATH_H
+#define __FASTMATH_H
+
+#include "LUT.h"
+
+namespace FastMath{
+    float FastSin(float theta);
+    float FastCos(float theta);
+    };
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastMath/LUT.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,8 @@
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/benkatz/code/FastPWM3/#51c979bca21e
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Inverter/Inverter.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,112 @@
+#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);
+    //Current_B = new AnalogIn(BSense);
+    //Current_C = new AnalogIn(CSense);
+    
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable the clock to GPIOA
+    RCC->APB1ENR |= 0x00000001; // enable TIM2 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);
+    
+    //PWM Setup
+    TIM2->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
+    TIM2->ARR = 0x8CA; // 
+    TIM2->CCER |= TIM_CCER_CC1NP;
+         
+    //ISR Setup     
+    NVIC->ISER[0] |= 1<< (TIM2_IRQn); // enable the TIM2 IRQ
+     
+    TIM2->DIER |= TIM_DIER_UIE; // enable update interrupt
+    TIM2->CR1 |= TIM_CR1_ARPE; // autoreload on, 
+    TIM2->EGR |= TIM_EGR_UG;   
+
+    // 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 outputs   
+     
+     EnableInverter();
+     SetDTC(0.0f, 0.0f, 0.0f);
+     wait(.2);
+     ZeroCurrent();
+    }
+
+void Inverter::SetDTC(float DTC_A, float DTC_B, float DTC_C){
+        PWM_A->write(DTC_A);
+        PWM_B->write(DTC_B);
+        PWM_C->write(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 += ADC1->DR;
+        I_C_Offset += ADC2->DR;
+        ADC1->CR2  |= 0x40000000; 
+        }
+    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;
+    }
+
+void Inverter::SampleCurrent(void){
+ //   Dbg->write(1);
+    GPIOC->ODR ^= (1 << 4);
+    I_B = _I_Scale*((float) (ADC1->DR) -  I_B_Offset);
+    I_C = _I_Scale*((float) (ADC2->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; 
+
+    //I_B = Current_B->read()*_I_Scale;
+    //I_C = Current_C->read()*_I_Scale;
+    GPIOC->ODR ^= (1 << 4);
+ //   Dbg->write(0);
+    }
+    
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Inverter/Inverter.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,26 @@
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,80 @@
+
+#include "mbed.h"
+#include "PositionSensor.h"
+#include <math.h>
+
+
+    
+PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) {
+    _CPR = CPR;
+    _offset = offset;
+    
+    // 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 = 0xf1f1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
+    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   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
+  
+    TIM3->CNT = 0x8000;  //reset the counter before we use it  
+    
+    ZPulse = new InterruptIn(PB_0);
+    ZSense = new DigitalIn(PB_0);
+    ZPulse->enable_irq();
+    ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
+    ZPulse->mode(PullDown);
+    //TIM5->CCMR1 = 0xf1f1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
+
+    
+    ZTest = new DigitalOut(PC_2);
+    ZTest->write(1);
+    //int state = 0;
+    
+    
+}
+ 
+float PositionSensorEncoder::GetMechPosition() {        //returns rotor angle in radians.
+    int raw = TIM3->CNT-0x8000;
+    if (raw < 0) raw += _CPR;
+    if (raw >= _CPR) raw -= _CPR;
+    return 6.28318530718f*(raw)/(float)_CPR + _offset;    
+}
+
+float PositionSensorEncoder::GetElecPosition() {        //returns rotor electrical angle in radians.
+    int raw = TIM3->CNT-0x8000;
+    if (raw < 0) raw += _CPR;
+    if (raw >= _CPR) raw -= _CPR;
+    float signed_elec = fmod((7.0f*(6.28318530718f*(raw)/(float)_CPR + _offset)), 6.28318530718f);
+    //float signed_elec = (7*(6.28318530718*(TIM3->CNT-0x8000)/(float)_CPR + _offset));
+    if (signed_elec < 0){
+        return signed_elec + 6.28318530718f;
+        }
+    else{
+        return signed_elec;
+        }
+}
+
+void PositionSensorEncoder::ZeroEncoderCount(void){
+    if (ZSense->read() == 1){
+        if (ZSense->read() == 1){
+            TIM3->CNT=0x8000;
+            state = !state;
+            ZTest->write(state);
+        }
+        }
+    }
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,26 @@
+#ifndef POSITIONSENSOR_H
+#define POSITIONSENSOR_H
+
+class PositionSensor {
+public:
+    virtual float GetMechPosition() {return 0.0f;}
+    virtual float GetElecPosition() {return 0.0f;}
+};
+  
+  
+class PositionSensorEncoder: public PositionSensor {
+public:
+    PositionSensorEncoder(int CPR, float offset);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+private:
+    InterruptIn *ZPulse;
+    DigitalIn *ZSense;
+    DigitalOut *ZTest;
+    virtual void ZeroEncoderCount(void);
+    int _CPR;
+    int state;
+    float _offset;
+};
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SVM/SVM.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,22 @@
+
+#include "mbed.h"
+#include "SVM.h"
+#include "Inverter.h"
+
+SPWM::SPWM(Inverter *inverter, float V_Bus){
+    _inverter = inverter;
+    _V_Bus = V_Bus;
+    }
+void SPWM::Update_DTC(float V_Alpha, float V_Beta){
+    float DTC_A = V_Alpha/_V_Bus + .5f;
+    float DTC_B = 0.5f*(-V_Alpha + 1.73205080757f*V_Beta)/_V_Bus + .5f;
+    float DTC_C = 0.5f*(-V_Alpha - 1.73205080757f*V_Beta)/_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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SVM/SVM.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,37 @@
+#ifndef SVM_H
+#define SVM_H
+#include "Inverter.h"
+
+class SVM{
+public:
+    virtual void Update_DTC(float V_Alpha, float V_Beta) = 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);
+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);
+
+private:
+    float _V_Bus;
+protected:
+    Inverter* _inverter;
+    };
+    
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Transforms/Transforms.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,35 @@
+#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 a = sine;
+    //float b = cosine;
+    *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);
+    *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);
+    } 
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Transforms/Transforms.h	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,11 @@
+
+#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/main.cpp	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "PositionSensor.h"
+#include "Inverter.h"
+#include "SVM.h"
+#include "FastMath.h"
+#include "Transforms.h"
+#include "CurrentRegulator.h"
+
+PositionSensorEncoder encoder(8192,0);
+Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
+
+CurrentRegulator foc(&inverter, &encoder, .005, .5);
+
+Ticker  testing;
+//SPWM spwm(&inverter, 2.0);
+
+//DigitalOut Dbg_pin(PA_15);
+
+using namespace FastMath;
+using namespace Transforms;
+
+float offset = 0;//-0.24;
+/*
+float v_alpha = 0;
+float v_beta = 0;
+
+float i_d = 0;
+float i_q = 1;
+float v_d = 0;
+float v_q = -.2;
+float f;
+float theta;
+ */  
+extern "C" void TIM2_IRQHandler(void) {
+  // flash on update event
+  if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
+      inverter.SampleCurrent();
+      }
+  TIM2->SR = 0x0; // reset the status register
+}
+
+/*
+void PrintEncoder(void){
+    printf("%f\n\r", encoder.GetElecPosition());
+    //printf("%f\n\r", encoder.GetMechPosition());
+
+    //printf("%d\n\r", TIM3->CNT-0x8000);
+    }
+*/
+    
+void Loop(void){
+    foc.Commutate();
+    }
+/*
+void voltage_foc(void){
+    theta = encoder.GetElecPosition() + offset;
+    InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
+    spwm.Update_DTC(v_alpha, v_beta);
+    //output.write(theta/6.28318530718f);
+    }
+ */
+/*
+void open_loop(void){
+    f = t.read();
+    v_alpha = 0.1f*sin(10.0f*f);
+    v_beta = 0.1f*cos(10.0f*f);
+    spwm.Update_DTC(v_alpha, v_beta);
+    }
+    */
+       
+int main() {
+    wait(1);
+
+    testing.attach(&Loop, .0001);
+    
+    //inverter.EnableInverter();
+
+    while(1) {
+        
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Feb 05 00:52:53 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4336505e4b1c
\ No newline at end of file