Zolovick_Larrea_Mouaffek / Mbed 2 deprecated ES202_Final_Project

this

Dependencies:   ContinuousServo TCS3472_I2C Tach mbed

main.cpp@1:50677ba704f5, 2018-04-30 (annotated)

Committer:
PlayaLarrea
Date:
Mon Apr 30 20:12:02 2018 +0000
Revision:
1:50677ba704f5
Parent:
0:389040618ce2 
FINAL CODE. STAYED IN BOUNDS, SENSED MINE AND FINISHED IN 29 SECONDS

Who changed what in which revision?

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