Search This Blog

Thursday, March 3, 2011

Method for controlling 4 steppers from Arduino and two Motor Shields from Adafruit

This is so I can control the X Y and Z axis stepper motors of a CNC from one single arduino and can avoid a synchronized communication network and multiple Arduinos.  The fourth motor control will be reserved for tool adjustments if needed, it is extra for now.

I wanted to be able to allow all the motors to hold while others were running.  This hack succeeded in getting me 4 motors with full control.  I'm not attempting to microstep, I'm just using this for single stepping, I think maybe the way the library deals with microstepping it may not work because the PWM leads are shared.

Using two of these motor control boards:
http://www.adafruit.com/index.php?main_page=product_info&cPath=17_21&products_id=81

We are going to stack two motor shields on top of the Arduino.  Using stacking headers on the first instead of the headers that come with it.  Got the headers from Adafruit too.    http://www.adafruit.com/index.php?main_page=product_info&cPath=17_21&products_id=85 .   I used two sets of stacking headers because in order to drive my steppers I stacked another set of H bridge drivers on top and used a small 16pin dip heat sink from digikey.

So here comes the very subtle and elegant hack!
On top motor shield, cut pin 12 out of the header pins so it doesn't connect to the top board.  Short pin 12 and 13 on the top motor shield, because pin 13 is the new motor latch for the top shield, and needs to connect to the shield where 12 used to.   A nip and a solder blob and Bob's your uncle.


With the stack, one board will use the latch on pin 12, and the other on pin 13.
All the commands are sent to both, one board ignores (never latches) the commands for the other.  Picked pin 13 for the new latch because it is in the same register as 12 and will be clocked the same way by the port writes.   I don't think this technique is extensible to more boards, because there are no more pins.  pin3 and the analog pins are in different ports and wouldn't be synchronous.

Here you can see the Arduino with two motor shields on top, connected to stepper motors.  The third is out of the picture.   Motor power comes from an old computer ATX power supply 5V.

Some links to facts on the motor shield board:
http://www.ladyada.net/images/mshield/mshieldv1-schem.png

Digital pin 2, and 13 are not used.
Digital pin 11: DC Motor #1 / Stepper #1 (activation/speed control)
Digital pin 3: DC Motor #2 / Stepper #1 (activation/speed control)
Digital pin 5: DC Motor #3 / Stepper #2 (activation/speed control)
Digital pin 6: DC Motor #4 / Stepper #2 (activation/speed control)
Digital pin 4, 7, 8 and 12 (and now 13) are used to drive the DC/Stepper motors via the 74HC595 serial-to-parallel latch




OK, now at the end will be the modified library posted, but here is what I did.
Coding wise this may not be the most efficient method because of the redundant libaries, but it made the hack very easy to do w/o risk of messing up the code.


Copy the AFMotor directory in the arduino/libraries directory to AFMotorA.  In the copy, change the names of AFMotor.h and AFMotor.cpp to AFMotorA.h and AFMotorA.cpp.  Note that you will have to restart the Arduino software to see the new library.  


Now find the line in the AFcontrol.h file in the libraries directory
#define MOTOR_LATCH 12
and change it to

#define MOTOR_LATCHA 13

We need to change the function names to differentiate which board they apply to.
In AFMotorA.h  globally replace the text "AFMotor" with "AFMotorA" everywhere it appears.

In AFMotorA.cpp, also globally replace the text "AFMotor" with "AFMotorA" everywhere it appears.
Also replace MOTOR_LATCH with MOTOR_LATCHA everywhere it appears.

Now you have two libraries, which address different boards.  So here is a stepper test program that demos the four motors working:
Example Sketch


// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!
// modified by tomw to drive 2 shields at once
// This requires a modified library 
// 
//with the second having MOTORLATCH on pin 13, renamed MOTORLATCHA
//and several functions renamed to AFMotorA 

#include <AFMotor.h>
#include <AFMotorA.h>

AF_Stepper motorX(48, 1);
AF_Stepper motorY(48, 2);
AF_StepperA motorZ(48, 1);
AF_StepperA motorA(48, 2);

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Stepper test!");

  motorX.setSpeed(60);  // 60 rpm   
  motorY.setSpeed(60);  // 60 rpm   
  motorZ.setSpeed(60);  // 60 rpm   
  motorA.setSpeed(60);  // 60 rpm  
  
  delay(5000);  // wait for the supply to come up
  
  //step motors one step and back to power the coils
  motorX.step(1, FORWARD, SINGLE); 
  motorY.step(1, FORWARD, SINGLE);
  motorZ.step(1, FORWARD, SINGLE); 
  motorA.step(1, FORWARD, SINGLE);
  motorX.step(1, BACKWARD, SINGLE); 
  motorY.step(1, BACKWARD, SINGLE);
  motorZ.step(1, BACKWARD, SINGLE); 
  motorA.step(1, BACKWARD, SINGLE);
}

void loop() { 
  Serial.println("Single coil steps");
  motorX.step(480, FORWARD, SINGLE); 
  delay(1000);
  motorY.step(480, FORWARD, SINGLE); 
  delay(1000);
  motorZ.step(480, FORWARD, SINGLE); 
  delay(1000);
  motorA.step(480, FORWARD, SINGLE); 
  delay(1000);
  motorX.step(480, BACKWARD, SINGLE); 
  delay(1000);
  motorY.step(480, BACKWARD, SINGLE); 
  delay(1000);
  motorZ.step(480, BACKWARD, SINGLE); 
  delay(1000);
  motorA.step(480, BACKWARD, SINGLE); 
  delay(1000);
 // Now all runing at once!  Bravo!
  int i;
  for (i=0;  i<480; i++) {
    motorX.step(2, FORWARD, SINGLE); 
    motorY.step(1, FORWARD, SINGLE); 
    motorZ.step(2, FORWARD, SINGLE); 
    motorA.step(1, FORWARD, SINGLE); 
  }

}

Modified Libraries follow, AFmotorA.h and AFmotorA.cpp
////////////////////////AFMotorA.h///////////////////////////////////////////////////

// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!

#ifndef _AFMotorA_h_
#define _AFMotorA_h_

#include <inttypes.h>
#include <avr/io.h>

// comment out this line to remove microstepping support
// for smaller library. Be sure to delete the old library objects!
#define MICROSTEPPING 1

#ifdef MICROSTEPPING
#define MICROSTEP 8
#endif

#define MOTOR12_64KHZ _BV(CS20)  // no prescale
#define MOTOR12_8KHZ _BV(CS21)   // divide by 8
#define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32
#define MOTOR12_1KHZ _BV(CS22)  // divide by 64

#define MOTOR34_64KHZ _BV(CS00)  // no prescale
#define MOTOR34_8KHZ _BV(CS01)   // divide by 8
#define MOTOR34_1KHZ _BV(CS01) | _BV(CS00)  // divide by 64

#define MOTOR1_A 2
#define MOTOR1_B 3
#define MOTOR2_A 1
#define MOTOR2_B 4
#define MOTOR4_A 0
#define MOTOR4_B 6
#define MOTOR3_A 5
#define MOTOR3_B 7

#define FORWARD 1
#define BACKWARD 2
#define BRAKE 3
#define RELEASE 4

#define SINGLE 1
#define DOUBLE 2
#define INTERLEAVE 3


/*
#define LATCH 4
#define LATCH_DDR DDRB
#define LATCH_PORT PORTB

#define CLK_PORT PORTD
#define CLK_DDR DDRD
#define CLK 4

#define ENABLE_PORT PORTD
#define ENABLE_DDR DDRD
#define ENABLE 7

#define SER 0
#define SER_DDR DDRB
#define SER_PORT PORTB
*/

// Arduino pin names
#define MOTORLATCHA 13
#define MOTORCLK 4
#define MOTORENABLE 7
#define MOTORDATA 8

class AFMotorControllerA
{
  public:
    AFMotorControllerA(void);
    void enable(void);
    friend class AF_DCMotorA;
    void latch_tx(void);
};

class AF_DCMotorA
{
 public:
  AF_DCMotorA(uint8_t motornum, uint8_t freq =  MOTOR34_8KHZ);
  void run(uint8_t);
  void setSpeed(uint8_t);

 private:
  uint8_t motornum, pwmfreq;
};

class AF_StepperA {
 public:
  AF_StepperA(uint16_t, uint8_t);
  void step(uint16_t steps, uint8_t dir,  uint8_t style = SINGLE);
  void setSpeed(uint16_t);
  uint8_t onestep(uint8_t dir, uint8_t style);
  void release(void);
  uint16_t revsteps; // # steps per revolution
  uint8_t steppernum;
  uint32_t usperstep, steppingcounter;
 private:
};

uint8_t getlatchstate(void);

#endif

//////////////////////AFMotorA.cpp////////////////////////////////////////////////

// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!


#include <avr/io.h>
#include "WProgram.h"
#include "AFMotorA.h"

static uint8_t latch_state;

#define MICROSTEPS 16  // 8, 16 & 32 are popular

//#define MOTORDEBUG 1

AFMotorControllerA::AFMotorControllerA(void) {
}

void AFMotorControllerA::enable(void) {
  // setup the latch
  /*
  LATCH_DDR |= _BV(LATCH);
  ENABLE_DDR |= _BV(ENABLE);
  CLK_DDR |= _BV(CLK);
  SER_DDR |= _BV(SER);
  */
  pinMode(MOTORLATCHA, OUTPUT);
  pinMode(MOTORENABLE, OUTPUT);
  pinMode(MOTORDATA, OUTPUT);
  pinMode(MOTORCLK, OUTPUT);

  latch_state = 0;

  latch_tx();  // "reset"

  //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs!
  digitalWrite(MOTORENABLE, LOW);
}


void AFMotorControllerA::latch_tx(void) {
  uint8_t i;

  //LATCH_PORT &= ~_BV(LATCH);
  digitalWrite(MOTORLATCHA, LOW);

  //SER_PORT &= ~_BV(SER);
  digitalWrite(MOTORDATA, LOW);

  for (i=0; i<8; i++) {
    //CLK_PORT &= ~_BV(CLK);
    digitalWrite(MOTORCLK, LOW);

    if (latch_state & _BV(7-i)) {
      //SER_PORT |= _BV(SER);
      digitalWrite(MOTORDATA, HIGH);
    } else {
      //SER_PORT &= ~_BV(SER);
      digitalWrite(MOTORDATA, LOW);
    }
    //CLK_PORT |= _BV(CLK);
    digitalWrite(MOTORCLK, HIGH);
  }
  //LATCH_PORT |= _BV(LATCH);
  digitalWrite(MOTORLATCHA, HIGH);
}

static AFMotorControllerA MC;


/******************************************
               MOTORS
******************************************/
inline void initPWM1(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer2A on PB3 (Arduino pin #11)
    TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a
    TCCR2B = freq & 0x7;
    OCR2A = 0;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 11 is now PB5 (OC1A)
    TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a
    TCCR1B = (freq & 0x7) | _BV(WGM12);
    OCR1A = 0;
#endif
    pinMode(11, OUTPUT);
}

inline void setPWM1(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer2A on PB3 (Arduino pin #11)
    OCR2A = s;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 11 is now PB5 (OC1A)
    OCR1A = s;
#endif
}

inline void initPWM2(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer2B (pin 3)
    TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b
    TCCR2B = freq & 0x7;
    OCR2B = 0;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 3 is now PE5 (OC3C)
    TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c
    TCCR3B = (freq & 0x7) | _BV(WGM12);
    OCR3C = 0;
#endif
    pinMode(3, OUTPUT);
}

inline void setPWM2(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer2A on PB3 (Arduino pin #11)
    OCR2B = s;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 11 is now PB5 (OC1A)
    OCR3C = s;
#endif
}

inline void initPWM3(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer0A / PD6 (pin 6)
    TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A
    //TCCR0B = freq & 0x7;
    OCR0A = 0;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 6 is now PH3 (OC4A)
    TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a
    TCCR4B = (freq & 0x7) | _BV(WGM12);
    //TCCR4B = 1 | _BV(WGM12);
    OCR4A = 0;
#endif
    pinMode(6, OUTPUT);
}

inline void setPWM3(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer0A on PB3 (Arduino pin #6)
    OCR0A = s;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 6 is now PH3 (OC4A)
    OCR4A = s;
#endif
}



inline void initPWM4(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer0B / PD5 (pin 5)
    TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a
    //TCCR0B = freq & 0x7;
    OCR0B = 0;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 5 is now PE3 (OC3A)
    TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a
    TCCR3B = (freq & 0x7) | _BV(WGM12);
    //TCCR4B = 1 | _BV(WGM12);
    OCR3A = 0;
#endif
    pinMode(5, OUTPUT);
}

inline void setPWM4(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    // use PWM from timer0A on PB3 (Arduino pin #6)
    OCR0B = s;
#elif defined(__AVR_ATmega1280__) 
    // on arduino mega, pin 6 is now PH3 (OC4A)
    OCR3A = s;
#endif
}

AF_DCMotorA::AF_DCMotorA(uint8_t num, uint8_t freq) {
  motornum = num;
  pwmfreq = freq;

  MC.enable();

  switch (num) {
  case 1:
    latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0
    MC.latch_tx();
    initPWM1(freq);
    break;
  case 2:
    latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0
    MC.latch_tx();
    initPWM2(freq);
    break;
  case 3:
    latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0
    MC.latch_tx();
    initPWM3(freq);
    break;
  case 4:
    latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0
    MC.latch_tx();
    initPWM4(freq);
    break;
  }
}

void AF_DCMotorA::run(uint8_t cmd) {
  uint8_t a, b;
  switch (motornum) {
  case 1:
    a = MOTOR1_A; b = MOTOR1_B; break;
  case 2:
    a = MOTOR2_A; b = MOTOR2_B; break;
  case 3:
    a = MOTOR3_A; b = MOTOR3_B; break;
  case 4:
    a = MOTOR4_A; b = MOTOR4_B; break;
  default:
    return;
  }
  
  switch (cmd) {
  case FORWARD:
    latch_state |= _BV(a);
    latch_state &= ~_BV(b); 
    MC.latch_tx();
    break;
  case BACKWARD:
    latch_state &= ~_BV(a);
    latch_state |= _BV(b); 
    MC.latch_tx();
    break;
  case RELEASE:
    latch_state &= ~_BV(a);
    latch_state &= ~_BV(b); 
    MC.latch_tx();
    break;
  }
}

void AF_DCMotorA::setSpeed(uint8_t speed) {
  switch (motornum) {
  case 1:
    setPWM1(speed); break;
  case 2:
    setPWM2(speed); break;
  case 3:
    setPWM3(speed); break;
  case 4:
    setPWM4(speed); break;
  }
}

/******************************************
               STEPPERS
******************************************/

AF_StepperA::AF_StepperA(uint16_t steps, uint8_t num) {
  MC.enable();

  revsteps = steps;
  steppernum = num;

  if (steppernum == 1) {
    latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
      ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
    MC.latch_tx();
    
    // enable both H bridges
    pinMode(11, OUTPUT);
    pinMode(3, OUTPUT);
    digitalWrite(11, HIGH);
    digitalWrite(3, HIGH);

#ifdef MICROSTEPPING
    // use PWM for microstepping support
    initPWM1(MOTOR12_64KHZ);
    initPWM2(MOTOR12_64KHZ);
    setPWM1(255);
    setPWM2(255);
#endif

  } else if (steppernum == 2) {
    latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
      ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
    MC.latch_tx();

    // enable both H bridges
    pinMode(5, OUTPUT);
    pinMode(6, OUTPUT);
    digitalWrite(5, HIGH);
    digitalWrite(6, HIGH);

#ifdef MICROSTEPPING    
    // use PWM for microstepping support
    // use PWM for microstepping support
    initPWM3(1);
    initPWM4(1);
    setPWM3(255);
    setPWM4(255);
#endif
  }
}

void AF_StepperA::setSpeed(uint16_t rpm) {
  usperstep = 60000000 / (revsteps * rpm);
  steppingcounter = 0;
}

void AF_StepperA::release(void) {
  if (steppernum == 1) {
    latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
      ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
    MC.latch_tx();
  } else if (steppernum == 2) {
    latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
      ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
    MC.latch_tx();
  }
}

void AF_StepperA::step(uint16_t steps, uint8_t dir,  uint8_t style) {
  uint32_t uspers = usperstep;
  uint8_t ret = 0;

  if (style == INTERLEAVE) {
    uspers /= 2;
  }
#ifdef MICROSTEPPING
 else if (style == MICROSTEP) {
    uspers /= MICROSTEPS;
    steps *= MICROSTEPS;
#ifdef MOTORDEBUG
    Serial.print("steps = "); Serial.println(steps, DEC);
#endif
  }
#endif

  while (steps--) {
    ret = onestep(dir, style);
    delay(uspers/1000); // in ms
    steppingcounter += (uspers % 1000);
    if (steppingcounter >= 1000) {
      delay(1);
      steppingcounter -= 1000;
    }
  }
#ifdef MICROSTEPPING
  if (style == MICROSTEP) {
    //Serial.print("last ret = "); Serial.println(ret, DEC);
    while ((ret != 0) && (ret != MICROSTEPS)) {
      ret = onestep(dir, style);
      delay(uspers/1000); // in ms
      steppingcounter += (uspers % 1000);
      if (steppingcounter >= 1000) {
delay(1);
steppingcounter -= 1000;
      } 
    }
  }
#endif

}

uint8_t AF_StepperA::onestep(uint8_t dir, uint8_t style) {
  uint8_t a, b, c, d;
  uint8_t step;
  uint8_t mstep = 0;
#ifdef MICROSTEPPING
  uint8_t ocrb, ocra;
#endif
  if (steppernum == 1) {
    a = _BV(MOTOR1_A);
    b = _BV(MOTOR2_A);
    c = _BV(MOTOR1_B);
    d = _BV(MOTOR2_B);

#ifdef MICROSTEPPING
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    ocra = OCR2A;
    ocrb = OCR2B;
#elif defined(__AVR_ATmega1280__) 
    ocra = OCR1A;
    ocrb = OCR3C;
#endif

    if (style == MICROSTEP) {
      //TCCR2B = _BV(CS21);
    }
#endif
  } else if (steppernum == 2) {
    a = _BV(MOTOR3_A);
    b = _BV(MOTOR4_A);
    c = _BV(MOTOR3_B);
    d = _BV(MOTOR4_B);

#ifdef MICROSTEPPING
#if defined(__AVR_ATmega8__) || \
    defined(__AVR_ATmega48__) || \
    defined(__AVR_ATmega88__) || \
    defined(__AVR_ATmega168__) || \
    defined(__AVR_ATmega328P__)
    ocra = OCR0A;
    ocrb = OCR0B;
#elif defined(__AVR_ATmega1280__) 
    ocra = OCR4A;
    ocrb = OCR3A;
#endif

    if (style == MICROSTEP) {
      //TCCR0B = _BV(CS00);
    }   
#endif

  } else {
    return 0;
  }

#ifdef MOTORDEBUG
  Serial.print("a = "); Serial.print(ocra, DEC);
  Serial.print(" b = "); Serial.print(ocrb, DEC);
  Serial.print("\t");
#endif

  // OK next determine what step we are at 
  if ((latch_state & (a | b)) == (a | b))
    step = 1 * MICROSTEPS; 
  else if ((latch_state & (b | c)) == (b | c))
    step = 3 * MICROSTEPS; 
  else if ((latch_state & (c | d)) == (c | d))
    step = 5 * MICROSTEPS;
  else if ((latch_state & (d | a)) == (d | a))
    step = 7 * MICROSTEPS;
  else if (latch_state & a)
    step = 0;
  else if (latch_state & b)
    step = 2 * MICROSTEPS;
  else if (latch_state & c)
    step = 4 * MICROSTEPS;
  else
    step = 6 * MICROSTEPS;

  //Serial.print("step "); Serial.print(step, DEC); Serial.print("\t");
  // next determine what sort of stepping procedure we're up to
  if (style == SINGLE) {
    if ((step/MICROSTEPS) % 2) { // we're at an odd step, weird
      if (dir == FORWARD)
step = (step + MICROSTEPS) % (8*MICROSTEPS);
      else
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
    } else {           // go to the next even step
      if (dir == FORWARD)
step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
      else
step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);  

    }
#ifdef MICROSTEPPING
    ocra = 255;
    ocrb = 255;
#endif

  } else if (style == DOUBLE) {
    if (! (step/MICROSTEPS % 2)) { // we're at an even step, weird
      if (dir == FORWARD)
step = (step + MICROSTEPS) % (8*MICROSTEPS);
      else
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
    } else {           // go to the next odd step
      if (dir == FORWARD)
step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
      else
step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
    }
#ifdef MICROSTEPPING
    ocra = 255;
    ocrb = 255;
#endif
  } else if (style == INTERLEAVE) {
     if (dir == FORWARD)
       step = (step + 1*MICROSTEPS) % (8*MICROSTEPS);
     else
       step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
#ifdef MICROSTEPPING
    ocra = 255;
    ocrb = 255;
#endif
  } 
#ifdef MICROSTEPPING
  else if (style == MICROSTEP) {

    //Serial.print("Step #"); Serial.print(step/MICROSTEPS, DEC); Serial.print("\t");
    if (dir == FORWARD) {

      // if at even step, make sure its 'odd'
      if (! (step/MICROSTEPS) % 2) {
step = (step + MICROSTEPS) % (8*MICROSTEPS);
      }

      // fix hiccups from changing direction
      if (((ocra == 255) && ((step/MICROSTEPS)%4 == 3)) ||
 ((ocrb == 255) && ((step/MICROSTEPS)%4 == 1))) {
step += 2*MICROSTEPS;
step %= 8*MICROSTEPS;
      }

      if ((step == MICROSTEPS) || (step == 5*MICROSTEPS)) {
// get the current microstep
if (ocrb == 255)
 mstep = MICROSTEPS;
else
 mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print("uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep++;
mstep %= (MICROSTEPS+1);
if (mstep == MICROSTEPS)
 ocrb = 255;
else 
 ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" -> "); Serial.println(mstep, DEC);
#endif
if (mstep == MICROSTEPS)
 step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
      } else {
// get the current microstep
if (ocrb == 255)
 mstep = MICROSTEPS;
else
 mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print("uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep += MICROSTEPS;
mstep %= (MICROSTEPS+1);
if (mstep == MICROSTEPS)
 ocrb = 255;
else 
 ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" +> "); Serial.println(mstep, DEC);
#endif
if (mstep == 0)
 step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
      }
    } else {

      // fix hiccups from changing direction
      if (((ocra == 255) && ((step/MICROSTEPS)%4 == 1)) ||
 ((ocrb == 255) && ((step/MICROSTEPS)%4 == 3))) {
step = (step + 6*MICROSTEPS);
step %= (8*MICROSTEPS);
      }

      // if at even step, make sure its 'odd'
      if (! (step/MICROSTEPS % 2)) {
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
      }
      if ((step == MICROSTEPS) || (step == 5*MICROSTEPS)) {
// get the current microstep
if (ocrb == 255)
 mstep = MICROSTEPS;
else
 mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print(" uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep += MICROSTEPS;
mstep %= (MICROSTEPS+1);
if (mstep == MICROSTEPS)
 ocrb = 255;
else
 ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" !> "); Serial.println(mstep, DEC);
#endif
if (mstep == 0)
 step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
      } else {
// get the current microstep
if (ocrb == 255)
 mstep = MICROSTEPS;
else
 mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print("uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep++;
mstep %= (MICROSTEPS + 1);
if (mstep == MICROSTEPS)
 ocrb = 255;
else 
 ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" *> "); Serial.println(mstep, DEC);
#endif
if (mstep == MICROSTEPS)
 step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
      }
    }
  }


  //Serial.print(" -> step = "); Serial.print(step/MICROSTEPS, DEC); Serial.print("\t");

  if (steppernum == 1) {
    setPWM1(ocra);
     setPWM2(ocrb);
  } else if (steppernum == 2) {
    setPWM3(ocra);
    setPWM4(ocrb);
  }

#endif  // microstepping

  // release all
  latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0

  //Serial.println(step, DEC);

  switch (step/MICROSTEPS) {
  case 0:
    latch_state |= a; // energize coil 1 only
    break;
  case 1:
    latch_state |= a | b; // energize coil 1+2
    break;
  case 2:
    latch_state |= b; // energize coil 2 only
    break;
  case 3:
    latch_state |= b | c; // energize coil 2+3
    break;
  case 4:
    latch_state |= c; // energize coil 3 only
    break; 
  case 5:
    latch_state |= c | d; // energize coil 3+4
    break;
  case 6:
    latch_state |= d; // energize coil 4 only
    break;
  case 7:
    latch_state |= d | a; // energize coil 1+4
    break;
  }
  MC.latch_tx();
  return mstep;
}







1 comment:

  1. Hi,
    i stumbled upon this very usefull information while searching for the possibility to control a small 3 axis cnc with 2 adafruit boards and single strepping does not provide enough torque but double coil does. So before i start modding my stepper shields a short question, did you ever tried the 'DOUBLE' coil mode?
    Many thanks in advance,
    Smai-Lee

    ReplyDelete