Initial commit
Dependencies: FastPWM Lamp_Intensity_Lock_withTempCo mbed
Fork of Lamp_Intensity_Lock_withTempCo by
main.cpp
- Committer:
- laserdad
- Date:
- 2017-03-13
- Revision:
- 4:eb26ac5c3bd5
- Parent:
- 3:a8ec3c6aeb08
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(); } }