//#include <TinyDebug.h>
// NOTE: Top 2 steppers are set to 1/4 microstepping.  Delete red wires to MS2 on each driver to restore full step

int latchPin = PB0;//Pin connected to ST_CP of each shift register
int clockPin = PB1;  //Pin connected to SH_CP of each shift register
int dataPin = PB2; //Pin connected to DS of first shift register

byte motStep[4] = {128,32,8,2};
byte motDir[4] = {64,16,4,1};

// Set motor reference to axis
byte x = motStep[0];
byte y = motStep[1];
byte z = motStep[2] + motStep[3]; // Linked motors, dual axis

// Direction byte mask
byte xd = motDir[0];
byte yd = motDir[1];
// Assumes linked motors rotate in the same direction, invert/xor (~) or not (!) the value to reverse it
// eg 'byte zd = motDir[2] + (motDir[3] ^ motDir[3]);'..or "~"...or "!"
byte zd = motDir[2] + !motDir[3]; 

void setup() {
  //Debug.begin();
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
}

void motMove(int steps, int speed, byte mots, byte dir) { // stepcount, delay millis(low is faster)
  for(int i = 0; i < steps; i++){
    digitalWrite(latchPin, LOW);
    shiftOut(dataPin, clockPin, LSBFIRST, mots);
    digitalWrite(latchPin, HIGH);
    delayMicroseconds(speed);
    digitalWrite(latchPin, LOW);
    shiftOut(dataPin, clockPin, LSBFIRST, dir);
    digitalWrite(latchPin, HIGH);
    delayMicroseconds(speed);
    
  }
}

void loop() {
  // If not explictly set, dir will be clockwise (normal)
  byte mots = x + y + z;
  byte dir = xd + !yd + zd; // x clockwise, y counter clockwise, z..whatever direction for linked

  motMove(200,2000,mots,dir); // Steps to move, delay between steps, axis combo, direction combo
  delay(2000);
}