Using Grbl’s Spindle PWM to Control a Servo

While I am waiting on some parts to arrive for a pen ‘bot, I started investigating ideas to control it. It will be using a hobby servo for pen up/down. Servos work great for things like this because they are easy to integrate and don’t need to be homed. They always know where they are in rotation.

I am probably going to use Grbl  and it outputs step and direction signals for motion.  A servo requires a PWM signal.  Since the spindle already uses PWM to set speed, this seems like a good place to start hacking. A PWM signal has a frequency and a duty cycle.  The frequency is how often the signal repeats and the duty cycle is how long the output signal is logic high (the rest of the time it is low). Servos want the signal to repeat every 20ms (50Hz). They want the duty cycle to be 1m long at one end of the rotation and 2ms at the other end.

  • 0 degree duty cycle = 1ms
  • 180 degree duty cycle 2ms
  • Frequency 50Hz (20ms)

PWM is a peripheral found on most micro controllers including Arduinos.  They use the timers built into CPU. You setup a few registers and the PWM runs independent of of the code. The spindle is using timer2.  All other timers are being used by other Grbl functions (on Unos), so they are not available. Timer2 is an 8 bit timer so it can only count up to 255.  To set the duty cycle you set a “compare” number that is 0-255. At the beginning of the cycle, the output will go high and the timer will begin counting.  When it reaches the compare value the output goes low. When it reaches 255, the cycle starts over again.

Issues

  • Resolution: Since the servo’s duty cycle range ( 1ms to 2ms ) is only a fraction of the frequency (1ms/20ms) , we only have 1/20 of the 256 counts at best. That is going to limit our resolution, but we only care about a rough pen up and pen down in this application.
  • Frequency: You don’t get to pick any frequency on Arduinos. You are limited to certain fractions of the CPU clock.  The allowable fractions are 1/1, 1/8, 1/64, 1/256 & 1/1024. We need to pick one that is close to getting us to 50Hz.

Setting the frequency.

The formula for the setting the frequency is..

Freq = F_CPU / (Prescaler * TimerResolution)    Eq #1

  • Freq: Our desired frequency of 50Hz
  • F_CPU: This is the frequency of the CPU. For most Arduino that is 16Mz
  • Prescaler: This is that fraction mentioned about.
  • TimerResolution: In our case this will be 256

This can be rewritten as this to find Prescaler…

Prescaler = F_CPU / (TimerResolution * Freq)  Eq #2

Prescaler = 16,000,000 / (256 * 50)

This yields 1250 as the desired prescaler. This is not an option so we pick the closest one of 1024.  If we plug that into Eq #1, we get 61Hz. That is close enough.

Determine the duty cycles.

We need to determine the compare values for each end of the servo rotation.  A unit of compare value is often called a “tick”. Each “tick” of the timer is…

Tick = 1 / (F_CPU / Prescaler) Eq3

Tick = 1 / (16,000,000 / 1024)

This yields a tick length of 0.000064 seconds. We then determine how many ticks are needed for each end of travel.

  • 0 degree = 0.001 / 0.000064. This yields 15.6 Ticks which needs to be rounded to 16
  • 180 degrees = 0.002 / 0.000064. This yields 31.25 Ticks which needs to be rounded to 31.

Setting The Resisters.

You need to deeply dive into the datasheets for these, but I’ll briefly explain them here.

TCCR2A = (1<<COM2A1) | ((1<<WGM20) | (1<<WGM21));
TCCR2B = (1<<CS22) | (1 <<CS21) | (1<<CS20);
OCR2A = 31;


  • First line: TCCR2A (Timer/Counter Control Register A for Timer 2).   WGM20 & WGM21 (Waveform Generation Modes) are setting the mode to Fast PWM.  The COM2A1 bit tells the CPU to clear the compare value when reached.
  • Second line: TCCR2B (Timer/Counter Control Register B for Timer 2) This sets the Clock Select bits to the value needed for a 1024 prescaler .
  • Third Line: Sets OCR2A (Output Compare Register A for Timer 2) to 31, which is the 180 degree value determined above.

Note: Grbl uses some aliases for the register names to make the code a little easier to read and more universal between Uno/Mega, so keep that in mind when reading the actual code.

Testing:

I loaded the firmware and hooked the output to my Saleae Logic 4 logic analyzer.

Here is the output at at the 0 degree rotation 1.087ms @ 61.07Hz

Here is the output at 180 degree rotation. 2.047 @ 61.07Hz

The Firmware.

I put everything into the a spindle_control.c file. Just replace it with the one Grbl comes with. Note: This is designed for Grbl 1.1f

 


/*
  spindle_control.c - spindle control methods
  Part of Grbl

  PEN_SERVO update by Bart Dring 8/2017
  Copyright (c) 2012-2017 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud

  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "grbl.h"

/*
Pen Servo: 

For a pen bot I want to use the spindle PWM to control a servo
When the spindle is on, the servo moves the pen down
When it is off the pen moves up

The spindle output is using a PWM, but we need to adjust that 

We only need a rough 

Use 1024 prescaler to get. ... 16,000,000 Mhz  / 1024 = 15625 Hz
It is an 8 bit timer so 15625 / 256 = 61 Hz. This is pretty close the the 50Hz recommended for servos
Each tick = 0.000064sec 
One end of servo is 0.001 sec (0.001 / 0.000064 = 15.6 ticks)
The other end is 0.002 sec (0.002 / 0.000064 = 31 ticks)


*/

#define PEN_SERVO
// these are full travel values. If you want to move less than full travel adjust these values
// If your servo is going the wrong way, swap them.
#define PEN_SERVO_DOWN     16      
#define PEN_SERVO_UP       31        

#ifdef VARIABLE_SPINDLE
  static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
#endif


void spindle_init()
{
  #ifdef VARIABLE_SPINDLE

    SPINDLE_PWM_DDR |= (1<<SPINDLE_PWM_BIT); // Configure as PWM output pin.
	
	#ifndef PEN_SERVO 
  
		// Configure variable spindle PWM and enable pin, if requried. On the Uno, PWM and enable are
		// combined unless configured otherwise.
		
		SPINDLE_TCCRA_REGISTER = SPINDLE_TCCRA_INIT_MASK; // Configure PWM output compare timer
		SPINDLE_TCCRB_REGISTER = SPINDLE_TCCRB_INIT_MASK;
    
	#else
		#ifdef CPU_MAP_ATMEGA2560
      	
		// not supported yet
		
	  #else
		
        SPINDLE_TCCRA_REGISTER = (1<<COM2A1) | ((1<<WGM20) | (1<<WGM21));
		TCCR2B = (1<<CS22) | (1 <<CS21) | (1<<CS20);
	    
	  #endif
	#endif
	
	
	
	#ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
      SPINDLE_ENABLE_DDR |= (1<<SPINDLE_ENABLE_BIT); // Configure as output pin.
    #else
      SPINDLE_DIRECTION_DDR |= (1<<SPINDLE_DIRECTION_BIT); // Configure as output pin.
    #endif

    pwm_gradient = SPINDLE_PWM_RANGE/(settings.rpm_max-settings.rpm_min);

  #else

    // Configure no variable spindle and only enable pin.
    SPINDLE_ENABLE_DDR |= (1<<SPINDLE_ENABLE_BIT); // Configure as output pin.
    SPINDLE_DIRECTION_DDR |= (1<<SPINDLE_DIRECTION_BIT); // Configure as output pin.

  #endif

  spindle_stop();
}


uint8_t spindle_get_state()
{
	#ifdef VARIABLE_SPINDLE
    #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
		  // No spindle direction output pin. 
			#ifdef INVERT_SPINDLE_ENABLE_PIN
			  if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
	    #else
	 			if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
	    #endif
    #else
      if (SPINDLE_TCCRA_REGISTER & (1<<SPINDLE_COMB_BIT)) { // Check if PWM is enabled.
        if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
        else { return(SPINDLE_STATE_CW); }
      }
    #endif
	#else
		#ifdef INVERT_SPINDLE_ENABLE_PIN
		  if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { 
		#else
		  if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) {
		#endif
      if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
      else { return(SPINDLE_STATE_CW); }
    }
	#endif
	return(SPINDLE_STATE_DISABLE);
}


// Disables the spindle and sets PWM output to zero when PWM variable spindle speed is enabled.
// Called by various main program and ISR routines. Keep routine small, fast, and efficient.
// Called by spindle_init(), spindle_set_speed(), spindle_set_state(), and mc_reset().
void spindle_stop()
{
	
  #ifndef PEN_SERVO	
	
	  #ifdef VARIABLE_SPINDLE
		SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
		#ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
		  #ifdef INVERT_SPINDLE_ENABLE_PIN
			SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
		  #else
			SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
		  #endif
		#endif
	  #else
		#ifdef INVERT_SPINDLE_ENABLE_PIN
		  SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
		#else
		  SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
		#endif
	  #endif
  
  #else	  
    SPINDLE_OCR_REGISTER = PEN_SERVO_UP;     
  #endif
  
}


#ifdef VARIABLE_SPINDLE
  // Sets spindle speed PWM output and enable pin, if configured. Called by spindle_set_state()
  // and stepper ISR. Keep routine small and efficient.
  void spindle_set_speed(uint8_t pwm_value)
  {
	
    #ifdef PEN_SERVO
		if (pwm_value == SPINDLE_PWM_OFF_VALUE) 
			spindle_stop();
		else // not off
			SPINDLE_OCR_REGISTER = PEN_SERVO_DOWN;
    #else	
	  
	  
		SPINDLE_OCR_REGISTER = pwm_value; // Set PWM output level.
		#ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
		  if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
			spindle_stop();
		  } else {
			SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
			#ifdef INVERT_SPINDLE_ENABLE_PIN
			  SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
			#else
			  SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
			#endif
		  }
		#else
		  if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
			SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
		  } else {
			SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled. } #endif #endif } #ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE // Called by spindle_set_state() and step segment generator. Keep routine small and efficient. uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit. { uint8_t pwm_value; rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value. // Calculate PWM register value based on rpm max/min settings and programmed rpm. if ((settings.rpm_min >= settings.rpm_max) || (rpm >= RPM_MAX)) {
        rpm = RPM_MAX;
        pwm_value = SPINDLE_PWM_MAX_VALUE;
      } else if (rpm <= RPM_MIN) { if (rpm == 0.0) { // S0 disables spindle pwm_value = SPINDLE_PWM_OFF_VALUE; } else { rpm = RPM_MIN; pwm_value = SPINDLE_PWM_MIN_VALUE; } } else { // Compute intermediate PWM value with linear spindle speed model via piecewise linear fit model. #if (N_PIECES > 3)
          if (rpm > RPM_POINT34) {
            pwm_value = floor(RPM_LINE_A4*rpm - RPM_LINE_B4);
          } else 
        #endif
        #if (N_PIECES > 2)
          if (rpm > RPM_POINT23) {
            pwm_value = floor(RPM_LINE_A3*rpm - RPM_LINE_B3);
          } else 
        #endif
        #if (N_PIECES > 1)
          if (rpm > RPM_POINT12) {
            pwm_value = floor(RPM_LINE_A2*rpm - RPM_LINE_B2);
          } else 
        #endif
        {
          pwm_value = floor(RPM_LINE_A1*rpm - RPM_LINE_B1);
        }
      }
      sys.spindle_speed = rpm;
      return(pwm_value);
    }
    
  #else 
  
    // Called by spindle_set_state() and step segment generator. Keep routine small and efficient.
    uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit.
    {
      uint8_t pwm_value;
      rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value.
      // Calculate PWM register value based on rpm max/min settings and programmed rpm.
      if ((settings.rpm_min >= settings.rpm_max) || (rpm >= settings.rpm_max)) {
        // No PWM range possible. Set simple on/off spindle control pin state.
        sys.spindle_speed = settings.rpm_max;
        pwm_value = SPINDLE_PWM_MAX_VALUE;
      } else if (rpm <= settings.rpm_min) {
        if (rpm == 0.0) { // S0 disables spindle
          sys.spindle_speed = 0.0;
          pwm_value = SPINDLE_PWM_OFF_VALUE;
        } else { // Set minimum PWM output
          sys.spindle_speed = settings.rpm_min;
          pwm_value = SPINDLE_PWM_MIN_VALUE;
        }
      } else { 
        // Compute intermediate PWM value with linear spindle speed model.
        // NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight.
        sys.spindle_speed = rpm;
        pwm_value = floor((rpm-settings.rpm_min)*pwm_gradient) + SPINDLE_PWM_MIN_VALUE;
      }
      return(pwm_value);
    }
    
  #endif
#endif


// Immediately sets spindle running state with direction and spindle rpm via PWM, if enabled.
// Called by g-code parser spindle_sync(), parking retract and restore, g-code program end,
// sleep, and spindle stop override.
#ifdef VARIABLE_SPINDLE
  void spindle_set_state(uint8_t state, float rpm)
#else
  void _spindle_set_state(uint8_t state)
#endif
{
  if (sys.abort) { return; } // Block during abort.
  if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm.
  
    #ifdef VARIABLE_SPINDLE
      sys.spindle_speed = 0.0;
    #endif
    spindle_stop();
  
  } else {
  
    #ifndef USE_SPINDLE_DIR_AS_ENABLE_PIN
      if (state == SPINDLE_ENABLE_CW) {
        SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
      } else {
        SPINDLE_DIRECTION_PORT |= (1<<SPINDLE_DIRECTION_BIT);
      }
    #endif
  
    #ifdef VARIABLE_SPINDLE
      // NOTE: Assumes all calls to this function is when Grbl is not moving or must remain off.
      if (settings.flags & BITFLAG_LASER_MODE) { 
        if (state == SPINDLE_ENABLE_CCW) { rpm = 0.0; } // TODO: May need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE);
      }
      spindle_set_speed(spindle_compute_pwm_value(rpm));
    #endif
    #if (defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && 
        !defined(SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED)) || !defined(VARIABLE_SPINDLE)
      // NOTE: Without variable spindle, the enable bit should just turn on or off, regardless
      // if the spindle speed value is zero, as its ignored anyhow.
      #ifdef INVERT_SPINDLE_ENABLE_PIN
        SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
      #else
        SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
      #endif    
    #endif
  
  }
  
  sys.report_ovr_counter = 0; // Set to report change immediately
}


// G-code parser entry-point for setting spindle state. Forces a planner buffer sync and bails 
// if an abort or check-mode is active.
#ifdef VARIABLE_SPINDLE
  void spindle_sync(uint8_t state, float rpm)
  {
    if (sys.state == STATE_CHECK_MODE) { return; }
    protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
    spindle_set_state(state,rpm);
  }
#else
  void _spindle_sync(uint8_t state)
  {
    if (sys.state == STATE_CHECK_MODE) { return; }
    protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
    _spindle_set_state(state);
  }
#endif

Share and Enjoy:
  • Print
  • Digg
  • StumbleUpon
  • del.icio.us
  • Facebook
  • Yahoo! Buzz
  • Twitter
  • Google Bookmarks
  1. No Comments

*