Record and Play
This example sketch lets you record and play back motion patterns. Press the record button and move the servo around. The sketch will read and store 5 seconds of position data. Press the play button to make the servo repeat the motion pattern.
Note: This example depends on the Bounce2 library, which needs to be installed separately.
Note: This sketch uses two external buttons. Hook the record button up between pin 4 and ground, and the play button between pin 5 and ground. No external resistors are needed.
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <HerkulexServo.h>
#include <Bounce2.h>
#define SAMPLE_SIZE 50
#define SAMPLE_INTERVAL 100
#define PLAYBACK_SPEED (SAMPLE_INTERVAL / 11.2 * 2)
#define PIN_SW_RX 8
#define PIN_SW_TX 9
#define PIN_BTN_REC 4
#define PIN_BTN_PLAY 5
SoftwareSerial servo_serial(PIN_SW_RX, PIN_SW_TX);
HerkulexServoBus herkulex_bus(servo_serial);
HerkulexServo my_servo(herkulex_bus, 12);
Bounce btn_rec;
Bounce btn_play;
uint16_t samples[SAMPLE_SIZE] = {};
uint8_t sample_idx = 0;
unsigned long last_sample = 0;
enum class State {
None = 0,
Idle,
Record,
Play
};
State current_state = State::Idle;
State last_state = State::None;
void setup() {
pinMode(PIN_BTN_REC, INPUT_PULLUP);
pinMode(PIN_BTN_PLAY, INPUT_PULLUP);
btn_rec.attach(PIN_BTN_REC);
btn_rec.interval(50);
btn_play.attach(PIN_BTN_PLAY);
btn_play.interval(50);
Serial.begin(115200);
servo_serial.begin(115200);
delay(500);
}
void loop() {
herkulex_bus.update();
btn_rec.update();
btn_play.update();
bool just_entered_state = (current_state != last_state);
last_state = current_state;
unsigned long now = millis();
switch (current_state) {
case State::Idle:
if (just_entered_state) {
my_servo.setLedColor(HerkulexLed::Off);
my_servo.setTorqueOff();
}
if (btn_rec.fell()) {
current_state = State::Record;
}
if (btn_play.fell()) {
current_state = State::Play;
}
break;
case State::Record:
if (just_entered_state) {
my_servo.setLedColor(HerkulexLed::Red);
my_servo.setTorqueOff();
last_sample = now;
sample_idx = 0;
}
if ( (now - last_sample) > SAMPLE_INTERVAL ) {
last_sample = now;
if (sample_idx == SAMPLE_SIZE) {
current_state = State::Idle;
return;
} else {
samples[sample_idx] = my_servo.getPosition();
sample_idx++;
}
}
break;
case State::Play:
if (just_entered_state) {
my_servo.setLedColor(HerkulexLed::Green);
my_servo.setTorqueOn();
last_sample = now;
sample_idx = 0;
}
if ( (now - last_sample) > SAMPLE_INTERVAL ) {
last_sample = now;
if (sample_idx == SAMPLE_SIZE) {
current_state = State::Idle;
return;
} else {
my_servo.setPosition(samples[sample_idx], PLAYBACK_SPEED);
sample_idx++;
}
}
break;
}
}