Zolovick_Larrea_Mouaffek / Mbed 2 deprecated ES202_Final_Project

this

Dependencies:   ContinuousServo TCS3472_I2C Tach mbed

main.cpp@0:389040618ce2, 2018-04-30 (annotated)

Committer:
georgezolovick
Date:
Mon Apr 30 17:16:55 2018 +0000
Revision:
0:389040618ce2
Child:
1:50677ba704f5
30Apr2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
georgezolovick 0:389040618ce2 1 #include "mbed.h"
georgezolovick 0:389040618ce2 2 #include "ContinuousServo.h"
georgezolovick 0:389040618ce2 3 #include "Tach.h"
georgezolovick 0:389040618ce2 4 #include "TCS3472_I2C.h"
georgezolovick 0:389040618ce2 5
georgezolovick 0:389040618ce2 6 ContinuousServo left(p23);
georgezolovick 0:389040618ce2 7 ContinuousServo right(p26);
georgezolovick 0:389040618ce2 8 AnalogIn sonar(p19);
georgezolovick 0:389040618ce2 9 PwmOut LB(p22); // PWM out signal to power LED on the sensor
georgezolovick 0:389040618ce2 10 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
georgezolovick 0:389040618ce2 11 DigitalOut hallpwr(p22);
georgezolovick 0:389040618ce2 12 DigitalIn hall(p21);
georgezolovick 0:389040618ce2 13 BusOut flash(LED1, LED2, LED3, LED4);
georgezolovick 0:389040618ce2 14 Ticker mine;
georgezolovick 0:389040618ce2 15 Ticker ultra;
georgezolovick 0:389040618ce2 16 Serial pc(USBTX,USBRX);
georgezolovick 0:389040618ce2 17
georgezolovick 0:389040618ce2 18 float wall = 100.0;
georgezolovick 0:389040618ce2 19 float analog;
georgezolovick 0:389040618ce2 20 float range;
georgezolovick 0:389040618ce2 21
georgezolovick 0:389040618ce2 22 int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
georgezolovick 0:389040618ce2 23 float PWMbrightness = 1.0; // float specifying brightness of LED (between 0.0 and 1.0)
georgezolovick 0:389040618ce2 24 int x;
georgezolovick 0:389040618ce2 25 float a,b,c;
georgezolovick 0:389040618ce2 26
georgezolovick 0:389040618ce2 27 void update()
georgezolovick 0:389040618ce2 28 {
georgezolovick 0:389040618ce2 29 a = 2.3*156.25*sonar.read();
georgezolovick 0:389040618ce2 30 //pc.printf("%f\r\n", a);
georgezolovick 0:389040618ce2 31 wait(0.01);
georgezolovick 0:389040618ce2 32 b = 2.3*156.25*sonar.read();
georgezolovick 0:389040618ce2 33 //pc.printf("%f\r\n", b);
georgezolovick 0:389040618ce2 34 wait(0.01);
georgezolovick 0:389040618ce2 35 c = 2.3*156.25*sonar.read();
georgezolovick 0:389040618ce2 36 //pc.printf("%f\r\n", c);
georgezolovick 0:389040618ce2 37 wait(0.01);
georgezolovick 0:389040618ce2 38 wall = (a+b+c)/3.0;
georgezolovick 0:389040618ce2 39 pc.printf("%f\r\n", wall);
georgezolovick 0:389040618ce2 40 }
georgezolovick 0:389040618ce2 41
georgezolovick 0:389040618ce2 42 void sweep()
georgezolovick 0:389040618ce2 43 {
georgezolovick 0:389040618ce2 44 if(hall.read() == 1){
georgezolovick 0:389040618ce2 45 flash = 8.0;
georgezolovick 0:389040618ce2 46 wait(0.1);
georgezolovick 0:389040618ce2 47 hallpwr = 0;
georgezolovick 0:389040618ce2 48 }
georgezolovick 0:389040618ce2 49 else{
georgezolovick 0:389040618ce2 50 flash = 0.0;
georgezolovick 0:389040618ce2 51 }
georgezolovick 0:389040618ce2 52 }
georgezolovick 0:389040618ce2 53
georgezolovick 0:389040618ce2 54 int main()
georgezolovick 0:389040618ce2 55 {
georgezolovick 0:389040618ce2 56 ultra.attach(&update, 0.1);
georgezolovick 0:389040618ce2 57
georgezolovick 0:389040618ce2 58 hall.mode(PullUp);
georgezolovick 0:389040618ce2 59 hallpwr = 1;
georgezolovick 0:389040618ce2 60 mine.attach(&sweep, 0.1);
georgezolovick 0:389040618ce2 61
georgezolovick 0:389040618ce2 62 rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
georgezolovick 0:389040618ce2 63 rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
georgezolovick 0:389040618ce2 64
georgezolovick 0:389040618ce2 65 wait(3);
georgezolovick 0:389040618ce2 66
georgezolovick 0:389040618ce2 67 while(wall>=6.9) { // main code for driving goes here
georgezolovick 0:389040618ce2 68 //x = hall.read();
georgezolovick 0:389040618ce2 69 //if(x == 1) {
georgezolovick 0:389040618ce2 70 // flash = 8.0;
georgezolovick 0:389040618ce2 71 // wait(0.25);
georgezolovick 0:389040618ce2 72 // x = 0;
georgezolovick 0:389040618ce2 73 //} else if (x == 0){
georgezolovick 0:389040618ce2 74 // flash = 0.0;
georgezolovick 0:389040618ce2 75 //}
georgezolovick 0:389040618ce2 76 //x = hall.read();
georgezolovick 0:389040618ce2 77
georgezolovick 0:389040618ce2 78 LB = PWMbrightness; // set brightness of sensor LED
georgezolovick 0:389040618ce2 79 rgb_sensor.getAllColors( rgb_data );
georgezolovick 0:389040618ce2 80 pc.printf("%d\r\n", rgb_data[1]);
georgezolovick 0:389040618ce2 81 left.speed(.15);
georgezolovick 0:389040618ce2 82 right.speed(-.25);
georgezolovick 0:389040618ce2 83 if(rgb_data[1]<900) {
georgezolovick 0:389040618ce2 84 left.speed(.1);
georgezolovick 0:389040618ce2 85 right.speed(-.25);
georgezolovick 0:389040618ce2 86 } else if(rgb_data[1]>1000) {
georgezolovick 0:389040618ce2 87 left.speed(.19);
georgezolovick 0:389040618ce2 88 right.speed(-.18);
georgezolovick 0:389040618ce2 89 } else{
georgezolovick 0:389040618ce2 90 left.speed(.15);
georgezolovick 0:389040618ce2 91 right.speed(-.25);
georgezolovick 0:389040618ce2 92 }
georgezolovick 0:389040618ce2 93 }
georgezolovick 0:389040618ce2 94 left.stop();
georgezolovick 0:389040618ce2 95 right.stop();
georgezolovick 0:389040618ce2 96 }