Merge pull request #2470 from thinkyhead/patch_servo_move

Patch servos code for move

Marlin/Conditionals.h

@@ -279,6 +279,8 @@
     #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
   #endif
 
+  #define SERVO_LEVELING (defined(ENABLE_AUTO_BED_LEVELING) && defined(DEACTIVATE_SERVOS_AFTER_MOVE))
+
    /**
     * Sled Options
     */ 

Marlin/Marlin_main.cpp

@@ -36,8 +36,6 @@
   #endif
 #endif // ENABLE_AUTO_BED_LEVELING
 
-#define SERVO_LEVELING (defined(ENABLE_AUTO_BED_LEVELING) && defined(DEACTIVATE_SERVOS_AFTER_MOVE))
-
 #ifdef MESH_BED_LEVELING
   #include "mesh_bed_leveling.h"
 #endif

Marlin/servo.cpp

@@ -35,7 +35,7 @@
 
  write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
  writeMicroseconds() - Sets the servo pulse width in microseconds
- move(pin, angel) - Sequence of attach(pin), write(angel).
+ move(pin, angle) - Sequence of attach(pin), write(angle).
                     With DEACTIVATE_SERVOS_AFTER_MOVE it waits SERVO_DEACTIVATION_DELAY and detaches.
  read()      - Gets the last written servo pulse width as an angle between 0 and 180.
  readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
@@ -59,7 +59,7 @@
 
 //#define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
 
-static servo_t servos[MAX_SERVOS];                          // static array of servo structures
+static ServoInfo_t servo_info[MAX_SERVOS];                  // static array of servo info structures
 static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
 
 uint8_t ServoCount = 0;                                     // the total number of attached servos
@@ -69,7 +69,7 @@ uint8_t ServoCount = 0;                                     // the total number
 #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
 #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
 #define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
-#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
+#define SERVO(_timer,_channel)  (servo_info[SERVO_INDEX(_timer,_channel)])       // macro to access servo class by timer and channel
 
 #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
 #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
@@ -232,34 +232,37 @@ static boolean isTimerActive(timer16_Sequence_t timer) {
 Servo::Servo() {
   if ( ServoCount < MAX_SERVOS) {
     this->servoIndex = ServoCount++;                    // assign a servo index to this instance
-    servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
+    servo_info[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
   }
   else
     this->servoIndex = INVALID_SERVO;  // too many servos
 }
 
-uint8_t Servo::attach(int pin) {
+int8_t Servo::attach(int pin) {
   return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
 }
 
-uint8_t Servo::attach(int pin, int min, int max) {
-  if (this->servoIndex < MAX_SERVOS ) {
-    if(pin > 0)
-      servos[this->servoIndex].Pin.nbr = pin;
-    pinMode(servos[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
-    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
-    this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
-    this->max = (MAX_PULSE_WIDTH - max) / 4;
-    // initialize the timer if it has not already been initialized
-    timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
-    if (!isTimerActive(timer)) initISR(timer);
-    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
-  }
+int8_t Servo::attach(int pin, int min, int max) {
+
+  if (this->servoIndex >= MAX_SERVOS) return -1;
+
+  if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin;
+  pinMode(servo_info[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
+
+  // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
+  this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
+  this->max = (MAX_PULSE_WIDTH - max) / 4;
+
+  // initialize the timer if it has not already been initialized
+  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if (!isTimerActive(timer)) initISR(timer);
+  servo_info[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
+
   return this->servoIndex;
 }
 
 void Servo::detach() {
-  servos[this->servoIndex].Pin.isActive = false;
+  servo_info[this->servoIndex].Pin.isActive = false;
   timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
   if (!isTimerActive(timer)) finISR(timer);
 }
@@ -287,7 +290,7 @@ void Servo::writeMicroseconds(int value) {
 
     uint8_t oldSREG = SREG;
     cli();
-    servos[channel].ticks = value;
+    servo_info[channel].ticks = value;
     SREG = oldSREG;
   }
 }
@@ -296,17 +299,21 @@ void Servo::writeMicroseconds(int value) {
 int Servo::read() { return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
 
 int Servo::readMicroseconds() {
-  return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
+  return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[this->servoIndex].ticks) + TRIM_DURATION;
 }
 
-bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
+bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
 
-uint8_t Servo::move(int pin, int value) {
-  uint8_t ret;
-  ret = this->attach(pin);
-  if (ret) {
+int8_t Servo::move(int pin, int value) {
+  int8_t ret;
+  #if SERVO_LEVELING
+    ret = this->attach(pin);
+  #else
+    ret = this->servoIndex;
+  #endif
+  if (ret >= 0) {
     this->write(value);
-    #ifdef DEACTIVATE_SERVOS_AFTER_MOVE && (SERVO_DEACTIVATION_DELAY > 0)
+    #if SERVO_LEVELING
       delay(SERVO_DEACTIVATION_DELAY);
       this->detach();
     #endif

Marlin/servo.h

@@ -112,17 +112,17 @@ typedef struct {
 typedef struct {
   ServoPin_t Pin;
   unsigned int ticks;
-} servo_t;
+} ServoInfo_t;
 
 class Servo {
   public:
     Servo();
-    uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
-    uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
+    int8_t attach(int pin);           // attach the given pin to the next free channel, set pinMode, return channel number (-1 on fail)
+    int8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
     void detach();
     void write(int value);             // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
     void writeMicroseconds(int value); // Write pulse width in microseconds
-    uint8_t move(int pin, int value);  // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure.
+    int8_t move(int pin, int value);  // attach the given pin to the next free channel, set pinMode, return channel number (-1 if attach fails)
                                        // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds.
                                        // if DEACTIVATE_SERVOS_AFTER_MOVE is defined waits SERVO_DEACTIVATION_DELAY, than detaches.
     int read();                        // returns current pulse width as an angle between 0 and 180 degrees