Revision 2:b5a85ba07057, committed 2018-04-30

Comitter:

PlayaLarrea

Date:

Mon Apr 30 15:38:53 2018 +0000

Parent:

1:1dd468e2ad40

Commit message:

gay

Changed in this revision

--- a/color.cpp	Wed Apr 25 14:13:24 2018 +0000
+++ b/color.cpp	Mon Apr 30 15:38:53 2018 +0000
@@ -3,63 +3,64 @@
 #include "Tach.h"
 #include "TCS3472_I2C.h"
 
-InterruptIn hall(p21);
+//InterruptIn hall_sensor(p21);
+DigitalOut hallpwr(p22);
+DigitalIn hall(p21);
 BusOut flash(LED1, LED2, LED3, LED4);
 TCS3472_I2C rgb_sensor(p9, p10);
 ContinuousServo left(p23);                  //Set up left wheel driver
 ContinuousServo right(p26);                 //Set up right wheel driver
-Tach tleft(p17,64);                         //Set up left tachometer
-Tach tright(p13, 64);                       //Set up right tachometer
-
-float wl;                                   //Left wheel velocity
-float wr;                                   //Right wheel velocity
-float e;                                    //Error
-
-float r = 1.3125;                           //radius of wheels
-float l = 4.0625;                           //width of wheel base
-float omega = 0.0;                          //angular velocity of wheels
-float v = 0.5;                              //translational velocity of wheels
-float kp = 1.0;                             //Proportional control gain
-
-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 rgb_data[4];
 float PWMbrightness = 1.0;
 float LB = 0.0;
+int x = 0.0;
  
-void mine() {
-    flash = 8.0;
-}
+//void mine() {
+//    flash = 8.0;
+//    wait(1.0);
+//    flash = 0;
+//}
  
 int main() {
     
-    hall.rise(&mine);
-    hall.fall(&mine);
+    hall.mode(PullUp);
+//    hall_sensor.rise(&mine);
+//    hall_sensor.fall(&mine);
+    hallpwr = 1;
     
     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
-        wl = tleft.getSpeed();              //Read the left tachometer
-        wr = tright.getSpeed();             //Read the right tachometer
-    
-        e = wl - (-wr);                     //Calculate the error
-        vr = vr - (kp*e);                   //Readjust the right wheel velocity
+        
+        x = hall.read();
+        
+        if(x == 1){
+            flash = 8.0;
+            }
+        else{
+            flash = 0.0;
+            }
+        x = hall.read();   
+            
+        left.speed(0.1);                     //Move the left wheel
+        right.speed(-0.1);                    //Move the right wheel
         
         LB = PWMbrightness;
         rgb_sensor.getAllColors(rgb_data);
-        
-        if(rgb_data[1]>999 && rgb_data[2]<999 && rgb_data[3]<999){
-        flash = 1.0;
+              
+        //if(rgb_data[1]>650){
+        if(rgb_data[0]>1000 && rgb_data[1]>650 && rgb_data[2]<300 && rgb_data[3]<300){
+        flash = 1.0; //red
         }
-        else if(rgb_data[1]<999 && rgb_data[2]>999 && rgb_data[3]<999){
-            flash = 2.0;
+        //else if(rgb_data[2]>650){
+        else if(rgb_data[0]>900 && rgb_data[1]<700 && rgb_data[2]>650 && rgb_data[3]<700){
+            flash = 2.0; //green
             }
-            else if(rgb_data[1]<999 && rgb_data[2]<999 && rgb_data[3]>999){
-                flash = 4.0;
+            //else if(rgb_data[3]>300){
+            else if(rgb_data[0]>900 && rgb_data[1]<300 && rgb_data[2]<300 && rgb_data[3]>300){
+                flash = 4.0; //blue
                 }
     }
 }
\ No newline at end of file