Untitled

// 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);
  }
}