/*
   move two servos combined
   https://forum.arduino.cc/t/mehrere-servos-synchron-ansteuern/1030870/20

   dynamic

   by noiasca
   2022-09-12

   ------------------------------------------------------------------------
   Modifications
   Hardware:
     - rotate upper servo (B)
     - remove switches, add poti
   Software
     - use MobaTools
     - use poti to move
*/

#include <MobaTools.h>

constexpr uint8_t servoAPin = 8;  // GPIO for Servo A
constexpr uint8_t servoBPin = 9;  // GPIO for Servo B
constexpr uint8_t potiPin = A0;   // analog input for poti to control movement

MoToServo servoA;
MoToServo servoB;
constexpr uint8_t servoSpeed = 30; 
constexpr uint8_t servoMin = 0; 
constexpr uint8_t servoMax = 90; 


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

  servoA.attach(servoAPin);
  servoB.attach(servoBPin);
  servoA.setSpeed(servoSpeed);
  servoB.setSpeed(servoSpeed);
  servoA.write(90);
  servoB.write(90);
}


void loop()
{
  // read poti
  uint16_t potiValue = analogRead(potiPin);
  static uint16_t lastPotiValue;

  if (potiValue != lastPotiValue)
  {
    lastPotiValue = potiValue;
    
    // map poti to desired servo movement
    int angleValue = map(potiValue, 0, 1023, servoMin, servoMax);

    // set servos
    servoA.write(angleValue);
    servoB.write(180-angleValue);

    Serial.print(F("Poti    :"));
    Serial.println(potiValue);
    Serial.print(F("Winkel A:"));
    Serial.println(angleValue);
    Serial.print(F("Winkel B:"));
    Serial.println(180-angleValue);
  }
}