Diff: main.cpp

Revision:

1:2d9d931c8484

Parent:

0:d4187a097285

Child:

2:1b142e2aa23e

--- a/main.cpp	Wed Dec 28 13:29:12 2016 +0000
+++ b/main.cpp	Mon Jan 09 14:35:41 2017 +0000
@@ -1,6 +1,9 @@
 #include "mbed.h"
+#include "FastPWM.h"
 
 #define TMD2772_Addr 0x72 //this is the 8-bit address
+#define MAX31725_Addr 0x92 //this is the 8-bit address
+
 const uint8_t ALSDataRegister = 0x14;
 const uint8_t waitIntervals = 100; //number of 2.73 ms waits
 const int measPeriod_ms = 699;
@@ -9,30 +12,37 @@
 I2C i2c(I2C_SDA,I2C_SCL);
 Serial pc(USBTX,USBRX); //open serial port (optionally add baud rate after specifying TX and RX pins)
 Timer t;
-PwmOut mypwm(PA_1);
+FastPWM mypwm(PA_1);
+
+
 
-void writeRegister(uint8_t addr, uint8_t reg, uint8_t val) {
- /*writes 1 byte to a single register*/
+//**********************************
+//declare subroutines
+void writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
+{
+    /*writes 1 byte to a single register*/
     char writeData[2];
-    writeData[0] = reg | 0x80;
+    writeData[0] = reg ;
     writeData[1] = val;
     i2c.write(addr,writeData, 2);
 }
 
-void writeBlock(uint8_t addr, uint8_t startReg, uint8_t *data, uint8_t numBytes) {
-/*writes data from an array beginning at the startReg*/
+void writeBlock(uint8_t addr, uint8_t startReg, uint8_t *data, uint8_t numBytes)
+{
+    /*writes data from an array beginning at the startReg*/
     char writeData[numBytes+1];
-    writeData[0]=startReg | 0x80;
-    for(int n=1;n<numBytes;n++) {
+    writeData[0]=startReg;
+    for(int n=1; n<numBytes; n++) {
         writeData[n]=data[n-1];
     }
     i2c.write(addr,writeData,numBytes+1);
 }
 
-void readBlock(uint8_t addr, uint8_t startReg, char *data, int numBytes) {
-    char writeData = startReg | 0x80;
-    i2c.write(addr,&writeData,1,true);
-    i2c.read(addr,data,numBytes);
+void readRegisters(uint8_t addr, uint8_t startReg, char *regData, int numBytes)
+{
+    char writeData = startReg;
+    i2c.write(addr,&writeData,1,true); //true is for repeated start
+    i2c.read(addr,regData,numBytes);
 }
 
 uint16_t LSB_MSB_2uint16(char *data) {
@@ -40,35 +50,63 @@
     return ((uint16_t)data[1] << 8) + (uint16_t)data[0];
 }
 
-void regDump(uint8_t Addr, uint8_t startByte, uint8_t endByte) {
-/*print the values of up to 20 registers*/
+uint16_t MSB_LSB_2uint16(char *data) {
+/*returns an unsinged 16 bit integer from a 2 data bytes, where the second byte is the MSB*/
+    return ((uint16_t)data[0] << 8) + (uint16_t)data[1];
+}
+
+void regDump(uint8_t Addr, uint8_t startByte, uint8_t endByte)
+{
+    /*print the values of up to 20 registers*/
     char regData[20];
     int numBytes;
     if (endByte>=startByte) {
         numBytes =  (endByte-startByte+1) < 20 ? (endByte-startByte+1) : 20;
-    }
-    else {
+    } else {
         numBytes=1;
-    }    
-        
-    regData[0] = startByte | 0x80;
-    i2c.write(TMD2772_Addr,regData,1,true);
-    i2c.read(TMD2772_Addr, regData, numBytes);
-    for(int n=0;n<numBytes;n++) {
-      pc.printf("%X \r\n", regData[n]);
+    }
+
+    regData[0] = startByte;
+    i2c.write(Addr,regData,1,true);
+    i2c.read(Addr, regData, numBytes);
+    for(int n=0; n<numBytes; n++) {
+        pc.printf("%X, %X \r\n", startByte+n, regData[n]);
     }
 }
 
 
+bool bitRead(uint16_t data, uint8_t bitNum)
+{
+    uint16_t mask = 1<<bitNum;
+    uint16_t masked_bit = data & mask;
+    return masked_bit >> bitNum;
+}
+
+float getTemp( int address) {
+    char tempData[2];
+    uint16_t tempBits; 
+    const float LSB =0.00390625;
+  // read temperature
+    readRegisters(MAX31725_Addr,0x00,tempData,2);
+    tempBits = MSB_LSB_2uint16(tempData);
+    if(bitRead(tempBits,15) == 1 )
+    {
+        return( (32768-tempBits)*LSB ); //negative temp
+    }
+    else {
+        return ( tempBits*LSB ); //positive temp
+    }
+} 
+
 
 void initTMD2772(void) {
-    writeRegister(TMD2772_Addr,0x00,0x0B);// Set power on, ALS enabled, Wait enabled, Interrupts enabled (register 0)
-    writeRegister(TMD2772_Addr,0x01,0x00);//ALS time register - 0x00 is max integration time of 699ms (register 1) 
-    writeRegister(TMD2772_Addr,0x03,0xFF-waitIntervals); // Wtime = 2.73 ms * delay peroids (subtract from 0xFF to enter into register)
-    writeRegister(TMD2772_Addr,0x0D,0x04); //optionally scale ALS gain by 0.16 by seleting 0x04;
-//    writeRegister(TMD2772_Addr,0x0D,0x00); //optionally scale ALS gain by 0.16 by seleting 0x04;
+    writeRegister(TMD2772_Addr,(0x00 | 0x80),0x0B);// Set power on, ALS enabled, Wait enabled, Interrupts enabled (register 0)
+    writeRegister(TMD2772_Addr,(0x01 | 0x80),0x00);//ALS time register - 0x00 is max integration time of 699ms (register 1) 
+    writeRegister(TMD2772_Addr,(0x03 | 0x80),0xFF-waitIntervals); // Wtime = 2.73 ms * delay peroids (subtract from 0xFF to enter into register)
+//    writeRegister(TMD2772_Addr,(0x0D | 0x80),0x04); //optionally scale ALS gain by 0.16 by seleting 0x04;
+    writeRegister(TMD2772_Addr,(0x0D | 0x80),0x00); //optionally scale ALS gain by 0.16 by seleting 0x04;
 
-    writeRegister(TMD2772_Addr,0x0F,0x00); //ALS gain is 1x
+    writeRegister(TMD2772_Addr,(0x0F | 0x80),0x00); //ALS gain is 1x
 }
 
 int main() {
@@ -76,48 +114,59 @@
     char data[4];
     uint16_t ch0Data;
     uint16_t ch1Data;
-    char reg2write;
-    float setpoint = 29500;
-    float step = 0.0001; //duty cycle change per sample
-    float dutyCycle=0.88;
+    float setpoint = 2.1148; //ch0/ch1 color setpoint ~0.88 duty cycle
+    float step; //duty cycle change per sample
+    float dutyCycle=0.926;
     float dutyCycleMin =0.8;
     float dutyCycleMax =0.99;
-    float pGain = 0.25;
+    float stepMax=0.0025;
+    float stepMin=-0.0025;
+//    float iGain = 0.05; //integral gain --adding this because when I blew on it, it couldn't recover
     float err;
-    float tol=2.5;
-    
+    float tol=1e-4; //tolerance within which to ignore changes in signal intensity
+    float pGain = 0.2; //proportional gain
+    float quadGain = 0; // 250 : 0.2 ratio relative to pGain
+    //float ch0Avg; //integral error
+//    float filterLength = 15;
+  
     //setup everything
-    mypwm.period_ms(5);
+    mypwm.period_us(400);
     mypwm.write(dutyCycle);
     i2c.frequency(400000); //set I2C frequency to 400kHz
     wait_ms(1000);
     initTMD2772();
-    regDump(TMD2772_Addr,0x00,0x0F);
+    regDump(TMD2772_Addr,(0x00 | 0x80),0x0F);
     pc.printf("Done initializing\r\n");
     wait_ms(700);
     
     //get initial filter value
-    reg2write=ALSDataRegister | 0x80;
-    i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
-    i2c.read(TMD2772_Addr,data,4);
+//    reg2write=ALSDataRegister | 0x80;
+    readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
+//    
+//    i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
+//    i2c.read(TMD2772_Addr,data,4);
     ch0Data = LSB_MSB_2uint16(data);
     ch1Data = LSB_MSB_2uint16(data+2);
     ratio = (float)ch0Data/(float)ch1Data;
     wait_ms(699);
     
+    
     while(1) {
         t.start();
-        reg2write=ALSDataRegister | 0x80;
-//        pc.printf("%X\r\n",reg2write);
-        i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
-        i2c.read(TMD2772_Addr,data,4);
+        readRegisters(TMD2772_Addr, (ALSDataRegister | 0x80), data ,4);
+        //reg2write=ALSDataRegister | 0x80;
+////        pc.printf("%X\r\n",reg2write);
+//        i2c.write(TMD2772_Addr,&reg2write,1,true); //1 byte of data, repeated start for read
+//        i2c.read(TMD2772_Addr,data,4);
         ch0Data = LSB_MSB_2uint16(data);
         ch1Data = LSB_MSB_2uint16(data+2);
         ratio = (float)ch0Data/(float)ch1Data;
-        err = ch0Data - setpoint;
-        pc.printf( "%U,%U, %f, %f, %f\r\n",ch0Data,ch1Data,ratio, dutyCycle, mypwm.read());
+        err = ratio - setpoint;
+        pc.printf( "%U,%U, %f, %f, %f\r\n",ch0Data,ch1Data,ratio,mypwm.read(),getTemp(MAX31725_Addr) );
         if (abs(err)>tol) {
-            step = (ch0Data-setpoint)/setpoint * pGain;
+            step = err * pGain + err/abs(err)*pow(err,2)*quadGain;
+            step = (step > stepMax) ? stepMax : step;
+            step = (step < stepMin) ? stepMin : step;
             dutyCycle -=step;
             dutyCycle = (dutyCycle < dutyCycleMin) ? dutyCycleMin : dutyCycle;
             dutyCycle = (dutyCycle > dutyCycleMax) ? dutyCycleMax : dutyCycle;