/* millisServoV1
   20/04/2024


*/
#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;




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 9 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]);
  }

}

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;
}