This robot consists in pick up different pieces in different places and bring it back to the line production depending on the assembler command. Also, it is going to be a line following robot with an ultrasonic sensor and an emergency stop button to provide safe during the process.

April 26, 2014

CODE

#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

}
at
Labels:

No comments:

Post a Comment

Subscribe to: Post Comments (Atom)