motion vallam

onam.ino

#include <Servo.h>

const int MIN_ANGLE = 30;   // lowest angle each arm swings to
const int MAX_ANGLE = 150;  // highest angle each arm swings to

const int NUM_SERVOS = 3;                              // how many servos total
const int SERVO_PINS[NUM_SERVOS]    = {9, 10, 11};      // signal pin for each servo, in order
const int STEP_INTERVAL[NUM_SERVOS] = {15, 20, 25};     // ms per degree of travel, per servo (lower = faster arm)

const bool SYNC_MODE = true;  // true = all arms locked together, false = staggered/independent

const int MIN_PULSE_US = 544;   // pulse width at MIN_ANGLE (recalibrate if your SG90 clones differ)
const int MAX_PULSE_US = 2400;  // pulse width at MAX_ANGLE (recalibrate if your SG90 clones differ)

Servo arms[NUM_SERVOS];
float pulse[NUM_SERVOS];       // current pulse width, fractional for smooth sub-degree motion
float velocity[NUM_SERVOS];    // us moved per ms, derived from STEP_INTERVAL
int   direction[NUM_SERVOS];
unsigned long lastUpdate[NUM_SERVOS];

void haltOnError() {
  pinMode(LED_BUILTIN, OUTPUT);
  while (true) {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(150);
    digitalWrite(LED_BUILTIN, LOW);
    delay(150);
  }
}

void setup() {
  if (MIN_ANGLE >= MAX_ANGLE || NUM_SERVOS < 1) haltOnError(); // bad config, refuse to run blind

  float pulseRange = MAX_PULSE_US - MIN_PULSE_US;
  float angleRange = MAX_ANGLE - MIN_ANGLE;

  for (int i = 0; i < NUM_SERVOS; i++) {
    arms[i].attach(SERVO_PINS[i], MIN_PULSE_US, MAX_PULSE_US);

    int interval = SYNC_MODE ? STEP_INTERVAL[0] : STEP_INTERVAL[i];
    velocity[i] = (pulseRange / angleRange) / interval;

    float startFraction = SYNC_MODE ? 0.0 : (float)i / NUM_SERVOS;
    pulse[i] = MIN_PULSE_US + startFraction * pulseRange;

    direction[i] = 1;
    lastUpdate[i] = micros();

    arms[i].writeMicroseconds((int)pulse[i]);
  }
}

void loop() {
  unsigned long now = micros();
  for (int i = 0; i < NUM_SERVOS; i++) {
    unsigned long elapsedUs = now - lastUpdate[i]; // unsigned subtraction, safe across micros() overflow
    lastUpdate[i] = now;

    float elapsedMs = elapsedUs / 1000.0;
    if (elapsedMs > 50) elapsedMs = 50; // guard against a single huge jump if loop ever stalls

    pulse[i] += direction[i] * velocity[i] * elapsedMs;

    if (pulse[i] >= MAX_PULSE_US || pulse[i] <= MIN_PULSE_US) {
      direction[i] *= -1;
    }

    pulse[i] = constrain(pulse[i], (float)MIN_PULSE_US, (float)MAX_PULSE_US);
    arms[i].writeMicroseconds((int)pulse[i]);
  }
}