const int in1 = 2;
const int in2 = 3;
const int in3 = 4;
const int in4 = 5;
const int L = 6;
const int U = 7;
const int R = 8;
const int D = 9;
int di;
int posY = 90;
#include <Servo.h>
#include <NewPing.h>
#define T 13
#define E 12
#define MD 50
NewPing sonar(T, E, MD);
Servo Y;
void setup()
{
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(L, INPUT);
pinMode(U, INPUT);
pinMode(R, INPUT);
pinMode(D, INPUT);
Y.attach(10);
}
void left() {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
void right() {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
void forwards() {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
void OFF() {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
void Distance() {
di = sonar.ping_cm();
if(di >= 10 && di <= 50) {
forwards();
}
else {
OFF();
}
}
void loop()
{
if(digitalRead(U) == 1 && digitalRead(D) == 0)
{
while(digitalRead(U) != 0 && digitalRead(D) != 1)
{
if(posY == 50)
{break;}
posY--;
Y.write(posY);
delay(3);
}
}
else if(digitalRead(U) == 0 && digitalRead(D) == 1)
{
while(digitalRead(U) != 1 && digitalRead(D) != 0)
{
if(posY == 160)
{break;}
posY++;
Y.write(posY);
delay(3);
}
}
if(digitalRead(L) == 0 && digitalRead(R) == 1) {
left();
}
else if(digitalRead(L) == 1 && digitalRead(R) == 0) {
right();
}
else {
Distance();
}
}
Comments
Please log in or sign up to comment.