Dan Allen / Mbed 2 deprecated Lamp_Intensity_Lock_withTempandCal

Initial commit

Dependencies:   FastPWM Lamp_Intensity_Lock_withTempCo mbed

Fork of Lamp_Intensity_Lock_withTempCo by Medic

main.cpp@4:eb26ac5c3bd5, 2017-03-13 (annotated)

Committer:
laserdad
Date:
Mon Mar 13 22:36:47 2017 +0000
Revision:
4:eb26ac5c3bd5
Parent:
3:a8ec3c6aeb08 
Added board heater. Switched 2 pins v.s orig AlsTempR3 schematic to deal w/mbed pin assigment fail. Got rid of fan enable (automatic on board). Added slot heater. Added light ramp on/off. Refactored for legibility.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
laserdad 0:d4187a097285 1 #include "mbed.h"
laserdad 1:2d9d931c8484 2 #include "FastPWM.h"
laserdad 0:d4187a097285 3
laserdad 0:d4187a097285 4 #define TMD2772_Addr 0x72 //this is the 8-bit address
laserdad 4:eb26ac5c3bd5 5 #define MAX31725_Addr 0x96 //this is the 8-bit address
laserdad 4:eb26ac5c3bd5 6 #define iGainNormal 0.005
laserdad 4:eb26ac5c3bd5 7 #define pGainNormal 0.15
laserdad 4:eb26ac5c3bd5 8 #define lampVsense_pin PA_0 //do not switch this with boardPWM or it will not compile
laserdad 4:eb26ac5c3bd5 9 #define PWM_pin PA_6
laserdad 4:eb26ac5c3bd5 10 #define v12Sense_pin PA_7
laserdad 4:eb26ac5c3bd5 11 #define vDiv 4.01
laserdad 4:eb26ac5c3bd5 12 #define cal 0.987
laserdad 4:eb26ac5c3bd5 13 #define boardPWM_pin PA_3
laserdad 4:eb26ac5c3bd5 14 #define debugPrint 1
laserdad 4:eb26ac5c3bd5 15 #define alsFilterLength 8
laserdad 4:eb26ac5c3bd5 16 #define slotHeaterPWM_pin PA_1
laserdad 1:2d9d931c8484 17
laserdad 4:eb26ac5c3bd5 18 //compiles with PA_6 as PWM and lampVsense as PA_0 and PWM_pin as PA_3
laserdad 4:eb26ac5c3bd5 19 //does not compile with PA_5 as lampvsense, boardPWM as PA_0 and PWM_pin as PA_3,
laserdad 4:eb26ac5c3bd5 20 //DNC for lampVsense PA_3, PWM PA_6 boardPWM PA_0
laserdad 0:d4187a097285 21 const uint8_t ALSDataRegister = 0x14;
laserdad 4:eb26ac5c3bd5 22 const uint8_t waitIntervals = 0; //number of 2.73 ms waits
laserdad 0:d4187a097285 23 const int measPeriod_ms = 699;
laserdad 4:eb26ac5c3bd5 24 const float tempRef = 40; //C temperature at which all optical powers are calculated to
laserdad 3:a8ec3c6aeb08 25 const float Ch0tempCo = 0.00228; // % per degree C
laserdad 3:a8ec3c6aeb08 26 const float Ch1tempCo = 0.001765; //% per degree C
laserdad 2:1b142e2aa23e 27 const float warmUp = 15; //number of measurements with a higher than usual pGain (to get to target faster);
laserdad 4:eb26ac5c3bd5 28 const float targetV = 10.9;
laserdad 4:eb26ac5c3bd5 29 const float VfilterLength = 8;
laserdad 4:eb26ac5c3bd5 30 const float tempFilterLength = 8;
laserdad 4:eb26ac5c3bd5 31 const uint8_t numLampMeas = 64;
laserdad 4:eb26ac5c3bd5 32 float ch0Data=0;
laserdad 4:eb26ac5c3bd5 33 float ch1Data=0;
laserdad 4:eb26ac5c3bd5 34 float vLamp;
laserdad 4:eb26ac5c3bd5 35 float v12;
laserdad 4:eb26ac5c3bd5 36 float lampVolts=0;
laserdad 4:eb26ac5c3bd5 37 float temperature=0;
laserdad 4:eb26ac5c3bd5 38 float instantTemperature=0;
laserdad 4:eb26ac5c3bd5 39 float dutyCycle =0.93; //initial duty cycle to try
laserdad 4:eb26ac5c3bd5 40 const float boardSetpointTemp = 40;
laserdad 4:eb26ac5c3bd5 41 float boardDutyCycle = 0.055;
laserdad 4:eb26ac5c3bd5 42 const float tempTol = 0.01; //tolerance within which to ignore temperature changes
laserdad 4:eb26ac5c3bd5 43 const float boardPropGain = 0.04;
laserdad 4:eb26ac5c3bd5 44 const float boardDerGain = 1.5;
laserdad 0:d4187a097285 45
laserdad 0:d4187a097285 46 I2C i2c(I2C_SDA,I2C_SCL);
laserdad 4:eb26ac5c3bd5 47 Serial pc(USBTX,USBRX,115200); //open serial port (optionally add baud rate after specifying TX and RX pins)
laserdad 0:d4187a097285 48 Timer t;
laserdad 4:eb26ac5c3bd5 49 FastPWM mypwm(PWM_pin);
laserdad 4:eb26ac5c3bd5 50 AnalogIn lampVsense(lampVsense_pin);
laserdad 4:eb26ac5c3bd5 51 AnalogIn v12Sense(v12Sense_pin);
laserdad 4:eb26ac5c3bd5 52 FastPWM boardPWM(boardPWM_pin);
laserdad 4:eb26ac5c3bd5 53 FastPWM slotHeaterPWM(slotHeaterPWM_pin);
laserdad 1:2d9d931c8484 54 //**********************************
laserdad 1:2d9d931c8484 55 //declare subroutines
laserdad 1:2d9d931c8484 56 void writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
laserdad 1:2d9d931c8484 57 {
laserdad 1:2d9d931c8484 58 /*writes 1 byte to a single register*/
laserdad 0:d4187a097285 59 char writeData[2];
laserdad 1:2d9d931c8484 60 writeData[0] = reg ;
laserdad 0:d4187a097285 61 writeData[1] = val;
laserdad 0:d4187a097285 62 i2c.write(addr,writeData, 2);
laserdad 0:d4187a097285 63 }
laserdad 0:d4187a097285 64
laserdad 1:2d9d931c8484 65 void writeBlock(uint8_t addr, uint8_t startReg, uint8_t *data, uint8_t numBytes)
laserdad 1:2d9d931c8484 66 {
laserdad 1:2d9d931c8484 67 /*writes data from an array beginning at the startReg*/
laserdad 0:d4187a097285 68 char writeData[numBytes+1];
laserdad 1:2d9d931c8484 69 writeData[0]=startReg;
laserdad 1:2d9d931c8484 70 for(int n=1; n<numBytes; n++) {
laserdad 0:d4187a097285 71 writeData[n]=data[n-1];
laserdad 0:d4187a097285 72 }
laserdad 0:d4187a097285 73 i2c.write(addr,writeData,numBytes+1);
laserdad 0:d4187a097285 74 }
laserdad 0:d4187a097285 75
laserdad 1:2d9d931c8484 76 void readRegisters(uint8_t addr, uint8_t startReg, char *regData, int numBytes)
laserdad 1:2d9d931c8484 77 {
laserdad 1:2d9d931c8484 78 char writeData = startReg;
laserdad 1:2d9d931c8484 79 i2c.write(addr,&writeData,1,true); //true is for repeated start
laserdad 1:2d9d931c8484 80 i2c.read(addr,regData,numBytes);
laserdad 0:d4187a097285 81 }
laserdad 0:d4187a097285 82
laserdad 0:d4187a097285 83 uint16_t LSB_MSB_2uint16(char *data) {
laserdad 0:d4187a097285 84 /*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
laserdad 0:d4187a097285 85 return ((uint16_t)data[1] << 8) + (uint16_t)data[0];
laserdad 0:d4187a097285 86 }
laserdad 0:d4187a097285 87
laserdad 1:2d9d931c8484 88 uint16_t MSB_LSB_2uint16(char *data) {
laserdad 1:2d9d931c8484 89 /*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
laserdad 1:2d9d931c8484 90 return ((uint16_t)data[0] << 8) + (uint16_t)data[1];
laserdad 1:2d9d931c8484 91 }
laserdad 1:2d9d931c8484 92
laserdad 1:2d9d931c8484 93 void regDump(uint8_t Addr, uint8_t startByte, uint8_t endByte)
laserdad 1:2d9d931c8484 94 {
laserdad 1:2d9d931c8484 95 /*print the values of up to 20 registers*/
laserdad 0:d4187a097285 96 char regData[20];
laserdad 0:d4187a097285 97 int numBytes;
laserdad 0:d4187a097285 98 if (endByte>=startByte) {
laserdad 0:d4187a097285 99 numBytes = (endByte-startByte+1) < 20 ? (endByte-startByte+1) : 20;
laserdad 1:2d9d931c8484 100 } else {
laserdad 0:d4187a097285 101 numBytes=1;
laserdad 1:2d9d931c8484 102 }
laserdad 1:2d9d931c8484 103
laserdad 1:2d9d931c8484 104 regData[0] = startByte;
laserdad 1:2d9d931c8484 105 i2c.write(Addr,regData,1,true);
laserdad 1:2d9d931c8484 106 i2c.read(Addr, regData, numBytes);
laserdad 1:2d9d931c8484 107 for(int n=0; n<numBytes; n++) {
laserdad 1:2d9d931c8484 108 pc.printf("%X, %X \r\n", startByte+n, regData[n]);
laserdad 0:d4187a097285 109 }
laserdad 0:d4187a097285 110 }
laserdad 0:d4187a097285 111
laserdad 0:d4187a097285 112
laserdad 1:2d9d931c8484 113 bool bitRead(uint16_t data, uint8_t bitNum)
laserdad 1:2d9d931c8484 114 {
laserdad 1:2d9d931c8484 115 uint16_t mask = 1<<bitNum;
laserdad 1:2d9d931c8484 116 uint16_t masked_bit = data & mask;
laserdad 1:2d9d931c8484 117 return masked_bit >> bitNum;
laserdad 1:2d9d931c8484 118 }
laserdad 1:2d9d931c8484 119
laserdad 1:2d9d931c8484 120 float getTemp( int address) {
laserdad 1:2d9d931c8484 121 char tempData[2];
laserdad 1:2d9d931c8484 122 uint16_t tempBits;
laserdad 4:eb26ac5c3bd5 123 const float tempLSB =0.00390625;
laserdad 1:2d9d931c8484 124 // read temperature
laserdad 1:2d9d931c8484 125 readRegisters(MAX31725_Addr,0x00,tempData,2);
laserdad 1:2d9d931c8484 126 tempBits = MSB_LSB_2uint16(tempData);
laserdad 1:2d9d931c8484 127 if(bitRead(tempBits,15) == 1 )
laserdad 1:2d9d931c8484 128 {
laserdad 4:eb26ac5c3bd5 129 return( (32768-tempBits)*tempLSB ); //negative temp
laserdad 1:2d9d931c8484 130 }
laserdad 1:2d9d931c8484 131 else {
laserdad 4:eb26ac5c3bd5 132 return ( tempBits*tempLSB ); //positive temp
laserdad 1:2d9d931c8484 133 }
laserdad 1:2d9d931c8484 134 }
laserdad 1:2d9d931c8484 135
laserdad 0:d4187a097285 136 void initTMD2772(void) {
laserdad 1:2d9d931c8484 137 writeRegister(TMD2772_Addr,(0x00 | 0x80),0x0B);// Set power on, ALS enabled, Wait enabled, Interrupts enabled (register 0)
laserdad 1:2d9d931c8484 138 writeRegister(TMD2772_Addr,(0x01 | 0x80),0x00);//ALS time register - 0x00 is max integration time of 699ms (register 1)
laserdad 1:2d9d931c8484 139 writeRegister(TMD2772_Addr,(0x03 | 0x80),0xFF-waitIntervals); // Wtime = 2.73 ms * delay peroids (subtract from 0xFF to enter into register)
laserdad 1:2d9d931c8484 140 // writeRegister(TMD2772_Addr,(0x0D | 0x80),0x04); //optionally scale ALS gain by 0.16 by seleting 0x04;
laserdad 1:2d9d931c8484 141 writeRegister(TMD2772_Addr,(0x0D | 0x80),0x00); //optionally scale ALS gain by 0.16 by seleting 0x04;
laserdad 0:d4187a097285 142
laserdad 1:2d9d931c8484 143 writeRegister(TMD2772_Addr,(0x0F | 0x80),0x00); //ALS gain is 1x
laserdad 0:d4187a097285 144 }
laserdad 0:d4187a097285 145
laserdad 2:1b142e2aa23e 146 float tempCorrectTMDCh0(float counts, float tempC)
laserdad 2:1b142e2aa23e 147 {
laserdad 2:1b142e2aa23e 148 float tDiff = tempC-tempRef;
laserdad 2:1b142e2aa23e 149 float delta = Ch0tempCo*tDiff; //the % difference observed vs. reference temperature
laserdad 2:1b142e2aa23e 150 return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
laserdad 2:1b142e2aa23e 151
laserdad 2:1b142e2aa23e 152 }
laserdad 2:1b142e2aa23e 153
laserdad 3:a8ec3c6aeb08 154 float tempCorrectTMDCh1(float counts, float tempC)
laserdad 3:a8ec3c6aeb08 155 {
laserdad 3:a8ec3c6aeb08 156 float tDiff = tempC-tempRef;
laserdad 3:a8ec3c6aeb08 157 float delta = Ch1tempCo*tDiff; //the % difference observed vs. reference temperature
laserdad 3:a8ec3c6aeb08 158 return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
laserdad 3:a8ec3c6aeb08 159
laserdad 3:a8ec3c6aeb08 160 }
laserdad 2:1b142e2aa23e 161
laserdad 4:eb26ac5c3bd5 162 float getAvgLampV(uint8_t numMeas)
laserdad 4:eb26ac5c3bd5 163 {
laserdad 4:eb26ac5c3bd5 164 float lampCounts=0;
laserdad 4:eb26ac5c3bd5 165 numMeas = (numMeas > 256) ? 256 : numMeas;
laserdad 4:eb26ac5c3bd5 166 for(int16_t n=0; n<numMeas; n++) {
laserdad 4:eb26ac5c3bd5 167 lampCounts += lampVsense.read();
laserdad 4:eb26ac5c3bd5 168
laserdad 4:eb26ac5c3bd5 169 }
laserdad 4:eb26ac5c3bd5 170 return lampCounts/numMeas;
laserdad 4:eb26ac5c3bd5 171 }
laserdad 4:eb26ac5c3bd5 172
laserdad 4:eb26ac5c3bd5 173 float getAvg12V(uint8_t numMeas)
laserdad 4:eb26ac5c3bd5 174 {
laserdad 4:eb26ac5c3bd5 175 float lampCounts=0;
laserdad 4:eb26ac5c3bd5 176 numMeas = (numMeas > 256) ? 256 : numMeas;
laserdad 4:eb26ac5c3bd5 177 for(int16_t n=0; n<numMeas; n++) {
laserdad 4:eb26ac5c3bd5 178 lampCounts += v12Sense.read();
laserdad 4:eb26ac5c3bd5 179
laserdad 4:eb26ac5c3bd5 180 }
laserdad 4:eb26ac5c3bd5 181 return lampCounts/numMeas;
laserdad 4:eb26ac5c3bd5 182 }
laserdad 4:eb26ac5c3bd5 183
laserdad 4:eb26ac5c3bd5 184 void updateLampVolts(uint8_t numMeas)
laserdad 4:eb26ac5c3bd5 185 {
laserdad 4:eb26ac5c3bd5 186 if(lampVolts == 0)
laserdad 4:eb26ac5c3bd5 187 {
laserdad 4:eb26ac5c3bd5 188 lampVolts = ( vDiv * cal * 3.3 * ( getAvg12V(numMeas) - getAvgLampV(numMeas) ) - lampVolts); //initialize lamp volts
laserdad 4:eb26ac5c3bd5 189 }
laserdad 4:eb26ac5c3bd5 190 else
laserdad 4:eb26ac5c3bd5 191 {
laserdad 4:eb26ac5c3bd5 192 lampVolts += ( vDiv * cal * 3.3 * ( getAvg12V(numMeas) - getAvgLampV(numMeas) ) - lampVolts)/VfilterLength; //update with IIR filter
laserdad 4:eb26ac5c3bd5 193 }
laserdad 4:eb26ac5c3bd5 194 }
laserdad 4:eb26ac5c3bd5 195
laserdad 4:eb26ac5c3bd5 196 void updateTemperature(void)
laserdad 4:eb26ac5c3bd5 197 {
laserdad 4:eb26ac5c3bd5 198 if (temperature == 0)
laserdad 4:eb26ac5c3bd5 199 {
laserdad 4:eb26ac5c3bd5 200 instantTemperature=getTemp(MAX31725_Addr);
laserdad 4:eb26ac5c3bd5 201 temperature = instantTemperature;
laserdad 4:eb26ac5c3bd5 202 }
laserdad 4:eb26ac5c3bd5 203 else
laserdad 4:eb26ac5c3bd5 204 {
laserdad 4:eb26ac5c3bd5 205 instantTemperature=getTemp(MAX31725_Addr);
laserdad 4:eb26ac5c3bd5 206 temperature += (instantTemperature-temperature)/tempFilterLength ;
laserdad 4:eb26ac5c3bd5 207 }
laserdad 4:eb26ac5c3bd5 208 }
laserdad 4:eb26ac5c3bd5 209
laserdad 4:eb26ac5c3bd5 210 void updateAlsData(void)
laserdad 4:eb26ac5c3bd5 211 {
laserdad 4:eb26ac5c3bd5 212 float ch0RawData;
laserdad 4:eb26ac5c3bd5 213 float ch1RawData;
laserdad 4:eb26ac5c3bd5 214 char data[4];
laserdad 4:eb26ac5c3bd5 215
laserdad 4:eb26ac5c3bd5 216 readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
laserdad 4:eb26ac5c3bd5 217 ch0RawData = (float) LSB_MSB_2uint16(data);
laserdad 4:eb26ac5c3bd5 218 ch1RawData = LSB_MSB_2uint16(data+2);
laserdad 4:eb26ac5c3bd5 219
laserdad 4:eb26ac5c3bd5 220 if(temperature==0) {//no temp measurement
laserdad 4:eb26ac5c3bd5 221 if(ch0Data==0) {//no prior ch0Data--initialize filter
laserdad 4:eb26ac5c3bd5 222 ch0Data = ch0RawData;
laserdad 4:eb26ac5c3bd5 223 ch1Data = ch1RawData;
laserdad 4:eb26ac5c3bd5 224 }
laserdad 4:eb26ac5c3bd5 225 else {//prior data exists, update w/IIR filter
laserdad 4:eb26ac5c3bd5 226 ch0Data += (ch0RawData-ch0Data)/alsFilterLength;
laserdad 4:eb26ac5c3bd5 227 ch1Data += (ch1RawData-ch0Data)/alsFilterLength;
laserdad 4:eb26ac5c3bd5 228 }
laserdad 4:eb26ac5c3bd5 229 }
laserdad 4:eb26ac5c3bd5 230 else { //temp meas exists
laserdad 4:eb26ac5c3bd5 231 if(ch0Data == 0) {
laserdad 4:eb26ac5c3bd5 232 ch0Data = (tempCorrectTMDCh0(ch0RawData,temperature)-ch0Data)/alsFilterLength; //initialize with temperature corrected the data
laserdad 4:eb26ac5c3bd5 233 ch1Data = (tempCorrectTMDCh1(ch1RawData,temperature)-ch1Data)/alsFilterLength; //initialize with temperature corrected the data
laserdad 4:eb26ac5c3bd5 234 }
laserdad 4:eb26ac5c3bd5 235 else {
laserdad 4:eb26ac5c3bd5 236 ch0Data += (tempCorrectTMDCh0(ch0RawData,temperature)-ch0Data)/alsFilterLength; //update IIR filter with temperature corrected data
laserdad 4:eb26ac5c3bd5 237 ch1Data += (tempCorrectTMDCh1(ch1RawData,temperature)-ch1Data)/alsFilterLength; //update IIR filter with temperature corrected data
laserdad 4:eb26ac5c3bd5 238 }
laserdad 4:eb26ac5c3bd5 239 }
laserdad 4:eb26ac5c3bd5 240 } //end updateCh0Data
laserdad 4:eb26ac5c3bd5 241
laserdad 4:eb26ac5c3bd5 242 void updateLampPWM(float err, float sumErr, float pGain, float iGain)
laserdad 4:eb26ac5c3bd5 243 {
laserdad 4:eb26ac5c3bd5 244 const float dutyCycleMin =0;
laserdad 4:eb26ac5c3bd5 245 const float dutyCycleMax =0.98;
laserdad 4:eb26ac5c3bd5 246 const float stepMax=0.005;
laserdad 4:eb26ac5c3bd5 247 const float stepMin=-0.005;
laserdad 4:eb26ac5c3bd5 248 float step; //duty cycle change per sample
laserdad 4:eb26ac5c3bd5 249
laserdad 4:eb26ac5c3bd5 250 step = err * pGain + sumErr*iGain;
laserdad 4:eb26ac5c3bd5 251 step = (step > stepMax) ? stepMax : step;
laserdad 4:eb26ac5c3bd5 252 step = (step < stepMin) ? stepMin : step;
laserdad 4:eb26ac5c3bd5 253 dutyCycle -= step;
laserdad 4:eb26ac5c3bd5 254 dutyCycle = (dutyCycle < dutyCycleMin) ? dutyCycleMin : dutyCycle;
laserdad 4:eb26ac5c3bd5 255 dutyCycle = (dutyCycle > dutyCycleMax) ? dutyCycleMax : dutyCycle;
laserdad 4:eb26ac5c3bd5 256 mypwm.write(dutyCycle); //update with new settings
laserdad 4:eb26ac5c3bd5 257 }
laserdad 4:eb26ac5c3bd5 258
laserdad 4:eb26ac5c3bd5 259
laserdad 4:eb26ac5c3bd5 260 void updateBoardPWM(float err, float pGain, float dGain)
laserdad 4:eb26ac5c3bd5 261 {
laserdad 4:eb26ac5c3bd5 262 static float prevTemp=temperature;
laserdad 4:eb26ac5c3bd5 263 const float dutyCycleMin =0;
laserdad 4:eb26ac5c3bd5 264 const float dutyCycleMax =0.1;
laserdad 4:eb26ac5c3bd5 265 const float stepMax=0.05;
laserdad 4:eb26ac5c3bd5 266 const float stepMin=-0.05;
laserdad 4:eb26ac5c3bd5 267 float step; //duty cycle change per sample
laserdad 4:eb26ac5c3bd5 268 float pGain1 = pGain;
laserdad 4:eb26ac5c3bd5 269 // if(err>0)
laserdad 4:eb26ac5c3bd5 270 // {
laserdad 4:eb26ac5c3bd5 271 // pGain1=0;
laserdad 4:eb26ac5c3bd5 272 // }
laserdad 4:eb26ac5c3bd5 273 // else
laserdad 4:eb26ac5c3bd5 274 // {
laserdad 4:eb26ac5c3bd5 275 // pGain1 = pGain;
laserdad 4:eb26ac5c3bd5 276 // }
laserdad 4:eb26ac5c3bd5 277 step = err * pGain1 + (temperature-prevTemp)/boardSetpointTemp*dGain;
laserdad 4:eb26ac5c3bd5 278 step = (step > stepMax) ? stepMax : step;
laserdad 4:eb26ac5c3bd5 279 step = (step < stepMin) ? stepMin : step;
laserdad 4:eb26ac5c3bd5 280 boardDutyCycle -= step;
laserdad 4:eb26ac5c3bd5 281 boardDutyCycle = (boardDutyCycle < dutyCycleMin) ? dutyCycleMin : boardDutyCycle;
laserdad 4:eb26ac5c3bd5 282 boardDutyCycle = (boardDutyCycle > dutyCycleMax) ? dutyCycleMax : boardDutyCycle;
laserdad 4:eb26ac5c3bd5 283 boardPWM.write(boardDutyCycle); //update with new settings
laserdad 4:eb26ac5c3bd5 284
laserdad 4:eb26ac5c3bd5 285 prevTemp = temperature;
laserdad 4:eb26ac5c3bd5 286
laserdad 4:eb26ac5c3bd5 287 }
laserdad 4:eb26ac5c3bd5 288
laserdad 4:eb26ac5c3bd5 289
laserdad 4:eb26ac5c3bd5 290 void rampLamp(float finalDutyCycle)
laserdad 4:eb26ac5c3bd5 291 {
laserdad 4:eb26ac5c3bd5 292 float currentDutyCycle;
laserdad 4:eb26ac5c3bd5 293 float step = 0.01; //increment or decrement duty cycle by 1% increments
laserdad 4:eb26ac5c3bd5 294 bool ramping = 1;
laserdad 4:eb26ac5c3bd5 295 currentDutyCycle = mypwm.read();
laserdad 4:eb26ac5c3bd5 296 while (ramping) {
laserdad 4:eb26ac5c3bd5 297 wait_ms(40);
laserdad 4:eb26ac5c3bd5 298 if(finalDutyCycle - currentDutyCycle > step)
laserdad 4:eb26ac5c3bd5 299 {
laserdad 4:eb26ac5c3bd5 300 currentDutyCycle += step;
laserdad 4:eb26ac5c3bd5 301 mypwm.write(currentDutyCycle);
laserdad 4:eb26ac5c3bd5 302 }
laserdad 4:eb26ac5c3bd5 303 else if (finalDutyCycle - currentDutyCycle < -step)
laserdad 4:eb26ac5c3bd5 304 {
laserdad 4:eb26ac5c3bd5 305 currentDutyCycle -= step;
laserdad 4:eb26ac5c3bd5 306 mypwm.write(currentDutyCycle);
laserdad 4:eb26ac5c3bd5 307 }
laserdad 4:eb26ac5c3bd5 308 else
laserdad 4:eb26ac5c3bd5 309 {
laserdad 4:eb26ac5c3bd5 310 ramping = 0;
laserdad 4:eb26ac5c3bd5 311 mypwm.write(finalDutyCycle);
laserdad 4:eb26ac5c3bd5 312 }
laserdad 4:eb26ac5c3bd5 313 }
laserdad 4:eb26ac5c3bd5 314 }
laserdad 4:eb26ac5c3bd5 315
laserdad 4:eb26ac5c3bd5 316
laserdad 4:eb26ac5c3bd5 317 float getNewSetpoint(float targetVoltage, float warmUpTime, float calTime)
laserdad 4:eb26ac5c3bd5 318 {
laserdad 4:eb26ac5c3bd5 319 warmUpTime = ( warmUpTime < 0 ) ? 0 : warmUpTime;
laserdad 4:eb26ac5c3bd5 320 calTime = (calTime <= warmUpTime) ? calTime= warmUpTime*2+1: calTime;
laserdad 4:eb26ac5c3bd5 321 const float calTime_ms = 1000*calTime; //convert seconds to ms
laserdad 4:eb26ac5c3bd5 322 const float warmUpTime_ms = 1000*warmUpTime;
laserdad 4:eb26ac5c3bd5 323
laserdad 4:eb26ac5c3bd5 324 const float tol =0.02;// volts tolerance on target voltage
laserdad 4:eb26ac5c3bd5 325 const uint8_t persist=8; //number of consecutive samples in range in order to output new setpoint
laserdad 4:eb26ac5c3bd5 326 uint8_t consecutiveInRange=0;
laserdad 4:eb26ac5c3bd5 327 const float propGain = 2e-3; //proportional gain
laserdad 4:eb26ac5c3bd5 328 float errVoltage;
laserdad 4:eb26ac5c3bd5 329 float tempErr;
laserdad 4:eb26ac5c3bd5 330 t.start();
laserdad 4:eb26ac5c3bd5 331
laserdad 4:eb26ac5c3bd5 332 //turn on heater and stabilize board at target temp
laserdad 4:eb26ac5c3bd5 333 rampLamp(dutyCycle);
laserdad 4:eb26ac5c3bd5 334 rampLamp(0.05); //low level power keeps fan on
laserdad 4:eb26ac5c3bd5 335 boardPWM.write(boardDutyCycle);
laserdad 4:eb26ac5c3bd5 336 while(t.read_ms() < 120*1000)
laserdad 4:eb26ac5c3bd5 337 {
laserdad 4:eb26ac5c3bd5 338 wait_ms(699);
laserdad 4:eb26ac5c3bd5 339 updateTemperature();
laserdad 4:eb26ac5c3bd5 340 tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
laserdad 4:eb26ac5c3bd5 341 if (abs(tempErr*boardSetpointTemp)>tempTol) {
laserdad 4:eb26ac5c3bd5 342 updateBoardPWM(tempErr,boardPropGain,boardDerGain);
laserdad 4:eb26ac5c3bd5 343 }
laserdad 4:eb26ac5c3bd5 344
laserdad 4:eb26ac5c3bd5 345 pc.printf( "%.4f, %.2f, %f, %f, %f, %U\r\n",lampVolts, ch0Data, mypwm.read(),boardPWM.read(),temperature,consecutiveInRange);
laserdad 4:eb26ac5c3bd5 346 }
laserdad 4:eb26ac5c3bd5 347
laserdad 4:eb26ac5c3bd5 348 rampLamp(dutyCycle);
laserdad 4:eb26ac5c3bd5 349 //begin adjust output to target voltage at desired sensor board temperature.
laserdad 4:eb26ac5c3bd5 350 while(t.read_ms() < calTime_ms) {
laserdad 4:eb26ac5c3bd5 351 wait_ms(699);
laserdad 4:eb26ac5c3bd5 352 updateTemperature();
laserdad 4:eb26ac5c3bd5 353 updateLampVolts(numLampMeas);
laserdad 4:eb26ac5c3bd5 354 updateAlsData();
laserdad 4:eb26ac5c3bd5 355 //#ifdef printDebug
laserdad 4:eb26ac5c3bd5 356 pc.printf( "%.4f, %.2f, %f, %f, %f, %U\r\n",lampVolts, ch0Data, mypwm.read(),boardPWM.read(),temperature,consecutiveInRange);
laserdad 4:eb26ac5c3bd5 357 //#endif
laserdad 4:eb26ac5c3bd5 358 errVoltage = lampVolts-targetVoltage;
laserdad 4:eb26ac5c3bd5 359 if (abs(errVoltage) < tol )
laserdad 4:eb26ac5c3bd5 360 {
laserdad 4:eb26ac5c3bd5 361 consecutiveInRange++;
laserdad 4:eb26ac5c3bd5 362 if ( consecutiveInRange >= persist)
laserdad 4:eb26ac5c3bd5 363 {
laserdad 4:eb26ac5c3bd5 364 if(t.read_ms() > warmUpTime_ms)
laserdad 4:eb26ac5c3bd5 365 {
laserdad 4:eb26ac5c3bd5 366 return floor(ch0Data)+0.5;
laserdad 4:eb26ac5c3bd5 367 }
laserdad 4:eb26ac5c3bd5 368 }
laserdad 4:eb26ac5c3bd5 369 }
laserdad 4:eb26ac5c3bd5 370 else
laserdad 4:eb26ac5c3bd5 371 {
laserdad 4:eb26ac5c3bd5 372 consecutiveInRange=0;
laserdad 4:eb26ac5c3bd5 373 }
laserdad 4:eb26ac5c3bd5 374 updateLampPWM(errVoltage,0,propGain,0);
laserdad 4:eb26ac5c3bd5 375 tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
laserdad 4:eb26ac5c3bd5 376 if (abs(tempErr*boardSetpointTemp)>tempTol) {
laserdad 4:eb26ac5c3bd5 377 updateBoardPWM(tempErr,boardPropGain,boardDerGain);
laserdad 4:eb26ac5c3bd5 378 }
laserdad 4:eb26ac5c3bd5 379
laserdad 4:eb26ac5c3bd5 380 } //end while
laserdad 4:eb26ac5c3bd5 381 return ch0Data;
laserdad 4:eb26ac5c3bd5 382 }
laserdad 4:eb26ac5c3bd5 383
laserdad 0:d4187a097285 384 int main() {
laserdad 0:d4187a097285 385 float ratio;
laserdad 4:eb26ac5c3bd5 386 static bool inRange=false;
laserdad 4:eb26ac5c3bd5 387 float setpoint = 37250.5; //ch0/ch1 color setpoint ~0.88 duty cycle
laserdad 1:2d9d931c8484 388 // float iGain = 0.05; //integral gain --adding this because when I blew on it, it couldn't recover
laserdad 0:d4187a097285 389 float err;
laserdad 4:eb26ac5c3bd5 390 float tempErr;
laserdad 4:eb26ac5c3bd5 391 const float tol=0.5; //tolerance within which to ignore changes in signal intensity
laserdad 4:eb26ac5c3bd5 392 float pGain; //proportional gain
laserdad 4:eb26ac5c3bd5 393 float iGain; // 250 : 0.2 ratio relative to pGain
laserdad 4:eb26ac5c3bd5 394 float sumErr=0;
laserdad 4:eb26ac5c3bd5 395 const float sumErrMax = .01;
laserdad 4:eb26ac5c3bd5 396
laserdad 0:d4187a097285 397 //setup everything
laserdad 1:2d9d931c8484 398 mypwm.period_us(400);
laserdad 4:eb26ac5c3bd5 399 boardPWM.period_us(400);
laserdad 4:eb26ac5c3bd5 400 slotHeaterPWM.period_us(400);
laserdad 4:eb26ac5c3bd5 401 slotHeaterPWM.write(0);
laserdad 4:eb26ac5c3bd5 402 //mypwm.write(dutyCycle);
laserdad 4:eb26ac5c3bd5 403 boardPWM.write(0);
laserdad 0:d4187a097285 404 i2c.frequency(400000); //set I2C frequency to 400kHz
laserdad 0:d4187a097285 405 wait_ms(1000);
laserdad 0:d4187a097285 406 initTMD2772();
laserdad 4:eb26ac5c3bd5 407 #ifdef printDebug
laserdad 4:eb26ac5c3bd5 408 regDump(TMD2772_Addr,(0x00 | 0x80),0x0F);
laserdad 4:eb26ac5c3bd5 409 #endif
laserdad 4:eb26ac5c3bd5 410 pc.printf("fan on\r\n");
laserdad 0:d4187a097285 411 pc.printf("Done initializing\r\n");
laserdad 0:d4187a097285 412 wait_ms(700);
laserdad 2:1b142e2aa23e 413 static int loopCount =0;
laserdad 0:d4187a097285 414
laserdad 4:eb26ac5c3bd5 415 wait_ms(measPeriod_ms);
laserdad 4:eb26ac5c3bd5 416
laserdad 4:eb26ac5c3bd5 417 //warmup and choose setpoint
laserdad 4:eb26ac5c3bd5 418 pc.printf("finding %f V setpoint\r\n",targetV);
laserdad 4:eb26ac5c3bd5 419 setpoint = getNewSetpoint(targetV, 300, 400);
laserdad 4:eb26ac5c3bd5 420 pc.printf( "new setpoint is %f counts\r\n",setpoint );
laserdad 0:d4187a097285 421
laserdad 4:eb26ac5c3bd5 422
laserdad 0:d4187a097285 423 while(1) {
laserdad 4:eb26ac5c3bd5 424 t.start();
laserdad 4:eb26ac5c3bd5 425 updateTemperature();
laserdad 4:eb26ac5c3bd5 426 updateAlsData();
laserdad 4:eb26ac5c3bd5 427 ratio = ch0Data/ch1Data;
laserdad 4:eb26ac5c3bd5 428 err = (ch0Data - setpoint)/setpoint;
laserdad 4:eb26ac5c3bd5 429 tempErr = (instantTemperature - boardSetpointTemp)/boardSetpointTemp;
laserdad 4:eb26ac5c3bd5 430 if(loopCount<warmUp)
laserdad 2:1b142e2aa23e 431 {
laserdad 4:eb26ac5c3bd5 432 sumErr=0; //no integral gain during initial warmup--takes too long to integate error out
laserdad 4:eb26ac5c3bd5 433 pGain=0.25;
laserdad 2:1b142e2aa23e 434 }
laserdad 2:1b142e2aa23e 435 else
laserdad 2:1b142e2aa23e 436 {
laserdad 4:eb26ac5c3bd5 437 sumErr += err; //use integral gain while the sensor is heating up to get to setpoint faster and avoid lag.
laserdad 4:eb26ac5c3bd5 438 pGain=pGainNormal;
laserdad 2:1b142e2aa23e 439 }
laserdad 4:eb26ac5c3bd5 440 inRange = (abs(sumErr)<sumErrMax) ? true : false;
laserdad 4:eb26ac5c3bd5 441 if(inRange)
laserdad 2:1b142e2aa23e 442 {
laserdad 4:eb26ac5c3bd5 443 iGain=0; //no need for iGain--inRange
laserdad 4:eb26ac5c3bd5 444 }
laserdad 4:eb26ac5c3bd5 445 else
laserdad 2:1b142e2aa23e 446 {
laserdad 4:eb26ac5c3bd5 447 iGain = iGainNormal;
laserdad 2:1b142e2aa23e 448 }
laserdad 4:eb26ac5c3bd5 449 updateLampVolts(numLampMeas);
laserdad 4:eb26ac5c3bd5 450 //#ifdef printDebug
laserdad 4:eb26ac5c3bd5 451 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 );
laserdad 4:eb26ac5c3bd5 452 //#endif
laserdad 4:eb26ac5c3bd5 453 if (abs(err*setpoint)>tol) {
laserdad 4:eb26ac5c3bd5 454 updateLampPWM(err,sumErr,pGain,iGain);
laserdad 0:d4187a097285 455 }
laserdad 4:eb26ac5c3bd5 456 if (abs(tempErr*boardSetpointTemp)>tempTol) {
laserdad 4:eb26ac5c3bd5 457 updateBoardPWM(tempErr,boardPropGain,boardDerGain);
laserdad 4:eb26ac5c3bd5 458 }
laserdad 4:eb26ac5c3bd5 459 loopCount++;
laserdad 0:d4187a097285 460 while(t.read_ms() < measPeriod_ms) {
laserdad 0:d4187a097285 461 //pc.printf("%U \r\n",t.read_ms());
laserdad 0:d4187a097285 462 }
laserdad 0:d4187a097285 463 t.reset();
laserdad 0:d4187a097285 464 }
laserdad 0:d4187a097285 465
laserdad 0:d4187a097285 466
laserdad 0:d4187a097285 467 }
laserdad 0:d4187a097285 468