Initial commit
Dependencies: FastPWM Lamp_Intensity_Lock_withTempCo mbed
Fork of Lamp_Intensity_Lock_withTempCo by
main.cpp@1:2d9d931c8484, 2017-01-09 (annotated)
- 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?
User | Revision | Line number | New 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,®2write,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,®2write,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 |