//Inclusion of libraries
#include <AccelStepper.h>
#include <Bounce2.h>

//definition of Arduino pin constants
const int ledEnable = 13; //the on board LED will show us the activation status of the motors
const int pinSwEnable = 7; //the button present in the joystick module that enables or disables the control
const int pinEnable = 8; //the pins that control the ENABLE state of the A4988 drivers are connected in series, which is why only one pin is needed to manage them both

unsigned long debounceDelay = 10; //milliseconds for the button debonuce

const int jX = A0; //analog pin that reads the values for the
const int stepX = 2; //digital pin that sends STEP signals to the X driver
int dirX = 1; //digital pin that sends the DIRECTION signal to the X driver
long speedX, valX, mapX; //motor movement management variables X

const int jY = A1; //analog pin that reads the Y values
const int stepY = 4; //digital pin that sends the STEP signals to the Y driver
int dirY = 3; //digital pin that sends the DIRECTION signal to the Y driver
long speedY, valY, mapY; //Y motor movement management variables

//variables used by the AccelStepper library
const int maxSpeed = 500; //according to the library documentation this value can be set up to 4000 for an Arduino UNO
const int minSpeed = 0; //minimum engine speed
const float acceleration = 10.0; //number of steps per second in acceleration

const int treshold = 30; //the reading of the potentiometers is never 100% reliable, this value helps to determine the point to be considered as "Stay still" in the movements
long tresholdUp, tresholdDown; //service variables to perform the task described above

boolean enabled, moveX, moveY, enable; //movement management variables

Bounce btnEnable = Bounce(); //instantiate a button from the Bounce library

//instantiate the engines
AccelStepper motorX(AccelStepper::DRIVER, stepX, dirX);
AccelStepper motorY(AccelStepper::DRIVER, stepY, dirY);

void setup() {
   //initialize values
   speedX = speedY = 0;
   enable = false;

   //definition of pin modes
   pinMode(ledEnable, OUTPUT);
   pinMode(pinEnable, OUTPUT);

   pinMode(pinSwEnable, INPUT_PULLUP); //switch input needs to be set to INPUT_PULLUP

   digitalWrite(ledEnable, enable);
   digitalWrite(pinEnable, !enable); //The A4988 drivers disable the motor commands if they receive a HIGH signal on the ENABLE pin, for this reason the value is opposite to that of the LED

   //configure the joystick button using the Bounce library
   btnEnable.attach(pinSwEnable);
   btnEnable.interval(debounceDelay);

   //calculate range of values within which to consider the joystick position as "Stay still"
   tresholdDown = (maxSpeed / 2) - treshold;
   tresholdUp = (maxSpeed / 2) + treshold;

   //configure engine parameters
   motorX.setMaxSpeed(maxSpeed);
   motorX.setSpeed(minSpeed);
   motorX.setAcceleration(acceleration); // fix

   motorY.setMaxSpeed(maxSpeed);
   motorY.setSpeed(minSpeed);
   motorY.setAcceleration(acceleration);
   Serial.begin(9600);
}

void loop() {

   //execute control and reading function of the button that determines the enabling status
   checkEnable();

   digitalWrite(ledEnable, enable); //show enabling status via the LED on pin 13
   digitalWrite(pinEnable, !enable); //set opposite value on the ENABLE pins of the drivers

   //perform analog reading of the values coming from the joystick potentiometers
   valX = analogRead(jX);
   valY = analogRead(jY);

   //maps the read values as a function of the minimum and maximum speed
   mapX = map(valX, 0, 1023, minSpeed, maxSpeed);
   mapY = map(valY, 0, 1023, minSpeed, maxSpeed);

   if (mapX <= tresholdDown) {
     //x goes backwards
     speedX = -map(mapX, tresholdDown, minSpeed, minSpeed, maxSpeed);
     moveX = true;
   } else if (mapX >= tresholdUp) {
     //x moves forward
     speedX = map(mapX, maxSpeed, tresholdUp, maxSpeed, minSpeed);
     moveX = true;
   } else {
     //x stays still
     speedX = 0;
     moveX = false;
   }

   if (mapY <= tresholdDown) {
     //y goes down
     speedY = -map(mapY, tresholdDown, minSpeed, minSpeed, maxSpeed);
     moveY = true;
   } else if (mapY >= tresholdUp) {
     //y goes up
     speedY = map(mapY, maxSpeed, tresholdUp, maxSpeed, minSpeed);
     moveY = true;
   } else {
     //y stays still
     speedY = 0;
     moveY = false;
   }

   if (moveX) {
     motorX.setSpeed(speedX);
     motorX.run();
   } else {
     motorX.stop();
   }

   if (moveY) {
     motorY.setSpeed(speedY);
     motorY.run();
   } else {
     motorY.stop();
   }
}


void checkEnable() {

   btnEnable.update();

   if (btnEnable.fell()) {
     enable = !enable;
   }

}