Obstacle avoiding with color recognition robot
PART LIST
Mechanical
Components
1) Acrylic sheet(for chassis)
2) Plastic wheels
3) Caster wheels
4) Motor mount heads 5) Nuts and bolts 6) Two-sided tape
7) Hax saw blade
8) Battery holder (4 cell)
9) Hot glue gun
Electronics
Components Part Numbers
1) color recognition sensor TCS3200
2) Arduino board ATMEGA 2560
3) Ultrasonic sensor HC-SR04
4) IR Sensor module
5) DC Motor(200 RPM)
6) H bridge LM298D
7) PCB
8) Battery (9V)
9) Li-ion Battery(7.4v)
10) LCD Display JHD 162A
11) Soldering wire and flux
12) Male and female headers
Here is the Arduino code for the robot according to above specifications
#include <LiquidCrystal.h>
#define trigPin 41
#define echoPin 43
#define S0 6
#define S1 5
#define S2 4
#define S3 3
#define OUT 2
int g_count = 0; // count the frequecy
int g_array[3]; // store the RGB value
int g_flag = 0; // filter of RGB queue
float g_SF[3]; // save the RGB Scale factor
LiquidCrystal lcd(24, 9, 10, 26,11, 28);
const int ir0=45;
const int ir1=47;
const int ir2=49;
const int ir3=51;
const int ir4=53;
const int a1=30;
const int a2=32;
const int a3=34;
const int a4=36;
int s[6];
//motor control
void motion(int l_speed, int r_speed)
{analogWrite(12,l_speed);
analogWrite(13,r_speed);
}
void fwd()
{
digitalWrite(a1, HIGH);digitalWrite(a2, LOW);//lft
digitalWrite(a3, HIGH);digitalWrite(a4, LOW);//rt
motion(200,200);Serial.print(" fwd\n");
}
void rt_turn()
{ digitalWrite(a1, LOW);digitalWrite(a2, HIGH);//lft
digitalWrite(a3,HIGH );digitalWrite(a4, LOW);//rt
motion(255,100);
Serial.print("rt turn\n");
}
void lt_turn()
{
digitalWrite(a1, HIGH);digitalWrite(a2, LOW);//lft
digitalWrite(a3, LOW);digitalWrite(a4, HIGH);//rt
motion(100,255);Serial.print("lt turn\n");
}
void rev()
{
digitalWrite(a1, LOW);digitalWrite(a2, HIGH);//lft
digitalWrite(a3, LOW);digitalWrite(a4, HIGH);//rt
motion(255,255);Serial.print(" rev\n");
}
void stop_()
{
digitalWrite(a1, LOW);digitalWrite(a2, LOW);//lft
digitalWrite(a3, LOW);digitalWrite(a4, LOW);//rt
motion(0,0);Serial.print(" stop\n");
}
//ultrasonic sensor
long us()
{ int duration;
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
return (duration/2) / 29.1;
}
//color sensor
// Init TSC230 and setting Frequency.
void TSC_Init()
{
pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
pinMode(OUT, INPUT);
digitalWrite(S0, LOW); // OUTPUT FREQUENCY SCALING 2%
digitalWrite(S1, HIGH);
}
// Select the filter color
void TSC_FilterColor(int Level01, int Level02)
{
if(Level01 != 0)
Level01 = HIGH;
if(Level02 != 0)
Level02 = HIGH;
digitalWrite(S2, Level01);
digitalWrite(S3, Level02);
}
void TSC_Count()
{
g_count ++ ;
}
void TSC_Callback()
{
switch(g_flag)
{
case 0:
/* Serial.println("->WB Start");*/
TSC_WB(LOW, LOW); //Filter without Red
break;
case 1:
/* Serial.print("->Frequency R=");
Serial.println(g_count);*/
g_array[0] = g_count;
TSC_WB(HIGH, HIGH); //Filter without Green
break;
case 2:
/*Serial.print("->Frequency G=");
Serial.println(g_count);*/
g_array[1] = g_count;
TSC_WB(LOW, HIGH); //Filter without Blue
break;
case 3:
/* Serial.print("->Frequency B=");
Serial.println(g_count);
Serial.println("->WB End");
*/
g_array[2] = g_count;
TSC_WB(HIGH, LOW); //Clear(no filter)
break;
default:
g_count = 0;
break;
}
}
void TSC_WB(int Level0, int Level1) //White Balance
{
g_count = 0;
g_flag ++;
TSC_FilterColor(Level0, Level1);
Timer3.setPeriod(500000); // set 1s period
}
void colour_loop()
{ g_flag = 0;
delay(4000);
if( g_array[0]<25&&g_array[1]<25&&g_array[2]<25)
{ Serial.print("BLACK");
lcd.print("BLACK colour");
}
else if( g_array[0]>125&&g_array[1]>125&&g_array[2]>125)
{ Serial.print("WHITE");
lcd.print("WHITE colour");
}
else if( (g_array[0]>80&&g_array[0]<255)&&g_array[1]>50&&g_array[1]<100&&(g_array[2]>80&&g_array[2]>80)&&(g_array[2]>80&&g_array[2]<149))
{ Serial.print("pink");
lcd.print("PINK colour");
}
else if( g_array[0]>g_array[1]&&g_array[0]>g_array[2])
{Serial.print("RED");
lcd.print("RED colour");
}
else if( g_array[1]>g_array[0]&&g_array[1]>g_array[2])
{Serial.print("GREEN");
lcd.print("GREEN colour");
}
else if( g_array[2]>g_array[0]&&g_array[2]>g_array[1])
{Serial.print("BLUE");
lcd.print("BLUE colour");
}
delay(4000);
}
void setup() {
Serial.begin (9600);
lcd.begin(16, 2);
lcd.print("hello world");
delay(1000);
pinMode(a1,OUTPUT);
pinMode(a2,OUTPUT);
pinMode(a3,OUTPUT);
pinMode(a4,OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
TSC_Init();
Timer3.initialize(); // defaulte is 1s
Timer3.attachInterrupt(TSC_Callback);
attachInterrupt(0, TSC_Count, RISING);
}
void loop()
{int dis=us();
if(dis>0&&dis <3)
s[0]=0;
else
{s[0]=1;}
s[1]=digitalRead(ir0);
s[2]=digitalRead(ir1);
s[3]=digitalRead(ir2);
s[4]=digitalRead(ir3);
s[5]=digitalRead(ir4);
if(((s[0])||(s[3]))&&((!(s[1]))&&(!(s[1]))&&(!(s[0]))&&(!(s[1]))))
{fwd();
}
else if((!s[0])&&(!s[3]))
{stop_();
// delay(10);
colour_loop();
// delay(10);
rev();
delay(500);
stop_();
delay(100);
lt_turn();
delay(500);
}
else if(((s[0])&&(s[1]))||(s[1]))
{lt_turn();
}
else if(((s[1])&&(s[2]))||(s[2]))
{lt_turn();
}
else if((s[0])&&(s[1])&&(s[5]))
{rev();
delay(500);
rt_turn();
}
else if(((s[0])&&(s[5]))||(s[5]))
{
rt_turn();
}
else if(((s[5])&&(s[4]))||(s[4]))
{
rt_turn();
}
else if((s[3])&&(s[4]))
{
rt_turn();
}
else
{fwd();}
lcd.clear();
}
NOTE: Before compiling this code you have to add <TimerThree.h> header file in Arduino IDE.
Thanks :)
No comments:
Post a Comment