// MADE BY CAKRAYP
// Sensor Ultrasonik untuk mengukur jarak tertentu.

#include <Servo.h>.
#define trigPin 9
#define echoPin 8

// Led RGB
#define led_RED 3
#define led_GREEN 5
#define led_BLUE 4

int antispam = 0;

Servo thisServo;

void setup() {
  Serial.begin(9600);
  Serial.println("<Arduino is ready>");
  Serial.println("Please test the distance with Ulrasonic...");

  // Ultrasonic Sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // LED RGB
  pinMode(led_RED, OUTPUT);   // LED Red
  pinMode(led_GREEN, OUTPUT); // LED Green
  pinMode(led_BLUE, OUTPUT);  // LED Blue

  // Servo
  thisServo.attach(2);  // Pin 2 (Servo dynamo)
}

void loop() {
  long duration, distance;  // Variables

  // Ultrasonic Controls
  digitalWrite(trigPin, LOW);
  digitalWrite(trigPin, HIGH);
  digitalWrite(trigPin, LOW);
  
  // Float distance
  duration = pulseIn(echoPin, HIGH);
  distance = (duration * 0.034) / 2;

  // if (distance != antispam) {
  //   Serial.print(distance);
  //   Serial.println(" CM");
  //   antispam = distance;
  // }

  // Let's go
  if (distance >= 0 && distance <= 100) {
    digitalWrite(led_RED, HIGH);
    digitalWrite(led_GREEN, LOW);
    digitalWrite(led_BLUE, LOW);
    thisServo.write(0);
  } else if (distance >= 100 && distance <= 200) {
    digitalWrite(led_RED, LOW);
    digitalWrite(led_GREEN, HIGH);
    digitalWrite(led_BLUE, LOW);
    thisServo.write(90);
  } else if (distance >= 200 && distance <= 350) {
    digitalWrite(led_RED, LOW);
    digitalWrite(led_GREEN, LOW);
    digitalWrite(led_BLUE, HIGH);
    thisServo.write(180);
  } else {
    digitalWrite(led_RED, LOW);
    digitalWrite(led_GREEN, HIGH);
    digitalWrite(led_BLUE, HIGH);
    // thisServo.write(0);

    // Servo berputar Mulai dari 180 derajat.
    int degress_Servo = 180;
    ServoRepeat(degress_Servo);
  }

  // You can use "delay(<time_miliseconds>)"
  // delay(1000);
}

void ServoRepeat(int deg) {
  int degress_Servo = deg;

  // Servo Berputar mulai dari 0 sampai 180
  for (int i = 0; i <= degress_Servo; i++) {
    thisServo.write(i);
    delay(10);
  }

  // Servo Berputar mulai 180 sampai 0
  for (int i = degress_Servo; i >= 0; i--) {
    thisServo.write(i);
    delay(10);
  }
}