#include <LiquidCrystal.h>

LiquidCrystal lcd(PA0, PA1, PA2, PA4, PA11, PA12);


#define dcservo1Pin D9
#define dcservo2Pin D10
#define dcservo3Pin D11



int Switch1 = PC13;
int Switch2 = PC14;

const int minPulseWidth = 500;  
const int maxPulseWidth = 2400;


void setup() {

  lcd.begin(16, 2);
  
  pinMode(Switch1, INPUT_PULLUP);
  pinMode(Switch2, INPUT_PULLUP);
  pinMode(dcservo1Pin, OUTPUT);
  pinMode(dcservo2Pin, OUTPUT);
  pinMode(dcservo3Pin, OUTPUT);


}

void loop() {
  lcd.setCursor(0, 0);  
  lcd.print("-HASIL DETEKSI-");
  

  
  
  if (digitalRead(Switch1) == LOW)
  {
    lcd.setCursor(0, 0);  
    lcd.print("-HASIL DETEKSI-");
    lcd.setCursor(3, 1);
    lcd.print("TRUK ODOL");
    delay(100);

    servoAngle2(0);
    delay(1000);
    servoAngle2(90);

    servoAngle1(0);
    delay(1000);
    servoAngle1(90);
    delay(150);
  }
  if (digitalRead(Switch2) == LOW)
  {
    lcd.setCursor(0, 0);  
    lcd.print("-HASIL DETEKSI-");
    lcd.setCursor(5, 1);
    lcd.print("AMAN");
    delay(100);

    servoAngle2(0);
    servoAngle3(0);
    delay(1000);
    servoAngle2(90);
    delay(100);
    servoAngle3(90);

    servoAngle1(0);
    delay(1000);
    servoAngle1(90);
    delay(150);
  }

 
}
void servoAngle1(int angle) {
  // Calculate the pulse width for the given angle
  int pulseWidth = map(angle, 0, 180, minPulseWidth, maxPulseWidth);
  //map() is a built-in function in Arduino environment.
  //It maps a number from one range to another.


  // Generate the PWM signal (1 ms to 2.4 ms pulse width)
  digitalWrite(dcservo1Pin, HIGH);
  delayMicroseconds(pulseWidth);
  digitalWrite(dcservo1Pin, LOW);
  delay(20 - pulseWidth / 1000);  // Rest of the 20ms period

}
void servoAngle2(int angle) {
  // Calculate the pulse width for the given angle
  int pulseWidth = map(angle, 0, 180, minPulseWidth, maxPulseWidth);
  //map() is a built-in function in Arduino environment.
  //It maps a number from one range to another.


  // Generate the PWM signal (1 ms to 2.4 ms pulse width)
  digitalWrite(dcservo2Pin, HIGH);
  delayMicroseconds(pulseWidth);
  digitalWrite(dcservo2Pin, LOW);
  delay(20 - pulseWidth / 1000);  // Rest of the 20ms period
}
void servoAngle3(int angle) {
  // Calculate the pulse width for the given angle
  int pulseWidth = map(angle, 0, 180, minPulseWidth, maxPulseWidth);
  //map() is a built-in function in Arduino environment.
  //It maps a number from one range to another.


  // Generate the PWM signal (1 ms to 2.4 ms pulse width)
  digitalWrite(dcservo3Pin, HIGH);
  delayMicroseconds(pulseWidth);
  digitalWrite(dcservo3Pin, LOW);
  delay(20 - pulseWidth / 1000);  // Rest of the 20ms period

}