// NewPing library is included
#include <NewPing.h>
// Here I declare what is connected to each pin on the Arduino
int ENA_LEFT = 22;
int IN1_LEFT = 24;
int IN2_LEFT = 26;
int IN3_LEFT = 28;
int IN4_LEFT = 30;
int ENB_LEFT = 32;
int ENA_RIGHT = 40;
int IN1_RIGHT = 42;
int IN2_RIGHT = 44;
int IN3_RIGHT = 46;
int IN4_RIGHT = 48;
int ENB_RIGHT = 50;
int Distance;
int TRIGGER_PIN = 51;
int ECHO_PIN = 53;
int MAX_DISTANCE = 500;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
// All pins are declared as outputs
// pins of the sensor (TRIGGER AND ECHO) doesn't have to be declared here
void setup() {
pinMode(ENA_LEFT, OUTPUT);
pinMode(IN1_LEFT, OUTPUT);
pinMode(IN2_LEFT, OUTPUT);
pinMode(IN3_LEFT, OUTPUT);
pinMode(IN4_LEFT, OUTPUT);
pinMode(ENB_LEFT, OUTPUT);
pinMode(ENA_RIGHT, OUTPUT);
pinMode(IN1_RIGHT, OUTPUT);
pinMode(IN2_RIGHT, OUTPUT);
pinMode(IN3_RIGHT, OUTPUT);
pinMode(IN4_RIGHT, OUTPUT);
pinMode(ENB_RIGHT, OUTPUT);
}
void loop() {
// The sensor measures de distance and stores in a variable called
// "Distance"
unsigned int uS = sonar.ping();
delay(32);
Distance = sonar.convert_cm(uS);
// If the distance is 57 or more the robot starts rotating in its own axis
// 57 is the distance in cm between a soda can and the sensor
// you can adjust this value if needed
if ((Distance > 57) or (Distance = 0)){
turn();
}
// If the distance is less than 57 cm it moves foward and then backwards
// 900 is the time in miliseconds the motors stay turned on
// while moving foward or backwards you can adjust this value if needed
else{
turn();
delay(50);
stop_();
foward();
delay(900);
stop_();
delay(400);
back();
delay(900);
stop_();
}
}
// functions that are called on voidloop()
void foward(){
digitalWrite(ENA_LEFT, HIGH);
digitalWrite(ENB_LEFT, HIGH);
digitalWrite(IN1_LEFT, HIGH);
digitalWrite(IN2_LEFT, LOW);
digitalWrite(IN3_LEFT, HIGH);
digitalWrite(IN4_LEFT, LOW);
digitalWrite(ENA_RIGHT, HIGH);
digitalWrite(ENB_RIGHT, HIGH);
digitalWrite(IN1_RIGHT, HIGH);
digitalWrite(IN2_RIGHT, LOW);
digitalWrite(IN3_RIGHT, HIGH);
digitalWrite(IN4_RIGHT, LOW);
}
void stop_(){
digitalWrite(ENA_LEFT, LOW);
digitalWrite(ENB_LEFT, LOW);
digitalWrite(ENA_RIGHT, LOW);
digitalWrite(ENB_RIGHT, LOW);
}
void back(){
digitalWrite(ENA_LEFT, HIGH);
digitalWrite(ENB_LEFT, HIGH);
digitalWrite(IN1_LEFT, LOW);
digitalWrite(IN2_LEFT, HIGH);
digitalWrite(IN3_LEFT, LOW);
digitalWrite(IN4_LEFT, HIGH);
digitalWrite(ENA_RIGHT, HIGH);
digitalWrite(ENB_RIGHT, HIGH);
digitalWrite(IN1_RIGHT, LOW);
digitalWrite(IN2_RIGHT, HIGH);
digitalWrite(IN3_RIGHT, LOW);
digitalWrite(IN4_RIGHT, HIGH);
}
void turn(){
digitalWrite(ENB_RIGHT, HIGH);
digitalWrite(ENA_RIGHT, HIGH);
digitalWrite(IN3_RIGHT, HIGH);
digitalWrite(IN4_RIGHT, LOW);
digitalWrite(IN1_RIGHT, HIGH);
digitalWrite(IN2_RIGHT, LOW);
}
Comments