majdku
3
const int trigPin = 2;
const int echoPin = 4;
const int LEDR = 13;
const int LEDG = 12;
long duration, distance, inches, cm;
const int MaxDistance = 15;
const int In1 = 3;
const int In2 = 5;
const int In3 = 6;
const int In4 = 11;
#include <Servo.h>;
Servo myServo;
int pos = 0;
void setup()
{
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(LEDR,OUTPUT);
pinMode(LEDG,OUTPUT);
pinMode(In1, OUTPUT);
pinMode(In2, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In4, OUTPUT);
myServo.attach(8);
}
void loop()
{
{
int speed1;
int speed2;
int speed3;
speed1 = 150;
speed2 = 180;
speed3 = 158;
while(1==1)
{
ping();
distance = cm;
if (distance < MaxDistance)
{
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, LOW);
analogWrite(In4, 0);
analogWrite(In3, 0);
analogWrite(In2, 0);
analogWrite(In1, 0);
delay(100);
analogWrite(In4, speed3);
analogWrite(In3, 0);
analogWrite(In2, speed1);
analogWrite(In1, 0);
delay(999);
analogWrite(In4, 0);
analogWrite(In3, 0);
analogWrite(In2, speed2);
analogWrite(In1, 0);
delay(600);
}
else
{
for (pos = 0; pos <= 180; pos += 1)
{
myservo.write(pos);
delay(15);
}
for (pos = 180; pos >= 0; pos -= 1)
{
myservo.write(pos);
delay(15);
}
digitalWrite(LEDG, HIGH);
digitalWrite(LEDR, LOW);
analogWrite(In4, 0);
analogWrite(In3, speed3);
analogWrite(In2, 0);
analogWrite(In1, speed1);
}
Serial.print(inches);
Serial.print("in, ");
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(10);
}
}
}
void ping ()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
inches = duration / 74 / 2;
cm = duration / 29 /2;
return;
}