Final
Dependencies: Motor TCS3472_I2C mbed
Diff: main.cpp
- Revision:
- 0:ebd2fdd6da3b
--- /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