/*
   Uno sketch to drive a stepper motor using the AccelStepper library.
   Function runSpeed() is used to run the motor at constant speed. A pot is read to vary the speed.
   Works with a ULN-2003 unipolar stepper driver, or a bipolar, constant voltage motor driver
   such as the L298 or TB6612, or a step/direction constant current driver like the a4988.
   A potentiometer is connected to analog input 0 and to gnd and 5v.

 The motor will rotate one direction. The higher the potentiometer value,
 the faster the motor speed. Because setSpeed() sets the delay between steps,
 you may notice the motor is less responsive to changes in the sensor value at
 low speeds.

 Created 30 Nov. 2009
 Modified 28 Oct 2010
 by Tom Igoe
 12/26/21  Modified to use AccelStepper.  --jkl

 */
// Include the AccelStepper Library


#include "AccelStepper.h" 
// Library created by Mike McCauley at http://www.airspayce.com/mikem/arduino/AccelStepper/

// AccelStepper Setup
AccelStepper stepperX(1, 3, 2);   // 1 = Easy Driver interface
                                  // NANO Pin 2 connected to STEP pin of Easy Driver
                                  // NANO Pin 3 connected to DIR pin of Easy Driver

// Define the Pins used
#define home_switch 12 // Pin 12 connected to Home Switch (MicroSwitch)
// enable pin por stepper driver
#define enable_driver 10  // Pin 12 connected to Home Switch (MicroSwitch)

//Define analog input pin for ultrasonic sensor
#define sensorin A0

#define estop_switch 11 // Pin 11 connected to Estop
// Stepper Travel Variables
long TravelX;  // Used to store the X value entered in the Serial Monitor
int move_finished=1;  // Used to check if move is completed
long initial_homing=-1;  // Used to Home Stepper at startup

const int maxSpeedLimit = 2000.0;  // set this to the maximum speed you want to use.
const int maxAccelLimit = 1000.0;  // set this to the maximum acc you want to use.
void setup() {
   Serial.begin(9600);  // Start the Serial monitor with speed of 9600 Bauds
   
   pinMode(home_switch, INPUT_PULLUP);//definition home switch must be updated to sensor
   pinMode(estop_switch, INPUT_PULLUP); //definition Estop switch
   pinMode(enable_driver, OUTPUT); //definition enable for stepper driver
   delay(5);  // Wait for driver wake up

   //  Set Max Speed and Acceleration of each Steppers at startup for homing
  stepperX.setMaxSpeed(100.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepperX.setAcceleration(100.0);  // Set Acceleration of Stepper

 // Start check Estop
 while(digitalRead(estop_switch)) {
  Serial.println("Stop Emergencia presionado . . ");
  digitalWrite(enable_driver, HIGH);
  delay(1000);
  digitalWrite(enable_driver, LOW); //enable stepper driver
 }
//  reset function

void(* resetFunc) (void) = 0;

// Start Homing procedure of Stepper Motor at startup

  Serial.print("Stepper is Homing . . . . . . . . . . . ");

  while (digitalRead(home_switch)) {  // Make the Stepper move CCW until the switch is activated   
    stepperX.moveTo(initial_homing);  // Set the position to move to
    initial_homing--;  // Decrease by 1 for next move if needed
    stepperX.run();  // Start moving the stepper
    if (digitalRead(estop_switch)) {
      digitalWrite(enable_driver, HIGH);
      resetFunc();

    }
    delay(5);
}

  stepperX.setCurrentPosition(0);  // Set the current position as zero for now
  stepperX.setMaxSpeed(100.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepperX.setAcceleration(100.0);  // Set Acceleration of Stepper
  initial_homing=1;

  while (!digitalRead(home_switch)) { // Make the Stepper move CW until the switch is deactivated
    stepperX.moveTo(initial_homing);  
    stepperX.run();
    initial_homing++;
    if (digitalRead(estop_switch)) {
      digitalWrite(enable_driver, HIGH);
      resetFunc();

    }
    delay(5);
  }
  
  stepperX.setCurrentPosition(0);
  Serial.println("Homing Completed");
  Serial.println("");
  stepperX.setMaxSpeed(maxSpeedLimit);      // Set Max Speed of Stepper (Faster for regular movements)
  stepperX.setAcceleration(maxAccelLimit);  // Set Acceleration of Stepper
  stepperX.setSpeed(maxSpeedLimit);    // initial speed target

}

void loop() {
float mSpeed;
int SensorReadedValue = analogRead(sensorin);
 
//Serial.print("Valor leido del sensor");
//Serial.print("   ");
//Serial.print(SensorReadedValue);
// map it to a the maximum speed range
float motorSpeed = map(SensorReadedValue, 0, 1023, maxSpeedLimit, -maxSpeedLimit);
//Serial.print("   ");
//Serial.print("motorSpeed");
//Serial.print("   ");
//Serial.print(motorSpeed);
//Serial.print("   ");
// set the motor speed:
stepperX.setSpeed(motorSpeed);

mSpeed = stepperX.speed();
//Serial.print("Valor velocidad");
//Serial.print("   ");
 // Serial.println(mSpeed);
delay(10);
stepperX.runSpeed();
}