fix servo control

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;
   }

src/ServoControl.h

@@ -20,6 +20,7 @@ public:
   static ServoControl *getInstance();
 
   int attach();
+  void detachServos();
   int setAngle(int pin, int angle);
 
 private:

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);

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