File content as of revision 15:e1a76426fb6a:

#include "mbed.h"
#include "TextLCD.h"
#include "PID.h"

#define PIDRATE 0.2
 
//Kc, Ti, Td, interval
PID PIDcontroller1(2.9, 1.0, 0.0, PIDRATE);//frequency PID
PID PIDcontroller2(0.4, 1.0, 0.0, PIDRATE);//amplitude PID

TextLCD             lcd(D2, D3, D4, D5, D6, D7); // rs, e, d4-d7
Serial              pc(USBTX, USBRX); // tx, rx
PwmOut              pwmDC(D9);
PwmOut              pwmSY(D13);
DigitalOut          relay1(D8);
DigitalOut          relay2(D14);
DigitalOut          relay3(D11);
DigitalOut          relay4(D12);

DigitalIn           userButton(USER_BUTTON);

AnalogIn            syncPin(A0);
AnalogIn            gridPin(A1);
AnalogIn            differentialPin(A2);
AnalogIn            potarDC(A3);
AnalogIn            potarSY(A4);
AnalogIn            currentPin(A5);

const float         sqrt2 = 1.414213562;

Timeout             timeout;
Ticker              tickerPWMDC;
Ticker              tickerPID;
volatile bool       looping = false;
volatile bool       synchronized = false;
volatile bool       mainLoop = true;
volatile float      PID1Output;
volatile float      PID2Output;


// ##############################################
// ########## PROTOTYPES ########################
// #############################################################################
void stopLooping(void);
float getVolageRMS(AnalogIn ana_pin);
float getVolageReadedMax(AnalogIn ana_pin);
float getFrequency(AnalogIn ana_pin);
void displayLCD(float syncRMS, float gridRMS, float syncFreq, float gridFreq);
void tickerPWMDCfunction();
void initPID1();
void initPID2();
void tickerPIDfunction();
float getPhaseAngle(AnalogIn voltage_pin, AnalogIn current_pin);


// ##############################################
// ########## MAIN ##############################
// #############################################################################
int main() {
    float syncRMS, gridRMS, syncFreq, gridFreq;
    
    relay1 = 1;//Relay off=1, on=0
    relay2 = 1;//Relay off=1, on=0
    relay3 = 1;//Relay off=1, on=0
    relay4 = 0;//Relay off=1, on=0

    //initialise PIDcontrollers
    initPID1();
    initPID2();
    
    
    
    
    while(mainLoop){
        pwmDC.period(0.0002f);
        pwmDC.write(1-0.00f); //(1-duty)
        pwmSY.period(0.0002f); 
        pwmSY.write(1-0.00f); //(1-duty)
        
        while(userButton==1){;}
        
        pwmDC.write(1-0.900f); //(1-duty)
        pwmSY.write(1-0.80f); //(1-duty)
        //tickerPWMDC.attach(&tickerPWMDCfunction, 0.1);
        
        //pc.printf("\n\nAccelerating\r\n");
        lcd.printf("ACCELERATING");
        wait(5);//wait so the motor get steady state
        //pwmDC.write(1-0.00f); //(1-duty)
        
        //manual synchronisation
        while(!synchronized){
            //measure and calculate desired value
            syncRMS = getVolageRMS(syncPin);
            gridRMS = getVolageRMS(gridPin);
            syncFreq = getFrequency(syncPin);
            gridFreq = getFrequency(gridPin);
            //Update the PID process variable.
            PIDcontroller1.setProcessValue(syncFreq);
            PIDcontroller2.setProcessValue(syncRMS);
            //Interrupt for a correct PID rate
            //tickerPID.attach(&tickerPIDfunction, PIDRATE);
            //display values on LCD
            displayLCD(syncRMS, gridRMS, syncFreq, gridFreq);
            //update PID values
            PID1Output = PIDcontroller1.compute();
            PID2Output = PIDcontroller2.compute();
            //drive PWMs with PID values
            pwmDC.write(1-PID1Output); //(1-duty)
            pwmSY.write(1-PID2Output); //(1-duty)
            pc.printf("PID1:%f \t syncFreq:%f \r\n", PID1Output, syncFreq);
            pc.printf("PID2:%f \t syncRMS:%f \r\n\n", PID2Output, syncRMS);
            wait(PIDRATE);
            //voltage and frequency matching
            if(abs(syncRMS-gridRMS)<0.5 && abs(syncFreq-gridFreq)<0.1){
                //pc.printf("voltage and freqency OK\r\n");
                lcd.locate(11,0);//(col,row)
                lcd.printf("V&fOK");
                while(!synchronized){//phase matching loop
                    //measure and calculate desired value
                    syncRMS = getVolageRMS(syncPin);
                    gridRMS = getVolageRMS(gridPin);
                    syncFreq = getFrequency(syncPin);
                    gridFreq = getFrequency(gridPin);
                    //display values on LCD
                    displayLCD(syncRMS, gridRMS, syncFreq, gridFreq);
                    //phase matching
                    if(getVolageReadedMax(differentialPin) < 0.050){
                        //pc.printf("SYNCHONIZATION OK\r\n\n");
                        lcd.locate(12,1);//(col,row)
                        lcd.printf("SYNC");
                        relay1 = 0;//Relay off=1, on=0 // to hear the noise
                        relay2 = 0;//Relay off=1, on=0 // to hear the noise
                        relay3 = 0;//Relay off=1, on=0 // to hear the noise
                        relay4 = 1;//Relay off=1, on=0 // to hear the noise
                        pwmDC.write(1-0.00f); //(1-duty)
                        pwmSY.write(1-0.00f); //(1-duty)
                        synchronized = true;
                        mainLoop = false;
                    }
                }
            }
        }
        while(true)
        getPhaseAngle(currentPin, gridPin);
    }
}







// ##############################################
// ########## FUNCTIONS #########################
// #############################################################################
// ISR to stop loping
void stopLooping(void) {
    looping = false;//looping is volatile bool
}

// #############################################################################
// ISR to update pwmDC with potarDC
void tickerPWMDCfunction(){
    float valuePotar1;
    float valuePotar2;
    valuePotar1 = potarDC.read();
    pwmDC.write(1-valuePotar1);
    valuePotar2 = potarSY.read();
    pwmSY.write(1-valuePotar2);
    //lcd.locate(12,0);//(col,row)
    //lcd.printf("%f",valuePotar);
}


// #############################################################################
// ISR to update PID
void tickerPIDfunction(){
    PID1Output = PIDcontroller1.compute();
    pwmDC.write(1-PID1Output); //(1-duty)
    pc.printf("PID1:%f\r\n\n", PID1Output);
}


// #############################################################################
void initPID1(){
    //Input 
    PIDcontroller1.setInputLimits(0.0, 100.0);
    //Pwm output from 0.0 to 1.0
    PIDcontroller1.setOutputLimits(0.0, 1.0);
    //If there's a bias.
    PIDcontroller1.setBias(0.70);
    PIDcontroller1.setMode(true);
    //We want the process variable to be 50Hz
    PIDcontroller1.setSetPoint(50);//50Hz
}

// #############################################################################
void initPID2(){
    //Input 
    PIDcontroller2.setInputLimits(0.0, 25.0);
    //Pwm output from 0.0 to 1.0
    PIDcontroller2.setOutputLimits(0.0, 1.0);
    //If there's a bias.
    PIDcontroller2.setBias(0.70);
    PIDcontroller2.setMode(true);
    //We want the process variable to be 50Hz
    PIDcontroller2.setSetPoint(getVolageRMS(gridPin));
}

// #############################################################################
float getVolageRMS(AnalogIn ana_pin){
    float v1;//readed voltage
    float v1Max = 0;//max readed voltage
    float VRMS; //RMS voltage
    looping = true;
    timeout.attach(callback(&stopLooping), 0.020);//T=20ms because f=50Hz
    while(looping){
        v1 = ana_pin.read()*3.3;
        if(v1 > v1Max){
            v1Max = v1;
        }
    }
    VRMS = (v1Max+0.685)*9.32/sqrt2;
    //pc.printf("VRMS: %f\r\n",VRMS);
    return VRMS;
}


// #############################################################################
float getVolageReadedMax(AnalogIn ana_pin){
    float v1;//readed voltage
    float v1Max = 0;//max readed voltage
    looping = true;
    timeout.attach(callback(&stopLooping), 0.025);//T=25>20ms because f=50Hz
    while(looping){
        v1 = ana_pin.read()*3.3;
        if(v1 > v1Max){
            v1Max = v1;
        }
    }
    return v1Max;
}


// #############################################################################
float getFrequency(AnalogIn ana_pin){
    float   freq; //frequency
    float   maxReadedVoltage;//maximum voltage readed by the ADC
    float   readedVoltage;//readed voltage
    int     nbrRisingEdge=0;// number of rising edge detected
    float   T;//Periode
    Timer   timer;
    maxReadedVoltage = getVolageReadedMax(ana_pin);
    //pc.printf("maxReadedVoltage: %f\r\n",maxReadedVoltage);
    bool aboveLine = true;
    bool allowedClicTimer = false;
    looping = true;
    timeout.attach(callback(&stopLooping), 1);//try to find rising edges during 1sec max
    while(nbrRisingEdge<2 and looping){
        readedVoltage = ana_pin.read()*3.3;
        if(readedVoltage<(maxReadedVoltage/2)){//rising edge detection ready
            aboveLine = false;
        }
        if((maxReadedVoltage/2)<readedVoltage && aboveLine==false){//rising edge detected
            allowedClicTimer = true;
            aboveLine = true;
        }
        if((maxReadedVoltage*2/3)<readedVoltage && allowedClicTimer==true){//rising edge detected
            allowedClicTimer = false;
            if(nbrRisingEdge==0)
                timer.start();
            if(nbrRisingEdge==1)
                timer.stop();
            nbrRisingEdge++;
        }
        
    }
    if(nbrRisingEdge!=2){
        lcd.locate(13,1);
        lcd.printf("f!%d",nbrRisingEdge);
    }
    T = timer.read();
    freq = 1/T;
    //pc.printf("T: %f\r\n",T);
    //pc.printf("freq: %f\r\n\n",freq);
    if(looping==false)
    freq = 0;
    return freq;
}


// #############################################################################
void displayLCD(float syncRMS, float gridRMS, float syncFreq, float gridFreq){
    lcd.locate(0,0);//(col,row)
    lcd.printf("                ");
    lcd.locate(0,1);//(col,row)
    lcd.printf("                ");
    lcd.locate(0,0);//(col,row)
    lcd.printf("G:%3.1f@%3.1f", gridRMS, gridFreq);
    lcd.locate(0,1);//(col,row)
    lcd.printf("S:%3.1f@%3.1f", syncRMS, syncFreq);
    
}


// #############################################################################
float getPhaseAngle(AnalogIn voltage_pin, AnalogIn current_pin){
    bool searchingInstant = true;
    Timer   timer1;
    float voltageValue;
    //float   maxReadedVoltageVoltage, maxReadedVoltageCurrent;//maximum voltage readed by the ADC
    //maxReadedVoltageVoltage = getVolageReadedMax(voltage_pin);
    //maxReadedVoltageCurrent= getVolageReadedMax(current_pin);
    //pc.printf("maxReadedVoltageVoltage: %f \t maxReadedVoltageCurrent: %f\r\n", maxReadedVoltageVoltage, maxReadedVoltageCurrent);
    while(searchingInstant){
        voltageValue = voltage_pin.read();
        //pc.printf("voltageValue voltage:%f\n\r",voltageValue);
        if(voltageValue < 0.50){
            timer1.start();
            searchingInstant = false;
        }
    }
    searchingInstant = true;
    while(searchingInstant){
        voltageValue = current_pin.read();
        //pc.printf("voltageValue current:%f\n\r",voltageValue);
        if(voltageValue < 0.050){
            timer1.stop();
            searchingInstant = false;
        }
    }
    pc.printf("phaseAngle:%f\n\r",timer1.read()*180/0.020);
    return 0;
}