Diff: main.cpp

Revision:

0:389040618ce2

Child:

1:50677ba704f5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 30 17:16:55 2018 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "ContinuousServo.h"
+#include "Tach.h"
+#include "TCS3472_I2C.h"
+
+ContinuousServo left(p23);
+ContinuousServo right(p26);
+AnalogIn sonar(p19);
+PwmOut LB(p22); // PWM out signal to power LED on the sensor
+TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
+DigitalOut hallpwr(p22);
+DigitalIn hall(p21);
+BusOut flash(LED1, LED2, LED3, LED4);
+Ticker mine;
+Ticker ultra;
+Serial pc(USBTX,USBRX);
+
+float wall = 100.0;
+float analog;
+float range;
+
+int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
+float PWMbrightness = 1.0; // float specifying brightness of LED (between 0.0 and 1.0)
+int x;
+float a,b,c;
+
+void update()
+{
+    a = 2.3*156.25*sonar.read();
+    //pc.printf("%f\r\n", a);
+    wait(0.01);
+    b = 2.3*156.25*sonar.read();
+    //pc.printf("%f\r\n", b);
+    wait(0.01);
+    c = 2.3*156.25*sonar.read();
+    //pc.printf("%f\r\n", c);
+    wait(0.01);
+    wall = (a+b+c)/3.0;
+    pc.printf("%f\r\n", wall);
+}
+
+void sweep()
+{
+    if(hall.read() == 1){
+        flash = 8.0;
+        wait(0.1);
+        hallpwr = 0;
+        }
+        else{
+            flash = 0.0;
+            }
+}
+    
+int main()
+{
+    ultra.attach(&update, 0.1);
+
+    hall.mode(PullUp);
+    hallpwr = 1;
+    mine.attach(&sweep, 0.1);
+    
+    rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
+    rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
+
+    wait(3);
+
+    while(wall>=6.9) { // main code for driving goes here
+        //x = hall.read();
+        //if(x == 1) {
+        //    flash = 8.0;
+        //    wait(0.25);
+        //    x = 0;
+        //} else if (x == 0){
+        //    flash = 0.0;
+        //}
+        //x = hall.read();
+
+        LB = PWMbrightness; // set brightness of sensor LED
+        rgb_sensor.getAllColors( rgb_data );
+        pc.printf("%d\r\n", rgb_data[1]);
+        left.speed(.15);
+        right.speed(-.25);
+        if(rgb_data[1]<900) {
+            left.speed(.1);
+            right.speed(-.25);
+        } else if(rgb_data[1]>1000) {
+            left.speed(.19);
+            right.speed(-.18);
+        } else{
+            left.speed(.15);
+            right.speed(-.25);
+        }
+    }
+    left.stop();
+    right.stop();
+}
\ No newline at end of file