#include "SmartServo.hpp"

SmartServo::SmartServo(int pin)
{
    myservo.attach(pin);
}

// Note: This blocks execution until servo finished moving.
void SmartServo::moveServoTo(int pos, int stepSize)
{
    int currentPos = myservo.read();
    for (int p = currentPos; p < pos; p += stepSize)
    {
        myservo.write(p);
        delay(10);
    }
    for (int p = currentPos; p > pos; p -= stepSize)
    {
        myservo.write(p);
        delay(10);
    }
}

void SmartServo::moveServoToZero()
{
    moveServoTo(0, 10);
}

void SmartServo::moveServoTo180()
{
    moveServoTo(180, 5);
}