main.cpp

00001 #include "mbed.h"
00002 #include "FastPWM.h"
00003 
00004 #define TMD2772_Addr 0x72 //this is the 8-bit address
00005 #define MAX31725_Addr 0x96 //this is the 8-bit address
00006 #define iGainNormal 0.005
00007 #define pGainNormal 0.15
00008 #define lampVsense_pin PA_0 //do not switch this with boardPWM or it will not compile
00009 #define PWM_pin PA_6
00010 #define v12Sense_pin PA_7
00011 #define vDiv 4.01
00012 #define cal 0.987
00013 #define boardPWM_pin PA_3 
00014 #define debugPrint 1
00015 #define alsFilterLength 8
00016 #define slotHeaterPWM_pin PA_1
00017 
00018 //compiles with PA_6 as PWM and lampVsense as PA_0 and PWM_pin as PA_3
00019 //does not compile with PA_5 as lampvsense, boardPWM as PA_0 and PWM_pin as PA_3, 
00020 //DNC for lampVsense PA_3, PWM PA_6 boardPWM PA_0
00021 const uint8_t ALSDataRegister = 0x14;
00022 const uint8_t waitIntervals = 0; //number of 2.73 ms waits
00023 const int measPeriod_ms = 699;
00024 const float tempRef = 40;  //C temperature at which all optical powers are calculated to
00025 const float Ch0tempCo = 0.00228; // % per degree C
00026 const float Ch1tempCo = 0.001765; //% per degree C
00027 const float warmUp = 15; //number of measurements with a higher than usual pGain (to get to target faster);
00028 const float targetV = 10.9;
00029 const float VfilterLength = 8;
00030 const float tempFilterLength = 8;
00031 const uint8_t numLampMeas = 64; 
00032 float ch0Data=0;
00033 float ch1Data=0;   
00034 float vLamp;
00035 float v12;
00036 float lampVolts=0;
00037 float temperature=0;
00038 float instantTemperature=0;
00039 float dutyCycle =0.93; //initial duty cycle to try
00040 const float boardSetpointTemp = 40;
00041 float boardDutyCycle = 0.055;
00042 const float tempTol = 0.01; //tolerance within which to ignore temperature changes
00043 const float boardPropGain = 0.04;
00044 const float boardDerGain = 1.5;
00045 
00046 I2C i2c(I2C_SDA,I2C_SCL);
00047 Serial pc(USBTX,USBRX,115200); //open serial port (optionally add baud rate after specifying TX and RX pins)
00048 Timer t;
00049 FastPWM mypwm(PWM_pin);
00050 AnalogIn   lampVsense(lampVsense_pin);
00051 AnalogIn   v12Sense(v12Sense_pin);
00052 FastPWM boardPWM(boardPWM_pin);
00053 FastPWM slotHeaterPWM(slotHeaterPWM_pin);
00054 //**********************************
00055 //declare subroutines
00056 void writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
00057 {
00058     /*writes 1 byte to a single register*/
00059     char writeData[2];
00060     writeData[0] = reg ;
00061     writeData[1] = val;
00062     i2c.write(addr,writeData, 2);
00063 }
00064 
00065 void writeBlock(uint8_t addr, uint8_t startReg, uint8_t *data, uint8_t numBytes)
00066 {
00067     /*writes data from an array beginning at the startReg*/
00068     char writeData[numBytes+1];
00069     writeData[0]=startReg;
00070     for(int n=1; n<numBytes; n++) {
00071         writeData[n]=data[n-1];
00072     }
00073     i2c.write(addr,writeData,numBytes+1);
00074 }
00075 
00076 void readRegisters(uint8_t addr, uint8_t startReg, char *regData, int numBytes)
00077 {
00078     char writeData = startReg;
00079     i2c.write(addr,&writeData,1,true); //true is for repeated start
00080     i2c.read(addr,regData,numBytes);
00081 }
00082 
00083 uint16_t LSB_MSB_2uint16(char *data) {
00084 /*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
00085     return ((uint16_t)data[1] << 8) + (uint16_t)data[0];
00086 }
00087 
00088 uint16_t MSB_LSB_2uint16(char *data) {
00089 /*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
00090     return ((uint16_t)data[0] << 8) + (uint16_t)data[1];
00091 }
00092 
00093 void regDump(uint8_t Addr, uint8_t startByte, uint8_t endByte)
00094 {
00095     /*print the values of up to 20 registers*/
00096     char regData[20];
00097     int numBytes;
00098     if (endByte>=startByte) {
00099         numBytes =  (endByte-startByte+1) < 20 ? (endByte-startByte+1) : 20;
00100     } else {
00101         numBytes=1;
00102     }
00103 
00104     regData[0] = startByte;
00105     i2c.write(Addr,regData,1,true);
00106     i2c.read(Addr, regData, numBytes);
00107     for(int n=0; n<numBytes; n++) {
00108         pc.printf("%X, %X \r\n", startByte+n, regData[n]);
00109     }
00110 }
00111 
00112 
00113 bool bitRead(uint16_t data, uint8_t bitNum)
00114 {
00115     uint16_t mask = 1<<bitNum;
00116     uint16_t masked_bit = data & mask;
00117     return masked_bit >> bitNum;
00118 }
00119 
00120 float getTemp( int address) {
00121     char tempData[2];
00122     uint16_t tempBits; 
00123     const float tempLSB =0.00390625;
00124   // read temperature
00125     readRegisters(MAX31725_Addr,0x00,tempData,2);
00126     tempBits = MSB_LSB_2uint16(tempData);
00127     if(bitRead(tempBits,15) == 1 )
00128     {
00129         return( (32768-tempBits)*tempLSB ); //negative temp
00130     }
00131     else {
00132         return ( tempBits*tempLSB ); //positive temp
00133     }
00134 } 
00135 
00136 void initTMD2772(void) {
00137     writeRegister(TMD2772_Addr,(0x00 | 0x80),0x0B);// Set power on, ALS enabled, Wait enabled, Interrupts enabled (register 0)
00138     writeRegister(TMD2772_Addr,(0x01 | 0x80),0x00);//ALS time register - 0x00 is max integration time of 699ms (register 1) 
00139     writeRegister(TMD2772_Addr,(0x03 | 0x80),0xFF-waitIntervals); // Wtime = 2.73 ms * delay peroids (subtract from 0xFF to enter into register)
00140 //    writeRegister(TMD2772_Addr,(0x0D | 0x80),0x04); //optionally scale ALS gain by 0.16 by seleting 0x04;
00141     writeRegister(TMD2772_Addr,(0x0D | 0x80),0x00); //optionally scale ALS gain by 0.16 by seleting 0x04;
00142 
00143     writeRegister(TMD2772_Addr,(0x0F | 0x80),0x00); //ALS gain is 1x
00144 }
00145 
00146 float tempCorrectTMDCh0(float counts, float tempC) 
00147 {
00148     float tDiff = tempC-tempRef;
00149     float delta = Ch0tempCo*tDiff; //the % difference observed vs. reference temperature
00150    return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
00151     
00152 }
00153 
00154 float tempCorrectTMDCh1(float counts, float tempC) 
00155 {
00156     float tDiff = tempC-tempRef;
00157     float delta = Ch1tempCo*tDiff; //the % difference observed vs. reference temperature
00158    return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
00159     
00160 }
00161 
00162 float getAvgLampV(uint8_t numMeas) 
00163 {
00164     float lampCounts=0;
00165     numMeas = (numMeas > 256) ? 256 : numMeas;
00166     for(int16_t n=0; n<numMeas; n++) {
00167         lampCounts += lampVsense.read();
00168     
00169     }    
00170     return lampCounts/numMeas;
00171 }
00172 
00173 float getAvg12V(uint8_t numMeas) 
00174 {
00175     float lampCounts=0;
00176     numMeas = (numMeas > 256) ? 256 : numMeas;
00177     for(int16_t n=0; n<numMeas; n++) {
00178         lampCounts += v12Sense.read();
00179     
00180     }    
00181     return lampCounts/numMeas;
00182 }
00183 
00184 void updateLampVolts(uint8_t numMeas) 
00185 {
00186     if(lampVolts == 0)
00187     {
00188         lampVolts =  ( vDiv * cal * 3.3 * ( getAvg12V(numMeas) - getAvgLampV(numMeas) ) - lampVolts); //initialize lamp volts  
00189     }
00190     else
00191     {
00192         lampVolts += ( vDiv * cal * 3.3 * ( getAvg12V(numMeas) - getAvgLampV(numMeas) ) - lampVolts)/VfilterLength; //update with IIR filter
00193     }
00194 }
00195 
00196 void updateTemperature(void) 
00197 {
00198     if (temperature == 0) 
00199     {
00200         instantTemperature=getTemp(MAX31725_Addr);
00201         temperature = instantTemperature;
00202     }
00203     else 
00204     {
00205         instantTemperature=getTemp(MAX31725_Addr);
00206         temperature += (instantTemperature-temperature)/tempFilterLength  ;  
00207     }
00208 }
00209 
00210 void updateAlsData(void)
00211 {
00212     float ch0RawData;
00213     float ch1RawData;
00214     char data[4];
00215         
00216     readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
00217     ch0RawData = (float) LSB_MSB_2uint16(data);
00218     ch1RawData = LSB_MSB_2uint16(data+2);
00219     
00220     if(temperature==0) {//no temp measurement
00221         if(ch0Data==0) {//no prior ch0Data--initialize filter        
00222             ch0Data = ch0RawData;
00223             ch1Data = ch1RawData;
00224         }
00225         else {//prior data exists, update w/IIR filter
00226             ch0Data += (ch0RawData-ch0Data)/alsFilterLength;
00227             ch1Data += (ch1RawData-ch0Data)/alsFilterLength;
00228         }
00229     }
00230     else { //temp meas exists
00231         if(ch0Data == 0) {
00232             ch0Data = (tempCorrectTMDCh0(ch0RawData,temperature)-ch0Data)/alsFilterLength; //initialize with temperature corrected the data 
00233             ch1Data = (tempCorrectTMDCh1(ch1RawData,temperature)-ch1Data)/alsFilterLength; //initialize with temperature corrected the data 
00234         }
00235         else {
00236             ch0Data += (tempCorrectTMDCh0(ch0RawData,temperature)-ch0Data)/alsFilterLength; //update IIR filter with temperature corrected data
00237             ch1Data += (tempCorrectTMDCh1(ch1RawData,temperature)-ch1Data)/alsFilterLength; //update IIR filter with temperature corrected data
00238         }
00239     }      
00240 } //end updateCh0Data
00241 
00242 void updateLampPWM(float err, float sumErr, float pGain, float iGain)
00243 {
00244     const float dutyCycleMin =0;
00245     const float dutyCycleMax =0.98;
00246     const float stepMax=0.005;
00247     const float stepMin=-0.005;          
00248     float step; //duty cycle change per sample 
00249        
00250     step = err * pGain + sumErr*iGain;
00251     step = (step > stepMax) ? stepMax : step;
00252     step = (step < stepMin) ? stepMin : step;
00253     dutyCycle -= step;
00254     dutyCycle = (dutyCycle < dutyCycleMin) ? dutyCycleMin : dutyCycle;
00255     dutyCycle = (dutyCycle > dutyCycleMax) ? dutyCycleMax : dutyCycle;
00256     mypwm.write(dutyCycle);   //update with new settings
00257 }
00258 
00259 
00260 void updateBoardPWM(float err, float pGain, float dGain)
00261 {
00262     static float prevTemp=temperature;
00263     const float dutyCycleMin =0;
00264     const float dutyCycleMax =0.1;
00265     const float stepMax=0.05;
00266     const float stepMin=-0.05;          
00267     float step; //duty cycle change per sample 
00268     float pGain1 = pGain;   
00269 //    if(err>0)
00270 //    {
00271 //        pGain1=0;
00272 //    }
00273 //    else 
00274 //    {
00275 //        pGain1 = pGain;  
00276 //    }    
00277     step = err * pGain1 + (temperature-prevTemp)/boardSetpointTemp*dGain;
00278     step = (step > stepMax) ? stepMax : step;
00279     step = (step < stepMin) ? stepMin : step;
00280     boardDutyCycle -= step;
00281     boardDutyCycle = (boardDutyCycle < dutyCycleMin) ? dutyCycleMin : boardDutyCycle;
00282     boardDutyCycle = (boardDutyCycle > dutyCycleMax) ? dutyCycleMax : boardDutyCycle;
00283     boardPWM.write(boardDutyCycle);   //update with new settings
00284 
00285     prevTemp = temperature;
00286     
00287 }
00288 
00289 
00290 void rampLamp(float finalDutyCycle) 
00291 {
00292     float currentDutyCycle;
00293     float step = 0.01; //increment or decrement duty cycle by 1% increments
00294     bool ramping = 1;
00295     currentDutyCycle = mypwm.read();
00296     while (ramping) {
00297         wait_ms(40);
00298         if(finalDutyCycle - currentDutyCycle > step) 
00299         {
00300             currentDutyCycle += step;
00301             mypwm.write(currentDutyCycle);
00302         }
00303         else if (finalDutyCycle - currentDutyCycle < -step)
00304         {
00305             currentDutyCycle -= step;
00306             mypwm.write(currentDutyCycle);
00307         }
00308         else
00309         {
00310             ramping = 0;
00311             mypwm.write(finalDutyCycle);
00312         }
00313     }
00314 }
00315 
00316 
00317 float getNewSetpoint(float targetVoltage, float warmUpTime, float calTime)
00318 {
00319     warmUpTime = ( warmUpTime < 0 ) ? 0 : warmUpTime;
00320     calTime = (calTime <= warmUpTime) ? calTime= warmUpTime*2+1: calTime;
00321     const float calTime_ms = 1000*calTime; //convert seconds to ms
00322     const float warmUpTime_ms = 1000*warmUpTime;
00323     
00324     const float tol =0.02;// volts tolerance on target voltage
00325     const uint8_t persist=8; //number of consecutive samples in range in order to output new setpoint
00326     uint8_t consecutiveInRange=0;
00327     const float propGain = 2e-3; //proportional gain
00328     float errVoltage;
00329     float tempErr;
00330     t.start();
00331     
00332     //turn on heater and stabilize board at target temp
00333     rampLamp(dutyCycle);
00334     rampLamp(0.05); //low level power keeps fan on
00335     boardPWM.write(boardDutyCycle);
00336     while(t.read_ms() < 120*1000) 
00337     {
00338         wait_ms(699);
00339         updateTemperature();
00340         tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
00341         if (abs(tempErr*boardSetpointTemp)>tempTol) {
00342             updateBoardPWM(tempErr,boardPropGain,boardDerGain);
00343         }
00344         
00345         pc.printf( "%.4f, %.2f, %f, %f, %f, %U\r\n",lampVolts, ch0Data, mypwm.read(),boardPWM.read(),temperature,consecutiveInRange);
00346     }
00347     
00348     rampLamp(dutyCycle);   
00349     //begin adjust output to target voltage at desired sensor board temperature.
00350     while(t.read_ms() < calTime_ms) { 
00351         wait_ms(699);
00352         updateTemperature();
00353         updateLampVolts(numLampMeas); 
00354         updateAlsData();
00355         //#ifdef printDebug 
00356             pc.printf( "%.4f, %.2f, %f, %f, %f, %U\r\n",lampVolts, ch0Data, mypwm.read(),boardPWM.read(),temperature,consecutiveInRange);
00357         //#endif
00358         errVoltage = lampVolts-targetVoltage;
00359         if (abs(errVoltage) < tol )
00360         {    
00361             consecutiveInRange++;
00362             if ( consecutiveInRange >= persist)
00363             {    
00364                 if(t.read_ms() > warmUpTime_ms) 
00365                 {
00366                     return floor(ch0Data)+0.5;
00367                 }
00368             }
00369         }
00370         else
00371         {
00372             consecutiveInRange=0;
00373         }
00374         updateLampPWM(errVoltage,0,propGain,0);
00375         tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
00376         if (abs(tempErr*boardSetpointTemp)>tempTol) {
00377             updateBoardPWM(tempErr,boardPropGain,boardDerGain);
00378         }
00379         
00380     } //end while
00381     return ch0Data;
00382 }
00383 
00384 int main() {
00385     float ratio;
00386     static bool inRange=false;
00387     float setpoint = 37250.5; //ch0/ch1 color setpoint ~0.88 duty cycle
00388 //    float iGain = 0.05; //integral gain --adding this because when I blew on it, it couldn't recover
00389     float err;
00390     float tempErr;
00391     const float tol=0.5; //tolerance within which to ignore changes in signal intensity
00392     float pGain; //proportional gain
00393     float iGain; // 250 : 0.2 ratio relative to pGain
00394     float sumErr=0;
00395     const float sumErrMax = .01;
00396     
00397     //setup everything
00398     mypwm.period_us(400);
00399     boardPWM.period_us(400);
00400     slotHeaterPWM.period_us(400);
00401     slotHeaterPWM.write(0);
00402     //mypwm.write(dutyCycle);
00403     boardPWM.write(0);
00404     i2c.frequency(400000); //set I2C frequency to 400kHz
00405     wait_ms(1000);
00406     initTMD2772();
00407     #ifdef printDebug
00408         regDump(TMD2772_Addr,(0x00 | 0x80),0x0F);
00409     #endif
00410     pc.printf("fan on\r\n");
00411     pc.printf("Done initializing\r\n");
00412     wait_ms(700);
00413     static int loopCount =0;
00414     
00415     wait_ms(measPeriod_ms);
00416     
00417     //warmup and choose setpoint
00418     pc.printf("finding %f V setpoint\r\n",targetV);
00419     setpoint = getNewSetpoint(targetV, 300, 400);
00420     pc.printf( "new setpoint is %f counts\r\n",setpoint );
00421     
00422 
00423     while(1) {
00424         t.start();
00425         updateTemperature();
00426         updateAlsData();
00427         ratio = ch0Data/ch1Data;
00428         err = (ch0Data - setpoint)/setpoint;
00429         tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
00430         if(loopCount<warmUp) 
00431         {
00432             sumErr=0;   //no integral gain during initial warmup--takes too long to integate error out
00433             pGain=0.25;
00434         }
00435         else 
00436         {
00437             sumErr += err; //use integral gain while the sensor is heating up to get to setpoint faster and avoid lag.
00438             pGain=pGainNormal;
00439         }
00440         inRange = (abs(sumErr)<sumErrMax) ? true : false;
00441         if(inRange) 
00442         {
00443             iGain=0; //no need for iGain--inRange
00444         } 
00445         else
00446         {
00447             iGain = iGainNormal;
00448         }
00449         updateLampVolts(numLampMeas); 
00450         //#ifdef printDebug
00451             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 );
00452         //#endif
00453         if (abs(err*setpoint)>tol) {
00454             updateLampPWM(err,sumErr,pGain,iGain);
00455         }
00456         if (abs(tempErr*boardSetpointTemp)>tempTol) {
00457             updateBoardPWM(tempErr,boardPropGain,boardDerGain);
00458         }
00459         loopCount++;
00460         while(t.read_ms() < measPeriod_ms) {
00461          //pc.printf("%U \r\n",t.read_ms());   
00462         }
00463         t.reset();
00464     }
00465     
00466     
00467 }
00468