#include "Ultrasonic.h"
#include<Servo.h>
#include<FastIO.h>
#include<I2CIO.h>
#include<LCD.h>
#include<LiquidCrystal.h>
#include<LiquidCrystal_I2C.h>
#include<LiquidCrystal_SR.h>
#include<LiquidCrystal_SR2W.h>
#include<LiquidCrystal_SR3W.h>
#include<Wire.h>
#define I2C_ADDR 0x3F // Define I2C Address where the PCF8574A is
#define BACKLIGHT_PIN 3
#defineEn_pin 2
#defineRw_pin 1
#defineRs_pin 0
#define D4_pin 4
#define D5_pin 5
#define D6_pin 6
#define D7_pin 7
int n = 1;
LiquidCrystal_I2C
lcd(I2C_ADDR,En_pin,Rw_pin,Rs_pin,D4_pin,D5_pin,D6_pin,D7_pin);
Servo myservo;
//#defineleftPin = tag1
//#definerightPin = tag10
intpos = 0;
intdelayPeriod = 1000; //
increasing this slows down the servo movement
intRFIDResetPin = 13;
int distance = 0;
int detector = 0;
Ultrasonic ultrasonic (10,12);
constint IN1=3;
constint IN2=5;
constint IN3=6;
constint IN4=11;
constintirLeft = 2; //Left line sensor is on pin A2
constintirRight = 3; //Right line sensor is on pin A3
constint buttonPin1 = 2;// the number of the pushbutton pin
constint buttonPin2 = 4;
constint buttonPin3 = 8;
constint buttonPin4 = 9;
int buttonState1 = 0;
int buttonState2 = 0;
int buttonState3 = 0;
int buttonState4 = 0;
int aux_pin1;
int aux_pin2;
int aux_pin3;
int aux_pin4;
intaux_pin_choose;
boolean Rotate=true;
//Register your RFID tags here
char tag1[13] =
"FE0070368D35";
char tag2[13] =
"FE0070E21975";
char tag3[13] =
"FE007086ABA3";
char tag4[13] =
"FE0070E21975";
char tag5[13] =
"FE00714637FE";
char tag6[13] =
"01023C000E31";
char tag7[13] =
"01023C0A3207";
char tag8[13] =
"FE007145E329";
char tag9[13] = "FE0070CCA8EA";
char tag10[13] = "FE0071564D94";
void setup()
{
lcd.begin (20,4);
// Switch on the backlight
lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
lcd.setBacklight(HIGH);
lcd.home (); //
go home
lcd.print("CargoRobot");
lcd.setCursor ( 0, 1 ); //
go to the 2nd line
lcd.print("Version 1.0");
lcd.setCursor ( 0, 2 ); //
go to the third line
lcd.print("Test & demonstration");
lcd.setCursor ( 0, 3 ); //
go to the fourth line
lcd.print("...: ");
delay(5000);
myservo.attach(7); // attaches
the servo on pin 9 to the servo object
myservo.write(pos); // center the servo
// initialize the pushbutton
pin as an input:
pinMode(buttonPin1, INPUT);
pinMode(buttonPin2, INPUT);
pinMode(buttonPin3, INPUT);
Serial.begin(9600);
pinMode(RFIDResetPin, OUTPUT);
digitalWrite(RFIDResetPin, HIGH);
//ONLY NEEDED IF CONTROLING
THESE PINS - EG. LEDs
// pinMode(2, OUTPUT);
//pinMode(23, OUTPUT);
//pinMode(4, OUTPUT);
//pinMode(25, OUTPUT);
//pinMode(26, OUTPUT);
//pinMode(27, OUTPUT);
//pinMode(8, OUTPUT);
//pinMode(29, OUTPUT);
//pinMode(30, OUTPUT);
//pinMode(31, OUTPUT);
// fIRST , define the Motor's
pin as an OUTPUT
pinMode( IN1 ,OUTPUT);// Right Motor 1st wire
pinMode( IN2 ,OUTPUT);// Right Motor 2nd wire
pinMode( IN3 ,OUTPUT);// left Motor 1st wire
pinMode( IN4 ,OUTPUT);// left Motor 2nd wire
//Here we set up variable that will hold the ADC value representing the
line sensor values
intleftSees = 0; //A2 ADC value (0 to 1023)
intrightSees = 0; //A3 ADC value (0 to 1023)
}
void forward(int Speed){
//When we want to let Motor To
Rotate clock wise
// just void this part on the
loop section .
analogWrite(IN1,0);
analogWrite(IN2,Speed);
analogWrite(IN3,Speed);
analogWrite(IN4,0);
}
void backward(int Speed){
//When we want to let Motor To
Rotate Counter clock wise
// just void this part on the
loop section .
analogWrite(IN1,Speed);
analogWrite(IN2,0);
analogWrite(IN3,0);
analogWrite(IN4,Speed);
}
void left(int Speed){
//When we want to let Motor To
Rotate Counter clock wise
// just void this part on the
loop section .
analogWrite(IN1,0);
analogWrite(IN2,Speed);
analogWrite(IN3,0);
analogWrite(IN4,Speed);
}
void right(int Speed){
//When we want to let Motor To
Rotate Counter clock wise
// just void this part on the
loop section .
analogWrite(IN1,Speed);
analogWrite(IN2,0);
analogWrite(IN3,Speed);
analogWrite(IN4,0);
}
void Stop(){
//When we want to let Motor To
Rotate clock wise
// just void this part on the
loop section
analogWrite(IN1,0);
analogWrite(IN2,0);
analogWrite(IN3,0);
analogWrite(IN4,0);
}
void loop(){
//delay(5000);
//Start by reading the left
sensor on A2
intleftEye = analogRead(irLeft);
//next read the right sensor
connected A3
intrightEye = analogRead(irRight);
//delay a little bit
delay(5);
distance=ultrasonic.Ranging(CM);
delay(20);
if (distance<=20){
Stop();
delay(20);
Serial.println("Sai da frente");
delay(150);
lcd.clear();
lcd.print("Get out the way");
lcd.setCursor ( 0, 2 );
}
//Next, we run the motors based
on the sensor reading
//If both sensors see black
(ADC value greater than 1000), then back up
//else if ((leftEye>=
500)&&(rightEye>= 500)) right(105);
//Otherwise, if only the left
sensor sees black, then turn off the left motor
//so the robot veer to the left
else if ((leftEye>= 500)&&(rightEye< 500)) left(80);
//Otherwise, if only the right
sensor sees black, then turn off the right motor
//so the robot veer to the
right
else if ((leftEye< 500)&&(rightEye>= 50)) right(80);
//Otherwise, move forward
else forward(75);
lcd.clear();
lcd.print("forward");
lcd.setCursor ( 0, 2 );
chartagString[13];
int index = 0;
boolean reading = false;
while(Serial.available()){
intreadByte = Serial.read(); //read next available byte
if(readByte == 2) reading = true; //begining of tag
if(readByte == 3) reading = false; //end of tag
if(reading &&readByte != 2 &&readByte != 10
&&readByte != 13){
//store the tag
tagString[index] = readByte;
index ++;
}
}
checkTag(tagString); //Check if it is a match
clearTag(tagString); //Clear the char of all value
resetReader(); //eset the RFID reader
}
voidcheckTag(char tag[]){
///////////////////////////////////
//Check the read tag against known tags
///////////////////////////////////
buttonState1 = digitalRead(buttonPin1);
buttonState2 = digitalRead(buttonPin2);
buttonState3 = digitalRead(buttonPin3);
buttonState4 = digitalRead(buttonPin4);
if (buttonState4 == LOW)
{
Serial.println("#4");
lcd.clear();
lcd.print("Emergency!!");
aux_pin4=1;
lcd.setCursor ( 0, 2 );
Stop();
}
if(strlen(tag) == 0) return; //empty, no need to contunue
if(compareTag(tag, tag1))
{
Serial.println(tag);
forward(90);
delay(500);
Stop();
// lcd.clear();
lcd.print("waiting comand");
lcd.setCursor ( 0, 1 );
if( pos> 0) pos = 0;
myservo.write(pos); // tell
servo to go to position in variable 'pos'
delay(delayPeriod);
aux_pin1=0;
aux_pin2=0;
aux_pin3=0;
while(1)
{
buttonState1 = digitalRead(buttonPin1);
buttonState2 = digitalRead(buttonPin2);
buttonState3 = digitalRead(buttonPin3);
if (buttonState1 == LOW)
{
Serial.println("#1");
lcd.clear();
lcd.print("Going to #1");
aux_pin1=1;
lcd.setCursor ( 0, 2 );
// delay(2000);
break;
}
else if (buttonState2 == LOW)
{
Serial.println("#2");
lcd.clear();
lcd.print("Going to #2");
aux_pin2=1;
lcd.setCursor ( 0, 2
);
// delay(1000);
break;
}
else if (buttonState3 == LOW)
{
Serial.println("#3");
lcd.clear();
lcd.print("Going to #3");
aux_pin3=1;
lcd.setCursor ( 0, 2
);
// delay(2000);
break;
}
}
}
// buttonState1 =
digitalRead(buttonPin1);
// if (buttonState1 == HIGH)
else if((compareTag(tag, tag3))&& (aux_pin1 == 1))
{
Serial.println("#1");
lcd.clear();
lcd.print("This is #1");
lcd.setCursor ( 0, 2 );
//delay(2000);
left(100);
while(1)
{
intleftEye = analogRead(irLeft);
//next read the right sensor
connected A3
intrightEye = analogRead(irRight);
//delay a little bit
delay(5);
(leftEye>= 500);
break;
// delay(2000);
}
}
else if ((compareTag(tag, tag4))&& (aux_pin2 == 1))
{
Serial.println("#2");
lcd.clear();
lcd.print("This is #2");
lcd.setCursor ( 0, 2 );
//delay(2000);
left(100);
while(1)
{
intleftEye = analogRead(irLeft);
//next read the right sensor
connected A3
intrightEye = analogRead(irRight);
//delay a little bit
delay(5);
(leftEye>= 500);
break;
// delay(2000);
}
}else if ((compareTag(tag, tag5))&& (aux_pin3 == 1))
{
Serial.println("#3");
lcd.clear();
lcd.print("Going to #3");
lcd.setCursor ( 0, 2 );
//delay(2000);
left(100);
while(1)
{
intleftEye = analogRead(irLeft);
//next read the right sensor
connected A3
intrightEye = analogRead(irRight);
//delay a little bit
delay(5);
(leftEye>= 500);
break;
//delay(2000);
}
}
else if (compareTag(tag, tag6)){
lightLED(27);
}else if(compareTag(tag, tag7)){
lightLED(28);
}else if(compareTag(tag, tag8))
{
//delay(500);
Serial.println("#1");
right(120);
// while(1)
//
// {
//
// intleftEye =
analogRead(irLeft);
//
// //next read the right sensor connected A3
// intrightEye =
analogRead(irRight);
//
// //delay a little bit
// delay(5);
// if(rightEye>= 500)
// break;
// delay(2000);
//
// }
delay(2000);
if( pos< 180)
pos = 100;
myservo.write(pos); // tell
servo to go to position in variable 'pos'
delay(delayPeriod);
lcd.clear();
lcd.print("tolling");
delay(1000);
}
else if(compareTag(tag, tag9))
{
Stop();
Serial.println("#2");
right(120);
while(1)
{
intleftEye = analogRead(irLeft);
//next read the right sensor
connected A3
intrightEye = analogRead(irRight);
//delay a little bit
delay(5);
(rightEye>= 500);
break;
delay(2000);
}
delay(2000);
if( pos< 180)
pos = 100;
myservo.write(pos); // tell
servo to go to position in variable 'pos'
delay(delayPeriod);
lcd.clear();
lcd.print("tolling");
delay(1000);
}
else if(compareTag(tag, tag10))
{
Stop();
Serial.println("#3");
delay(1000);
right(120);
while(1)
{
intleftEye = analogRead(irLeft);
//next read the right sensor
connected A3
intrightEye = analogRead(irRight);
//delay a little bit
delay(5);
(rightEye>= 500);
break;
delay(2000);
}
delay(2000);
if( pos< 180)
pos = 100;
myservo.write(pos); // tell
servo to go to position in variable 'pos'
delay(delayPeriod);
lcd.clear();
lcd.print("tolling");
delay(1000);
}
else if (Serial.println(tag)); //read out any unknown tag
}
voidlightLED(int pin){
///////////////////////////////////
//Turn on LED on pin "pin" for 250ms
///////////////////////////////////
Serial.println(pin);
digitalWrite(pin, HIGH);
delay(250);
digitalWrite(pin, LOW);
}
voidresetReader(){
///////////////////////////////////
//Reset the RFID reader to read again.
///////////////////////////////////
digitalWrite(RFIDResetPin, LOW);
digitalWrite(RFIDResetPin, HIGH);
delay(150);
}
voidclearTag(char one[]){
///////////////////////////////////
//clear the char array by filling with null - ASCII 0
//Will think same tag has been read otherwise
///////////////////////////////////
for(inti = 0; i<strlen(one); i++){
one[i] = 0;
}
}
booleancompareTag(char one[], char two[]){
///////////////////////////////////
//compare two value to see if same,
//strcmp not working 100% so we do this
///////////////////////////////////
if(strlen(one) == 0) return false; //empty
for(inti = 0; i< 12; i++){
if(one[i] != two[i]) return false;
}
return true; //no mismatches
}
