// https://wokwi.com/projects/429712639479753729
//
// Based on the https://hackaday.io/project/183713/instructions
// for https://hackaday.io/project/183279-accelstepper-the-missing-manual/details
// example Exper3 for step 6 with code from UnoAccelStepperExper3.ino
// from https://hackaday.io/project/183713/files
//
// This code uses a small state machine to switch between moving, cruising, and stopping
//  The state machine allows switching between acceleration and cruiseing modes,
// as well as changing behavior based on time, speed
// and also changing the acceleration curves and targets in different steps
// 
//
// Other example simulations https://forum.arduino.cc/t/wokwi-simulations-for-arduino-built-in-examples/1304754

// Include the AccelStepper Library
#include <AccelStepper.h>
#include <elapsedMillis.h>

// Motor Connections (unipolar motor driver)
const int In1 = 8;
const int In2 = 9;
const int In3 = 10;
const int In4 = 11;
// Motor Connections (constant voltage bipolar H-bridge motor driver)
const int AIn1 = 8;
const int AIn2 = 9;
const int BIn1 = 10;
const int BIn2 = 11;
// Motor Connections (constant current, step/direction bipolar motor driver)
const int dirPin = 4;
const int stepPin = 5;

// Creates an instance - Pick the version you want to use and un-comment it. That's the only required change.
//AccelStepper myStepper(AccelStepper::FULL4WIRE, AIn1, AIn2, BIn1, BIn2);  // works for TB6612 (Bipolar, constant voltage, H-Bridge motor driver)
//AccelStepper myStepper(AccelStepper::FULL4WIRE, In1, In3, In2, In4);    // works for ULN2003 (Unipolar motor driver)
AccelStepper myStepper(AccelStepper::DRIVER, stepPin, dirPin);           // works for a4988 (Bipolar, constant current, step/direction driver)

// define the time limit to run before stopping
const int timeLimit = 10;  // number of seconds

// State definitions
#define RSPD 01
#define RJSTR 02
#define STOPPED 03
// State variable
int state;

elapsedMillis printTime;

void setup() {
  Serial.begin(115200);
  state = RJSTR;   // initial state is run, just run
  // set the maximum speed, acceleration factor,
  // and the target position
  myStepper.setMaxSpeed(400.);
  myStepper.setAcceleration(50.0);
  myStepper.moveTo(10000);
}
int lCount = 0; // elapsed seconds
void loop() {
  float mSpeed;
  if (printTime >= 1000) {
    printTime = 0;
    lCount++;
    mSpeed = myStepper.speed();
    Serial.print(mSpeed);
    Serial.print("  ");
    Serial.println(myStepper.currentPosition());
    switch (state) {
      //digitalWrite(markerPin,HIGH);    // just for testing
      case RSPD:
        if (lCount >= timeLimit) {
          myStepper.setAcceleration(200.0);  // this makes motor stop much quicker!
          myStepper.stop();
          myStepper.runToPosition();   // go immediately to stop position!
          state = STOPPED;
        }
        break;
      case RJSTR:
        if (abs(mSpeed) >= 200.0) {
          state = RSPD;   // switch to run speed state when target speed is reached
        }
        break;
      case STOPPED:
        if (lCount > 2 * timeLimit) {
          // this is an addition to the stock example, to make it pause and bounce back
          lCount = 0;
          state = RJSTR;
          myStepper.setAcceleration(50.0);
          myStepper.move(myStepper.currentPosition() > 0 ? -10000 : 10000);
        }
        break;
    }
  }
  switch (state) {
    case RSPD:
      myStepper.runSpeed();
      break;
    case RJSTR:
      myStepper.run();
      break;
    case STOPPED:
      break;
  }
}