Initial commit

Dependencies:   FastPWM Lamp_Intensity_Lock_withTempCo mbed

Fork of Lamp_Intensity_Lock_withTempCo by Medic

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