Diff: main.cpp

Revision:

0:ebd2fdd6da3b

diff -r 000000000000 -r ebd2fdd6da3b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 03 15:50:20 2016 +0000
@@ -0,0 +1,176 @@
+// FORMAT_CODE_START
+#include "mbed.h"       //include libraries
+#include "Motor.h"
+#include "TCS3472_I2C.h"
+
+Motor lmot(p25,p27,p28);   //Set up motor
+Motor rmot(p26,p30,p29);
+AnalogIn dsensor(p16);
+AnalogIn diodes[]= {p17,p19,p20};
+PwmOut LB(p22); // PWM out signal to power LED on the sensor
+TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
+Serial pc(USBTX,USBRX);
+//DigitalOut led(LED1);
+AnalogIn hall_sensor(p15); // Melexis US1818 Hall effect sensor on pin 20
+DigitalOut status_led(LED1); // blinky light
+
+static float rspeed=.6/3;
+float vin=0.0;
+static float lspeed=.7/3;
+
+void turnLeft()
+{
+    rmot.speed(0);
+    lmot.speed(-1*lspeed*2);
+}
+
+void turnRight()
+{
+    rmot.speed(-1*rspeed*1.5);
+    lmot.speed(0);
+}
+void goStraight()
+{
+    rmot.speed(rspeed);
+    lmot.speed(lspeed);
+}
+
+void goBackwards()
+{
+    rmot.speed(-1*rspeed);
+    lmot.speed(-1*lspeed);
+}
+
+void stopMotors()
+{
+    rmot.speed(0);
+    lmot.speed(0);
+    wait(.05);
+}
+
+
+int main()
+{
+    pc.baud(115200); // Set Baud rate of serial connection
+    int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
+    float diode_data[3];
+    float distance=0;
+    float PWMbrightness = 1.0; // declare a float specifying brightness of LED (between 0.0 and 1.0)
+    int diodeturn; //0=turn right, 1=straight, 2=turn left
+    int colorturn;
+    float prevdist=0;
+    int counter=0;
+    bool usediode;
+    status_led = 0;
+    int x=0;
+    int y=0;
+    int count=0;
+
+    while(1) {
+        LB = PWMbrightness; // set brightness of sensor LED
+        //Read Data
+        rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
+        for(int i=0; i<=2; i++) {            //read the value of the infrared diodes
+            diode_data[i]=diodes[i];
+        }
+        prevdist=distance;
+        distance=dsensor;
+
+        //Filter Data
+        if(diode_data[0]>diode_data[1] && diode_data[0]>diode_data[2]) {
+            diodeturn=0;
+        } else if(diode_data[2]>diode_data[0] && diode_data[2]>diode_data[1]) {
+            diodeturn=2;
+        } else {
+            diodeturn=1;
+        }
+        if ((rgb_data[1]>= rgb_data[2]) && (rgb_data[1]>=rgb_data[3]))   {
+            colorturn=2;
+        } else if ((rgb_data[2]>= rgb_data[1]) && (rgb_data[2]>=rgb_data[3]))   {
+            colorturn=0;
+        } else {
+            colorturn=1;
+            
+        }
+        printf("\nColorturn: %d\n",colorturn);
+        printf("Diodeturn: %d\n",diodeturn);
+        
+        if(rgb_data[0]>10000){//Tells if it's in the light area
+            usediode=true;
+            //led=1;
+            pc.printf("Use Diode\n");
+        } else {
+            usediode=false;
+        
+            pc.printf("Use Color\n");
+        }
+        //Hall Effect
+        vin = hall_sensor.read(); // this line actually reads the sensor
+        // NB the sensor is a digital out; here it is read as an analog only for testing
+
+        if (vin>0.5 && x==0) {
+            
+            status_led = 1; // blinky light on
+            y=0;
+            count=count+1;
+            if(count>10) {
+                x=1;
+            }
+        }
+        if(vin>.5 && x==1) {
+            
+            status_led = 0; // blinky light off
+            count=0;
+        } if (vin<.5&&y==0) {
+            
+            status_led = 1; // blinky light off
+            x=0;
+            count=count+1;
+            if(count>10) {
+                y=1;
+            }
+        }
+        if(vin<.5 && y==1) {
+            status_led = 0; // blinky light off
+            count=0;
+        }
+   
+        //Follow Course
+        if(distance>.0125 && prevdist>.0125) {
+            if(!usediode) {
+                if(colorturn==0) {
+                    turnRight();
+                    pc.printf("C Turn Right\n");
+                } else if(colorturn==2) {
+                    turnLeft();
+                    pc.printf("C Turn Left");
+                } else { //On track
+                    goStraight();
+                    pc.printf("C Straigt\n");
+                }//close course control
+            } else  { //if !usediode
+                if(diodeturn==0) {
+                   // turnRight();
+                   rmot.speed(0);
+                    lmot.speed(lspeed);
+                    pc.printf("D Turn Right\n");
+                } else if(diodeturn==2) {
+                    //turnLeft();
+                    rmot.speed(rspeed);
+                    lmot.speed(0);
+                    pc.printf("D Turn Left\n");
+                } else { //On track
+                    goStraight();
+                    pc.printf("D Straight\n");
+                }//close course control
+            }
+        } else {
+            stopMotors();
+        }
+
+        wait(.15);
+        stopMotors();
+        counter++;
+        wait(.4);
+    }//end while
+}//end main
\ No newline at end of file