/*
  Independent speed control of 6 servos on an ESP32-C3
  https://wokwi.com/projects/355852275661848577
  by dlloydev, February 2023.
*/

#include <pwmWrite.h>

Pwm pwm = Pwm();

const int servoPin[] = {6, 7, 8, 9, 18, 19};
const int speedPin[] = {0, 1, 2, 3, 4, 5};
float speed[] = {0, 0, 0, 0, 0, 0};
float pos[] = {0, 0, 0, 0, 0, 0};
bool dir[] = {0, 0, 0, 0, 0, 0};

void setup() {
}

void loop() {
  for (int i = 0; i < 6; i++) {
    speed[i] = (analogRead(speedPin[i])) / 2047.0;  // 0-200% speed
    pwm.writeServo(servoPin[i], pos[i]);
    delay(5);
    if (pos[i] < 0) pos[i] = 0;
    if (dir[i]) {
      if (pos[i] <= 180.0) {
        pos[i] += speed[i];
      } else {
        dir[i] = false;
      }
    } else {
      if (pos[i] >= 0.5) {
        pos[i] -= speed[i];
      } else {
        dir[i] = true;
      }
    }
  }
}