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@3:a8ec3c6aeb08, 2017-01-13 (annotated)

Committer:
laserdad
Date:
Fri Jan 13 22:30:02 2017 +0000
Revision:
3:a8ec3c6aeb08
Parent:
2:1b142e2aa23e
Child:
4:eb26ac5c3bd5
updated tempco for both channels;

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 1:2d9d931c8484 5 #define MAX31725_Addr 0x92 //this is the 8-bit address
laserdad 1:2d9d931c8484 6
laserdad 0:d4187a097285 7 const uint8_t ALSDataRegister = 0x14;
laserdad 0:d4187a097285 8 const uint8_t waitIntervals = 100; //number of 2.73 ms waits
laserdad 0:d4187a097285 9 const int measPeriod_ms = 699;
laserdad 2:1b142e2aa23e 10 const float tempRef = 29.5; //C temperature at which all optical powers are calculated to
laserdad 3:a8ec3c6aeb08 11 const float Ch0tempCo = 0.00228; // % per degree C
laserdad 3:a8ec3c6aeb08 12 const float Ch1tempCo = 0.001765; //% per degree C
laserdad 2:1b142e2aa23e 13 const float warmUp = 15; //number of measurements with a higher than usual pGain (to get to target faster);
laserdad 0:d4187a097285 14
laserdad 0:d4187a097285 15 I2C i2c(I2C_SDA,I2C_SCL);
laserdad 0:d4187a097285 16 Serial pc(USBTX,USBRX); //open serial port (optionally add baud rate after specifying TX and RX pins)
laserdad 0:d4187a097285 17 Timer t;
laserdad 1:2d9d931c8484 18 FastPWM mypwm(PA_1);
laserdad 1:2d9d931c8484 19
laserdad 1:2d9d931c8484 20
laserdad 0:d4187a097285 21
laserdad 1:2d9d931c8484 22 //**********************************
laserdad 1:2d9d931c8484 23 //declare subroutines
laserdad 1:2d9d931c8484 24 void writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
laserdad 1:2d9d931c8484 25 {
laserdad 1:2d9d931c8484 26 /*writes 1 byte to a single register*/
laserdad 0:d4187a097285 27 char writeData[2];
laserdad 1:2d9d931c8484 28 writeData[0] = reg ;
laserdad 0:d4187a097285 29 writeData[1] = val;
laserdad 0:d4187a097285 30 i2c.write(addr,writeData, 2);
laserdad 0:d4187a097285 31 }
laserdad 0:d4187a097285 32
laserdad 1:2d9d931c8484 33 void writeBlock(uint8_t addr, uint8_t startReg, uint8_t *data, uint8_t numBytes)
laserdad 1:2d9d931c8484 34 {
laserdad 1:2d9d931c8484 35 /*writes data from an array beginning at the startReg*/
laserdad 0:d4187a097285 36 char writeData[numBytes+1];
laserdad 1:2d9d931c8484 37 writeData[0]=startReg;
laserdad 1:2d9d931c8484 38 for(int n=1; n<numBytes; n++) {
laserdad 0:d4187a097285 39 writeData[n]=data[n-1];
laserdad 0:d4187a097285 40 }
laserdad 0:d4187a097285 41 i2c.write(addr,writeData,numBytes+1);
laserdad 0:d4187a097285 42 }
laserdad 0:d4187a097285 43
laserdad 1:2d9d931c8484 44 void readRegisters(uint8_t addr, uint8_t startReg, char *regData, int numBytes)
laserdad 1:2d9d931c8484 45 {
laserdad 1:2d9d931c8484 46 char writeData = startReg;
laserdad 1:2d9d931c8484 47 i2c.write(addr,&writeData,1,true); //true is for repeated start
laserdad 1:2d9d931c8484 48 i2c.read(addr,regData,numBytes);
laserdad 0:d4187a097285 49 }
laserdad 0:d4187a097285 50
laserdad 0:d4187a097285 51 uint16_t LSB_MSB_2uint16(char *data) {
laserdad 0:d4187a097285 52 /*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
laserdad 0:d4187a097285 53 return ((uint16_t)data[1] << 8) + (uint16_t)data[0];
laserdad 0:d4187a097285 54 }
laserdad 0:d4187a097285 55
laserdad 1:2d9d931c8484 56 uint16_t MSB_LSB_2uint16(char *data) {
laserdad 1:2d9d931c8484 57 /*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
laserdad 1:2d9d931c8484 58 return ((uint16_t)data[0] << 8) + (uint16_t)data[1];
laserdad 1:2d9d931c8484 59 }
laserdad 1:2d9d931c8484 60
laserdad 1:2d9d931c8484 61 void regDump(uint8_t Addr, uint8_t startByte, uint8_t endByte)
laserdad 1:2d9d931c8484 62 {
laserdad 1:2d9d931c8484 63 /*print the values of up to 20 registers*/
laserdad 0:d4187a097285 64 char regData[20];
laserdad 0:d4187a097285 65 int numBytes;
laserdad 0:d4187a097285 66 if (endByte>=startByte) {
laserdad 0:d4187a097285 67 numBytes = (endByte-startByte+1) < 20 ? (endByte-startByte+1) : 20;
laserdad 1:2d9d931c8484 68 } else {
laserdad 0:d4187a097285 69 numBytes=1;
laserdad 1:2d9d931c8484 70 }
laserdad 1:2d9d931c8484 71
laserdad 1:2d9d931c8484 72 regData[0] = startByte;
laserdad 1:2d9d931c8484 73 i2c.write(Addr,regData,1,true);
laserdad 1:2d9d931c8484 74 i2c.read(Addr, regData, numBytes);
laserdad 1:2d9d931c8484 75 for(int n=0; n<numBytes; n++) {
laserdad 1:2d9d931c8484 76 pc.printf("%X, %X \r\n", startByte+n, regData[n]);
laserdad 0:d4187a097285 77 }
laserdad 0:d4187a097285 78 }
laserdad 0:d4187a097285 79
laserdad 0:d4187a097285 80
laserdad 1:2d9d931c8484 81 bool bitRead(uint16_t data, uint8_t bitNum)
laserdad 1:2d9d931c8484 82 {
laserdad 1:2d9d931c8484 83 uint16_t mask = 1<<bitNum;
laserdad 1:2d9d931c8484 84 uint16_t masked_bit = data & mask;
laserdad 1:2d9d931c8484 85 return masked_bit >> bitNum;
laserdad 1:2d9d931c8484 86 }
laserdad 1:2d9d931c8484 87
laserdad 1:2d9d931c8484 88 float getTemp( int address) {
laserdad 1:2d9d931c8484 89 char tempData[2];
laserdad 1:2d9d931c8484 90 uint16_t tempBits;
laserdad 1:2d9d931c8484 91 const float LSB =0.00390625;
laserdad 1:2d9d931c8484 92 // read temperature
laserdad 1:2d9d931c8484 93 readRegisters(MAX31725_Addr,0x00,tempData,2);
laserdad 1:2d9d931c8484 94 tempBits = MSB_LSB_2uint16(tempData);
laserdad 1:2d9d931c8484 95 if(bitRead(tempBits,15) == 1 )
laserdad 1:2d9d931c8484 96 {
laserdad 1:2d9d931c8484 97 return( (32768-tempBits)*LSB ); //negative temp
laserdad 1:2d9d931c8484 98 }
laserdad 1:2d9d931c8484 99 else {
laserdad 1:2d9d931c8484 100 return ( tempBits*LSB ); //positive temp
laserdad 1:2d9d931c8484 101 }
laserdad 1:2d9d931c8484 102 }
laserdad 1:2d9d931c8484 103
laserdad 0:d4187a097285 104
laserdad 0:d4187a097285 105 void initTMD2772(void) {
laserdad 1:2d9d931c8484 106 writeRegister(TMD2772_Addr,(0x00 | 0x80),0x0B);// Set power on, ALS enabled, Wait enabled, Interrupts enabled (register 0)
laserdad 1:2d9d931c8484 107 writeRegister(TMD2772_Addr,(0x01 | 0x80),0x00);//ALS time register - 0x00 is max integration time of 699ms (register 1)
laserdad 1:2d9d931c8484 108 writeRegister(TMD2772_Addr,(0x03 | 0x80),0xFF-waitIntervals); // Wtime = 2.73 ms * delay peroids (subtract from 0xFF to enter into register)
laserdad 1:2d9d931c8484 109 // writeRegister(TMD2772_Addr,(0x0D | 0x80),0x04); //optionally scale ALS gain by 0.16 by seleting 0x04;
laserdad 1:2d9d931c8484 110 writeRegister(TMD2772_Addr,(0x0D | 0x80),0x00); //optionally scale ALS gain by 0.16 by seleting 0x04;
laserdad 0:d4187a097285 111
laserdad 1:2d9d931c8484 112 writeRegister(TMD2772_Addr,(0x0F | 0x80),0x00); //ALS gain is 1x
laserdad 0:d4187a097285 113 }
laserdad 0:d4187a097285 114
laserdad 2:1b142e2aa23e 115 float tempCorrectTMDCh0(float counts, float tempC)
laserdad 2:1b142e2aa23e 116 {
laserdad 2:1b142e2aa23e 117 float tDiff = tempC-tempRef;
laserdad 2:1b142e2aa23e 118 float delta = Ch0tempCo*tDiff; //the % difference observed vs. reference temperature
laserdad 2:1b142e2aa23e 119 return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
laserdad 2:1b142e2aa23e 120
laserdad 2:1b142e2aa23e 121 }
laserdad 2:1b142e2aa23e 122
laserdad 3:a8ec3c6aeb08 123 float tempCorrectTMDCh1(float counts, float tempC)
laserdad 3:a8ec3c6aeb08 124 {
laserdad 3:a8ec3c6aeb08 125 float tDiff = tempC-tempRef;
laserdad 3:a8ec3c6aeb08 126 float delta = Ch1tempCo*tDiff; //the % difference observed vs. reference temperature
laserdad 3:a8ec3c6aeb08 127 return counts *(1-delta); //the count value equivalent if measured at reference temperature (less counts if temp is higher)
laserdad 3:a8ec3c6aeb08 128
laserdad 3:a8ec3c6aeb08 129 }
laserdad 2:1b142e2aa23e 130
laserdad 0:d4187a097285 131 int main() {
laserdad 0:d4187a097285 132 float ratio;
laserdad 0:d4187a097285 133 char data[4];
laserdad 0:d4187a097285 134 uint16_t ch0Data;
laserdad 0:d4187a097285 135 uint16_t ch1Data;
laserdad 2:1b142e2aa23e 136 float setpoint = 37250; //ch0/ch1 color setpoint ~0.88 duty cycle
laserdad 2:1b142e2aa23e 137 static float step; //duty cycle change per sample
laserdad 2:1b142e2aa23e 138 float dutyCycle=0.9275;
laserdad 0:d4187a097285 139 float dutyCycleMin =0.8;
laserdad 2:1b142e2aa23e 140 float dutyCycleMax =0.98;
laserdad 1:2d9d931c8484 141 float stepMax=0.0025;
laserdad 1:2d9d931c8484 142 float stepMin=-0.0025;
laserdad 1:2d9d931c8484 143 // float iGain = 0.05; //integral gain --adding this because when I blew on it, it couldn't recover
laserdad 0:d4187a097285 144 float err;
laserdad 2:1b142e2aa23e 145 float tol=1; //tolerance within which to ignore changes in signal intensity
laserdad 2:1b142e2aa23e 146 float pGain = 0.25; //proportional gain
laserdad 2:1b142e2aa23e 147 // float quadGain = 0;*/
laserdad 2:1b142e2aa23e 148 static float temperature = 0;
laserdad 1:2d9d931c8484 149 //float ch0Avg; //integral error
laserdad 1:2d9d931c8484 150 // float filterLength = 15;
laserdad 2:1b142e2aa23e 151 float tempFilterLength = 8;
laserdad 0:d4187a097285 152 //setup everything
laserdad 1:2d9d931c8484 153 mypwm.period_us(400);
laserdad 0:d4187a097285 154 mypwm.write(dutyCycle);
laserdad 0:d4187a097285 155 i2c.frequency(400000); //set I2C frequency to 400kHz
laserdad 0:d4187a097285 156 wait_ms(1000);
laserdad 0:d4187a097285 157 initTMD2772();
laserdad 1:2d9d931c8484 158 regDump(TMD2772_Addr,(0x00 | 0x80),0x0F);
laserdad 0:d4187a097285 159 pc.printf("Done initializing\r\n");
laserdad 0:d4187a097285 160 wait_ms(700);
laserdad 2:1b142e2aa23e 161 static int loopCount =0;
laserdad 0:d4187a097285 162
laserdad 0:d4187a097285 163 //get initial filter value
laserdad 1:2d9d931c8484 164 // reg2write=ALSDataRegister | 0x80;
laserdad 1:2d9d931c8484 165 readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
laserdad 1:2d9d931c8484 166 //
laserdad 1:2d9d931c8484 167 // i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
laserdad 1:2d9d931c8484 168 // i2c.read(TMD2772_Addr,data,4);
laserdad 0:d4187a097285 169 ch0Data = LSB_MSB_2uint16(data);
laserdad 0:d4187a097285 170 ch1Data = LSB_MSB_2uint16(data+2);
laserdad 0:d4187a097285 171 ratio = (float)ch0Data/(float)ch1Data;
laserdad 0:d4187a097285 172 wait_ms(699);
laserdad 0:d4187a097285 173
laserdad 1:2d9d931c8484 174
laserdad 0:d4187a097285 175 while(1) {
laserdad 2:1b142e2aa23e 176 loopCount++;
laserdad 2:1b142e2aa23e 177 if (loopCount<warmUp)
laserdad 2:1b142e2aa23e 178 {
laserdad 2:1b142e2aa23e 179 pGain =0.25;
laserdad 2:1b142e2aa23e 180 }
laserdad 2:1b142e2aa23e 181 else
laserdad 2:1b142e2aa23e 182 {
laserdad 2:1b142e2aa23e 183 pGain =0.2;
laserdad 2:1b142e2aa23e 184 }
laserdad 0:d4187a097285 185 t.start();
laserdad 1:2d9d931c8484 186 readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
laserdad 1:2d9d931c8484 187 //reg2write=ALSDataRegister | 0x80;
laserdad 1:2d9d931c8484 188 //// pc.printf("%X\r\n",reg2write);
laserdad 1:2d9d931c8484 189 // i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
laserdad 1:2d9d931c8484 190 // i2c.read(TMD2772_Addr,data,4);
laserdad 0:d4187a097285 191 ch0Data = LSB_MSB_2uint16(data);
laserdad 2:1b142e2aa23e 192 if (temperature ==0)
laserdad 2:1b142e2aa23e 193 {
laserdad 2:1b142e2aa23e 194 temperature = getTemp(MAX31725_Addr);
laserdad 2:1b142e2aa23e 195 }
laserdad 2:1b142e2aa23e 196 else
laserdad 2:1b142e2aa23e 197 {
laserdad 2:1b142e2aa23e 198 temperature += (getTemp(MAX31725_Addr)-temperature)/tempFilterLength ;
laserdad 2:1b142e2aa23e 199 }
laserdad 2:1b142e2aa23e 200 ch0Data = tempCorrectTMDCh0(ch0Data,temperature);
laserdad 0:d4187a097285 201 ch1Data = LSB_MSB_2uint16(data+2);
laserdad 3:a8ec3c6aeb08 202 ch1Data = tempCorrectTMDCh1(ch0Data,temperature);
laserdad 0:d4187a097285 203 ratio = (float)ch0Data/(float)ch1Data;
laserdad 2:1b142e2aa23e 204 err = ch0Data - setpoint;
laserdad 2:1b142e2aa23e 205 pc.printf( "%U,%U, %f, %f, %f, %f\r\n",ch0Data,ch1Data,ratio,mypwm.read(),temperature, step);
laserdad 0:d4187a097285 206 if (abs(err)>tol) {
laserdad 2:1b142e2aa23e 207 step = err/setpoint * pGain ;
laserdad 1:2d9d931c8484 208 step = (step > stepMax) ? stepMax : step;
laserdad 1:2d9d931c8484 209 step = (step < stepMin) ? stepMin : step;
laserdad 2:1b142e2aa23e 210 dutyCycle -= step;
laserdad 0:d4187a097285 211 dutyCycle = (dutyCycle < dutyCycleMin) ? dutyCycleMin : dutyCycle;
laserdad 0:d4187a097285 212 dutyCycle = (dutyCycle > dutyCycleMax) ? dutyCycleMax : dutyCycle;
laserdad 0:d4187a097285 213 //update with new settings
laserdad 0:d4187a097285 214 mypwm.write(dutyCycle);
laserdad 0:d4187a097285 215 }
laserdad 0:d4187a097285 216 while(t.read_ms() < measPeriod_ms) {
laserdad 0:d4187a097285 217 //pc.printf("%U \r\n",t.read_ms());
laserdad 0:d4187a097285 218 }
laserdad 0:d4187a097285 219 t.reset();
laserdad 0:d4187a097285 220 }
laserdad 0:d4187a097285 221
laserdad 0:d4187a097285 222
laserdad 0:d4187a097285 223 }
laserdad 0:d4187a097285 224