init

src/pen.cpp

@@ -0,0 +1,37 @@
+#include "pen.h"
+#include "config.h"
+#include <ESP32Servo.h>
+
+static Servo   s_servo;
+static bool    s_penDown   = false;
+static uint8_t s_angleUp   = SERVO_ANGLE_UP;
+static uint8_t s_angleDown = SERVO_ANGLE_DOWN;
+
+void pen_init() {
+    s_servo.attach(PIN_SERVO);
+    pen_up();
+}
+
+void pen_up() {
+    s_servo.write(s_angleUp);
+    delay(SERVO_SETTLE_MS);
+    s_penDown = false;
+}
+
+void pen_down() {
+    s_servo.write(s_angleDown);
+    delay(SERVO_SETTLE_MS);
+    s_penDown = true;
+}
+
+bool pen_is_down() {
+    return s_penDown;
+}
+
+void pen_set_down_angle(uint8_t angle) {
+    s_angleDown = angle;
+}
+
+void pen_set_up_angle(uint8_t angle) {
+    s_angleUp = angle;
+}