From 923b05ffd2c583c6515889ae7b2105755b742206 Mon Sep 17 00:00:00 2001
From: gabrielburnworth <gabrielburnworth@users.noreply.github.com>
Date: Thu, 7 Feb 2019 15:22:36 -0800
Subject: [PATCH] fix servo control

---
 src/ServoControl.cpp               | 38 +++++++++++++++++++++++++++++-
 src/ServoControl.h                 |  1 +
 src/farmbot_arduino_controller.cpp |  1 +
 src/pins.h                         |  2 +-
 4 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/src/ServoControl.cpp b/src/ServoControl.cpp
index 9cf110b..9babab8 100644
--- a/src/ServoControl.cpp
+++ b/src/ServoControl.cpp
@@ -9,7 +9,13 @@ D11 D6 D5 D4
 
 static ServoControl *instance;
 
-Servo servos[2];
+#if defined(RAMPS_V14)
+  Servo servos[2];
+#endif
+
+#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
+  Servo servos[4];
+#endif
 
 ServoControl *ServoControl::getInstance()
 {
@@ -28,6 +34,20 @@ int ServoControl::attach()
 {
   servos[0].attach(SERVO_0_PIN);
   servos[1].attach(SERVO_1_PIN);
+  #if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
+    servos[2].attach(SERVO_2_PIN);
+    servos[3].attach(SERVO_3_PIN);
+  #endif
+}
+
+void ServoControl::detachServos()
+{
+  servos[0].detach();
+  servos[1].detach();
+  #if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
+    servos[2].detach();
+    servos[3].detach();
+  #endif
 }
 
 int ServoControl::setAngle(int pin, int angle)
@@ -52,11 +72,27 @@ int ServoControl::setAngle(int pin, int angle)
   switch (pin)
   {
   case 4:
+    servos[0].attach(SERVO_0_PIN);
     servos[0].write(angle);
     break;
   case 5:
+    servos[1].attach(SERVO_1_PIN);
     servos[1].write(angle);
     break;
+  case 6:
+    #if defined(RAMPS_V14)
+      return 1;
+    #endif
+    servos[2].attach(SERVO_2_PIN);
+    servos[2].write(angle);
+    break;
+  case 11:
+    #if defined(RAMPS_V14)
+      return 1;
+    #endif
+    servos[3].attach(SERVO_3_PIN);
+    servos[3].write(angle);
+    break;
   default:
     return 1;
   }
diff --git a/src/ServoControl.h b/src/ServoControl.h
index 4ae529c..8fcf45e 100644
--- a/src/ServoControl.h
+++ b/src/ServoControl.h
@@ -20,6 +20,7 @@ public:
   static ServoControl *getInstance();
 
   int attach();
+  void detachServos();
   int setAngle(int pin, int angle);
 
 private:
diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp
index 203908e..7522652 100644
--- a/src/farmbot_arduino_controller.cpp
+++ b/src/farmbot_arduino_controller.cpp
@@ -434,6 +434,7 @@ void loop()
   {
     StepperControl::getInstance()->disableMotorsEmergency();
     PinControl::getInstance()->resetPinsUsed();
+    ServoControl::getInstance()->detachServos();
     if (debugMessages)
     {
       Serial.print(COMM_REPORT_COMMENT);
diff --git a/src/pins.h b/src/pins.h
index a241d80..b76d15e 100644
--- a/src/pins.h
+++ b/src/pins.h
@@ -202,7 +202,7 @@
   #define SERVO_0_PIN 4
   #define SERVO_1_PIN 5
   #define SERVO_2_PIN 6
-  #define SERVO_3_PIN 7
+  #define SERVO_3_PIN 11
 
   // Encoder X channel A: pin 16, port H1
   #define ENC_X_A_PORT PINH