From 3b23ccd366338f87d021ef183edfb4d3695c28c5 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 19 Jul 2015 11:24:23 -0700 Subject: [PATCH 1/2] Patch servos code for move - Have `Servo::attach` explicitly return -1 if it fails - Check for -1 in `Servo::move` because `servoIndex` might be 0 - Make `attach` / `detach` calls conditional on `SERVO_LEVELING` - Move `SERVO_LEVELING` define to `Conditionals.h` --- Marlin/Conditionals.h | 2 ++ Marlin/Marlin_main.cpp | 2 -- Marlin/servo.cpp | 47 ++++++++++++++++++++++++------------------ Marlin/servo.h | 6 +++--- 4 files changed, 32 insertions(+), 25 deletions(-) diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h index 6741298ba..f31ffcd40 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 9b20f29cb..3c4e5dbb7 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 97def9ea0..752435397 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) @@ -238,23 +238,26 @@ Servo::Servo() { 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) 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 + return this->servoIndex; } @@ -301,12 +304,16 @@ int Servo::readMicroseconds() { bool Servo::attached() { return servos[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 b37000393..ec7b662f4 100644 --- a/Marlin/servo.h +++ b/Marlin/servo.h @@ -117,12 +117,12 @@ typedef struct { 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 From eacfe132aa5218c501055c0d3a4ca2359dd7caa4 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 19 Jul 2015 11:35:48 -0700 Subject: [PATCH 2/2] Change the name of servos[] and servo_t --- Marlin/servo.cpp | 20 ++++++++++---------- Marlin/servo.h | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/Marlin/servo.cpp b/Marlin/servo.cpp index 752435397..ed8b05d14 100644 --- a/Marlin/servo.cpp +++ b/Marlin/servo.cpp @@ -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,7 +232,7 @@ 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 @@ -246,8 +246,8 @@ int8_t Servo::attach(int pin, int min, int max) { if (this->servoIndex >= MAX_SERVOS) return -1; - if (pin > 0) servos[this->servoIndex].Pin.nbr = pin; - pinMode(servos[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output + 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 @@ -256,13 +256,13 @@ int8_t Servo::attach(int pin, int min, int max) { // 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 + 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); } @@ -290,7 +290,7 @@ void Servo::writeMicroseconds(int value) { uint8_t oldSREG = SREG; cli(); - servos[channel].ticks = value; + servo_info[channel].ticks = value; SREG = oldSREG; } } @@ -299,10 +299,10 @@ 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; } int8_t Servo::move(int pin, int value) { int8_t ret; diff --git a/Marlin/servo.h b/Marlin/servo.h index ec7b662f4..b0b5ce5b1 100644 --- a/Marlin/servo.h +++ b/Marlin/servo.h @@ -112,7 +112,7 @@ typedef struct { typedef struct { ServoPin_t Pin; unsigned int ticks; -} servo_t; +} ServoInfo_t; class Servo { public: