use ServoEasing

1 files changed, 22 insertions, 21 deletions

diff --git a/main.ino b/main.ino
index f7066d3..315e9ad 100644
--- a/main.ino
+++ b/main.ino
@@ -1,6 +1,6 @@
 // Pill dispenser
 
-// Requires libraries EasyButton, BlinkControl, FastLED, Servo
+// Requires libraries EasyButton, BlinkControl, FastLED, ServoEasing
 // Optionally requires libraries GxEPD2, GEM
 
 #include <EasyButton.h>
@@ -19,13 +19,13 @@ const byte BUTTON_PIN_MOVE = BUTTON_1;
 const byte BUTTON_PIN_SELECT = BUTTON_2;
 const byte BUTTON_PIN_ACTION = BUTTON_3;
 BlinkControl led(LED_PIN);
-#define SERVO_PIN 3
+#define SERVO_PIN 5 // see ServoEasing PinDefinitionsAndMore.h
 #else
 const byte BUTTON_PIN_MOVE = 4;
 const byte BUTTON_PIN_SELECT = 5;
 const byte BUTTON_PIN_ACTION = 2;
 BlinkControl led(LED_BUILTIN); // pin 13 is built-in LED on many Arduino boards
-#define SERVO_PIN 3
+#define SERVO_PIN 9 // see ServoEasing PinDefinitionsAndMore.h
 #endif
 
 #define LEDS_PIN 6
@@ -77,14 +77,13 @@ GEMPage menuPageMain("Main Menu");
 GEM_adafruit_gfx menu(displayEPaper, GEM_POINTER_ROW, GEM_ITEMS_COUNT_AUTO);
 #endif
 
-#include <Servo.h>
+#define MAX_EASING_SERVOS 1
+#include "ServoEasing.hpp"
 
-Servo servo;
+ServoEasing servo;
 
-int servo_angle = 89;
-int servo_angle_delta = 3;
-const int servo_angle_min = 0;
-const int servo_angle_max = 90;
+#define SERVO_POS_INIT 0
+#define SERVO_POS_MAX 90
 
 int servo_should_move = 0;
 
@@ -124,8 +123,16 @@ void setup() {
   menu.drawMenu();
 #endif
 
-  servo.attach(SERVO_PIN);
-  servo.write(servo_angle);  // place serve at initial position
+  Serial.println(F("Attach servo at pin " STR(SERVO_PIN)));
+  if (servo.attach(SERVO_PIN, SERVO_POS_INIT) == INVALID_SERVO) {
+    led.blink2();
+    Serial.println(F("Error attaching servo"));
+  }
+
+  // Wait for servo to reach start position.
+  delay(500);
+
+  servo.setSpeed(90); // This speed is taken if no further speed argument is given.
 }
 
 void loop() {
@@ -206,15 +213,9 @@ void printData() {
 // source: https://robojax.com/learn/arduino/?vid=robojax_Servo_PB1_move_return
 void servoLoop() {
   if (servo_should_move == 1) {
-    servo_angle = servo_angle + servo_angle_delta;
-    if (servo_angle >= servo_angle_max) {
-      servo_angle_delta = -servo_angle_delta;
-      servo_should_move = 0;
-    }
-    if (servo_angle <= servo_angle_min) {
-      servo_angle_delta = -servo_angle_delta;
-    }
-    servo.write(servo_angle);
-    delay(10);
+    servo.easeTo(SERVO_POS_MAX);
+    delay(500);
+    servo.easeTo(SERVO_POS_INIT);
+    servo_should_move = 0;
   }
 }