Diff: color.cpp

Revision:

1:1dd468e2ad40

Parent:

0:7d319151aaa0

Child:

2:b5a85ba07057

diff -r 7d319151aaa0 -r 1dd468e2ad40 color.cpp
--- a/color.cpp	Wed Apr 25 13:33:13 2018 +0000
+++ b/color.cpp	Wed Apr 25 14:13:24 2018 +0000
@@ -3,7 +3,6 @@
 #include "Tach.h"
 #include "TCS3472_I2C.h"
 
-DigitalIn hall(p21);
 InterruptIn hall(p21);
 BusOut flash(LED1, LED2, LED3, LED4);
 TCS3472_I2C rgb_sensor(p9, p10);
@@ -25,10 +24,12 @@
 float vl = ((omega*l)+(2*v))/(2*r);         //Calculates left wheel velocity
 float vr = ((omega*l)-(2*v))/(2*r);         //Calculates right wheel velocity
 
-int counter = 0.0;
+int rgb_data[4];
+float PWMbrightness = 1.0;
+float LB = 0.0;
  
 void mine() {
-    flash = 16.0;
+    flash = 8.0;
 }
  
 int main() {
@@ -36,6 +37,9 @@
     hall.rise(&mine);
     hall.fall(&mine);
     
+    rgb_sensor.enablePowerAndRGBC();
+    rgb_sensor.setIntegrationTime(100);
+    
     while(1) {           // wait around, interrupts will interrupt this!
         left.speed(vl);                     //Move the left wheel
         right.speed(vr);                    //Move the right wheel
@@ -45,8 +49,17 @@
         e = wl - (-wr);                     //Calculate the error
         vr = vr - (kp*e);                   //Readjust the right wheel velocity
         
-        if(rgb_sensor){
-        flash = counter;
+        LB = PWMbrightness;
+        rgb_sensor.getAllColors(rgb_data);
+        
+        if(rgb_data[1]>999 && rgb_data[2]<999 && rgb_data[3]<999){
+        flash = 1.0;
         }
+        else if(rgb_data[1]<999 && rgb_data[2]>999 && rgb_data[3]<999){
+            flash = 2.0;
+            }
+            else if(rgb_data[1]<999 && rgb_data[2]<999 && rgb_data[3]>999){
+                flash = 4.0;
+                }
     }
 }
\ No newline at end of file