CurrentRegulator.cpp

00001 #include "Transforms.h"
00002 #include "CurrentRegulator.h"
00003 #include "Inverter.h"
00004 #include "SVM.h"
00005 #include "Transforms.h"
00006 #include "PositionSensor.h"
00007 
00008 using namespace Transforms;
00009 
00010 CurrentRegulator::CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, PositionSensor *velocity_sensor, float L, float Kp, float Ki){
00011     _Inverter = inverter;
00012     PWM = new SVPWM(inverter, 2.0);
00013     _PositionSensor = position_sensor;
00014     _VelocitySensor = velocity_sensor;
00015     IQ_Ref = 0;
00016     Q_Max = 40.0f;
00017     ID_Ref = 0;
00018     V_Q = 0;
00019     V_D = 0;
00020     V_Alpha = 0;
00021     V_Beta = 0;
00022     V_A = 0;
00023     V_B = 0;
00024     V_C = 0;
00025     I_Q = 0;
00026     I_D = 0;
00027     I_A = 0;
00028     I_B = 0;
00029     I_C = 0;
00030     I_Alpha = 0;
00031     I_Beta = 0;
00032     IQ_Old = 0;
00033     ID_Old = 0;
00034     count = 0;
00035     _L = L;
00036     _Kp = Kp;
00037     _Ki = Ki;
00038     Q_Integral = 0;
00039     D_Integral = 0;
00040     Int_Max = .9;
00041     DTC_Max = .97;
00042     //theta_elec = _PositionSensor->GetElecPosition();
00043 
00044     
00045     }
00046 
00047 void CurrentRegulator::SendSPI(){
00048     
00049     
00050     }
00051     
00052 float CurrentRegulator::GetQ(){
00053     return I_Q;
00054     }
00055     
00056 void CurrentRegulator::Reset(void){
00057     IQ_Ref = 0;
00058     ID_Ref = 0;
00059     V_Q = 0;
00060     V_D = 0;
00061     V_Alpha = 0;
00062     V_Beta = 0;
00063     V_A = 0;
00064     V_B = 0;
00065     V_C = 0;
00066     I_Q = 0;
00067     I_D = 0;
00068     I_A = 0;
00069     I_B = 0;
00070     I_C = 0;
00071     I_Alpha = 0;
00072     I_Beta = 0;
00073     //pc->printf("%f, %f\n\r", Q_Integral, D_Integral);
00074     wait(.03);
00075     Q_Integral = 0;
00076     D_Integral = 0;    
00077     }
00078     
00079 void CurrentRegulator::UpdateRef(float D, float Q){
00080     if(Q>Q_Max){
00081         Q = Q_Max;
00082         }
00083     else if(Q<-Q_Max){
00084         Q = -Q_Max;
00085         }
00086     if(D>Q_Max){
00087         D = Q_Max;
00088         }
00089     else if(D<-Q_Max){
00090         D = -Q_Max;
00091         }
00092     IQ_Ref = Q;
00093     ID_Ref = D;
00094     }
00095 
00096 void CurrentRegulator::SampleCurrent(){
00097     _Inverter->GetCurrent(&I_A, &I_B, &I_C);
00098     Clarke(I_A, I_B, &I_Alpha, &I_Beta);
00099     float ID_Sample, IQ_Sample;
00100     //Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
00101     
00102     Park(I_Alpha, I_Beta, theta_elec, &ID_Sample, &IQ_Sample);    
00103     I_D = .5f*ID_Sample + .5f*ID_Old;
00104     I_Q = .5f*IQ_Sample + .5f*IQ_Old;
00105     ID_Old = I_D;
00106     IQ_Old = I_Q;
00107     //count += 1;
00108     //if(count > 10000) {
00109     //    count=0;
00110     //    printf("I_A:  %f        I_C:  %f       I_C:  %f\n\r", I_A, I_B, I_C);
00111     //IQ_Ref = -IQ_Ref;
00112     //    }
00113 
00114     //DAC->DHR12R1 = (int) (I_Q*490.648f) + 2048;
00115     //DAC->DHR12R1 = (int) (I_Alpha*4096.0f) + 2048;
00116     }
00117     
00118 void CurrentRegulator::Update(){
00119         float Q_Error = IQ_Ref - I_Q;
00120         float D_Error = ID_Ref - I_D;
00121         float w_elec = _VelocitySensor->GetElecVelocity();
00122         
00123         Q_Integral += Q_Error*_Ki*_Kp;
00124         D_Integral += D_Error*_Ki*_Kp;
00125         
00126         if (Q_Integral > Int_Max) Q_Integral = Int_Max;
00127         else if(Q_Integral < -Int_Max) Q_Integral = -Int_Max;
00128         if (D_Integral > Int_Max) D_Integral = Int_Max;
00129         else if(D_Integral < -Int_Max) D_Integral = -Int_Max;       
00130          
00131         V_Q = Q_Integral + _Kp*Q_Error;
00132         //V_Q = V_Q - w_elec*I_D;
00133         V_D = D_Integral + _Kp*D_Error;     
00134         //V_D = V_D + w_elec*I_Q;   //decoupling needs moar testing
00135     }
00136         
00137 void CurrentRegulator::SetVoltage(){
00138     InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
00139     InvClarke(V_Alpha, V_Beta, &V_A, &V_B, &V_C);
00140     PWM->Update_DTC(V_A, V_B, V_C);
00141     }
00142     
00143     
00144 void CurrentRegulator::Commutate(){
00145     count += 1;
00146     //GPIOC->ODR = (1 << 4); //Toggle pin for debugging
00147     theta_elec = _PositionSensor->GetElecPosition();
00148     _PositionSensor->GetMechPosition();
00149     SampleCurrent(); //Grab most recent current sample
00150     Update();   //Run control loop
00151     SetVoltage();   //Set inverter duty cycles
00152     //GPIOC->ODR = (0 << 4); //Toggle pin for debugging
00153 
00154     
00155     if (count==1000){
00156         //printf("%f\n\r", V_A);
00157         count = 0;
00158         }
00159 
00160     
00161     
00162       
00163       }