diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index 6741298baf8c7a45fbb727619763ef56095f34b3..f31ffcd4036ced5cc23e9198351b7b9d3e7ddcc3 100644
--- a/Marlin/Conditionals.h
+++ b/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
*/
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 9b20f29cb6e1aab8d26df51a10e90adbb6ce70e3..3c4e5dbb7304c3b1b172f47afd55f7992991bc67 100644
--- a/Marlin/Marlin_main.cpp
+++ b/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
diff --git a/Marlin/servo.cpp b/Marlin/servo.cpp
index 97def9ea032d06ffe77046d15ab8c93c921ca9be..ed8b05d14963103a91fe82ebd7d08d2ca1293fe3 100644
--- a/Marlin/servo.cpp
+++ b/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
diff --git a/Marlin/servo.h b/Marlin/servo.h
index b3700039332b8a0e7c4b2b38557731604bfd70e4..b0b5ce5b17d97673cc1d362fe6783dd7de3f8876 100644
--- a/Marlin/servo.h
+++ b/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