Revision 0:4e1c4df6aabd, committed 2016-02-05

Comitter:

benkatz

Date:

Fri Feb 05 00:52:53 2016 +0000

Child:

1:b8bceb4daed5

Commit message:

It works!

Changed in this revision

--- /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