Fix Servo

MK/module/MK_Main.cpp

@@ -5683,15 +5683,7 @@ inline void gcode_M226() {
       if (code_seen('S')) {
         servo_position = code_value_short();
         if (servo_index >= 0 && servo_index < NUM_SERVOS && servo_index != DONDOLO_SERVO_INDEX) {
-          Servo *srv = &servo[servo_index];
-          #if HAS(SERVO_ENDSTOPS)
-            srv->attach(0);
-          #endif
-            srv->write(servo_position);
-          #if HAS(SERVO_ENDSTOPS)
-            delay_ms(SERVO_DEACTIVATION_DELAY);
-            srv->detach();
-          #endif
+          servo[servo_index].move(servo_position);
         }
         else if(servo_index == DONDOLO_SERVO_INDEX) {
           Servo *srv = &servo[servo_index];
@@ -5709,15 +5701,7 @@ inline void gcode_M226() {
       if (code_seen('S')) {
         servo_position = code_value_short();
         if (servo_index >= 0 && servo_index < NUM_SERVOS) {
-          Servo *srv = &servo[servo_index];
-          #if HAS(SERVO_ENDSTOPS)
-            srv->attach(0);
-          #endif
-          srv->write(servo_position);
-          #if HAS(SERVO_ENDSTOPS)
-            delay_ms(SERVO_DEACTIVATION_DELAY);
-            srv->detach();
-          #endif
+          servo[servo_index].move(servo_position);
         }
         else {
           ECHO_SM(ER, "Servo ");

MK/module/servo/servo.cpp

@@ -27,11 +27,10 @@
 #define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays
 
 static servo_t servos[MAX_SERVOS];                          // static array of servo 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
 
-static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
-
 // convenience macros
 #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
@@ -213,8 +212,7 @@ static boolean isTimerActive(timer16_Sequence_t timer)
 
 /****************** end of static functions ******************************/
 
-Servo::Servo()
-{
+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
@@ -224,64 +222,49 @@ Servo::Servo()
   }
 }
 
-uint8_t Servo::attach(int pin)
-{
+uint8_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)
-{
-  timer16_Sequence_t timer;
-
-  if (this->servoIndex < MAX_SERVOS) {
-    pinMode(pin, OUTPUT);                                   // set servo pin to output
-    servos[this->servoIndex].Pin.nbr = pin;
-    // 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
-    timer = SERVO_INDEX_TO_TIMER(servoIndex);
-    if (isTimerActive(timer) == false) {
-      initISR(timer);
-    }
-    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
-  }
+uint8_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;
 }
 
-void Servo::detach()
-{
-  timer16_Sequence_t timer;
-
+void Servo::detach() {
   servos[this->servoIndex].Pin.isActive = false;
-  timer = SERVO_INDEX_TO_TIMER(servoIndex);
-  if(isTimerActive(timer) == false) {
-    finISR(timer);
-  }
+  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if (!isTimerActive(timer)) finISR(timer);
 }
 
-void Servo::write(int value)
-{
-  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
-  if (value < MIN_PULSE_WIDTH)
-  {
-    if (value < 0)
-      value = 0;
-    else if (value > 180)
-      value = 180;
-
+void Servo::write(int value) {
+  if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+    if (value < 0) value = 0;
+    if (value > 180) value = 180;
     value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
   }
   this->writeMicroseconds(value);
 }
 
-void Servo::writeMicroseconds(int value)
-{
+void Servo::writeMicroseconds(int value) {
   // calculate and store the values for the given channel
   byte channel = this->servoIndex;
-  if( (channel < MAX_SERVOS) )   // ensure channel is valid
-  {
-    if (value < SERVO_MIN())          // ensure pulse width is valid
+  if (channel < MAX_SERVOS) {  // ensure channel is valid
+    if (value < SERVO_MIN())   // ensure pulse width is valid
       value = SERVO_MIN();
     else if (value > SERVO_MAX())
       value = SERVO_MAX();
@@ -300,13 +283,7 @@ void Servo::writeMicroseconds(int value)
 int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
 
 int Servo::readMicroseconds() {
-  unsigned int pulsewidth;
-  if (this->servoIndex != INVALID_SERVO)
-    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION;
-  else
-    pulsewidth  = 0;
-
-  return pulsewidth;
+  return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
 }
 
 bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }

MK/module/servo/servo.h

@@ -89,7 +89,7 @@ class 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.
     void detach();
-    void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds
+    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
     void move(int value);              // attach the servo, then move to value
                                        // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
@@ -97,10 +97,11 @@ class Servo {
     int read();                        // returns current pulse width as an angle between 0 and 180 degrees
     int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
     bool attached();                   // return true if this servo is attached, otherwise false
+
   private:
-     uint8_t servoIndex;               // index into the channel data for this servo
-     int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
-     int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   
+    uint8_t servoIndex;               // index into the channel data for this servo
+    int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH
+    int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
 };
 
 #endif