#include "Arduino.h"
#include "lego_servo.h"

lego_servo::Servo::Servo(int pin_a, int pin_b) {
    ipin_a = pin_a;
    ipin_b = pin_b
}

void lego_servo::Servo::begin()
{
    pinMode(ipin_a,OUTPUT);
    pinMode(ipin_b,OUTPUT);
    digitalWrite(ipin_a,0);
    digitalWrite(ipin_b,0);
}

void lego_servo::Servo::position(int pos)
{
    switch (pos) {
        case 0:
            digitalWrite(ipin_a,0);
            digitalWrite(ipin_b,0);
            digitalWrite(LED_BUILTIN,1);
            break;
        case 1:
            digitalWrite(ipin_a,1);
            digitalWrite(ipin_b,0);
            digitalWrite(LED_BUILTIN,0);
            break;
        case 2:
            digitalWrite(ipin_a,0);
            digitalWrite(ipin_b,1);
            digitalWrite(LED_BUILTIN,1);
            break;
    }
}