Diff: main.cpp

Revision:

4:104acd180bb1

Parent:

3:b65b394375b7

Child:

5:507618748cb4

--- a/main.cpp	Tue Nov 29 03:37:35 2016 +0000
+++ b/main.cpp	Fri Dec 02 07:38:22 2016 +0000
@@ -7,6 +7,14 @@
 //   https://developer.mbed.org/components/LSM9DS1-IMU/
 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
 
+//Color sensor, used to assist robot positioning
+// - wired using i2c according to
+//   https://developer.mbed.org/users/raj1995/notebook/adafruit-tcs34725-rgb-color-sensor/
+I2C color_sensor(p28, p27); //pins for I2C communication (SDA, SCL)
+int color_addr = 41 << 1;
+int color_thresh = 10000;//threshold for green before it triggers
+bool on_color = false;
+
 //pc serial connection, for debugging purposes
 Serial pc(USBTX, USBRX);
 
@@ -20,9 +28,34 @@
     IMU.calibrateMag(0);   
 }
 
+//verify color sensor and initialize
+void initColorSensor(){
+    pc.baud(9600);
+    
+    // Connect to the Color sensor and verify
+    color_sensor.frequency(200000);
+    
+    char id_regval[1] = {146};
+    char data[1] = {0};
+    color_sensor.write(color_addr,id_regval,1, true);
+    color_sensor.read(color_addr,data,1,false);
+
+    // Initialize color sensor
+    
+    char timing_register[2] = {129,0};
+    color_sensor.write(color_addr,timing_register,2,false);
+    
+    char control_register[2] = {143,0};
+    color_sensor.write(color_addr,control_register,2,false);
+    
+    char enable_register[2] = {128,3};
+    color_sensor.write(color_addr,enable_register,2,false);   
+}
+
 //initialization for all components
 void init(){
     initIMU();    
+    initColorSensor();
 }
 
 // Sends a request to the server and waits for a path response
@@ -53,6 +86,21 @@
     return sum / target_samples;
 }
 
+//returns true when color is entered (subsequently returns false until color is exited and entered again)
+bool colorEntered(){
+    char green_reg[1] = {152};
+    char green_data[2] = {0,0};
+    color_sensor.write(color_addr,green_reg,1, true);
+    color_sensor.read(color_addr,green_data,2, false);
+    
+    int green_value = ((int)green_data[1] << 8) | green_data[0];
+
+    bool is_green = green_value > color_thresh;
+    bool color_triggered = !on_color && is_green;
+    on_color = is_green;
+    return color_triggered;
+}
+
 //technologies for navigation may include
 // - Line following https://www.sparkfun.com/products/11769
 // - color sensor (indicates when end of movement is reached)