// multiple servo presets - Doug Hoffman 08/01/2022

// twelve servo objects can be created on most boards
#include <Servo.h> // include the Servo library

// create servo objects to control a servo
Servo myServo1;
Servo myServo2;
Servo myServo3;
Servo myServo4;
Servo myServo5;

int AnalogPin0 = A0; // Analog Button pin number
int ButtonValue = 0; // Analog Value from button press

// analog readings - less than values for each preset
int Analog0 = 50; // loop - do nothing
int Analog1 = 100;
int Analog2 = 150;
int Analog3 = 200;
int Analog4 = 250;
int Analog5 = 350;
int Analog6 = 550;
int Analog7 = 1050;

// variables to store the servo Pre set positions
int Servo1 = 0; // servo 1
int Servo2 = 0; // servo 2
int Servo3 = 0; // servo 3
int Servo4 = 0; // servo 4
int Servo5 = 0; // servo 5

// presets hold the values for all servos
String  Preset0 = "500,500,500,500,500";
String  Preset1 = "900,600,1300,600,1000";
String  Preset2 = "1300,1000,800,600,1100";
String  Preset3 = "1700,1400,900,700,1200";
String  Preset4 = "2000,1700,1000,900,1300";
String  Preset5 = "2200,2000,600,800,900";
String  Preset6 = "2500,2500,2500,2500,2500";

String ServoValues = "";
String StringArray[9];
int StringCount = 0;




void setup()
{
  Serial.begin(115200); // open serial port

  // attaches the Arduino pins to the servo object
  myServo1.attach(9);
  myServo2.attach(10);
  myServo3.attach(11);
  myServo4.attach(12);
  myServo5.attach(13);

  // Servos go to zero on start up
  ServoValues = Preset0; // go to zero
  SplitStringsSub();
  MoveServosSub();
  Serial.println("Servo's set to zero");
  
} // end setup





void loop()
{

  // Pre Set buttons
  ButtonValue = analogRead(AnalogPin0); // Read the analogue input
  //Serial.println(ButtonValue);

  //Move servo's and display the values in the Serial monitor
  if (ButtonValue < Analog0)
  {
    // do nothing-no buttons pressed
  }

  else if (ButtonValue < Analog1)
  {
    ServoValues = Preset0;
    SplitStringsSub();
    MoveServosSub();
  }

  else if (ButtonValue < Analog2)
  {
    ServoValues = Preset1;
    SplitStringsSub();
    MoveServosSub();
  }

  else if (ButtonValue < Analog3)
  {
    ServoValues = Preset2;
    SplitStringsSub();
    MoveServosSub();
  }

  else if (ButtonValue < Analog4)
  {
    ServoValues = Preset3;
    SplitStringsSub();
    MoveServosSub();
  }

  else if (ButtonValue < Analog5)
  {
    ServoValues = Preset4;
    SplitStringsSub();
    MoveServosSub();
  }

  else if (ButtonValue < Analog6)
  {
    ServoValues = Preset5;
    SplitStringsSub();
    MoveServosSub();
  }

  else if (ButtonValue < Analog7)
  {
    ServoValues = Preset6;
    SplitStringsSub();
    MoveServosSub();
  }

  else
  {
    // do nothing
  }

  delay(100); //Delay for stability
} // end loop




// Move servos
void MoveServosSub()
{
  Serial.print("Analog Value=");
  Serial.println(ButtonValue);

  myServo1.writeMicroseconds(Servo1);
  Serial.print("Servo 1=");
  Serial.println(myServo1.readMicroseconds());

  myServo2.writeMicroseconds(Servo2);
  Serial.print("Servo 2=");
  Serial.println(myServo2.readMicroseconds());

  myServo3.writeMicroseconds(Servo3);
  Serial.print("Servo 3=");
  Serial.println(myServo3.readMicroseconds());

  myServo4.writeMicroseconds(Servo4);
  Serial.print("Servo 4=");
  Serial.println(myServo4.readMicroseconds());

  myServo5.writeMicroseconds(Servo5);
  Serial.print("Servo 5=");
  Serial.println(myServo5.readMicroseconds());
  
  Serial.println("------------------");
} // end move servos




void SplitStringsSub()
{
  StringCount = 0;

  // Split the string into substrings
  // example string "900,600,1300,600,1000"
  while (ServoValues.length() > 0)
  {
    int index = ServoValues.indexOf(','); // split string at commas

    if (index == -1) // No comma found
    {
      StringArray[StringCount++] = ServoValues;
      break;
    }

    else
    {
      StringArray[StringCount++] = ServoValues.substring(0, index);
      ServoValues = ServoValues.substring(index + 1);
    }
  } // end while



  // Show the resulting substrings
  for (int i = 0; i < StringCount; i++)
  {
    //    Serial.print(i);
    //    Serial.print("-");
    //    Serial.println(StringArray[i]);
  }

  // convert strings to integers
  Servo1 = StringArray[0].toInt();
  Servo2 = StringArray[1].toInt();
  Servo3 = StringArray[2].toInt();
  Servo4 = StringArray[3].toInt();
  Servo5 = StringArray[4].toInt();

} // end split strings