File content as of revision 2:f71d1fb67922:

#include "CurrentRegulation.h"
#include "BLDCmotorDriver.h"
#include "PI.h"

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,
                    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){
    setValues(1e-3, 1e3, 40e3, 20e3, 3.3);
    //measure.attach_us(this, &CurrentRegulation::measuring, 500);
}

void CurrentRegulation::setValues(double R_sh, double R_1, double R_fs, double R_ft, double V_ref){
    this->R_sh = R_sh;
    this->R_1 = R_1;
    this->R_fs = R_fs;
    this->R_ft = R_ft;
    this->V_ref = V_ref;    
}

double CurrentRegulation::calculateCurrentA(){
    double V_outa = (V_ref - (V_ref/2))/(1 - 0) * I_A.read() + (V_ref/2);
    return I_A_ = (R_1 * V_outa)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
}

double CurrentRegulation::calculateCurrentB(){
    double V_outb = (V_ref - (V_ref/2))/(1 - 0) * I_B.read() + (V_ref/2);
    return I_B_ = (R_1 * V_outb)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
}

double CurrentRegulation::calculateCurrentC(){
    double V_outc = (V_ref - (V_ref/2))/(1 - 0) * I_C.read() + (V_ref/2);
    return I_C_ = (R_1 * V_outc)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
}

double CurrentRegulation::calculateTotalCurrent(){
    double V_outt = (V_ref - (V_ref/2))/(1 - 0) * I_TOTAL.read()+(V_ref/2);
    return I_TOTAL_ = (R_1 * V_outt)/(R_sh * R_fs) - (R_1 * (V_ref/2))/(R_sh * R_fs);
}

double CurrentRegulation::phaseCurrent(int currentSector){
    switch(currentSector){  
           case 0:                
                I_Ar = calculateCurrentC();
                calculateTotalCurrent();
                break;
           case 1:            
                I_Ar = calculateCurrentC(); 
                calculateTotalCurrent();
                break;
           case 2:   
                I_Ar = calculateCurrentA();   
                calculateTotalCurrent();
                break;
            case 3:             
                I_Ar = calculateCurrentA(); 
                calculateTotalCurrent();
                break;
            case 4:              
                I_Ar = calculateCurrentB();  
                calculateTotalCurrent();
                break;    
            case 5:              
                I_Ar = calculateCurrentB();  
                calculateTotalCurrent();
                break;
        }
    return I_Ar;
}

void CurrentRegulation::getValue(){
    sector = m.getSector();
    procesValue = phaseCurrent(sector);
}

void CurrentRegulation::measuring(){
    measure.attach_us(this, &CurrentRegulation::getValue, 500);
}

double CurrentRegulation::calculateKr(){
    T_pv = 1e-4;
    T_ch = 25e-6;
    zeta = 1.2;
    K_ch = 50.0;
    K_pv = 1.0;
    T_I = 1.428e-3;
    K_a = 4.76;
    T_suma = (T_pv + T_ch);
    K_oR = 1/(4 * (zeta * zeta) * T_suma);
    return K_R = (K_oR * T_I)/(K_a * K_ch * K_pv);
}

/*void CurrentRegulation::input(double in){
    setPoint = 41.25 * in;
}
*/

void CurrentRegulation::setOutput(double in){
    K_R = calculateKr();
    T_d = 500e-6;
    PI reg(K_R, T_I, T_d);
    setPoint = 41.25 * in;
    u = setPoint - procesValue;
    reg.in(u); 
    output = reg.out();
    m.setDutyCycle(output);
}