/* PCA9685MillisV1
    25/04/2024

  Control PCA9685 Servo movement with non blocking code.

  Arduino UNo
  Pins

  Arduino UNO: A4 (SDA), A5 (SCL)

*/

#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"

//Create Adafruit_PWMServoDriver object: servoBoard
Adafruit_PWMServoDriver servoBoard = Adafruit_PWMServoDriver();

//settings for PCA9685 defaults for servos
#define USMIN 600      // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400     // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50  // Analog servos run at ~50 Hz updates

unsigned long currentMillis;  //stores current board time in milliseconds
unsigned long resetMillis;    // keeps track of next time to check if servos have finished moving
int resetTimer = 2000;        //checks every 2 seconds

unsigned long servoMillis[16];

//definitions to make servoData[] easier to work with
#define SERVOTARGET 0
#define SERVOCURRENT 1
#define SERVOSPEED 2

unsigned int servoData[16][3];  //target angle, current angle, speed

void setup() {
  Serial.begin(9600);
  Serial.println("PCA9685MillisV1");

  servoBoard.begin();
  servoBoard.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
}

void loop() {

  currentMillis = millis();
  servoControl();  //servo control function


  //This code just checks if the servo has reached it's destination
  //If it has it sets a random destination and speed.
  if (currentMillis - resetMillis > resetTimer) {  //checks every 2 seconds
    resetMillis = currentMillis;
    //check to see if servo has finished movement and if it has give it a new destination value and speed
    for (int q = 0; q < 16; q++) {
      if (servoData[q][SERVOTARGET] == servoData[q][SERVOCURRENT]) {  //has the servo reached it's final position
        //if yes set a new position and speed

       
        servoData[q][SERVOTARGET] = random(0, 179);  //set new position to move to
        servoData[q][SERVOSPEED] = random(0, 70);    //set new position to move to
        Serial.print("Servo: ");
        Serial.print(q);
        Serial.print(" Tgt: ");
        Serial.print(servoData[q][SERVOTARGET]);
        Serial.print(" Speed: ");
        Serial.println(servoData[q][SERVOSPEED]);
      }
    }
    Serial.println(" ");
  }

  /* NOTE on changing servo target and speed.

    Changing the value of servoData[q][SERVOTARGET] at any time will cause the servo to instantly start moving to the new
    setting even if it has not arrived at the last destination.
    
    changing the value of servoData[q][SERVOSPEED] will change the current servo speed. This could be used for 
    servo acceleration deceleration as required.

    to find out how many steps are left find the difference between servoData[q][SERVOTARGET] and servoData[q][SERVOCURRENT]

  */
}

//this function deals with moving all the servos at the correct speed.
void servoControl() {
  int sendPos;
  for (int q = 0; q < 16; q++) {                                    //cycle through all the boards
    if (servoData[q][SERVOTARGET] != servoData[q][SERVOCURRENT]) {  //does this servo need to move?

      if (currentMillis - servoMillis[q] >= servoData[q][SERVOSPEED]) {  //is it time to move


        servoMillis[q] = currentMillis;  //reset this servos timer
        if (servoData[q][SERVOTARGET] > 179) {
          servoData[q][SERVOTARGET] = 179;
        }                                                              //limit check in case someone tried to move servo more than 179 degrees
        if (servoData[q][SERVOCURRENT] < servoData[q][SERVOTARGET]) {  //work out the direction servo needs to move in
          servoData[q][SERVOCURRENT]++;
        } else {
          servoData[q][SERVOCURRENT]--;
        }
        sendPos = map(servoData[q][SERVOCURRENT], 0, 179, USMIN, USMAX);
        servoBoard.writeMicroseconds(q, sendPos);
      }
    }
  }
}