Primjena ultrazvučnog senzora kod automobila

Kod - code

#include <Servo.h>
#include <NewPing.h>
const int LeftMotorForward = 7;
const int LeftMotorBackward = 6;
const int RightMotorForward = 4;
const int RightMotorBackward = 5;
#define trig_pin A1
#define echo_pin A2
#define maximum_distance 200
boolean goesForward = false;
int distance = 100;
NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name
void setup(){
pinMode(RightMotorForward, OUTPUT);
pinMode(LeftMotorForward, OUTPUT);
pinMode(LeftMotorBackward, OUTPUT);
pinMode(RightMotorBackward, OUTPUT);
servo_motor.attach(10); //our servo pin
servo_motor.write(90);
delay(1000);
distance = readPing();
delay(100);
//distance = readPing();
//delay(100);
//distance = readPing();
//delay(100);
//distance = readPing();
//delay(100);
}
void loop(){
int distanceRight = 0;
int distanceLeft = 0;
delay(50);
if (distance <= 12){
moveStop();
delay(200);
moveBackward();
delay(100);
moveStop();
delay(200);
distanceRight = lookRight();
delay(1000);
distanceLeft = lookLeft();
delay(1000);
//if (distance <= distanceLeft){
if (distanceRight <= distanceLeft){
moveLeft();
moveStop();
}
else{
moveRight();
moveStop();
}
}
else{
moveForward();
}
distance = readPing();
}

int lookRight(){
servo_motor.write(0);
delay(500);
int distance = readPing();
delay(200);
servo_motor.write(90);
return distance;
}
int lookLeft(){
servo_motor.write(180);
delay(500);
int distance = readPing();
delay(200);
servo_motor.write(90);
return distance;
delay(200);
}
int readPing(){
delay(100);
int cm = sonar.ping_cm();
if (cm==0){
cm=250;
}
return cm;
}
void moveStop(){
digitalWrite(RightMotorForward, LOW);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
digitalWrite(LeftMotorBackward, LOW);
}

void moveForward(){

if(!goesForward){
goesForward=true;
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(RightMotorBackward, LOW);
}
}
void moveBackward(){
goesForward=false;
digitalWrite(LeftMotorForward, LOW);
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorForward, LOW);
digitalWrite(RightMotorBackward, HIGH);
}
void moveRight(){
digitalWrite(LeftMotorForward, LOW);
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(RightMotorBackward, LOW);
delay(700);
}
void moveLeft(){
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorForward, LOW);
digitalWrite(RightMotorBackward, HIGH);
delay(700);
}

Odgovori

Vaša adresa e-pošte neće biti objavljena. Obavezna polja su označena sa *