Embedded Files
Circuit diagram for Arduino powered car designed to simulate an autonomous vehicle interacting with a traffic light. All red wires are power, black wires are ground, green wires are signal inputs, and yellow wires are signal outputs.
ARDUINO CODE
// TRAFFIC STOP CODE FOR ARDUINO
// Written by Brent Lawson
// 05-02-20
#include <Wire.h>
#include "Adafruit_TCS34725.h"
#define Left_Pin A0
#define Center_Pin A1
#define Right_Pin A2
#define IN1 7 //Right motor(K1/K2) direction Pin 1
#define IN2 8 //Right motor(K1/K2) direction Pin 2
#define IN3 9 //Left motor(K3/K4) direction Pin 1
#define IN4 10 //Left motor(K3/K4) direction Pin 2
#define ENA 5 //ENA PWM speed pin for Right motor(K1/K2)
#define ENB 6 //ENB PWM speed pin for Left motor(K3/K4)
#define TURN_TIME 75
#define SPEED 150
#define MOVE_TIME 100
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_1X);
String read_sensor_values()
{ int sensorvalue=8;
int sensor_left,sensor_center,sensor_right;
sensor_left= digitalRead(Left_Pin);
sensor_center=digitalRead(Center_Pin);
sensor_right=digitalRead(Right_Pin);
sensorvalue +=sensor_left*4+sensor_center*2+sensor_right;
String senstr= String(sensorvalue,BIN);
return senstr.substring(1,4);
}
/*motor control*/
void go_back() //motor rotate clockwise -->robot go back
{
digitalWrite(IN4,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN1,HIGH);
}
void go_ahead() //motor rotate counterclockwise -->robot go ahead
{
digitalWrite(IN4,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN1,LOW);
}
void stop_Stop() //motor brake -->robot stop
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4,LOW);
set_Motorspeed(0,0);
}
void turn_right() //left motor rotate clockwise and right motor rotate counterclockwise -->robot turn right
{
digitalWrite(IN4,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN2,HIGH);
digitalWrite(IN1,LOW);
set_Motorspeed(SPEED,0);
}
void turn_left() //left motor rotate counterclockwise and right motor rotate clockwise -->robot turn left
{
digitalWrite(IN4,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN2,LOW);
digitalWrite(IN1,HIGH);
set_Motorspeed(0,SPEED);
}
/*set motor speed */
void set_Motorspeed(int lspeed,int rspeed) //change motor speed
{
analogWrite(ENB,lspeed);//lspeed:0-255
analogWrite(ENA,rspeed);//rspeed:0-255
}
void setup() {
/******L298N******/
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(Left_Pin, INPUT);
pinMode(Center_Pin, INPUT);
pinMode(Right_Pin, INPUT);
Serial.begin(9600);
tcs.begin();
//delay(2000);
}
void auto_tracking(){
String sensorval= read_sensor_values();
if (sensorval=="100" or sensorval=="110") {
Serial.println("autotrack: turn right");
turn_right(); //Turn right
set_Motorspeed(SPEED*2/3,SPEED*3/4);
delay(TURN_TIME);
stop_Stop();
delay(50);
}
if (sensorval=="011" or sensorval=="001") {
turn_left(); //Turn left
set_Motorspeed(SPEED*6/8,SPEED*2/3);
delay(TURN_TIME);
stop_Stop();
delay(50);
}
if (sensorval=="010" or sensorval=="111"){
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(MOVE_TIME);
stop_Stop();
delay(50);
}
if (sensorval=="000" or sensorval=="101"){
stop_Stop();
delay(50);
}
}
void color_measurement (uint16_t& r, uint16_t& g, uint16_t& b, double& red_percent, double& green_percent, double& blue_percent, double& color_mag) {
double r_d, g_d, b_d;
double red_back = 13; // Background R Value measured before testing
double green_back = 7; // Background G Value measured before testing
double blue_back = 5; // Background B Value measured before testing
r_d = r*1.0;
r_d = r_d - red_back;
g_d = g*1.0;
g_d = g_d - green_back;
b_d = b*1.0;
b_d = b_d - blue_back;
color_mag = sqrt(sq(r_d) + sq(g_d) + sq(b_d));
red_percent = r_d/color_mag;
green_percent = g_d/color_mag;
blue_percent = b_d/color_mag;
if(isnan(red_percent) || red_percent < 0.0) {
red_percent = 0.0;
}
if(isnan(blue_percent) || blue_percent < 0.0) {
blue_percent = 0.0;
}
if( isnan(green_percent) || green_percent < 0.0) {
green_percent = 0.0;
}
}
void loop(){
String sensorval2=read_sensor_values();
uint16_t r, g, b, c, colorTemp, lux;
double red_percent=0;
double green_percent=0;
double blue_percent=0;
double color_mag;
char input_val;
int direction_input=51;
//int direction_input;
int sens_black = 0;
int i=0;
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
// NO LIGHT
while( color_mag < 10.0 ) {
if(direction_input == -1) {
direction_input = Serial.read();
}
Serial.print("INPUT: ");
Serial.print(direction_input);
Serial.print(" || AUTOTRACKING");
Serial.print(" || NO LIGHT || ");
Serial.print("Color Mag: ");
Serial.print(color_mag);
Serial.print(" , ");
Serial.print("Red: ");
//Serial.print(red_percent);
Serial.print(r);
Serial.print(" , ");
Serial.print("Green: ");
//Serial.print(green_percent);
Serial.print(g);
Serial.print(" , ");
Serial.print("Blue: ");
//Serial.println(blue_percent);
Serial.println(b);
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
auto_tracking();
sensorval2= read_sensor_values();
}
// RED LIGHT
if(red_percent > 0.85 && color_mag > 10) {
sensorval2 = read_sensor_values();
Serial.println(sensorval2);
while (red_percent > 0.85 && (sensorval2 == "010" || sensorval2 == "001" || sensorval2 == "100" || sensorval2 == "011" || sensorval2 == "110")) {
if(direction_input == -1) {
direction_input = Serial.read();
}
Serial.print("INPUT: ");
Serial.print(direction_input);
Serial.print(" || AUTOTRACKING ||");
Serial.print("RED LIGHT || ");
Serial.print("Color Mag: ");
Serial.print(color_mag);
Serial.print(" , ");
Serial.print("Red: ");
Serial.print(red_percent);
Serial.print(" , ");
Serial.print("Green: ");
Serial.print(green_percent);
Serial.print(" , ");
Serial.print("Blue: ");
Serial.println(blue_percent);
auto_tracking();
sensorval2= read_sensor_values();
Serial.println(sensorval2);
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
}
if(red_percent > 0.85 && sensorval2 == "000") {
delay(1);
Serial.println("STOPPING AT RED LIGHT");
stop_Stop();
}
while(red_percent > 0.85) {
if (direction_input < 0) {
direction_input = Serial.read();
}
Serial.print("INPUT: ");
Serial.print(direction_input);
Serial.print(" || WAITING ");
Serial.print(" || ");
Serial.print("RED LIGHT || ");
Serial.print("Color Mag: ");
Serial.print(color_mag);
Serial.print(" , ");
Serial.print("Red: ");
Serial.print(red_percent);
Serial.print(" , ");
Serial.print("Green: ");
Serial.print(green_percent);
Serial.print(" , ");
Serial.print("Blue: ");
Serial.println(blue_percent);
delay(10);
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
}
if( direction_input == 49 && sensorval2 == "000" ) {
Serial.println("LIGHT TURNED GREEN-> GOING LEFT");
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(410);
turn_right(); //Turn left
set_Motorspeed(SPEED*2/3,SPEED*6/8);
delay(230);
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(420);
turn_right(); //Turn left
set_Motorspeed(SPEED*2/3,SPEED*6/8);
delay(100);
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
while (color_mag > 10.0 && i<10) {
Serial.println("SEES GREEN BUT IGNORES AND JUST AUTOTRACKS");
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
i++;
}
} else if ( direction_input == 50 && sensorval2 == "000") {
Serial.println("LIGHT TURNED GREEN-> GOING RIGHT");
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(410);
turn_left(); //Turn left
set_Motorspeed(SPEED*6/8,SPEED*2/3);
delay(230);
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(420);
turn_left(); //Turn left
set_Motorspeed(SPEED*6/8,SPEED*2/3);
delay(100);
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
while (color_mag > 10.0 && i<10) {
Serial.println("SEES GREEN BUT IGNORES AND JUST AUTOTRACKS");
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
i++;
}
} else if ( (direction_input == 51 || direction_input == -1) && sensorval2 == "000") {
Serial.println("LIGHT TURNED GREEN-> GOING STRAIGHT");
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(550);
sensorval2 = read_sensor_values();
}
//GREEN LIGHT
} else if(green_percent > 0.70 && color_mag > 10.0) {
sensorval2 = read_sensor_values();
while (green_percent > 0.7 && (sensorval2 == "010" || sensorval2 == "001" || sensorval2 == "100")) {
if(direction_input == -1) {
direction_input = Serial.read();
}
Serial.print("INPUT: ");
Serial.print(direction_input);
Serial.print(" || AUTOTRACKING ||");
Serial.print("GREEN LIGHT || ");
Serial.print("Color Mag: ");
Serial.print(color_mag);
Serial.print(" , ");
Serial.print("Red: ");
Serial.print(red_percent);
Serial.print(" , ");
Serial.print("Green: ");
Serial.print(green_percent);
Serial.print(" , ");
Serial.print("Blue: ");
Serial.println(blue_percent);
auto_tracking();
sensorval2= read_sensor_values();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
}
//if (green_percent > 0.7) {
if( direction_input == 49 && sensorval2 == "000") {
Serial.println("CONTINUING THROUGH GREEN LIGHT-> turning left");
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(410);
turn_right(); //Turn left
set_Motorspeed(SPEED*2/3,SPEED*6/8);
delay(230);
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(420);
turn_right(); //Turn left
set_Motorspeed(SPEED*2/3,SPEED*6/8);
delay(100);
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
while (color_mag > 10.0 && i<10) {
Serial.println("SEES GREEN BUT IGNORES AND JUST AUTOTRACKS left");
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
i++;
}
} else if ( direction_input == 50 && sensorval2 == "000") {
Serial.println("CONTINUING THROUGH GREEN LIGHT-> turning right");
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(410);
turn_left(); //Turn left
set_Motorspeed(SPEED*6/8,SPEED*2/3);
delay(230);
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(420);
turn_left(); //Turn left
set_Motorspeed(SPEED*6/8,SPEED*2/3);
delay(100);
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
while (color_mag > 10.0 && i < 10) {
Serial.println("SEES GREEN BUT IGNORES AND JUST AUTOTRACKS right");
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
i++;
}
} else if ( (direction_input == 51 || direction_input == -1) && sensorval2 == "000") {
Serial.println("CONTINUING THROUGH GREEN LIGHT-> staying straight");
sensorval2 = read_sensor_values();
go_ahead();
set_Motorspeed(SPEED*4/8,SPEED*4/8);
delay(550);
sensorval2 = read_sensor_values();
}
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
} else {
Serial.print("AUTOTRACKING ||");
Serial.print("Color Mag: ");
Serial.print(color_mag);
Serial.print(" , ");
Serial.print("Red: ");
Serial.print(red_percent);
Serial.print(" , ");
Serial.print("Green: ");
Serial.print(green_percent);
Serial.print(" , ");
Serial.print("Blue: ");
Serial.println(blue_percent);
auto_tracking();
tcs.getRawData(&r, &g, &b, &c);
color_measurement(r, g, b, red_percent, green_percent, blue_percent, color_mag);
}
}
Page updated
Report abuse