#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 16
Adafruit_SSD1306 display(OLED_RESET);
#define trigPin 23
#define echoPin 22
#define redLED 18
#define orangeLED 19
#define greenLED 21
#define servoPin 13
#define LOADCELL_DOUT_PIN 35
#define LOADCELL_CLK_PIN 34
#include "HX711.h"
HX711 scale;

long duration;
int distance_cm;
int angle;
int weight;
bool flag = false;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redLED, OUTPUT);
  pinMode(orangeLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  pinMode(servoPin, OUTPUT);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_CLK_PIN);
  scale.set_scale();
}
void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance_cm = duration * 0.034 / 2;
  if (distance_cm < 60) {
    digitalWrite(redLED, HIGH);
    digitalWrite(orangeLED, LOW);
    digitalWrite(greenLED, LOW);
  } else if (distance_cm >= 60 && distance_cm <= 240) {
    digitalWrite(redLED, LOW);
    digitalWrite(orangeLED, HIGH);
    digitalWrite(greenLED, LOW);
  } else if (distance_cm > 240) {
    digitalWrite(redLED, LOW);
    digitalWrite(orangeLED, LOW);
    digitalWrite(greenLED, HIGH);
  }
  angle = map(distance_cm, 60, 240, 0, 180);
  if (flag) {
    weight = scale.get_units();
    Serial.println(weight);
    flag = false;
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("Distance(cm): ");
  display.print(distance_cm);
  display.setCursor(0, 12);
  display.print("Angle: ");
  display.print(angle);
  display.setCursor(0, 24);