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@2:1b142e2aa23e, 2017-01-13 (annotated)

Committer:
laserdad
Date:
Fri Jan 13 14:41:31 2017 +0000
Revision:
2:1b142e2aa23e
Parent:
1:2d9d931c8484
Child:
3:a8ec3c6aeb08
working revision. Initial commit.

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