Saturday, 25 September 2010

Arduino Stepper Driver v2

Here is a quick post of the Arduino code (interrupt service routine) I used to trial run the stepper motor. It's quick and dirty, so beware! It uses timer 3 to create the step signal. The main loop is responsible for acceleration. It does so by monitoring time and updating speed. Obviously, this is not perfect, but it's a start.


// Quick and Dirty Stepper Interrupt Service Routine
// Lets your Arduino Mega drive a stepper motor
// via a timer.
//
//
//
// Copyright (C) 2010 cncorbust.com
// This program 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.
// This program 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 this program.  If not, see www.gnu.org.
//
//
// Uses timer 3
//
// Pin 5 outputs step signal
// Pin 11 outputs direction signal
//
const int directionPin = 11;
const int stepPin = 5; // tied to timer 3; don't change this.

volatile long pos3;
volatile long delay3;
volatile int dir3; // -1 0 1

// Keep track of position
ISR(TIMER3_COMPA_vect)
{
  pos3+=dir3;
}

// call this to query current position
long getMotorPosition()
{
  cli();
  long r = pos3;
  sei();
  return r;
}

// Call to get started
void initMotor()
{
  digitalWrite(directionPin,0);
  pinMode(directionPin,OUTPUT);

  cli();
  pos3 = 0;
  dir3 = 0;
 
  TCCR3A = B01000001;
  TCCR3B = B00010000;  // no clock source, so stopped
  TIMSK3 = B00000010;
  OCR3AH = 255;  // just in case someone starts it without setting speed
  OCR3AL = 255;
  TCNT3H = 0;  // reset timer to trigger update of TOP from OCR3A
  TCNT3L = 0;
  pinMode(stepPin,OUTPUT);
  sei();
}

// set to 0 to stop motor
// set to negative value to move backwards
// be careful not to exceed max values (which depend on prescale factor)
void setMotorStepDelay(long microsecondsPerStep)
{
  if(microsecondsPerStep==0)
  {
    cli();
    dir3 = 0;
    delay3 = 0L;
    TCCR3B = B00010000;  // no clock source, so timer is stopped
    sei();
    return;
  }

  // 1/64 prescale:  x/1m*16m/4/64 (/4 because of timer mode)
  // long clockTicksPerStep = microsecondsPerStep / 16;  

  // 1/8 prescale: x/1m*16m/4/8
  long clockTicksPerStep = microsecondsPerStep / 2;

  // Just in case. Still many ways to break this...
  if(clockTicksPerStep>65535)
    clockTicksPerStep = 65535;
  else if(clockTicksPerStep<-65535)
    clockTicksPerStep = -65535;
 
  // horribly long time to block interrupts below...
  // Which part of "quick and dirty" did you not understand?

  cli();

  // TCCR3B = B00010011;  // 1/64 prescale
  TCCR3B = B00010010;  // 1/8 prescale
 
  if(clockTicksPerStep<0)
  {
    clockTicksPerStep = -clockTicksPerStep;
    dir3 = -1;
    digitalWrite(directionPin,1);  // 1 is backward on my setup
  }
  else
  {
    dir3 = 1;
    digitalWrite(directionPin,0);
  }

  OCR3AH = clockTicksPerStep >> 8;   // set delay, high byte first
  OCR3AL = clockTicksPerStep;
  if(delay3==0)
  {
    // we're starting from a stopped state, so kill the previous timer step...
    // reset timer to trigger update of TOP from OCR3A
    TCNT3H = 0;
    TCNT3L = 0;
  }
 
  delay3 = microsecondsPerStep;
 
  sei();
}


//------------------------------------------------------------------
//------------------------------------------------------------------
//------------------------------------------------------------------

// Your program here....


void setup()
{
  pinMode(13,OUTPUT); // led pin
  initMotor();
}

// Helper method to move motor slowly
void adjustPosition()
{
  digitalWrite(13,HIGH);  // Led indicator
  setMotorStepDelay(500); // 2000 micro-steps per second
  while(true);
}

// Main routine. Moves stepper back and forward
void doMainRun()
{
  int direction = -1;
  while(true)
  {
    // A 1 second stand-still, to engage/disengage motor by hand
    digitalWrite(13,HIGH);
    delay(1000);
    digitalWrite(13,LOW);
 
    long acceleration = 120000L; // steps per second per second
    long maxSpeed = 10L * 200L * 25L; // steps per second
    long travelDistance = 200L * 10L * 40;  // how far at full speed (roughly only!)
 
    long fullSpeedDuration = travelDistance * 1000L / maxSpeed; // duration in ms
    long baseTime;
    long dt;
    long speed;
 
    // speed up
    baseTime = millis();
    do
    {
    dt = millis() - baseTime;
    speed = dt * acceleration / 1000L;
    if(speed>maxSpeed) speed = maxSpeed;
    if(speed==0)
      setMotorStepDelay(0);
    else
      setMotorStepDelay(1000000L/speed*direction);
    } while(speed<maxSpeed);

    // go at full throttle
    delay(fullSpeedDuration); // let it run for a while

    // slow down
    baseTime = millis();
    do
    {
    dt = millis() - baseTime;
    speed = maxSpeed - dt * acceleration / 1000L;
    if(speed>maxSpeed) speed = maxSpeed;
    if(speed<=0)
      setMotorStepDelay(0);
    else
      setMotorStepDelay(1000000L/speed*direction);
    } while(speed>0);

    // Now go in reverse and do it all again.
    // It's like the share market...
    direction = -direction;
  }
}

void loop()
{
  doMainRun();
//  adjustPosition();
}

1 comment:

Andy said...

Excellent, thanks for posting your code! Hopefully I'll have time to try it out tomorrow.