Diff: main.cpp

Revision:

4:eb26ac5c3bd5

Parent:

3:a8ec3c6aeb08

--- a/main.cpp	Fri Jan 13 22:30:02 2017 +0000
+++ b/main.cpp	Mon Mar 13 22:36:47 2017 +0000
@@ -2,23 +2,55 @@
 #include "FastPWM.h"
 
 #define TMD2772_Addr 0x72 //this is the 8-bit address
-#define MAX31725_Addr 0x92 //this is the 8-bit address
+#define MAX31725_Addr 0x96 //this is the 8-bit address
+#define iGainNormal 0.005
+#define pGainNormal 0.15
+#define lampVsense_pin PA_0 //do not switch this with boardPWM or it will not compile
+#define PWM_pin PA_6
+#define v12Sense_pin PA_7
+#define vDiv 4.01
+#define cal 0.987
+#define boardPWM_pin PA_3 
+#define debugPrint 1
+#define alsFilterLength 8
+#define slotHeaterPWM_pin PA_1
 
+//compiles with PA_6 as PWM and lampVsense as PA_0 and PWM_pin as PA_3
+//does not compile with PA_5 as lampvsense, boardPWM as PA_0 and PWM_pin as PA_3, 
+//DNC for lampVsense PA_3, PWM PA_6 boardPWM PA_0
 const uint8_t ALSDataRegister = 0x14;
-const uint8_t waitIntervals = 100; //number of 2.73 ms waits
+const uint8_t waitIntervals = 0; //number of 2.73 ms waits
 const int measPeriod_ms = 699;
-const float tempRef = 29.5;  //C temperature at which all optical powers are calculated to
+const float tempRef = 40;  //C temperature at which all optical powers are calculated to
 const float Ch0tempCo = 0.00228; // % per degree C
 const float Ch1tempCo = 0.001765; //% per degree C
 const float warmUp = 15; //number of measurements with a higher than usual pGain (to get to target faster);
+const float targetV = 10.9;
+const float VfilterLength = 8;
+const float tempFilterLength = 8;
+const uint8_t numLampMeas = 64; 
+float ch0Data=0;
+float ch1Data=0;   
+float vLamp;
+float v12;
+float lampVolts=0;
+float temperature=0;
+float instantTemperature=0;
+float dutyCycle =0.93; //initial duty cycle to try
+const float boardSetpointTemp = 40;
+float boardDutyCycle = 0.055;
+const float tempTol = 0.01; //tolerance within which to ignore temperature changes
+const float boardPropGain = 0.04;
+const float boardDerGain = 1.5;
 
 I2C i2c(I2C_SDA,I2C_SCL);
-Serial pc(USBTX,USBRX); //open serial port (optionally add baud rate after specifying TX and RX pins)
+Serial pc(USBTX,USBRX,115200); //open serial port (optionally add baud rate after specifying TX and RX pins)
 Timer t;
-FastPWM mypwm(PA_1);
-
-
-
+FastPWM mypwm(PWM_pin);
+AnalogIn   lampVsense(lampVsense_pin);
+AnalogIn   v12Sense(v12Sense_pin);
+FastPWM boardPWM(boardPWM_pin);
+FastPWM slotHeaterPWM(slotHeaterPWM_pin);
 //**********************************
 //declare subroutines
 void writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
@@ -88,20 +120,19 @@
 float getTemp( int address) {
     char tempData[2];
     uint16_t tempBits; 
-    const float LSB =0.00390625;
+    const float tempLSB =0.00390625;
   // read temperature
     readRegisters(MAX31725_Addr,0x00,tempData,2);
     tempBits = MSB_LSB_2uint16(tempData);
     if(bitRead(tempBits,15) == 1 )
     {
-        return( (32768-tempBits)*LSB ); //negative temp
+        return( (32768-tempBits)*tempLSB ); //negative temp
     }
     else {
-        return ( tempBits*LSB ); //positive temp
+        return ( tempBits*tempLSB ); //positive temp
     }
 } 
 
-
 void initTMD2772(void) {
     writeRegister(TMD2772_Addr,(0x00 | 0x80),0x0B);// Set power on, ALS enabled, Wait enabled, Interrupts enabled (register 0)
     writeRegister(TMD2772_Addr,(0x01 | 0x80),0x00);//ALS time register - 0x00 is max integration time of 699ms (register 1) 
@@ -128,91 +159,304 @@
     
 }
 
+float getAvgLampV(uint8_t numMeas) 
+{
+    float lampCounts=0;
+    numMeas = (numMeas > 256) ? 256 : numMeas;
+    for(int16_t n=0; n<numMeas; n++) {
+        lampCounts += lampVsense.read();
+    
+    }    
+    return lampCounts/numMeas;
+}
+
+float getAvg12V(uint8_t numMeas) 
+{
+    float lampCounts=0;
+    numMeas = (numMeas > 256) ? 256 : numMeas;
+    for(int16_t n=0; n<numMeas; n++) {
+        lampCounts += v12Sense.read();
+    
+    }    
+    return lampCounts/numMeas;
+}
+
+void updateLampVolts(uint8_t numMeas) 
+{
+    if(lampVolts == 0)
+    {
+        lampVolts =  ( vDiv * cal * 3.3 * ( getAvg12V(numMeas) - getAvgLampV(numMeas) ) - lampVolts); //initialize lamp volts  
+    }
+    else
+    {
+        lampVolts += ( vDiv * cal * 3.3 * ( getAvg12V(numMeas) - getAvgLampV(numMeas) ) - lampVolts)/VfilterLength; //update with IIR filter
+    }
+}
+
+void updateTemperature(void) 
+{
+    if (temperature == 0) 
+    {
+        instantTemperature=getTemp(MAX31725_Addr);
+        temperature = instantTemperature;
+    }
+    else 
+    {
+        instantTemperature=getTemp(MAX31725_Addr);
+        temperature += (instantTemperature-temperature)/tempFilterLength  ;  
+    }
+}
+
+void updateAlsData(void)
+{
+    float ch0RawData;
+    float ch1RawData;
+    char data[4];
+        
+    readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
+    ch0RawData = (float) LSB_MSB_2uint16(data);
+    ch1RawData = LSB_MSB_2uint16(data+2);
+    
+    if(temperature==0) {//no temp measurement
+        if(ch0Data==0) {//no prior ch0Data--initialize filter        
+            ch0Data = ch0RawData;
+            ch1Data = ch1RawData;
+        }
+        else {//prior data exists, update w/IIR filter
+            ch0Data += (ch0RawData-ch0Data)/alsFilterLength;
+            ch1Data += (ch1RawData-ch0Data)/alsFilterLength;
+        }
+    }
+    else { //temp meas exists
+        if(ch0Data == 0) {
+            ch0Data = (tempCorrectTMDCh0(ch0RawData,temperature)-ch0Data)/alsFilterLength; //initialize with temperature corrected the data 
+            ch1Data = (tempCorrectTMDCh1(ch1RawData,temperature)-ch1Data)/alsFilterLength; //initialize with temperature corrected the data 
+        }
+        else {
+            ch0Data += (tempCorrectTMDCh0(ch0RawData,temperature)-ch0Data)/alsFilterLength; //update IIR filter with temperature corrected data
+            ch1Data += (tempCorrectTMDCh1(ch1RawData,temperature)-ch1Data)/alsFilterLength; //update IIR filter with temperature corrected data
+        }
+    }      
+} //end updateCh0Data
+
+void updateLampPWM(float err, float sumErr, float pGain, float iGain)
+{
+    const float dutyCycleMin =0;
+    const float dutyCycleMax =0.98;
+    const float stepMax=0.005;
+    const float stepMin=-0.005;          
+    float step; //duty cycle change per sample 
+       
+    step = err * pGain + sumErr*iGain;
+    step = (step > stepMax) ? stepMax : step;
+    step = (step < stepMin) ? stepMin : step;
+    dutyCycle -= step;
+    dutyCycle = (dutyCycle < dutyCycleMin) ? dutyCycleMin : dutyCycle;
+    dutyCycle = (dutyCycle > dutyCycleMax) ? dutyCycleMax : dutyCycle;
+    mypwm.write(dutyCycle);   //update with new settings
+}
+
+
+void updateBoardPWM(float err, float pGain, float dGain)
+{
+    static float prevTemp=temperature;
+    const float dutyCycleMin =0;
+    const float dutyCycleMax =0.1;
+    const float stepMax=0.05;
+    const float stepMin=-0.05;          
+    float step; //duty cycle change per sample 
+    float pGain1 = pGain;   
+//    if(err>0)
+//    {
+//        pGain1=0;
+//    }
+//    else 
+//    {
+//        pGain1 = pGain;  
+//    }    
+    step = err * pGain1 + (temperature-prevTemp)/boardSetpointTemp*dGain;
+    step = (step > stepMax) ? stepMax : step;
+    step = (step < stepMin) ? stepMin : step;
+    boardDutyCycle -= step;
+    boardDutyCycle = (boardDutyCycle < dutyCycleMin) ? dutyCycleMin : boardDutyCycle;
+    boardDutyCycle = (boardDutyCycle > dutyCycleMax) ? dutyCycleMax : boardDutyCycle;
+    boardPWM.write(boardDutyCycle);   //update with new settings
+
+    prevTemp = temperature;
+    
+}
+
+
+void rampLamp(float finalDutyCycle) 
+{
+    float currentDutyCycle;
+    float step = 0.01; //increment or decrement duty cycle by 1% increments
+    bool ramping = 1;
+    currentDutyCycle = mypwm.read();
+    while (ramping) {
+        wait_ms(40);
+        if(finalDutyCycle - currentDutyCycle > step) 
+        {
+            currentDutyCycle += step;
+            mypwm.write(currentDutyCycle);
+        }
+        else if (finalDutyCycle - currentDutyCycle < -step)
+        {
+            currentDutyCycle -= step;
+            mypwm.write(currentDutyCycle);
+        }
+        else
+        {
+            ramping = 0;
+            mypwm.write(finalDutyCycle);
+        }
+    }
+}
+
+
+float getNewSetpoint(float targetVoltage, float warmUpTime, float calTime)
+{
+    warmUpTime = ( warmUpTime < 0 ) ? 0 : warmUpTime;
+    calTime = (calTime <= warmUpTime) ? calTime= warmUpTime*2+1: calTime;
+    const float calTime_ms = 1000*calTime; //convert seconds to ms
+    const float warmUpTime_ms = 1000*warmUpTime;
+    
+    const float tol =0.02;// volts tolerance on target voltage
+    const uint8_t persist=8; //number of consecutive samples in range in order to output new setpoint
+    uint8_t consecutiveInRange=0;
+    const float propGain = 2e-3; //proportional gain
+    float errVoltage;
+    float tempErr;
+    t.start();
+    
+    //turn on heater and stabilize board at target temp
+    rampLamp(dutyCycle);
+    rampLamp(0.05); //low level power keeps fan on
+    boardPWM.write(boardDutyCycle);
+    while(t.read_ms() < 120*1000) 
+    {
+        wait_ms(699);
+        updateTemperature();
+        tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
+        if (abs(tempErr*boardSetpointTemp)>tempTol) {
+            updateBoardPWM(tempErr,boardPropGain,boardDerGain);
+        }
+        
+        pc.printf( "%.4f, %.2f, %f, %f, %f, %U\r\n",lampVolts, ch0Data, mypwm.read(),boardPWM.read(),temperature,consecutiveInRange);
+    }
+    
+    rampLamp(dutyCycle);   
+    //begin adjust output to target voltage at desired sensor board temperature.
+    while(t.read_ms() < calTime_ms) { 
+        wait_ms(699);
+        updateTemperature();
+        updateLampVolts(numLampMeas); 
+        updateAlsData();
+        //#ifdef printDebug 
+            pc.printf( "%.4f, %.2f, %f, %f, %f, %U\r\n",lampVolts, ch0Data, mypwm.read(),boardPWM.read(),temperature,consecutiveInRange);
+        //#endif
+        errVoltage = lampVolts-targetVoltage;
+        if (abs(errVoltage) < tol )
+        {    
+            consecutiveInRange++;
+            if ( consecutiveInRange >= persist)
+            {    
+                if(t.read_ms() > warmUpTime_ms) 
+                {
+                    return floor(ch0Data)+0.5;
+                }
+            }
+        }
+        else
+        {
+            consecutiveInRange=0;
+        }
+        updateLampPWM(errVoltage,0,propGain,0);
+        tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
+        if (abs(tempErr*boardSetpointTemp)>tempTol) {
+            updateBoardPWM(tempErr,boardPropGain,boardDerGain);
+        }
+        
+    } //end while
+    return ch0Data;
+}
+
 int main() {
     float ratio;
-    char data[4];
-    uint16_t ch0Data;
-    uint16_t ch1Data;
-    float setpoint = 37250; //ch0/ch1 color setpoint ~0.88 duty cycle
-    static float step; //duty cycle change per sample
-    float dutyCycle=0.9275;
-    float dutyCycleMin =0.8;
-    float dutyCycleMax =0.98;
-    float stepMax=0.0025;
-    float stepMin=-0.0025;
+    static bool inRange=false;
+    float setpoint = 37250.5; //ch0/ch1 color setpoint ~0.88 duty cycle
 //    float iGain = 0.05; //integral gain --adding this because when I blew on it, it couldn't recover
     float err;
-    float tol=1; //tolerance within which to ignore changes in signal intensity
-    float pGain = 0.25; //proportional gain
-//    float quadGain = 0;*/
-    static float temperature = 0;
-    //float ch0Avg; //integral error
-//    float filterLength = 15;
-    float tempFilterLength = 8;
+    float tempErr;
+    const float tol=0.5; //tolerance within which to ignore changes in signal intensity
+    float pGain; //proportional gain
+    float iGain; // 250 : 0.2 ratio relative to pGain
+    float sumErr=0;
+    const float sumErrMax = .01;
+    
     //setup everything
     mypwm.period_us(400);
-    mypwm.write(dutyCycle);
+    boardPWM.period_us(400);
+    slotHeaterPWM.period_us(400);
+    slotHeaterPWM.write(0);
+    //mypwm.write(dutyCycle);
+    boardPWM.write(0);
     i2c.frequency(400000); //set I2C frequency to 400kHz
     wait_ms(1000);
     initTMD2772();
-    regDump(TMD2772_Addr,(0x00 | 0x80),0x0F);
+    #ifdef printDebug
+        regDump(TMD2772_Addr,(0x00 | 0x80),0x0F);
+    #endif
+    pc.printf("fan on\r\n");
     pc.printf("Done initializing\r\n");
     wait_ms(700);
     static int loopCount =0;
     
-    //get initial filter value
-//    reg2write=ALSDataRegister | 0x80;
-    readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
-//    
-//    i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
-//    i2c.read(TMD2772_Addr,data,4);
-    ch0Data = LSB_MSB_2uint16(data);
-    ch1Data = LSB_MSB_2uint16(data+2);
-    ratio = (float)ch0Data/(float)ch1Data;
-    wait_ms(699);
+    wait_ms(measPeriod_ms);
+    
+    //warmup and choose setpoint
+    pc.printf("finding %f V setpoint\r\n",targetV);
+    setpoint = getNewSetpoint(targetV, 300, 400);
+    pc.printf( "new setpoint is %f counts\r\n",setpoint );
     
-    
+
     while(1) {
-        loopCount++;
-        if (loopCount<warmUp) 
+        t.start();
+        updateTemperature();
+        updateAlsData();
+        ratio = ch0Data/ch1Data;
+        err = (ch0Data - setpoint)/setpoint;
+        tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
+        if(loopCount<warmUp) 
         {
-            pGain =0.25;
+            sumErr=0;   //no integral gain during initial warmup--takes too long to integate error out
+            pGain=0.25;
         }
         else 
         {
-            pGain =0.2;
+            sumErr += err; //use integral gain while the sensor is heating up to get to setpoint faster and avoid lag.
+            pGain=pGainNormal;
         }
-        t.start();
-        readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
-        //reg2write=ALSDataRegister | 0x80;
-////        pc.printf("%X\r\n",reg2write);
-//        i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
-//        i2c.read(TMD2772_Addr,data,4);
-        ch0Data = LSB_MSB_2uint16(data);
-        if (temperature ==0) 
+        inRange = (abs(sumErr)<sumErrMax) ? true : false;
+        if(inRange) 
         {
-            temperature = getTemp(MAX31725_Addr);
-        }
-        else 
+            iGain=0; //no need for iGain--inRange
+        } 
+        else
         {
-            temperature += (getTemp(MAX31725_Addr)-temperature)/tempFilterLength  ;  
+            iGain = iGainNormal;
         }
-        ch0Data = tempCorrectTMDCh0(ch0Data,temperature);
-        ch1Data = LSB_MSB_2uint16(data+2);
-        ch1Data = tempCorrectTMDCh1(ch0Data,temperature);
-        ratio = (float)ch0Data/(float)ch1Data;
-        err = ch0Data - setpoint;
-        pc.printf( "%U,%U, %f, %f, %f, %f\r\n",ch0Data,ch1Data,ratio,mypwm.read(),temperature, step);
-        if (abs(err)>tol) {
-            step = err/setpoint * pGain ;
-            step = (step > stepMax) ? stepMax : step;
-            step = (step < stepMin) ? stepMin : step;
-            dutyCycle -= step;
-            dutyCycle = (dutyCycle < dutyCycleMin) ? dutyCycleMin : dutyCycle;
-            dutyCycle = (dutyCycle > dutyCycleMax) ? dutyCycleMax : dutyCycle;
-            //update with new settings
-            mypwm.write(dutyCycle);
+        updateLampVolts(numLampMeas); 
+        //#ifdef printDebug
+            pc.printf( "%.2f,%.2f, %f, %f, %f, %f, %f, %U, %f, %f\r\n",ch0Data,ch1Data,ratio,mypwm.read(),boardPWM.read(),temperature,sumErr,inRange, iGain*sumErr, lampVolts );
+        //#endif
+        if (abs(err*setpoint)>tol) {
+            updateLampPWM(err,sumErr,pGain,iGain);
         }
+        if (abs(tempErr*boardSetpointTemp)>tempTol) {
+            updateBoardPWM(tempErr,boardPropGain,boardDerGain);
+        }
+        loopCount++;
         while(t.read_ms() < measPeriod_ms) {
          //pc.printf("%U \r\n",t.read_ms());   
         }