#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);
 
}

1 comments:

 
Cyber Code © 2014. All Rights Reserved. Powered by Blogger
Top