/**
 * Author Teemu Mäntykallio
 * Initializes the library and runs the stepper
 * motor in alternating directions.
 * Serial only version
 */

#include <TMCStepper.h>

#define M1_EN_PIN 7
#define M2_EN_PIN 6
#define M3_EN_PIN 5
#define M4_EN_PIN 4

#define R_SENSE 0.11f  // Match to your driver \
                       // SilentStepStick series use 0.11 \
                       // UltiMachine Einsy and Archim2 boards use 0.2 \
                       // Panucatt BSD2660 uses 0.1 \
                       // Watterott TMC5160 uses 0.075

HardwareSerial& SERIAL_PORT = Serial1;

TMC2209Stepper driver[4] = { TMC2209Stepper(&SERIAL_PORT, R_SENSE, 0),
                             TMC2209Stepper(&SERIAL_PORT, R_SENSE, 1),
                             TMC2209Stepper(&SERIAL_PORT, R_SENSE, 2),
                             TMC2209Stepper(&SERIAL_PORT, R_SENSE, 3) };


void setup() {
  int m;

  pinMode(M1_EN_PIN, OUTPUT);
  pinMode(M2_EN_PIN, OUTPUT);
  pinMode(M3_EN_PIN, OUTPUT);
  pinMode(M4_EN_PIN, OUTPUT);
  Serial.begin(115200);
  SERIAL_PORT.begin(19200, SERIAL_8N1, 20, 21);

  for (m = 0; m < 4; m++) {
    driver[m].begin();
    driver[m].VACTUAL(0);
    driver[m].toff(5);  // Enables driver in software
    //driver[m].rms_current(600);        // Set motor RMS current
    driver[m].microsteps(256);  // Set microsteps to 1/16th

    //driver[m].en_spreadCycle(false);   // Toggle spreadCycle on TMC2208/2209/2224
    // driver[m].pwm_autoscale(true);     // Needed for stealthChop
  }
  digitalWrite(M1_EN_PIN, LOW);  // Enable driver in hardware
  digitalWrite(M2_EN_PIN, LOW);  // Enable driver in hardware
  digitalWrite(M3_EN_PIN, LOW);  // Enable driver in hardware
  digitalWrite(M4_EN_PIN, LOW);  // Enable driver in hardware
}

bool shaft = false;

void loop() {
  float ratio=1/0.715f;
  int m=1;
  // Run 5000 steps and switch direction in software
  driver[m].VACTUAL(256*ratio*200); // Will pulse steps at the frequency/0.715 (but argument is a int). Depend on microsteps config, and steps per rotation. Ex for 1 turn per second
  delay(10000);
  //Serial.print("Steps: ");
  //Serial.println(driver[m].MSCNT());
  //delay(1);
  //driver[m].VACTUAL(0);
  shaft = !shaft;
  driver[1].shaft(shaft);
}
