// A simple Arduino sketch for Docket.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#define BAUD_RATE 115200
#define FREQUENCY 60
#define MINIMUM_POSITION 105
#define MAXIMUM_POSITION 525
#define STEP 5
#define STEP_DELAY 20
Adafruit_PWMServoDriver driver;
int current_position;
void setup() {
driver = Adafruit_PWMServoDriver();
Serial.begin(BAUD_RATE);
driver.begin();
driver.setPWMFreq(FREQUENCY);
current_position = -1;
}
void loop() {
if (Serial.available()) {
int next_position = Serial.parseInt();
if (next_position < MINIMUM_POSITION) {
next_position = MINIMUM_POSITION;
} else if (next_position > MAXIMUM_POSITION) {
next_position = MAXIMUM_POSITION;
}
move(next_position);
Serial.print(0);
}
}
void move(int next_position) {
if (current_position == -1) {
current_position = next_position;
driver.setPWM(0, 0, current_position);
return;
}
while (next_position != current_position) {
if (current_position > next_position) {
current_position -= STEP;
if (current_position < next_position) {
current_position = next_position;
}
} else {
current_position += STEP;
if (current_position > next_position) {
current_position = next_position;
}
}
driver.setPWM(0, 0, current_position);
delay(STEP_DELAY);
}
}