First attempt at refactoring of original servo code. Only adding servo support as a start to keep things simple.

Marlin/Configuration.h

@@ -406,6 +406,23 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 // Support for the BariCUDA Paste Extruder.
 //#define BARICUDA
 
+/*********************************************************************\
+*
+* R/C SERVO support
+*
+* Sponsored by TrinityLabs, Reworked by codexmas
+*
+**********************************************************************/
+
+// Number of servos
+//
+// If you select a configuration below, this will receive a default value and does not need to be set manually
+// set it manually if you have more servos than extruders and wish to manually control some
+// leaving it undefined or defining as 0 will disable the servo subsystem
+// If unsure, leave commented / disabled
+//
+// #define NUM_SERVOS 3
+
 #include "Configuration_adv.h"
 #include "thermistortables.h"
 

Marlin/Marlin_main.cpp

@@ -40,6 +40,10 @@
 #include "language.h"
 #include "pins_arduino.h"
 
+#if (defined NUM_SERVOS) && (NUM_SERVOS > 0)
+#include "Servo.h"
+#endif
+
 #if DIGIPOTSS_PIN > -1
 #include <SPI.h>
 #endif
@@ -121,6 +125,7 @@
 // M220 S<factor in percent>- set speed factor override percentage
 // M221 S<factor in percent>- set extrude factor override percentage
 // M240 - Trigger a camera to take a photograph
+// M280 - set servo position absolute. P: servo index, S: angle or microseconds
 // M300 - Play beepsound S<frequency Hz> P<duration ms>
 // M301 - Set PID parameters P I and D
 // M302 - Allow cold extrudes
@@ -225,6 +230,10 @@ static uint8_t tmp_extruder;
 
 bool Stopped=false;
 
+#if (defined NUM_SERVOS) && (NUM_SERVOS > 0)
+  Servo servos[NUM_SERVOS];
+#endif
+
 //===========================================================================
 //=============================ROUTINES=============================
 //===========================================================================
@@ -331,6 +340,25 @@ void suicide()
   #endif
 }
 
+void servo_init()
+{
+  #if (NUM_SERVOS >= 1) && defined (SERVO0_PIN) && (SERVO0_PIN > -1)
+    servos[0].attach(SERVO0_PIN);
+  #endif
+  #if (NUM_SERVOS >= 2) && defined (SERVO1_PIN) && (SERVO1_PIN > -1)
+    servos[1].attach(SERVO1_PIN);
+  #endif
+  #if (NUM_SERVOS >= 3) && defined (SERVO2_PIN) && (SERVO2_PIN > -1)
+    servos[2].attach(SERVO2_PIN);
+  #endif
+  #if (NUM_SERVOS >= 4) && defined (SERVO3_PIN) && (SERVO3_PIN > -1)
+    servos[3].attach(SERVO3_PIN);
+  #endif
+  #if (NUM_SERVOS >= 5)
+    #error "TODO: enter initalisation code for more servos"
+  #endif
+}
+
 void setup()
 {
   setup_killpin();
@@ -379,6 +407,7 @@ void setup()
   watchdog_init();
   st_init();    // Initialize stepper, this enables interrupts!
   setup_photpin();
+  servo_init();
 
   lcd_init();
 
@@ -1485,6 +1514,37 @@ void process_commands()
     }
     break;
 
+    #if (defined NUM_SERVOS) && (NUM_SERVOS > 0)
+    case 280: // M280 - set servo position absolute. P: servo index, S: angle or microseconds
+      {
+        int servo_index = -1;
+        int servo_position = 0;
+        if (code_seen('P'))
+          servo_index = code_value();
+        if (code_seen('S')) {
+          servo_position = code_value();
+          if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
+            servos[servo_index].write(servo_position);
+          }
+          else {
+            SERIAL_ECHO_START;
+            SERIAL_ECHO("Servo ");
+            SERIAL_ECHO(servo_index);
+            SERIAL_ECHOLN(" out of range");
+          }
+        }
+        else if (servo_index >= 0) {
+          SERIAL_PROTOCOL(MSG_OK);
+          SERIAL_PROTOCOL(" Servo ");
+          SERIAL_PROTOCOL(servo_index);
+          SERIAL_PROTOCOL(": ");
+          SERIAL_PROTOCOL(servos[servo_index].read());
+          SERIAL_PROTOCOLLN("");
+        }
+      }
+      break;
+    #endif // NUM_SERVOS > 0
+
     #if defined(LARGE_FLASH) && LARGE_FLASH == true && defined(BEEPER) && BEEPER > -1
     case 300: // M300
     {

Marlin/Servo.h

@@ -59,28 +59,34 @@
 // Say which 16 bit timers can be used and in what order
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
 #define _useTimer5
-#define _useTimer1
+//#define _useTimer1
 #define _useTimer3
 #define _useTimer4
-typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ;
+//typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer5, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ;
 
 #elif defined(__AVR_ATmega32U4__)
-#define _useTimer1
-typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+//#define _useTimer1
+#define _useTimer3
+//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
 
 #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
 #define _useTimer3
-#define _useTimer1
-typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ;
+//#define _useTimer1
+//typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
 
 #elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
 #define _useTimer3
-#define _useTimer1
-typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ;
+//#define _useTimer1
+//typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
 
 #else  // everything else
-#define _useTimer1
-typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+//#define _useTimer1
+//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _Nbr_16timers } timer16_Sequence_t ;
 #endif
 
 #define Servo_VERSION           2      // software version of this library

Marlin/pins.h

@@ -383,6 +383,11 @@
 #define HEATER_BED_PIN     8    // BED
 #define TEMP_BED_PIN       14   // ANALOG NUMBERING
 
+#define SERVO0_PIN         11
+#define SERVO1_PIN         6
+#define SERVO2_PIN         5
+#define SERVO3_PIN         4
+
 #ifdef ULTRA_LCD
 
   #ifdef NEWPANEL