#include "HX711.h"
#include <Servo.h>

HX711 scale1; // Load cell 1
HX711 scale2; // Load cell 2
Servo servoMotor;

const int LOADCELL_DOUT_PIN1 = A1;
const int LOADCELL_SCK_PIN1 = A2;
const int LOADCELL_DOUT_PIN2 = A3;
const int LOADCELL_SCK_PIN2 = A4;

void setup() {
  Serial.begin(9600);

  scale1.begin(LOADCELL_DOUT_PIN1, LOADCELL_SCK_PIN1);
  scale2.begin(LOADCELL_DOUT_PIN2, LOADCELL_SCK_PIN2);
  
  servoMotor.attach(9);
  
  scale1.set_scale(); // Menyesuaikan dengan kalibrasi load cell
  scale2.set_scale();
  
  scale1.tare(); // Menyetel titik nol
  scale2.tare();
}

void loop() {
  float weight1 = scale1.get_units(10);
  float weight2 = scale2.get_units(10);

  float d1 = 10.0; // Jarak load cell 1 dari titik referensi
  float d2 = 30.0; // Jarak load cell 2 dari titik referensi

  float totalWeight = weight1 + weight2;
  float CG = (weight1 * d1 + weight2 * d2) / totalWeight;

  Serial.print("CG Position: ");
  Serial.println(CG);

  int servoPosition = map(CG, 0, 40, 0, 180); // Misalnya, skala dari 0 ke 40 cm ke 0 sampai 180 derajat
  servoMotor.write(servoPosition);

  delay(500);
}