/*
 * Arduio Uno control stepper motor drive for att and phase in LINAC
 * limit travel range by 2 limits switch and control moving my 2 switchs
 * Created by Mr.Chatchabhumi Dhammatong
 *
 *
 * Thank you for tutorial as below:
 * Tutorial page: https://arduinogetstarted.com/tutorials/arduino-stepper-motor-and-limit-switch
 */

#include <ezButton.h>
#include <AccelStepper.h>
//#include "Arduino_DebugUtils.h"

// axial Atten 2
#define M1_DIR 8
#define M1_STEP 9
#define AXIS1_UP_LIM A0
#define AXIS1_LO_LIM A1
#define AXIS1_UP_SW A2
#define AXIS1_LO_SW A3

// axial Phase 2
#define M2_DIR 10
#define M2_STEP 11
#define AXIS2_UP_LIM A4
#define AXIS2_LO_LIM A5
#define AXIS2_UP_SW A6
#define AXIS2_LO_SW A7

#define DEBUG 1
#define DIRECTION_CCW -1
#define DIRECTION_CW 1

#define STATE_STOP 0
#define STATE_STANDBY 10
#define STATE_CHANGE_DIR 1
#define STATE_MOVE 2
#define STATE_MOVING 3
#define STATE_MOV_CW 4
#define STATE_MOV_CCW 5
#define STATE_LIMIT_1 6
#define STATE_LIMIT_2 7
#define STATE_QUICKSTOP 99

#define MAX_POSITION 0x7FFFFFFF // maximum of position we can set (long type)
#define MIN_POSITION 0          // mm

#define MAX_SPEED 1000.0 // step per secound
#define ACCELERATE 400   // step per sec^2
#define INIT_SPEED 100.0
#define RUN_SPEED 600.0
#define STEP_PER_REV 18000
#define DISTANCE_REV 1.0
#define SLEW_RATE_SPEED 100.0

struct axial
{
    int stepperState = STATE_STOP;
    int direction = DIRECTION_CW;
    long targetPos = 0;
    long maxPos = MAX_POSITION;
    int runSpeed = RUN_SPEED;
    long pressTime = 0;
};

axial Axial[2];
AccelStepper stepper[] = {
    AccelStepper(AccelStepper::DRIVER, M1_STEP, M1_DIR),
    AccelStepper(AccelStepper::DRIVER, M2_STEP, M2_DIR)
    };

ezButton upperLimitSwitch[] = {
    ezButton(AXIS1_UP_LIM),
    ezButton(AXIS2_UP_LIM)
    };

ezButton lowerLimitSwitch[] = {
    ezButton(AXIS1_LO_LIM),
    ezButton(AXIS2_LO_LIM)
    };

ezButton CW_Switch[] = {
    ezButton(AXIS1_UP_SW),
    ezButton(AXIS2_UP_SW)
    };

ezButton CCW_Switch[] = {
    ezButton(AXIS1_LO_SW),
    ezButton(AXIS2_LO_SW)
    };

void setup()
{
    // Serial.begin(115200);

    for (byte i = 0; i < 2; i++)
    {
        upperLimitSwitch[i].setDebounceTime(20); // set debounce time to 10 milliseconds
        lowerLimitSwitch[i].setDebounceTime(20); // set debounce time to 10 milliseconds
        CW_Switch[i].setDebounceTime(50);        // set debounce time to 50 milliseconds
        CCW_Switch[i].setDebounceTime(50);       // set debounce time to 50 milliseconds

        stepper[i].disableOutputs();            // disable outputs
        stepper[i].setMaxSpeed(MAX_SPEED);      // set the maximum speed
        stepper[i].setAcceleration(ACCELERATE); // set acceleration
        stepper[i].setSpeed(INIT_SPEED);        // set initial speed
        stepper[i].setCurrentPosition(0.0);       // set position
        Axial[i].pressTime = 0;
    }
}

void loop()
{
        
    for (byte j = 0; j < 2; j++)
    {
        upperLimitSwitch[j].loop(); // MUST call the loop() function
        lowerLimitSwitch[j].loop(); // MUST call the loop() function
        CW_Switch[j].loop();        // MUST call the loop() function first
        CCW_Switch[j].loop();       // MUST call the loop() function first
    }

    for (byte j = 0; j < 2; j++)
    {
        if (CW_Switch[j].isPressed())
        {
            Axial[j].stepperState = STATE_MOV_CW;
    
        }
        if (CW_Switch[j].isReleased())
        {
            Axial[j].stepperState = STATE_STOP;
        }

        if (CCW_Switch[j].isPressed())
        {
            Axial[j].stepperState = STATE_MOV_CCW;
            //Axial[j].pressTime=0;
        }

        if (CCW_Switch[j].isReleased())
        {
            Axial[j].stepperState = STATE_STOP;
        }

        if (lowerLimitSwitch[j].isPressed())
        {
            Axial[j].stepperState = STATE_QUICKSTOP;
        }
        if (lowerLimitSwitch[j].isReleased())
        {
        }
        if (upperLimitSwitch[j].isPressed())
        {
            Axial[j].stepperState = STATE_QUICKSTOP;
            Axial[j].maxPos = stepper[j].currentPosition();
        }
        if (upperLimitSwitch[j].isReleased())
        {
            stepper[j].setCurrentPosition(0.0);
        }
    }

    for (byte i = 0; i < 2; i++)
    {
        switch (Axial[i].stepperState)
        {
        case STATE_QUICKSTOP:
            stepper[i].stop(); // STOP
            stepper[i].setSpeed(0.0);
            stepper[i].runSpeed();
            break;

        case STATE_STOP:
            stepper[i].setSpeed(0.0);
            stepper[i].runSpeed();
            Axial[i].stepperState = STATE_STANDBY;
            stepper[i].runSpeed();

            break;

        case STATE_STANDBY:
            stepper[i].disableOutputs(); // disable outputs
            stepper[i].setSpeed(0.0);
            Axial[i].stepperState = STATE_STANDBY;
            break;

        case STATE_MOV_CW:
            if (upperLimitSwitch[i].getState())
            { // checking the limits are touched?
                Axial[i].direction = DIRECTION_CW;
                stepper[i].setSpeed(Axial[i].direction * INIT_SPEED);
                stepper[i].enableOutputs();
                Axial[i].stepperState = STATE_MOVE;
                Axial[i].pressTime = millis();
            }
            else
            {
            }

            break;
        
    case STATE_MOV_CCW:
        if (lowerLimitSwitch[i].getState())
        { // checking the limits are touched?
            Axial[i].direction = DIRECTION_CCW;
            stepper[i].setSpeed(Axial[i].direction * INIT_SPEED);
            stepper[i].enableOutputs();
            Axial[i].stepperState = STATE_MOVE;
            Axial[i].pressTime=millis();
        }
        else
        {
        }

        break;

    case STATE_MOVE:
        stepper[i].runSpeed();
        Axial[i].stepperState = STATE_MOVE;
        if(millis()-Axial[i].pressTime >=1000){
          if (Axial[i].direction ==DIRECTION_CW){
             stepper[i].setSpeed((stepper[i].speed() < MAX_SPEED)? stepper[i].speed() +SLEW_RATE_SPEED:MAX_SPEED );
            
          }else{
            stepper[i].setSpeed(stepper[i].speed() > (-1*MAX_SPEED)? stepper[i].speed() -SLEW_RATE_SPEED :-1*MAX_SPEED );
          }
          Axial[i].pressTime=millis();
        }

        break;

    case STATE_MOVING: // without this state, the move will stop after reaching maximum position

        break;
    }
}
}