4 servos for all boards

FarmBot · Feb 8, 2019 · 2c30fb1 · 2c30fb1
1 parent 523fb7c
commit 2c30fb1
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 31 deletions.
diff --git a/src/ServoControl.cpp b/src/ServoControl.cpp
@@ -9,13 +9,7 @@ D11 D6 D5 D4
 
 static ServoControl *instance;
 
-#if defined(RAMPS_V14)
-  Servo servos[2];
-#endif
-
-#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
-  Servo servos[4];
-#endif
+Servo servos[4];
 
 ServoControl *ServoControl::getInstance()
 {
@@ -34,20 +28,16 @@ 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
+  servos[2].attach(SERVO_2_PIN);
+  servos[3].attach(SERVO_3_PIN);
 }
 
 void ServoControl::detachServos()
 {
   servos[0].detach();
   servos[1].detach();
-  #if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
-    servos[2].detach();
-    servos[3].detach();
-  #endif
+  servos[2].detach();
+  servos[3].detach();
 }
 
 int ServoControl::setAngle(int pin, int angle)
@@ -80,21 +70,13 @@ int ServoControl::setAngle(int pin, int angle)
     servos[1].write(angle);
     break;
   case 6:
-    #if defined(RAMPS_V14)
-      return 1;
-    #else
-      servos[2].attach(SERVO_2_PIN);
-      servos[2].write(angle);
-      break;
-    #endif
+    servos[2].attach(SERVO_2_PIN);
+    servos[2].write(angle);
+    break;
   case 11:
-    #if defined(RAMPS_V14)
-      return 1;
-    #else
-      servos[3].attach(SERVO_3_PIN);
-      servos[3].write(angle);
-      break;
-    #endif
+    servos[3].attach(SERVO_3_PIN);
+    servos[3].write(angle);
+    break;
   default:
     return 1;
   }

diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp
@@ -165,8 +165,11 @@ void setup()
     pinMode(AUX4_47, INPUT_PULLUP);
     pinMode(AUX4_32, INPUT_PULLUP);
 
-    //pinMode(SERVO_0_PIN , OUTPUT);
-    //pinMode(SERVO_1_PIN , OUTPUT);
+    pinMode(SERVO_0_PIN, OUTPUT);
+    pinMode(SERVO_1_PIN, OUTPUT);
+    pinMode(SERVO_2_PIN, OUTPUT);
+    pinMode(SERVO_3_PIN, OUTPUT);
+
   #endif
 
   #if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)

diff --git a/src/pins.h b/src/pins.h
@@ -65,6 +65,8 @@
 
   #define SERVO_0_PIN 4
   #define SERVO_1_PIN 5
+  #define SERVO_2_PIN 6
+  #define SERVO_3_PIN 11
 
   #define AUX1_00 0
   #define AUX1_01 1