/* millisServoV2
   20/04/2024

   This version is controlling 2 servo on independent timings


*/
#include <Servo.h>

Servo servo1;  // create servo object to control a servo
//This will store the data for servo1
unsigned int servo1Data[3];//moveTo, currentPos, speed
unsigned long servo1millis;

Servo servo2;  // create servo object to control a servo
//This will store the data for servo1
unsigned int servo2Data[3];//moveTo, currentPos, speed
unsigned long servo2millis;

Servo servo3;  // create servo object to control a servo
//This will store the data for servo1
unsigned int servo3Data[3];//moveTo, currentPos, speed
unsigned long servo3millis;




unsigned long currentMillis;//Holds the current board time in milliseconds

void setup() {
  Serial.begin(9600);
  Serial.println("millisServoV1");
  servo1.attach(8);  // attaches the servo on pin 8 to the servo object
  servo2.attach(9);  // attaches the servo on pin 9 to the servo object
  servo3.attach(10);  // attaches the servo on pin 10 to the servo object
}

void loop() {
  int servoFinished;
  currentMillis = millis();

  servoFinished = servo1Control();//controls servo 1
  if(servoFinished < 1){//If 0 is returned the servo has reached it's destination
    servo1Data[0] = random(179); //pick a random angle between 0 - 179
    servo1Data[2] = random(1,100); // pick a random speed between 1 and 100...speed in milliseconds per degree moved
    Serial.print("Servo 1 Next Pos: ");
    Serial.print(servo1Data[0]);
    Serial.print(" Servo 1 Next Speed: ");
    Serial.println(servo1Data[2]);
  }

  servoFinished = servo2Control();//controls servo 2
  if(servoFinished < 1){//If 0 is returned the servo has reached it's destination
    
    if(servo2Data[0] > 0){
      servo2Data[0] = 0;
    }else{
      servo2Data[0] = 179;
    }
    servo2Data[2] = 30; // speed
    Serial.print("Servo 2 Next Pos: ");
    Serial.print(servo2Data[0]);
    Serial.print(" Servo 2 Next Speed: ");
    Serial.println(servo2Data[2]);
  }

  servoFinished = servo3Control();//controls servo 3
  if(servoFinished < 1){//If 0 is returned the servo has reached it's destination
    
    if(servo3Data[0] > 0){
      servo3Data[0] = 0;
    }else{
      servo3Data[0] = 100;
    }
    servo3Data[2] = 10; // speed
    Serial.print("Servo 3 Next Pos: ");
    Serial.print(servo3Data[0]);
    Serial.print(" Servo 3 Next Speed: ");
    Serial.println(servo3Data[2]);
  }

  

}

int servo1Control(){
  int returnPos;
  returnPos = 0;//return 0 if reached destination
  if(servo1Data[0] > 179){
    servo1Data[0] = 179;//limit check in case soeone tried to move servo more than 179 degrees  
  }
  if(servo1Data[0] != servo1Data[1]){//is there still a distance to move
    if(currentMillis - servo1millis >= servo1Data[2]){//is it time to move
      servo1millis = currentMillis;
      if(servo1Data[0] > servo1Data[1]){
       servo1Data[1]++;
      }else{
       servo1Data[1]--;
      }
      servo1.write(servo1Data[1]);
      
    }
    returnPos = 1;//return 1 if still moving
  }
  return returnPos;
}

int servo2Control(){
  int returnPos;
  returnPos = 0;//return 0 if reached destination
  if(servo2Data[0] > 179){
    servo2Data[0] = 179;//limit check in case soeone tried to move servo more than 179 degrees  
  }
  if(servo2Data[0] != servo2Data[1]){//is there still a distance to move
    if(currentMillis - servo2millis >= servo2Data[2]){//is it time to move
      servo2millis = currentMillis;
      if(servo2Data[0] > servo2Data[1]){
       servo2Data[1]++;
      }else{
       servo2Data[1]--;
      }
      servo2.write(servo2Data[1]);
      
    }
    returnPos = 1;//return 1 if still moving
  }
  return returnPos;
}

int servo3Control(){
  int returnPos;
  returnPos = 0;//return 0 if reached destination
  if(servo3Data[0] > 179){
    servo3Data[0] = 179;//limit check in case soeone tried to move servo more than 179 degrees  
  }
  if(servo3Data[0] != servo3Data[1]){//is there still a distance to move
    if(currentMillis - servo3millis >= servo2Data[2]){//is it time to move
      servo3millis = currentMillis;
      if(servo3Data[0] > servo3Data[1]){
       servo3Data[1]++;
      }else{
       servo3Data[1]--;
      }
      servo3.write(servo3Data[1]);
    }
    returnPos = 1;//return 1 if still moving
  }
  return returnPos;
}