/*
  Tests three continuous SG90 servos
  // Datasheet: https://datasheetspdf.com/pdf/791970/TowerPro/SG90/1
  // Example: https://protosupplies.com/product/servo-motor-micro-sg90-360-degree-continuous-rotation/
*/

#include <pwmWrite.h>

const int servoPin1 = 18;
const int servoPin2 = 19;
const int servoPin3 = 21;

Pwm pwm = Pwm();

void setup() {
  pwm.attach(servoPin1, 0, 1000, 2000);
  pwm.attach(servoPin2, 1, 1000, 2000);
  pwm.attach(servoPin3, 2, 1000, 2000);
}

void loop() {

  //Testing servo1 ...
  pwm.writeServo(servoPin1, 0); //Spin in one direction
  delay(2000);
  pwm.writeServo(servoPin1, 90); // Stop
  delay(500);
  pwm.writeServo(servoPin1, 180); // Spin in opposite direction
  delay(2000);
  pwm.writeServo(servoPin1, 90); // Stop
  delay(500);

  //Testing servo2 ...
  pwm.writeServo(servoPin2, 0); //Spin in one direction
  delay(2000);
  pwm.writeServo(servoPin2, 90); // Stop
  delay(500);
  pwm.writeServo(servoPin2, 180); // Spin in opposite direction
  delay(2000);
  pwm.writeServo(servoPin2, 90); // Stop
  delay(500);

  //Testing servo3 ...
  pwm.writeServo(servoPin3, 0); //Spin in one direction
  delay(2000);
  pwm.writeServo(servoPin3, 90); // Stop
  delay(500);
  pwm.writeServo(servoPin3, 180); // Spin in opposite direction
  delay(2000);
  pwm.writeServo(servoPin3, 90); // Stop
  delay(500);

  //Testing all servos ...
  pwm.writeServo(servoPin1, 0); //Spin in one direction
  pwm.writeServo(servoPin2, 0); //Spin in one direction
  pwm.writeServo(servoPin3, 0); //Spin in one direction
  delay(2000);
  pwm.writeServo(servoPin1, 90); // Stop
  pwm.writeServo(servoPin2, 90); // Stop
  pwm.writeServo(servoPin3, 90); // Stop
  delay(500);
  pwm.writeServo(servoPin1, 180); // Spin in opposite direction
  pwm.writeServo(servoPin2, 180); // Spin in opposite direction
  pwm.writeServo(servoPin3, 180); // Spin in opposite direction
  delay(2000);
  pwm.writeServo(servoPin1, 90); // Stop
  pwm.writeServo(servoPin2, 90); // Stop
  pwm.writeServo(servoPin3, 90); // Stop
  delay(500);
}