/*
  Controls three servos with different easing settings
  https://wokwi.com/projects/360276061783595009
  by dlloydev, March 2023.

  ⚪ The orange servo moves from 0 to 180 deg at 140 deg/s with sigmoid motion.
*/

#include "pwmWrite.h"

const int servoPin = 10;

// units in degrees per second
float speed = 140.0;

// When easing constant (ke) < 1.0, return value is normalized, when 1.0, returns pulse width (μs)
// ke = 0.0 is linear, between 0.0 and 1.0 is tunable sigmoid, 1.0 is normal response
// Normalized Tunable Sigmoid: https://www.desmos.com/calculator/ejkcwglzd1
float ke = 0.6;

// go to position (degrees)
uint8_t pos = 180;

// duration of travel in ms is degrees to move / speed * 1000
float dur = 180.0 / speed * 1000.0;

float ye;

Pwm pwm = Pwm();

void setup() {
  Serial.begin(115200);
}

void loop() {

  if (ye <= 0.0 ) pos = 180;
  else if (ye >= 1.0) pos = 0;

  ye = pwm.writeServo(servoPin, pos, speed, ke);
  Serial.println(ye);
  delay(6);
}