Justin Rhodes
baseline build
Dependencies: FastPWM mbed-os mbed
Diff: PowerController.cpp
- Revision:
- 0:8a420ac6394e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerController.cpp Mon Jun 19 15:55:51 2017 +0000
@@ -0,0 +1,358 @@
+
+#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;
+}
+