Babysitter Autopilot Arduino Source

#include <Math.h>
#include <Servo.h>

int servoPin = 3; // Servo PWM
int ledPin = 13; // Led indicator of pulse
int speedPotPin = A0; // Adjusts speed (frequency/period)
int powerPotPin = A1; // Adjusts power (amplitude)

unsigned long lastTime;
double lastServoTime = 0;
Servo myservo;

void setup() {
  pinMode(ledPin, OUTPUT);
  myservo.attach(servoPin);
  lastTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  unsigned long timePassed = currentTime - lastTime;
  int speedPotValue = analogRead(speedPotPin);
  int powerPotValue = analogRead(powerPotPin);

  int servoSpeed = map(speedPotValue, 0, 1023, 10, 400);
  double powerScale = (double)powerPotValue / 1023.0;
  double servoTime = fmod(lastServoTime + (double)timePassed / 10000.0 * (double)servoSpeed, 2 * M_PI); // Servo time between 0 and 2*pi
  double servoSin = sin(servoTime);
  double servoPosition = (servoSin + 1.0) * 90.0 * powerScale; // Between 0 - 180

  myservo.write((int)servoPosition);
  digitalWrite(ledPin, servoSin > 0 ? HIGH : LOW);

  lastTime = currentTime;
  lastServoTime = servoTime;
  delay(10);
}