Sweep
This example sweeps a servo back and forth from -90° to +90°. Herkulex servos
use a number between 0
and 1023
to indicate position. To go from degrees to
Herkulex units, use the following formula:
uint16_t pos = 512 + uint16_t(pos_degree / 0.325f);
Note: Be sure to change SERVO_ID
to the ID of the servo you want to sweep.
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <HerkulexServo.h>
#define PIN_SW_RX 8
#define PIN_SW_TX 9
#define SERVO_ID 12
SoftwareSerial servo_serial(PIN_SW_RX, PIN_SW_TX);
HerkulexServoBus herkulex_bus(servo_serial);
HerkulexServo my_servo(herkulex_bus, SERVO_ID);
unsigned long last_update = 0;
unsigned long now = 0;
bool toggle = false;
void setup() {
Serial.begin(115200);
servo_serial.begin(115200);
delay(500);
my_servo.setTorqueOn(); // turn power on
}
void loop() {
herkulex_bus.update();
now = millis();
if ( (now - last_update) > 1000) {
// called every 1000 ms
if (toggle) {
// move to -90° over a duration of 560ms, set LED to green
// 512 - 90°/0.325 = 235
my_servo.setPosition(235, 50, HerkulexLed::Green);
} else {
// move to +90° over a duration of 560ms, set LED to blue
// 512 + 90°/0.325 = 789
my_servo.setPosition(789, 50, HerkulexLed::Blue);
}
last_update = now;
toggle = !toggle;
}
}