#define EN 0   //PA4

#define DIR_1 3  // PA7
#define STEP_1 8 // PA1

#define DIR_2 2  // PA6
#define STEP_2 9 // PA2

#define DIR_3 1  // PA5
#define STEP_3 10 // PA3

#define MICROSTEP 1 // microstepping mode

#include <SpeedyStepper.h>

// idea to control: use a global timer and write a speed manager for each motor. -> use SpeedyStepper library, code refered to example 6
// Write a program that calculates the speed required to move in a certain direction

SpeedyStepper stepperRight;
SpeedyStepper stepperLeft;
SpeedyStepper stepperFront;



void setup() {
  // put your setup code here, to run once:
  pinMode(EN, OUTPUT);
  stepperRight.connectToPins(STEP_1, DIR_1);
  stepperLeft.connectToPins(STEP_3, DIR_3);
  stepperFront.connectToPins(STEP_2, DIR_2);

  digitalWrite(EN, LOW);
}

void loop() {
  // put your main code here, to run repeatedly:

  //This should prep it to rotate 1 rotation
  stepperRight.setSpeedInStepsPerSecond(1000); // 5 RPS
  stepperRight.setAccelerationInStepsPerSecondPerSecond(1000);
  stepperRight.setupRelativeMoveInSteps(1000);

  stepperLeft.setSpeedInStepsPerSecond(1000); // 5 RPS
  stepperLeft.setAccelerationInStepsPerSecondPerSecond(1000);
  stepperLeft.setupRelativeMoveInSteps(-1000);

  while((!stepperRight.motionComplete()) || (!stepperLeft.motionComplete())) {
    stepperRight.processMovement();
    stepperLeft.processMovement();
  }

  delay(1000);
}