Arduino Example Snippet
#define SERVO1_PIN 1
#define SERVO2_PIN 14
#define SERVO3_PIN 41
#define SERVO_FREQ_HZ 50
#define PWM_BITS 12
#define PWM_MAX ((1 << PWM_BITS) - 1)
// Typical servo pulse range (adjust if needed)
#define SERVO_MIN_US 500
#define SERVO_MAX_US 2500
static inline int clampi(int v, int lo, int hi) {
if (v < lo) return lo;
if (v > hi) return hi;
return v;
}
static inline int angleToUs(int angle) {
angle = clampi(angle, 0, 180);
long us = SERVO_MIN_US + (long)(SERVO_MAX_US - SERVO_MIN_US) * angle / 180;
return (int)us;
}
static inline int usToDuty(int us) {
// 50 Hz -> period 20,000 us
const int period_us = 1000000 / SERVO_FREQ_HZ; // 20000
long duty = (long)us * PWM_MAX / period_us;
return (int)clampi((int)duty, 0, PWM_MAX);
}
static inline void servoWriteAngle(int pin, int angle) {
int us = angleToUs(angle);
int duty = usToDuty(us);
analogWrite(pin, duty);
}
void setup() {
pinMode(SERVO1_PIN, OUTPUT);
pinMode(SERVO2_PIN, OUTPUT);
pinMode(SERVO3_PIN, OUTPUT);
// IMPORTANT: core 3.3.5 requires pin argument
analogWriteFrequency(SERVO1_PIN, SERVO_FREQ_HZ);
analogWriteFrequency(SERVO2_PIN, SERVO_FREQ_HZ);
analogWriteFrequency(SERVO3_PIN, SERVO_FREQ_HZ);
analogWriteResolution(SERVO1_PIN, PWM_BITS);
analogWriteResolution(SERVO2_PIN, PWM_BITS);
analogWriteResolution(SERVO3_PIN, PWM_BITS);
}
void loop() {
for (int a = 0; a <= 180; a++) {
servoWriteAngle(SERVO1_PIN, a);
servoWriteAngle(SERVO2_PIN, a);
servoWriteAngle(SERVO3_PIN, a);
delay(15);
}
for (int a = 180; a >= 0; a--) {
servoWriteAngle(SERVO1_PIN, a);
servoWriteAngle(SERVO2_PIN, a);
servoWriteAngle(SERVO3_PIN, a);
delay(15);
}
}