From 6e451b3b9a61f81a26577b32e4c935c0455537e6 Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Tue, 6 Aug 2019 11:49:56 +0300
Subject: [PATCH] fix servo glitches on low voltage

---
 Firmware/MotorControl/src/Pen.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/Firmware/MotorControl/src/Pen.cpp b/Firmware/MotorControl/src/Pen.cpp
index 9c95e44..d62a086 100644
--- a/Firmware/MotorControl/src/Pen.cpp
+++ b/Firmware/MotorControl/src/Pen.cpp
@@ -8,16 +8,22 @@ Pen::Pen(int pin, int posEngaged, int posDisengaged)
 
 void Pen::engage() {
     if (!engaged) {
-        servo.write(posEngaged);
+        servo.attach(pin);
         delay(5);
+        servo.write(posEngaged);
+        delay(200);
+        servo.detach();
     }
     engaged = true;
 }
 
 void Pen::disengage() {
     if (engaged) {
-        servo.write(posDisengaged);
+        servo.attach(pin);
         delay(5);
+        servo.write(posDisengaged);
+        delay(200);
+        servo.detach();
     }
     engaged = false;
 }
@@ -25,8 +31,9 @@ void Pen::disengage() {
 void Pen::init() {
     servo.attach(pin);
     servo.write(posDisengaged);
-    delay(5);
+    delay(200);
     engaged = false;
+    servo.detach();
 }
 
 bool Pen::getEngaged() { return engaged; }