#include <Servo.h>

Servo pendulumServo;

const int servoPin = 11;
const float g = 9.81;       // accelerazione gravitazionale
const float L = 0.2;        // lunghezza del pendolo in metri
const float thetaMax = 40.0; // ampiezza massima in gradi
const int rampDuration = 15000; // durata rampa in millisecondi (30 sec)

unsigned long startTime;

void setup() {
  pendulumServo.attach(servoPin);
  startTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  float t = (currentTime - startTime) / 1000.0; // tempo in secondi

  // Calcolo della frequenza angolare
  float omega = sqrt(g / L);

  // Calcolo dell'ampiezza attuale in base alla rampa
  float rampProgress = min((currentTime - startTime) / (float)rampDuration, 1.0);
  float currentThetaMax = thetaMax * rampProgress;

  // Calcolo dell'angolo del pendolo
  float angle = currentThetaMax * cos(omega * t);

  // Mappatura dell'angolo per il servo (0-180 gradi)
  int servoAngle = map(angle, -thetaMax, thetaMax, 60, 120); // centro a 90°
  pendulumServo.write(servoAngle);

  delay(20); // aggiornamento ogni 20 ms
}