//  Control 3 servos independently with momentary push buttons
// https://forum.arduino.cc/t/control-3-servos-independently-with-momentary-push-buttons/1438199

#include  <MobaTools.h>
#define LED_BUILTIN_PACE 200

// code written to demonstrate managing N servos with left and right pushbuttons, and varying endpoints.
// limitations - as presently arranged, min position must always be left, and must always be less than max position, else all will collapse in a heap.
// changed using MobaTools by @MicroBahner - the limitation about min/max is no longer valid.
// so the names should be changed to leftPos and rightPos ;-)
struct mystruct {   // defines a structure for all data relating to one servo
  int minpos;       // min value for servo
  int maxpos;       // max value for servo
  int leftb;        // left button index
  int rightb;       // right button index
  int servop;       // pin for servo
  MoToServo myservo; // reserves space for the data needed to manage a servo
}; // end mystruct

const byte buttonPins[] = {2, 3, 4, 5, 6, 7};
MoToButtons ctrlButtons(buttonPins, sizeof(buttonPins), 20, 500 );

mystruct servos[] = { // creates 3 instances of that structure in an array
  {110, 70, 0, 1, 8}, // Note that min pos is larger than max pos => servo will rotate the opposite direction compared to others
  {60, 120, 2, 3, 9},
  {0, 180, 4, 5, 10}, // N.B. compiler generates a warning about an uninitialized component; ignore this
};
const int servoNum = sizeof(servos) / sizeof(servos[0]);   // create constant indicating number of servos managed
const int movement = 4000; // time to move from 0 to 180°

void setup() {
  Serial.begin(115200);  // sets up serial for Serial Monitor use, reporting
  pinMode(LED_BUILTIN, OUTPUT);
  for (int index = 0; index < servoNum; index++) {     // manage all setup for each servo sequentially
    servos[index].myservo.attach(servos[index].servop,600,2400);// attach         (begins updates for servo)
    servos[index].myservo.write(90); // write middle position (presets servo appropriately)
    servos[index].myservo.setSpeedTime(movement); // set speed of servo
  } // end setup loop
  while (!Serial) {}
  Serial.println("System Ready");
} // end setup

void loop() {
  static uint32_t nextTime = 0;
  uint32_t currentTime = millis();
  if(nextTime < currentTime) {
    nextTime += LED_BUILTIN_PACE;
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
  ctrlButtons.processButtons();
  for (int index = 0; index < servoNum; index++) {  // check, update all servos sequentially
    if (ctrlButtons.pressed(servos[index].leftb) ) {
      servos[index].myservo.write(servos[index].minpos);    // write position
      Serial.print("Servo "); Serial.print( index+1 );
      Serial.print(" left at "); Serial.println(servos[index].myservo.read());
    }
    if (ctrlButtons.pressed(servos[index].rightb) ) {
      servos[index].myservo.write(servos[index].maxpos);    // write position
      Serial.print("Servo "); Serial.print( index+1 );
      Serial.print(" right at "); Serial.println(servos[index].myservo.read());
    }
    if (ctrlButtons.released(servos[index].leftb) || ctrlButtons.released(servos[index].rightb) ) {
      servos[index].myservo.stop();
      Serial.print("Servo "); Serial.print( index+1 );
      Serial.print(" stopped at "); Serial.println(servos[index].myservo.read());
    }
   Serial.flush();
  } // end servo update loop
}   // end loop