#include <AccelStepper.h>
#include "parameters.h"

// defines pins
#define stepPin1  5
#define stepPin2  6
#define freewheel 2
#define dirPin1   0
#define dirPin2   1

// Define the stepper motor and the pins that is connected to
AccelStepper stepper1(1, stepPin1, dirPin1); // (Type of driver: with 2 pins, STEP, DIR)
AccelStepper stepper2(1, stepPin2, dirPin2); // (Type of driver: with 2 pins, STEP, DIR)

float desiredSpeed1;
float desiredSpeed2;

void setup()
{
  // Sets the two pins as Outputs
  pinMode(stepPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);

  Serial.begin(9600);

  stepper1.setMaxSpeed(1000);
  stepper2.setMaxSpeed(1000);

  // desiredSpeed = 1 mm/s * 400 step/rev * 16 microstep/step / (mm/rev) 
  desiredSpeed1 = desiredTranslationSpeed      * 400    / (diameterWheel*3.14159 * microstepping); // microstep / second
  desiredSpeed2 = ratioPushPull * desiredSpeed1;

  Serial.print(" desiredSpeed1: ");
  Serial.print(desiredSpeed1);
  Serial.println("microStep / s");
  Serial.print(" desiredSpeed2: ");
  Serial.print(desiredSpeed2);
  Serial.println("microStep / s");
}

void loop()
{
  stepper1.setSpeed(desiredSpeed1);
  stepper2.setSpeed(desiredSpeed2);
  stepper1.runSpeed();
  stepper2.runSpeed();
}