arduino: mottion following using 2 ultrasonic sensor
#include <Servo.h>
#include <LiquidCrystal.h>
LiquidCrystal lcd(2,3,4,5,6,7);
Servo myservo;
const int Lin = 10, Rin = 12, Lout = 11, Rout = 13, serv = 9; //setting sensor pins and servo pin
// establish variables for duration
// and the distance result in inches
long Rduration, Lduration, Rinches, Linches;
int threshold = 10; //Sensor threshold in inches
int angle = 80; //Initial angle
boolean debug = false; //Serial communication for debuging. Set to true for serial communication.
void setup() {
// initialize serial communication:
if (debug)
{
lcd.begin(16, 2);
lcd.print(" PEMANTAUAN ");
lcd.setCursor(0, 1);
lcd.print(" J MALING ");
delay(2000);
lcd.clear();
lcd.print("Kondisi=");
}
myservo.attach(9); //attach servo to pin 9
}
void loop()
{
pinMode(Rout, OUTPUT);
digitalWrite(Rout, LOW);
delayMicroseconds(2);
digitalWrite(Rout, HIGH);
delayMicroseconds(5);
digitalWrite(Rout, LOW);
Rduration = pulseIn(Rin, HIGH);
pinMode(Lout, OUTPUT);
digitalWrite(Lout, LOW);
delayMicroseconds(2);
digitalWrite(Lout, HIGH);
delayMicroseconds(5);
digitalWrite(Lout, LOW);
Lduration = pulseIn(Lin, HIGH);
// convert the time into a distance
Rinches = microsecondsToInches(Rduration);
Linches = microsecondsToInches(Lduration);
if (debug)
{
lcd.print("Kiri: ");
lcd.print(Linches);
lcd.print(" in");
lcd.print("Kanan: ");
lcd.print(Rinches);
lcd.print(" in");
}
follow();
}
long microsecondsToInches(long microseconds)
{
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
return microseconds / 74 / 2;
}
void follow()
{
if (Linches <= threshold || Rinches <= threshold)
{
if (Linches + 2 < Rinches)
{
angle = angle - 2;
}
if (Rinches + 2 < Linches)
{
angle = angle + 2;
}
}
if (angle > 160)
{
angle = 160;
}
if (angle < 0)
{
angle = 0;
}
myservo.write(angle);
}
$-)
ReplyDelete