File content as of revision 0:8a420ac6394e:

#include "mbed.h"
#include "PowerController.h"
#include "MillisecondCounter.h"
#include "CMSerial.h"
#include "math.h"
#include "DebugPort.h"

DebugPort pc;
PowerController::PowerController(void) :  led3(LED3),
    led4(LED4),
    ADC(),
    acquisitionTimer(this, &PowerController::AcquireData),
    pidTimer(this, &PowerController::ProcessPID)
{

    thread.start(this, &PowerController::ThreadEntry);


}

void PowerController::ThreadEntry(void)
{

    ioControl.Activate();
    ioControl.Deactivate();

    for(;;) {
        led3 = (GetTime_ms() / 1000) & 1;
        Device.deviceConfig.voltageSensed = 0.0;
        Device.deviceConfig.currentSensed = 0.0;
        Device.deviceConfig.sensedResistance = 0.0;
        Device.deviceConfig.sensedPower = 0.0;
        // Wait for an activation
        if( ioControl.isActivateSwitchOn() && ioControl.hasFalling()) {
            Device.deviceConfig.doneBit = 0;
            // Start acquiring data
            myTime = GetTime_ms();
            loopTimer = 0;
            Device.ClearAcquisitionData();
            switch (Device.deviceConfig.outputMode) {
                case OMRamp:
                    ExecuteRamp();
                    break;
                case OMConstantVoltage:
                    ExecuteConstantVoltage();
                    break;
                case OMConstantPower:
                    ExecuteConstantPower();
                    break;
                case OMCalibrate:
                    Calibrate();
                    break;
                default:
                    break;
            }
            Device.deviceConfig.doneBit = 1;
            ioControl.SetOutputVoltage(0);
            ioControl.Deactivate();

            led4 = 0;

            // pause to get a couple more samples
            rtos::Thread::wait(Device.deviceConfig.msSamplePeriod * 2);
            acquisitionTimer.stop();
        }
        // let others run
        rtos::Thread::wait(1);
    }
}

void PowerController::ExecuteRamp(void)
{
    // Set initial output voltage.
    ioControl.SetOutputVoltage(Device.deviceConfig.startVoltage);
    ioControl.SetPWMFrequency(Device.deviceConfig.pwmFrequency);

    loopTimer = 0;
    double resistance;
    unsigned powerStartTime = GetTime_ms();
    led4 = 1;
    if(Device.deviceConfig.msOpenLoopDuration > 0) {
        // Begin activation
        pid.SetTarget(Device.deviceConfig.startVoltage); // used to put the voltage into the Acquisition Data
        // Start acquiring data
        acquisitionTimer.start(Device.deviceConfig.msSamplePeriod);


        ioControl.Activate();
        //OPEN LOOP Control loop


        for (;;) {
            loopTimer = GetTime_ms() - powerStartTime;
            if (loopTimer > (Device.deviceConfig.msOpenLoopDuration))
                break;
            if (!ioControl.isActivateSwitchOn())
                break;
        }
    }
    if(Device.deviceConfig.msRampDuration >0) {
        resistance = Device.deviceConfig.startVoltage/ADC.GetSensedCurrent();

        // Calculate the voltage from the difference needed from the end and start power.
        double endVoltage = sqrt(Device.deviceConfig.rampEndPower * resistance);
        double startVoltage = sqrt(Device.deviceConfig.rampStartPower * resistance);
        double voltageDelta = endVoltage - startVoltage;


        // Calculate the voltagePerMS to find the step value for the ramp time
        double voltagePerMs = voltageDelta / (Device.deviceConfig.msRampDuration);

        // Set the Ramp starting Voltage
        ioControl.ChangeOutputVoltage( startVoltage );

        // We want to loop until our Ramp duration is complete or the activation switch has been depressed.
        // Because we don't know how long the loop takes, we need to check the loop time and set the new output
        // voltage based on the number of milliseconds that have passed during the loop.  Since our discrete voltage
        // increments are the change needed/millisecond, if our loop is faster than 1 ms, we should see a nice clean
        // ramp.  If it is slower than a millisecond, the ramp will be jagged.
        unsigned startTime = GetTime_ms();
        unsigned elapsedTime = 0;
        double setVoltage = startVoltage;

        // Begin activation
        pid.SetTarget(setVoltage); // used to put the voltage into the Acquisition Data
        for (;;) {
            // Determine how long it took us to go through the loop. The first time this should be zero
            elapsedTime = GetTime_ms() - startTime;
            loopTimer = GetTime_ms() - powerStartTime;
            // Break out of the loop if we have activated longer than the ramp duration + the open loop duration.
            if ( elapsedTime > ( Device.deviceConfig.msRampDuration  ) + ( Device.deviceConfig.msOpenLoopDuration )) {
                break;
            }
            // Double check that the user is still activating.
            if ( !ioControl.isActivateSwitchOn() ) {
                break;
            }

            // Now, set out output voltage to the last voltage plus however many steps we need to catch up.  That is, if it took
            // 4 ms to get through the loop the first time, we should set the new voltage to the last voltage + 4 ms worth of steps.
            setVoltage = (startVoltage + (voltagePerMs * elapsedTime));
            pid.SetTarget(setVoltage); // used to put the voltage into the Acquisition Data only
            // Make sure that we don't exceed the max voltage
            if ( setVoltage < endVoltage ) {
                ioControl.ChangeOutputVoltage( setVoltage );
            }
            rtos::Thread::wait(1);
        }
    }
    //start PID Control

    pid.Reset();
    pid.SetOutputRange(0,3.3);
    pid.SetTarget(Device.deviceConfig.rampEndPower);
    Device.deviceConfig.voltageSensed = ADC.GetSensedVoltage();
    Device.deviceConfig.currentSensed = ADC.GetSensedCurrent();
    pid.SetPID(Device.deviceConfig.kp,Device.deviceConfig.ki,Device.deviceConfig.kd);
    ioControl.SetLimits(Device.deviceConfig.vLim, Device.deviceConfig.iLim);
    for(;;) {
        loopTimer = GetTime_ms() - powerStartTime;
        if(loopTimer > (Device.deviceConfig.TotalDurationSeconds ))
            break;
        if(!ioControl.isActivateSwitchOn())
            break;
        Device.deviceConfig.voltageSensed = ADC.GetSensedVoltage();
        Device.deviceConfig.currentSensed = ADC.GetSensedCurrent();        
        Device.deviceConfig.sensedResistance = ADC.GetResistance();
        Device.deviceConfig.sensedPower =  Device.deviceConfig.voltageSensed  * Device.deviceConfig.currentSensed ;
        if(Device.deviceConfig.sensedResistance > 200)
            pid.SetTarget(25);
        else
            pid.SetTarget(Device.deviceConfig.rampEndPower);
        
        ProcessPID();
        // set output
        double pidout = pid.GetOutput();
        ioControl.SetHVControl(pidout);
        rtos::Thread::wait(1);
    }


    pidTimer.stop();
    ioControl.Deactivate();
}
//----------------------------------------------------------------------------------------------------------------------------------
void PowerController::ExecuteConstantVoltage(void)
{
    led4 = 1;
    int powerStartTime = GetTime_ms();
    ioControl.SetOutputVoltage(Device.deviceConfig.constantVoltage);
    ioControl.SetPWMFrequency(Device.deviceConfig.pwmFrequency);
    ioControl.SetLimits(Device.deviceConfig.vLim, Device.deviceConfig.iLim);
    pid.SetTarget(Device.deviceConfig.constantVoltage); // so Target voltage shows in Windows
    ioControl.Activate();

    acquisitionTimer.start(Device.deviceConfig.msSamplePeriod);
    for (;;) {
        loopTimer = GetTime_ms() - powerStartTime;
        if (loopTimer > (Device.deviceConfig.TotalDurationSeconds ))
            break;
        if (!ioControl.isActivateSwitchOn())
            break;
        Device.deviceConfig.voltageSensed = ADC.GetSensedVoltage();
        Device.deviceConfig.currentSensed = ADC.GetSensedCurrent();
        Device.deviceConfig.sensedResistance = ADC.GetResistance();
        pc.Print("R: ",  Device.deviceConfig.sensedResistance);
    }

}
//--------------------------------------------------------------------------------------------------------------------------------------------------
void PowerController::ExecuteConstantPower(void)
{

// Set our initial time
    pid.Reset();
    pid.SetPID(Device.deviceConfig.kp,Device.deviceConfig.ki,Device.deviceConfig.kd);
    pid.SetOutputRange(0,3.3);
    pid.SetTarget(Device.deviceConfig.constantPower);

    // set output voltage
    ioControl.SetOutputVoltage(10);
    ioControl.SetPWMFrequency(Device.deviceConfig.pwmFrequency);
    ioControl.SetLimits(Device.deviceConfig.vLim, Device.deviceConfig.iLim);
// Start acquiring data
    acquisitionTimer.start(Device.deviceConfig.msSamplePeriod);
    loopTimer = 0;
    ioControl.Activate();
    rtos::Thread::wait(1);

    Device.deviceConfig.voltageSensed = ADC.GetSensedVoltage();
    Device.deviceConfig.currentSensed = ADC.GetSensedCurrent();
    led4 = 1;
    unsigned powerStartTime = GetTime_ms();
    for(;;) {
        loopTimer = GetTime_ms() - powerStartTime;

        if(loopTimer > (Device.deviceConfig.TotalDurationSeconds ))
            break;
        if(!ioControl.isActivateSwitchOn())
            break;
        Device.deviceConfig.voltageSensed = ADC.GetSensedVoltage();
        Device.deviceConfig.currentSensed = ADC.GetSensedCurrent();
        Device.deviceConfig.sensedResistance = ADC.GetResistance();
        Device.deviceConfig.sensedPower = Device.deviceConfig.voltageSensed  * Device.deviceConfig.currentSensed;
        if(ADC.GetResistance() > 200) {
            pid.SetTarget(25);
        } else {
            pid.SetTarget(Device.deviceConfig.constantPower);
        }
        ProcessPID();
        // set output
        double pidout = pid.GetOutput();
        ioControl.SetHVControl(pidout);
        rtos::Thread::wait(1);
    }


    pidTimer.stop();


}
//-----------------------------------------------------------------------------------------------------------------
void PowerController::Calibrate(void)
{

    Device.deviceConfig.pwrCalHigh = 1;
    Device.deviceConfig.pwrCalLow = 1;
    ioControl.HVcalControl(Device.deviceConfig.voltageCal/10000);
    ioControl.Activate();

    for(;;) {
        ioControl.HVcalControl(Device.deviceConfig.voltageCal/10000);

        if (!ioControl.isActivateSwitchOn())
            break;
        rtos::Thread::wait(1);
    }
    acquisitionTimer.start(Device.deviceConfig.msSamplePeriod);
    Device.deviceConfig.pwrCalLow = ADC.GetSensedPower();
    Device.deviceConfig.vCalLow = ADC.GetSensedVoltage();

    acquisitionTimer.stop();
    ioControl.Deactivate();
    for(;;) {
        if( ioControl.isActivateSwitchOn() && ioControl.hasFalling())
            break;
        rtos::Thread::wait(1);
    }
    ioControl.HVcalControl(Device.deviceConfig.voltageCal/10000);
    ioControl.Activate();

    for(;;) {
        ioControl.HVcalControl(Device.deviceConfig.voltageCal/10000);

        if (!ioControl.isActivateSwitchOn())
            break;
        rtos::Thread::wait(1);
    }
    acquisitionTimer.start(Device.deviceConfig.msSamplePeriod);
    Device.deviceConfig.pwrCalHigh = ADC.GetSensedPower();
    Device.deviceConfig.vCalHigh = ADC.GetSensedVoltage();

    acquisitionTimer.stop();
    ioControl.Deactivate();

    FILE *fp = fopen("/local/out.txt", "w");  // Open "out.txt" on the local file system for writing
    fprintf(fp," %f \n\r",Device.deviceConfig.pwrCalLow );
    fprintf(fp," %f \n\r",Device.deviceConfig.pwrCalHigh );
    fprintf(fp," %f \n\r",Device.deviceConfig.vCalLow );
    fprintf(fp," %f \n\r",Device.deviceConfig.vCalHigh );
    if(Device.deviceConfig.kp != 10000) {
        fprintf(fp," %f \n\r",Device.deviceConfig.kp );
        fprintf(fp," %f \n\r",Device.deviceConfig.ki);
        fprintf(fp," %f \n\r",Device.deviceConfig.kd);
    } else {
        fprintf(fp," %f \n\r",0.0 );
        fprintf(fp," %f \n\r",0.0);
        fprintf(fp," %f \n\r",0.0);
    }
    fclose(fp);
}
//------------------------------------------------------------------------------------------------------------------

void PowerController::AcquireData(void)
{
    int elaspedTime = GetTime_ms() - myTime;
    AcquisitionData data;
    data.timestampMilliseconds = elaspedTime;
    data.vSense = Device.deviceConfig.voltageSensed ;
    data.iSense = Device.deviceConfig.currentSensed;
    data.vRMS = ADC.GetRMSVoltage();
    data.iRMS = ADC.GetRMSCurrent();
    data.targetVoltage = pid.GetTarget();
    data.power = Device.deviceConfig.sensedPower;
    data.resistence = Device.deviceConfig.sensedResistance;
    Device.AddAcquisitionSample(data);
}

void PowerController::ProcessPID(void)
{
    double snsV =  Device.deviceConfig.voltageSensed;
    double snsI =  Device.deviceConfig.currentSensed;
    double snsPowerCal = snsV * snsI;
    double snsPower = OffsetAndGain(Device.deviceConfig.pwrCalLow,Device.deviceConfig.pwrCalHigh ,50,100,snsPowerCal);
    double hvcontrol = ioControl.GetHVControl();
    rtos::Thread::wait(10);
    pc.Print("P: ", snsPowerCal);
    pc.Print("Pcal: ", snsPower);
    pc.Print("H: ", hvcontrol);
    pid.AddSample(snsPower, hvcontrol);
}

double PowerController::OffsetAndGain(double min,  double max,  double refMin,  double refMax, double inVal)
{
    return ((inVal - min) * (refMax - refMin))/ (max - min) + refMin;
}