Revision 5:3dd9b0a35aee, committed 2013-12-03

Comitter:

nolanwagener

Date:

Tue Dec 03 07:39:38 2013 +0000

Parent:

4:f38d0db83e1c

Child:

6:d0f91dd99cf0

Commit message:

first version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADJD-S371_ColourSens.lib	Tue Dec 03 07:39:38 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/MichaelW/code/ADJD-S371_ColourSens/#7944a9bbbe4f

--- a/main.cpp	Thu Nov 21 00:25:39 2013 +0000
+++ b/main.cpp	Tue Dec 03 07:39:38 2013 +0000
@@ -1,101 +1,303 @@
 #include "mbed.h"
 #include "m3pi.h"
+#include <ADJDColourSensor.h>
 
-DigitalOut sensorLED(p8);
-I2C colorSensor(p28, p27); //SDA, SCL
+#define RED 49 //((const unsigned char *)"1")
+#define GREEN 50 //((const unsigned char *) "2")
+#define BLUE 51 //((const unsigned char *) "3")
+#define BROWN 52 //((cont unsigned char *) "4")
+#define BLACK 53 // ((const unsigned char *) "5")
+#define YELLOW 54
+#define WHITE 55
+#define TEST 56 // ((const unsigned char *) "6")
+#define NUM_OF_WEIGHTS 7
+#define NUM_OF_TESTS 25
+
+//DigitalOut sensorLED(p8);
+//I2C colorSensor(p28, p27); //SDA, SCL
 Serial bt(p13, p14); // tx, rx
+ADJDColourSensor colorSensor(p28, p27, p8);
 
-const int addr = 0x74;
+//const int addr_write = 0x74 << 1;
+//const int addr_read = addr_write + 0x1;
 m3pi m3pi;
 
-int read(char lowByteRegister) {
-        char cmd[1];
-        char echo[2];
+void normalize(double* feature) {
+    double magnitude = sqrt(feature[0] * feature[0] + feature[1] * feature[1] + feature[2] * feature[2] + feature[3] * feature[3]);
+    feature[0] /= magnitude;
+    feature[1] /= magnitude;
+    feature[2] /= magnitude;
+    feature[3] /= magnitude;
+}
+
+void takeMeasurements(int numberOfTests) {
+    int redTotal = 0;
+    int greenTotal = 0;
+    int blueTotal = 0;
+    int clearTotal = 0;
+    for(int i = 0; i < numberOfTests; i++) {
+        colorSensor.readColors();
+        redTotal += colorSensor.red();
+        greenTotal += colorSensor.green();
+        blueTotal += colorSensor.blue();
+        clearTotal += colorSensor.clear();
+    }
+    bt.printf("R:%d, G:%d, B:%d, C:%d\r", redTotal / numberOfTests, greenTotal / numberOfTests, blueTotal / numberOfTests, clearTotal / numberOfTests);
+}
+
+double max(double* values) {
+    double currentMax = values[0];
+    
+    for(int i = 1; i < NUM_OF_WEIGHTS; i++) {
+        if(values[i] > currentMax)
+            currentMax = values[i];
+    }
+    
+    return currentMax;
+    
+}
+
+int max(int* values) {
+    int currentMax = values[0];
+    
+    for(int i = 1; i < NUM_OF_WEIGHTS; i++) {
+        if(values[i] > currentMax)
+            currentMax = values[i];
+    }
+    
+    return currentMax;
+    
+}
+
+void correctWeights(double* wrongWeights, double* properWeights) {
+    double colors[] = { (double) colorSensor.red(), (double) colorSensor.green(), (double) colorSensor.blue() };
+    normalize(colors);
+    wrongWeights[0] -= colors[0]; //colorSensor.red();
+    wrongWeights[1] -= colors[1]; //colorSensor.green();
+    wrongWeights[2] -= colors[2]; //colorSensor.blue();
+    properWeights[0] += colors[0]; //colorSensor.red();
+    properWeights[1] += colors[1]; //colorSensor.green();
+    properWeights[2] += colors[2]; //colorSensor.blue();
+    
+    normalize(wrongWeights);
+    normalize(properWeights);
+}
+
+void correctWeights(int* wrongWeights, int* properWeights) {
+    wrongWeights[0] -= colorSensor.red();
+    wrongWeights[1] -= colorSensor.green();
+    wrongWeights[2] -= colorSensor.blue();
+    properWeights[0] += colorSensor.red();
+    properWeights[1] += colorSensor.green();
+    properWeights[2] += colorSensor.blue();
+}
+
+
+void sendData(char* color, double* redWeights, double* greenWeights, double* blueWeights, double* brownWeights, double* blackWeights, double* yellowWeights, double* whiteWeights) {
+    bt.printf("%s,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r", color, colorSensor.red(), colorSensor.green(), colorSensor.blue(), colorSensor.clear(),redWeights[0], redWeights[1], redWeights[2], redWeights[3], greenWeights[0], greenWeights[1], greenWeights[2], greenWeights[3], blueWeights[0], blueWeights[1], blueWeights[2], blueWeights[3], brownWeights[0], brownWeights[1], brownWeights[2], brownWeights[3], blackWeights[0], blackWeights[1], blackWeights[2], blackWeights[3], yellowWeights[0], yellowWeights[1], yellowWeights[2], yellowWeights[3], whiteWeights[0], whiteWeights[1], whiteWeights[2], whiteWeights[3]);
+}
+
+void sendData(char* color, int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* yellowWeights, int* whiteWeights) {
+    bt.printf("%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r", color, colorSensor.red(), colorSensor.green(), colorSensor.blue(), colorSensor.clear(),redWeights[0], redWeights[1], redWeights[2], redWeights[3], greenWeights[0], greenWeights[1], greenWeights[2], greenWeights[3], blueWeights[0], blueWeights[1], blueWeights[2], blueWeights[3], brownWeights[0], brownWeights[1], brownWeights[2], brownWeights[3], blackWeights[0], blackWeights[1], blackWeights[2], blackWeights[3], yellowWeights[0], yellowWeights[1], yellowWeights[2], yellowWeights[3], whiteWeights[0], whiteWeights[1], whiteWeights[3], whiteWeights[3]);
+}
+
+void calibrate(double* weights) {
+    int redTotal = 0;
+    int greenTotal = 0;
+    int blueTotal = 0;
+    int clearTotal = 0;
+    for(int i = 0; i < NUM_OF_TESTS; i++) {
+        colorSensor.readColors();
+        redTotal += colorSensor.red();
+        greenTotal += colorSensor.green();
+        blueTotal += colorSensor.blue();
+        clearTotal += colorSensor.clear();
+    }
+    weights[0] = (double) redTotal;
+    weights[1] = (double) greenTotal;
+    weights[2] = (double) blueTotal;
+    weights[3] = (double) clearTotal;
+    normalize(weights);
+
+
+}
 
-        cmd[0] = lowByteRegister;
-        colorSensor.write(addr,cmd,1);
-        colorSensor.read(addr,echo,2);
-        int result_low = (int) echo[0];
-        
-        /*
-        cmd[0] = lowByteRegister + 1;
-        colorSensor.write(addr,cmd,1);
-        colorSensor.read(addr,echo,1);
-        int result_high = (int) echo[0];
-        bt.printf("hi:%0x ",result_high);
-        
-        return (result_high << 8) | result_low;
-        */
-        
-        return result_low;
+/*
+void calibrate(int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* desiredWeights) {
+    colorSensor.readColors();
+    int redProduct = colorSensor.dotProduct(redWeights);
+    int greenProduct = colorSensor.dotProduct(greenWeights);
+    int blueProduct = colorSensor.dotProduct(blueWeights);
+    int brownProduct = colorSensor.dotProduct(brownWeights);
+    int blackProduct = colorSensor.dotProduct(blackWeights);
+    int products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct};
+    int maxProduct = max(products);
+    char* color;
+    if(colorSensor.dotProduct(desiredWeights) == maxProduct) {
+        if(redWeights == desiredWeights) {
+            color = "Red";
+        } else if(greenWeights == desiredWeights) {
+            color = "Green";
+        } else if(blueWeights == desiredWeights) {
+            color = "Blue";
+        } else if(brownWeights == desiredWeights) {
+            color = "Brown";
+        } else if(blackWeights == desiredWeights) {
+            color = "Black";
+        }
+    } else if(redProduct == maxProduct) {
+        correctWeights(redWeights, desiredWeights);
+        color = "Red";
+    } else if(greenProduct == maxProduct) {
+        correctWeights(greenWeights, desiredWeights);
+        color = "Green";
+    } else if(blueProduct == maxProduct) {
+        correctWeights(blueWeights, desiredWeights);
+        color = "Blue";
+    } else if(brownProduct == maxProduct) {
+        correctWeights(brownWeights, desiredWeights);
+        color = "Brown";
+    } else if(blackProduct == maxProduct) {
+        correctWeights(blackWeights, desiredWeights);
+        color = "Black";
+    }
+    //sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights);
+} */
+
+void measure(double* redWeights, double* greenWeights, double* blueWeights, double* brownWeights, double* blackWeights, double* yellowWeights, double* whiteWeights) {
+    colorSensor.readColors();
+    double redProduct = colorSensor.dotProduct(redWeights);
+    double greenProduct = colorSensor.dotProduct(greenWeights);
+    double blueProduct = colorSensor.dotProduct(blueWeights);
+    double brownProduct = colorSensor.dotProduct(brownWeights);
+    double blackProduct = colorSensor.dotProduct(blackWeights);
+    double yellowProduct = colorSensor.dotProduct(yellowWeights);
+    double whiteProduct = colorSensor.dotProduct(whiteWeights);
+    double products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct, yellowProduct, whiteProduct};
+    double maxProduct = max(products);
+    char* color;
+    if(redProduct == maxProduct) {
+        color = "Red";
+    } else if(greenProduct == maxProduct) {
+        color = "Green";
+    } else if(blueProduct == maxProduct) {
+        color = "Blue";
+    } else if(brownProduct == maxProduct) {
+        color = "Brown";
+    } else if(blackProduct == maxProduct) {
+        color = "Black";
+    } else if(yellowProduct == maxProduct) {
+        color = "Yellow";
+    } else if(whiteProduct == maxProduct) {
+        color = "White";
+    }
+    sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
 }
 
+
+void measure(int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* yellowWeights, int* whiteWeights) {
+    colorSensor.readColors();
+    int redProduct = colorSensor.dotProduct(redWeights);
+    int greenProduct = colorSensor.dotProduct(greenWeights);
+    int blueProduct = colorSensor.dotProduct(blueWeights);
+    int brownProduct = colorSensor.dotProduct(brownWeights);
+    int blackProduct = colorSensor.dotProduct(blackWeights);
+    int yellowProduct = colorSensor.dotProduct(yellowWeights);
+    int whiteProduct = colorSensor.dotProduct(whiteWeights);
+    int products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct, yellowProduct, whiteProduct};
+    int maxProduct = max(products);
+    char* color;
+    if(redProduct == maxProduct) {
+        color = "Red";
+    } else if(greenProduct == maxProduct) {
+        color = "Green";
+    } else if(blueProduct == maxProduct) {
+        color = "Blue";
+    } else if(brownProduct == maxProduct) {
+        color = "Brown";
+    } else if(blackProduct == maxProduct) {
+        color = "Black";
+    } else if(yellowProduct == maxProduct) {
+        color = "Yellow";
+    } else if(whiteProduct == maxProduct) {
+        color = "White";
+    }
+    sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+}
+
+
+
 int main() {
 
     bt.baud(115200);
-
-    char cmd[2];
-    sensorLED = 1;
-    m3pi.locate(0,1);
-    m3pi.printf("Start");
-    wait(1);
+    colorSensor.setIntegrationTimeSlot(0xff, 0xf0, 0xff, 0xff);
+    colorSensor.setCapacitors(12, 7, 2, 9);
+    //colorSensor.setIntegrationTimeSlot(0xff, 0x110, 0x104, 0xff);
+    //colorSensor.setCapacitors(7, 7, 7, 7);
+    
+    
+    /*
+    int redWeights[] = {100, -50, -50};
+    int greenWeights[] = {-50, 100, -50};
+    int blueWeights[] = {-50, -50, 100};
+    int brownWeights[] = {1, 0, 1};
+    int blackWeights[] = {1, 1, 1};
+    double redWeights[] = {311.4, 233.2, 217.6};
+    */
+    double redWeights[] = {318.8, 242.6, 227.4, 712.4};
+    double greenWeights[] = {206.2, 258.2, 242.6, 640.4};
+    double blueWeights[] = {190.8, 223.4, 307.6, 630.8};
+    double brownWeights[] = {-205.6, -215.0, -212.4, -588.0};
+    double blackWeights[] = {175.6, 187.6, 196.8, 530.4};
+    double yellowWeights[] = {480.8, 542.6, 315.8, 1021.2};
+    double whiteWeights[] = {455.6, 584.8, 578.8, 1021.8};
+    
+    
+    normalize(redWeights); normalize(greenWeights); normalize(blueWeights); normalize(brownWeights); normalize(blackWeights); normalize(yellowWeights); normalize(whiteWeights);
+    int message;
     
     while(1) {
-        cmd[0] = 0x00;
-        cmd[1] = 0x01;
-        colorSensor.write(addr,cmd,2);
+        //takeMeasurements(20);
         
-        cmd[0] = 0x06;
-        cmd[1] = 0x04;
-        colorSensor.write(addr,cmd,2);
         
-        int red = read(0x06);
-        // int green = read(0x42);
-        // int blue = read(0x44);
-
-        // Print for debugging
-        
-        bt.printf("R:%0x ",red);
-        //bt.printf("G:%0x ",green);
-        //bt.printf("B:%0x     ",blue);
-        wait(1);
- 
- /*       
-        cmd[0] = 0x06;
-        colorSensor.write(addr,cmd,1);
-        colorSensor.read(addr,cmd,1);
-        red = cmd[0];
+        message = bt.getc();
+        //bt.printf("\r%d\r", message);
         
-        cmd[0] = 0x07;
-        colorSensor.write(addr,cmd,1);
-        colorSensor.read(addr,cmd,1);
-        green = cmd[0];
-        
-        cmd[0] = 0x08;
-        colorSensor.write(addr,cmd,1);
-        colorSensor.read(addr,cmd,1);
-        blue = cmd[0];
-        
-        bt.printf("Rc:%d ",red);
-        bt.printf("Gc:%d ",green);
-        bt.printf("Bc:%d      ",blue);
-*/        
-  
- /*       
-        m3pi.cls();
-        m3pi.locate(0,1);
-        m3pi.printf("R:%d",red);
-        wait(1.5);
-        
-        m3pi.cls();
-        m3pi.locate(0,1);
-        m3pi.printf("G:%d",green);
-        wait(1.5);
-        
-        m3pi.cls();
-        m3pi.locate(0,1);
-        m3pi.printf("B:%d",blue);    
-        wait(1.5);    
-*/
+        switch(message) {
+        case RED:
+            calibrate(redWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case GREEN:
+            calibrate(greenWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case BLUE:
+            calibrate(blueWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case BROWN:
+            calibrate(brownWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case BLACK:
+            calibrate(blackWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case YELLOW:
+            calibrate(yellowWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case WHITE:
+            calibrate(whiteWeights);
+            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        case TEST:
+            measure(redWeights,greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
+            break;
+        default:
+            break;
+        } 
     }
+    
 }