Ben Keegan, Mitch Bond, Nick Dilleumber, Rixon Fletcher / Mbed 2 deprecated Autonomous_Vehicle

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Motor Servo TCS3472_I2C mbed

main.cpp@0:a4ce7d166c62, 2015-04-28 (annotated)

Committer:
m173090
Date:
Tue Apr 28 01:46:49 2015 +0000
Revision:
0:a4ce7d166c62
Autonomous Vehicle Project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m173090 0:a4ce7d166c62 1 //Ben Keegan, Mitch Bond, Nick Dilleumber, Rixon Fletcher
m173090 0:a4ce7d166c62 2 //ES202 Principles of Mechatronics
m173090 0:a4ce7d166c62 3 //Autonomous Vehicle Project
m173090 0:a4ce7d166c62 4 //26 APR 2015
m173090 0:a4ce7d166c62 5
m173090 0:a4ce7d166c62 6 #include "mbed.h"
m173090 0:a4ce7d166c62 7 #include "Servo.h"
m173090 0:a4ce7d166c62 8 #include "Motor.h"
m173090 0:a4ce7d166c62 9 #include "TCS3472_I2C.h"
m173090 0:a4ce7d166c62 10
m173090 0:a4ce7d166c62 11 //Actuators and Ultrasonic Sensor Pins
m173090 0:a4ce7d166c62 12 Motor M(p26, p29, p30);
m173090 0:a4ce7d166c62 13 Servo S(p23);
m173090 0:a4ce7d166c62 14 AnalogIn coke(p20); // sets analog in to pin 20
m173090 0:a4ce7d166c62 15 //Color Sensor Pins
m173090 0:a4ce7d166c62 16 PwmOut LB(p22); // PWM out signal to power LED on the sensor
m173090 0:a4ce7d166c62 17 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
m173090 0:a4ce7d166c62 18 //IR Sensor Pins
m173090 0:a4ce7d166c62 19 AnalogIn photod1(p15);
m173090 0:a4ce7d166c62 20 AnalogIn photod2(p16);
m173090 0:a4ce7d166c62 21 AnalogIn photod3(p17);
m173090 0:a4ce7d166c62 22
m173090 0:a4ce7d166c62 23 //Serial pc(USBTX,USBRX); // Establish serial connection with computer
m173090 0:a4ce7d166c62 24
m173090 0:a4ce7d166c62 25 float volt = 0.0; // float volt
m173090 0:a4ce7d166c62 26 int distance = 100.0; //float distance set at a value that will allow for initial entry into loop
m173090 0:a4ce7d166c62 27 float cof[2] = {489.1001, 2.0247}; // coefficients from matlab calibration
m173090 0:a4ce7d166c62 28 int count = 0;
m173090 0:a4ce7d166c62 29 int there = 0;
m173090 0:a4ce7d166c62 30
m173090 0:a4ce7d166c62 31 int main()
m173090 0:a4ce7d166c62 32 {
m173090 0:a4ce7d166c62 33 rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
m173090 0:a4ce7d166c62 34 rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
m173090 0:a4ce7d166c62 35
m173090 0:a4ce7d166c62 36 //pc.baud(921600); // Set Baud rate of serial connection
m173090 0:a4ce7d166c62 37 //pc.format(8, SerialBase::None, 1);
m173090 0:a4ce7d166c62 38 int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
m173090 0:a4ce7d166c62 39 float PWMbrightness = 0.75; // declare a float specifying brightness of LED (between 0.0 and 1.0)
m173090 0:a4ce7d166c62 40 float r; //photodiode 1
m173090 0:a4ce7d166c62 41 float m; //photodiode 2
m173090 0:a4ce7d166c62 42 float l; //photodiode 3
m173090 0:a4ce7d166c62 43 int mult=1000; //multiplier to amplify signal
m173090 0:a4ce7d166c62 44
m173090 0:a4ce7d166c62 45 S= 0.63;
m173090 0:a4ce7d166c62 46 wait(0.1);
m173090 0:a4ce7d166c62 47 M.speed(0);
m173090 0:a4ce7d166c62 48 while(!there) {// While the distance to the front is sufficient
m173090 0:a4ce7d166c62 49 volt = coke;
m173090 0:a4ce7d166c62 50 distance = floor(cof[0]*pow(volt,1) + cof[1]); // determines distance based on calibration coefficients and rounds it down
m173090 0:a4ce7d166c62 51
m173090 0:a4ce7d166c62 52 r=mult*photod1.read();
m173090 0:a4ce7d166c62 53 m=mult*photod2.read();
m173090 0:a4ce7d166c62 54 l=mult*photod3.read();
m173090 0:a4ce7d166c62 55
m173090 0:a4ce7d166c62 56 LB = PWMbrightness; // set brightness of sensor LED
m173090 0:a4ce7d166c62 57 rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
m173090 0:a4ce7d166c62 58 //int unf = rgb_data[0];
m173090 0:a4ce7d166c62 59 int red = rgb_data[1];
m173090 0:a4ce7d166c62 60 int green = rgb_data[2];
m173090 0:a4ce7d166c62 61 int blue = rgb_data[3];
m173090 0:a4ce7d166c62 62 //pc.printf("%d %d %d\n",red, green, blue);
m173090 0:a4ce7d166c62 63 // Print data to serial port
m173090 0:a4ce7d166c62 64 //pc.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
m173090 0:a4ce7d166c62 65 M.speed(-0.2);
m173090 0:a4ce7d166c62 66
m173090 0:a4ce7d166c62 67 if (red > 150 && green < 100){
m173090 0:a4ce7d166c62 68 S = 0.34;
m173090 0:a4ce7d166c62 69 M.speed(0.25);
m173090 0:a4ce7d166c62 70 wait(.5);
m173090 0:a4ce7d166c62 71 S= 0.84;
m173090 0:a4ce7d166c62 72 //M.speed(-0.2);
m173090 0:a4ce7d166c62 73 M.speed(-0.25);
m173090 0:a4ce7d166c62 74 //wait(0.1);
m173090 0:a4ce7d166c62 75 //M.speed(0);
m173090 0:a4ce7d166c62 76 //wait(0.1);
m173090 0:a4ce7d166c62 77 //S =0.63;
m173090 0:a4ce7d166c62 78 //M.speed(-0.2);
m173090 0:a4ce7d166c62 79 //pc.printf("Turn Left\n");
m173090 0:a4ce7d166c62 80 }
m173090 0:a4ce7d166c62 81 else if(green > 200){ //&& green < 200
m173090 0:a4ce7d166c62 82 S = 0.84;
m173090 0:a4ce7d166c62 83 M.speed(0.25);
m173090 0:a4ce7d166c62 84 wait(0.5);
m173090 0:a4ce7d166c62 85 S= 0.34;
m173090 0:a4ce7d166c62 86 M.speed(-0.25);
m173090 0:a4ce7d166c62 87 //m.speed(0.1);
m173090 0:a4ce7d166c62 88 //wait(2);
m173090 0:a4ce7d166c62 89 //S =0.63;
m173090 0:a4ce7d166c62 90 //m.speed(-0.05);
m173090 0:a4ce7d166c62 91 //pc.printf("Turn Right\n");
m173090 0:a4ce7d166c62 92 }
m173090 0:a4ce7d166c62 93 else { /*(green-red)<50 && (blue-green)<50)*/
m173090 0:a4ce7d166c62 94 //pc.printf("Go Straight\n");
m173090 0:a4ce7d166c62 95 S= 0.63;
m173090 0:a4ce7d166c62 96 //M.speed(-0.175);
m173090 0:a4ce7d166c62 97 }
m173090 0:a4ce7d166c62 98
m173090 0:a4ce7d166c62 99 if (count > 10000){ //Ensure vehicle won't stop short due to people in way when it starts
m173090 0:a4ce7d166c62 100 if (distance < 17){
m173090 0:a4ce7d166c62 101 M.speed(0);
m173090 0:a4ce7d166c62 102 if (r > 200){ //Adjust to face lighthouse if right photodiode reads high
m173090 0:a4ce7d166c62 103 S = 0.84;
m173090 0:a4ce7d166c62 104 M.speed(0.25);
m173090 0:a4ce7d166c62 105 wait(0.5);
m173090 0:a4ce7d166c62 106 S= 0.34;
m173090 0:a4ce7d166c62 107 M.speed(-0.25);
m173090 0:a4ce7d166c62 108 wait(0.05);
m173090 0:a4ce7d166c62 109 M.speed(0);
m173090 0:a4ce7d166c62 110 }
m173090 0:a4ce7d166c62 111 there = 1;
m173090 0:a4ce7d166c62 112 }
m173090 0:a4ce7d166c62 113 }
m173090 0:a4ce7d166c62 114 count +=1;
m173090 0:a4ce7d166c62 115 }// end while
m173090 0:a4ce7d166c62 116 M.speed(0);
m173090 0:a4ce7d166c62 117 //pc.printf("Stop/n");
m173090 0:a4ce7d166c62 118 } // end main