/*
  Forum: https://forum.arduino.cc/t/stepper-motor-precision/1213294
  Wokwi: https://wokwi.com/projects/387378409878463489

*/


#include <Arduino.h>
#include <Wire.h>

#include <AccelStepper.h>

#define DEG_PER_STEP 1.8
#define STEP_PER_REVOLUTION (360 / DEG_PER_STEP)
#define DIR_PIN 18
#define STEP_PIN 19
#define TOUCHABLE 12
#define TOUCHABLE_THRESHOLD 60
#define MOTOR_INTERFACE_TYPE 1

#define MAX_SPEED 1500
#define SPEED 1000
#define ACCELERATION 6000
#define ROUNDS 20

//Just to simulate the Touch with a button
const byte buttonPin = 14;

// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(MOTOR_INTERFACE_TYPE, STEP_PIN, DIR_PIN);

void printSpeed();
void constantMove();

void setup() {
  Serial.begin(115200);
  pinMode(buttonPin, INPUT_PULLUP);

  // Set the maximum speed in steps per second:
  stepper.setMaxSpeed(MAX_SPEED);
  stepper.setAcceleration(ACCELERATION);
  stepper.setCurrentPosition(0);
}


uint32_t lastTime = millis();
int32_t lastPos = 0;
bool isRuning = false;

void loop() {

  int touch = touchRead(TOUCHABLE);
  touch = isTouchedByButton();
  if (touch < TOUCHABLE_THRESHOLD && !isRuning) {
    Serial.printf("Touched: %d, Starting... \n", touch);

    int posToGo = ROUNDS * STEP_PER_REVOLUTION;
    int speed = SPEED;

    if (posToGo == stepper.currentPosition()) {
      posToGo = 0;
      speed = -speed;
    }

    stepper.moveTo(posToGo);
    stepper.setSpeed(speed);

    isRuning = true;
    delay(200);
  }

  if (stepper.distanceToGo() == 0 && isRuning == true) {
    Serial.printf("Finished! CurrentPos: %d\n", stepper.currentPosition());
    isRuning = false;
  }

  printSpeed();

  //constantMove();
  stepper.run();
}

void constantMove() {
  stepper.setSpeed(SPEED);
  stepper.runSpeed();
}

void printSpeed() {

  const int32_t pos = stepper.currentPosition();
  static const int printPerRounds = 5;
  if (pos % (( int )STEP_PER_REVOLUTION * printPerRounds) == 0 && lastPos != pos) {
    const uint32_t currentTime = millis();
    Serial.printf("Current pos = %d,  Time = %d \n", stepper.currentPosition(), (currentTime - lastTime));
    lastTime = millis();
    lastPos = pos;
  }
}

int isTouchedByButton(){
  if (digitalRead(buttonPin) == LOW) {
    return 0;
  }
  return 100;

}