From b6349c262310793c047365a77adbb943a8d87ea1 Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Thu, 19 Mar 2020 14:24:18 +0300
Subject: [PATCH] [AVR] smooth servo

---
 Firmware/MotorControl/src/Pen.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/Firmware/MotorControl/src/Pen.cpp b/Firmware/MotorControl/src/Pen.cpp
index e9d784a..3d81dcb 100644
--- a/Firmware/MotorControl/src/Pen.cpp
+++ b/Firmware/MotorControl/src/Pen.cpp
@@ -8,18 +8,20 @@ Pen::Pen(int pin, int posEngaged, int posDisengaged)
 
 void Pen::engage() {
     if (!engaged) {
-        servo.attach(pin);
-        servo.write(posEngaged);
-        delay(200);
+        for(int i = posDisengaged; i > posEngaged; i--) {
+            servo.write(i);
+            delay(5);
+        }
     }
     engaged = true;
 }
 
 void Pen::disengage() {
     if (engaged) {
-        servo.attach(pin);
-        servo.write(posDisengaged);
-        delay(200);
+        for (int i = posEngaged; i > posDisengaged; i++) {
+            servo.write(i);
+            delay(5);
+        }
     }
     engaged = false;
 }