From effbfbad2a5975969c5cb2e47cb0cc16d79b6023 Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Mon, 5 Aug 2019 22:05:57 +0300
Subject: [PATCH] pen engaged smaller angle

---
 Firmware/MotorControl/src/Pen.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Firmware/MotorControl/src/Pen.cpp b/Firmware/MotorControl/src/Pen.cpp
index 855e40e..95bc83c 100644
--- a/Firmware/MotorControl/src/Pen.cpp
+++ b/Firmware/MotorControl/src/Pen.cpp
@@ -9,6 +9,7 @@ Pen::Pen(int pin, int posEngaged, int posDisengaged)
 void Pen::engage() {
     if (!engaged) {
         servo.write(posEngaged);
+        delay(5);
     }
     engaged = true;
 }
@@ -16,6 +17,7 @@ void Pen::engage() {
 void Pen::disengage() {
     if (engaged) {
         servo.write(posDisengaged);
+        delay(5);
     }
     engaged = false;
 }
@@ -23,9 +25,10 @@ void Pen::disengage() {
 void Pen::init() {
     servo.attach(pin);
     servo.write(posDisengaged);
+    delay(5);
     engaged = false;
 }
 
 bool Pen::getEngaged() { return engaged; }
 
-Pen pen(7, 120, 170);
+Pen pen(7, 100, 175);