37 lines
801 B
C++
37 lines
801 B
C++
#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;
|
|
}
|
|
}
|