CurrentRegulation.cpp

00001 #include "CurrentRegulation.h"
00002 #include "BLDCmotorDriver.h"
00003 #include "PI.h"
00004 
00005 CurrentRegulation::CurrentRegulation(PinName pI_A, PinName pI_B, PinName pI_C, PinName pI_TOTAL, PinName pGH_A, PinName pGH_B, PinName pGH_C, PinName pGL_A, PinName pGL_B, PinName pGL_C,
00006                     PinName pH1, PinName pH2, PinName pH3, PinName pFault):I_A(pI_A), I_B(pI_B), I_C(pI_C), I_TOTAL(pI_TOTAL), m(pGH_A, pGH_B, pGH_C, pGL_A, pGL_B, pGL_C, pH1, pH2, pH3, LED1){
00007     setValues(1e-3, 1e3, 40e3, 20e3, 3.3);
00008     //measure.attach_us(this, &CurrentRegulation::measuring, 500);
00009 }
00010 
00011 void CurrentRegulation::setValues(double R_sh, double R_1, double R_fs, double R_ft, double V_ref){
00012     this->R_sh = R_sh;
00013     this->R_1 = R_1;
00014     this->R_fs = R_fs;
00015     this->R_ft = R_ft;
00016     this->V_ref = V_ref;    
00017 }
00018 
00019 double CurrentRegulation::calculateCurrentA(){
00020     double V_outa = (V_ref - (V_ref/2))/(1 - 0) * I_A.read() + (V_ref/2);
00021     return I_A_ = (R_1 * V_outa)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
00022 }
00023 
00024 double CurrentRegulation::calculateCurrentB(){
00025     double V_outb = (V_ref - (V_ref/2))/(1 - 0) * I_B.read() + (V_ref/2);
00026     return I_B_ = (R_1 * V_outb)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
00027 }
00028 
00029 double CurrentRegulation::calculateCurrentC(){
00030     double V_outc = (V_ref - (V_ref/2))/(1 - 0) * I_C.read() + (V_ref/2);
00031     return I_C_ = (R_1 * V_outc)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
00032 }
00033 
00034 double CurrentRegulation::calculateTotalCurrent(){
00035     double V_outt = (V_ref - (V_ref/2))/(1 - 0) * I_TOTAL.read()+(V_ref/2);
00036     return I_TOTAL_ = (R_1 * V_outt)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
00037 }
00038 
00039 double CurrentRegulation::phaseCurrent(int currentSector){
00040     switch(currentSector){  
00041            case 0:                
00042                 I_Ar = calculateCurrentC();
00043                 calculateTotalCurrent();
00044                 break;
00045            case 1:            
00046                 I_Ar = calculateCurrentC(); 
00047                 calculateTotalCurrent();
00048                 break;
00049            case 2:   
00050                 I_Ar = calculateCurrentA();   
00051                 calculateTotalCurrent();
00052                 break;
00053             case 3:             
00054                 I_Ar = calculateCurrentA(); 
00055                 calculateTotalCurrent();
00056                 break;
00057             case 4:              
00058                 I_Ar = calculateCurrentB();  
00059                 calculateTotalCurrent();
00060                 break;    
00061             case 5:              
00062                 I_Ar = calculateCurrentB();  
00063                 calculateTotalCurrent();
00064                 break;
00065         }
00066     return I_Ar;
00067 }
00068 
00069 void CurrentRegulation::getValue(){
00070     sector = m.getSector();
00071     procesValue = phaseCurrent(sector);
00072 }
00073 
00074 void CurrentRegulation::measuring(){
00075     measure.attach_us(this, &CurrentRegulation::getValue, 500);
00076 }
00077 
00078 double CurrentRegulation::calculateKr(){
00079     T_pv = 1e-4;
00080     T_ch = 25e-6;
00081     zeta = 1.2;
00082     K_ch = 50.0;
00083     K_pv = 1.0;
00084     T_I = 1.428e-3;
00085     K_a = 4.76;
00086     T_suma = (T_pv + T_ch);
00087     K_oR = 1/(4 * (zeta * zeta) * T_suma);
00088     return K_R = (K_oR * T_I)/(K_a * K_ch * K_pv);
00089 }
00090 
00091 /*void CurrentRegulation::input(double in){
00092     setPoint = 41.25 * in;
00093 }
00094 */
00095 
00096 void CurrentRegulation::setOutput(double in){
00097     K_R = calculateKr();
00098     T_d = 500e-6;
00099     PI reg(K_R, T_I, T_d);
00100     setPoint = 41.25 * in;
00101     u = setPoint - procesValue;
00102     reg.in(u); 
00103     output = reg.out();
00104     m.setDutyCycle(output);
00105 }