#include <Arduino.h>


#include <AccelStepper.h>

// https://learn.watterott.com/de/silentstepstick/pinconfig/tmc2130/


#define EN_PIN           D0
#define ROLL_STEP_PIN D8
#define ROLL_DIR_PIN D7

#define PITCH_STEP_PIN D6
#define PITCH_DIR_PIN D5

AccelStepper stepperPitch(AccelStepper::DRIVER, PITCH_STEP_PIN, PITCH_DIR_PIN);

AccelStepper stepperRoll(AccelStepper::DRIVER, ROLL_STEP_PIN, ROLL_DIR_PIN);

unsigned long stepsPerRotationPitch=16*42*400/12; //12z to 42z, 16x microstepping

unsigned long stepsPerRotationRoll=16*85*400/17;//17z to 85z, 16x microstepping

void setup() {
  Serial.begin(115200);
  
  stepperPitch.setMaxSpeed(15000);
  stepperPitch.setAcceleration(80000.0);
  stepperPitch.moveTo(stepsPerRotationPitch/2);

  stepperRoll.setMaxSpeed(15000);
  stepperRoll.setAcceleration(80000.0);
  stepperRoll.moveTo(stepsPerRotationRoll/8);
}

void loop() {
  static unsigned long last_change;

  if (stepperPitch.distanceToGo() == 0) {
    if (stepperPitch.currentPosition()>100) {
      Serial.print("Pitch Moving to: "); Serial.println(0);
      stepperPitch.moveTo(0);
    }else{
      Serial.print("Pitch Moving to: "); Serial.println(stepsPerRotationPitch/2);
      stepperPitch.moveTo(stepsPerRotationPitch/2);
    }
  }

  if (stepperRoll.distanceToGo() == 0) {
    if (stepperRoll.currentPosition()>100) {
      Serial.print("Roll Moving to: "); Serial.println(0);
      stepperRoll.moveTo(0);
    }else{
      Serial.print("Roll Moving to: "); Serial.println(stepsPerRotationRoll/8);
      stepperRoll.moveTo(stepsPerRotationRoll/8);
    }
  }
  
  bool resultPitch = stepperPitch.run();
  bool resultRoll = stepperRoll.run();

  
  if ( millis() > last_change+500 ) {
    last_change=millis();
    //Serial.print("run="); Serial.print(result);
    //Serial.print(" dist="); Serial.print(stepperPitch.distanceToGo());
    //Serial.print(" pos="); Serial.print(stepperPitch.currentPosition());
    //Serial.println();
  }
  
}