#include <AccelStepper.h>
#include <Stepper.h>
#include <Servo.h>
#include <Bounce2.h>
#define STEPS 32
#define IN1  9
#define IN2  10
#define IN3  11
#define IN4  12

const int ledEnable = 13;         //led on board
const int pinSwEnable = 7;        //bottone joystick 1
const int pinSwServo = 6;         //bottone joystick 2
const int pinEnable = 8;          //abilita 2 driver A4988
int angolo=0;                     //angolo partenza servo
unsigned long debounceDelay = 10; //millisecondi per il debonuce del bottone
const int jX1 = A3;               //asse X joystick 2

const int jX = A0;                //asse X joystick 1
const int stepX = 2;              //invia segnali step MOTORE 1
const int dirX = 3;               //invia segnali direzione MOTORE 1
long speedX, valX, mapX;          //variabili movimenti MOTORE 1

const int jY = A1;                //asse Y joystick 1
const int stepY = 4;              //invia segnali step MOTORE 2
const int dirY = 5;               //invia segnali direzione MOTORE 2
long speedY, valY, mapY;          //variabili movimenti MOTORE 2

const int jY2 = A2;               //asse Y joystick 2
const int stepY2 = 1;             //invia segnali step MOTORE 3
const int dirY2 = 0;              //invia segnali direzione MOTORE 3
long speedY2, valY2, mapY2;       //variabili movimenti MOTORE 3

const int maxSpeed = 400;         //velocità massima da 1 a 4000 MOTORE 1&2
const int minSpeed = 0;           //velocità minima MOTORE 1&2
const float accelerazione = 50.0; //numero di step al secondo in accelerazione

const int treshold = 30;          //zona morta dei joystick
long tresholdUp, tresholdDown;    //variabili di servizio per espletare il compito descritto sopra

boolean abilitato, muoviX, muoviY, muoviY2, enable;  //variabili di gestione dei movimenti

Bounce btnEnable = Bounce();      //istanzia un bottone dalla libreria Bounce

AccelStepper motoreX(AccelStepper::DRIVER, stepX, dirX);
AccelStepper motoreY(AccelStepper::DRIVER, stepY, dirY);
AccelStepper motoreY2(AccelStepper::DRIVER, stepY2, dirY2);

Stepper stepper(STEPS, IN4, IN2,  IN3, IN1);

Servo servo;

void setup() {

  pinMode(pinSwServo, INPUT_PULLUP);  //input switch joystick 
  servo.attach(A5);                  
  
  speedX = speedY = speedY2 = 0;
  enable = false;
  
  pinMode(ledEnable, OUTPUT);
  pinMode(pinEnable, OUTPUT);

  pinMode(pinSwEnable, INPUT_PULLUP); //input switch joystick 

  digitalWrite(ledEnable, enable);
  digitalWrite(pinEnable, !enable);   //logica inversa (!)

  btnEnable.attach(pinSwEnable);
  btnEnable.interval(debounceDelay);

 
  tresholdDown = (maxSpeed / 2) - treshold; //calcola range valori entro i quali la posizione joystick è morta
  tresholdUp   = (maxSpeed / 2) + treshold;

  motoreX.setMaxSpeed(maxSpeed);
  motoreX.setSpeed(minSpeed);
  motoreX.setAcceleration(accelerazione);

  motoreY.setMaxSpeed(maxSpeed);
  motoreY.setSpeed(minSpeed);
  motoreY.setAcceleration(accelerazione);

  motoreY2.setMaxSpeed(maxSpeed);
  motoreY2.setSpeed(minSpeed);
  motoreY2.setAcceleration(accelerazione);
}

void loop() {


{
  int val = analogRead(jX1);
  if(  (val > 500)  && (val < 523) )//se è nel mezzo fermo
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2,  LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  }
  else
  {
    while (val  >= 523)
    {
      int speed_  = map(val, 523, 1023, 5, 1000);//velocità rotazione
      stepper.setSpeed(speed_);

      stepper.step(1);
 
      val  = analogRead(jX1);
    }
    while (val <= 500)
    {
      int speed_ = map(val, 500, 0, 5, 1000);
      stepper.setSpeed(speed_);
  
      stepper.step(-1);
 
      val  = analogRead(jX1);
    }
 }
} 


  
{
  if(digitalRead(pinSwServo)== HIGH) {
    angolo = 180;
  }
  else{
    angolo = 0;
  }
  servo.write(angolo);
}


  
  checkEnable();

  digitalWrite(ledEnable, enable);  //mostra stato di abilitazione tramite il led su pin 13
  digitalWrite(pinEnable, !enable); //imposta valore opposto sui pin ENABLE dei driver

  //esegui lettura analogica dei valori provenienti dai potenziometri del joystick
  valX = analogRead(jX);
  valY = analogRead(jY);
  valY2 = analogRead(jY2);

  //mappa i valori letti in funzione della velocità inima e massima
  mapX = map(valX, 0, 1023, minSpeed, maxSpeed);
  mapY = map(valY, 0, 1023, minSpeed, maxSpeed);
  mapY2 = map(valY2, 0, 1023, minSpeed, maxSpeed);

  //esegui funzione di comando dei motori
  pilotaMotori(mapX, mapY, mapY2);

}

void pilotaMotori(long mapX, long mapY, long mapY2) {

  if (mapX <= tresholdDown) {
    //x va indietro
    speedX = -map(mapX, tresholdDown, minSpeed,   minSpeed, maxSpeed);
    muoviX = true;
  } else if (mapX >= tresholdUp) {
    //x va avanti
    speedX = map(mapX,  maxSpeed, tresholdUp,  maxSpeed, minSpeed);
    muoviX = true;
  } else {
    //x sta fermo
    speedX = 0;
    muoviX = false;
  }

  if (mapY <= tresholdDown) {
    //y va giù
    speedY = -map(mapY, tresholdDown, minSpeed,   minSpeed, maxSpeed);
    muoviY = true;
  } else if (mapY >= tresholdUp) {
    //y va su
    speedY = map(mapY,  maxSpeed, tresholdUp,  maxSpeed, minSpeed);
    muoviY = true;
  } else {
    //y sta fermo
    speedY = 0;
    muoviY = false;
  }

  if (mapY2 <= tresholdDown) {
    //y va giù
    speedY2 = -map(mapY2, tresholdDown, minSpeed,   minSpeed, maxSpeed);
    muoviY2 = true;
  } else if (mapY2 >= tresholdUp) {
    //y va su
    speedY2 = map(mapY2,  maxSpeed, tresholdUp,  maxSpeed, minSpeed);
    muoviY2 = true;
  } else {
    //y sta fermo
    speedY2 = 0;
    muoviY2 = false;
  }


  if (muoviX) {
    motoreX.setSpeed(speedX);
    motoreX.run();
  } else {
    motoreX.stop();
  }

  if (muoviY) {
    motoreY.setSpeed(speedY);
    motoreY.run();
  } else {
    motoreY.stop();
  }

  if (muoviY2) {
    motoreY2.setSpeed(speedY2);
    motoreY2.run();
  } else {
    motoreY2.stop();
  }
}


void checkEnable() {

  btnEnable.update();

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