File content as of revision 4:eb26ac5c3bd5:

#include "mbed.h"
#include "FastPWM.h"

#define TMD2772_Addr 0x72 //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 = 0; //number of 2.73 ms waits
const int measPeriod_ms = 699;
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,115200); //open serial port (optionally add baud rate after specifying TX and RX pins)
Timer t;
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)
{
    /*writes 1 byte to a single register*/
    char writeData[2];
    writeData[0] = reg ;
    writeData[1] = val;
    i2c.write(addr,writeData, 2);
}

void writeBlock(uint8_t addr, uint8_t startReg, uint8_t *data, uint8_t numBytes)
{
    /*writes data from an array beginning at the startReg*/
    char writeData[numBytes+1];
    writeData[0]=startReg;
    for(int n=1; n<numBytes; n++) {
        writeData[n]=data[n-1];
    }
    i2c.write(addr,writeData,numBytes+1);
}

void readRegisters(uint8_t addr, uint8_t startReg, char *regData, int numBytes)
{
    char writeData = startReg;
    i2c.write(addr,&writeData,1,true); //true is for repeated start
    i2c.read(addr,regData,numBytes);
}

uint16_t LSB_MSB_2uint16(char *data) {
/*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
    return ((uint16_t)data[1] << 8) + (uint16_t)data[0];
}

uint16_t MSB_LSB_2uint16(char *data) {
/*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
    return ((uint16_t)data[0] << 8) + (uint16_t)data[1];
}

void regDump(uint8_t Addr, uint8_t startByte, uint8_t endByte)
{
    /*print the values of up to 20 registers*/
    char regData[20];
    int numBytes;
    if (endByte>=startByte) {
        numBytes =  (endByte-startByte+1) < 20 ? (endByte-startByte+1) : 20;
    } else {
        numBytes=1;
    }

    regData[0] = startByte;
    i2c.write(Addr,regData,1,true);
    i2c.read(Addr, regData, numBytes);
    for(int n=0; n<numBytes; n++) {
        pc.printf("%X, %X \r\n", startByte+n, regData[n]);
    }
}


bool bitRead(uint16_t data, uint8_t bitNum)
{
    uint16_t mask = 1<<bitNum;
    uint16_t masked_bit = data & mask;
    return masked_bit >> bitNum;
}

float getTemp( int address) {
    char tempData[2];
    uint16_t tempBits; 
    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)*tempLSB ); //negative temp
    }
    else {
        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) 
    writeRegister(TMD2772_Addr,(0x03 | 0x80),0xFF-waitIntervals); // Wtime = 2.73 ms * delay peroids (subtract from 0xFF to enter into register)
//    writeRegister(TMD2772_Addr,(0x0D | 0x80),0x04); //optionally scale ALS gain by 0.16 by seleting 0x04;
    writeRegister(TMD2772_Addr,(0x0D | 0x80),0x00); //optionally scale ALS gain by 0.16 by seleting 0x04;

    writeRegister(TMD2772_Addr,(0x0F | 0x80),0x00); //ALS gain is 1x
}

float tempCorrectTMDCh0(float counts, float tempC) 
{
    float tDiff = tempC-tempRef;
    float delta = Ch0tempCo*tDiff; //the % difference observed vs. reference temperature
   return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
    
}

float tempCorrectTMDCh1(float counts, float tempC) 
{
    float tDiff = tempC-tempRef;
    float delta = Ch1tempCo*tDiff; //the % difference observed vs. reference temperature
   return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
    
}

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;
    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 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);
    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();
    #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;
    
    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) {
        t.start();
        updateTemperature();
        updateAlsData();
        ratio = ch0Data/ch1Data;
        err = (ch0Data - setpoint)/setpoint;
        tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
        if(loopCount<warmUp) 
        {
            sumErr=0;   //no integral gain during initial warmup--takes too long to integate error out
            pGain=0.25;
        }
        else 
        {
            sumErr += err; //use integral gain while the sensor is heating up to get to setpoint faster and avoid lag.
            pGain=pGainNormal;
        }
        inRange = (abs(sumErr)<sumErrMax) ? true : false;
        if(inRange) 
        {
            iGain=0; //no need for iGain--inRange
        } 
        else
        {
            iGain = iGainNormal;
        }
        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());   
        }
        t.reset();
    }
    
    
}