/*########################################################*/
/* Controllo stepmotor per il nastro trasportatore a      */
/* scopo didattico (il docente dovrà spiegarne il         */
/* funzionamento).*/
/* vengono utilizzati tre pulsanti:                       */
/*  - pulsante rosso P1 per halt e stop del motore;       */
/*  - pulsante verde P2 per controllare il senso          */
/*    di rotazione;                                       */
/*  - pulsante grigio per controllare il microstepping    */
/*    (quindi la velocità) del motore.                    */
/* Il programma utilizza per il tasto P1 tre funzioni     */
/* logiche con cui si tenta di simulare il comportamento  */
/* di un debounce hardware basato su flip flop; per il    */
/* tasto P2 la chiamata a funzione dove con tre funzioni  */
/* logiche simili a quelle per il tasto P1; per il tasto  */
/* P2 l'interrupt del microcontrollore.                   */
/*                                                        */
/*     driver A4988 e DRV8825                             */
/*         ,-----------,                                  */
/*  ENABLE |o         o| V_MOTORE                         */
/*     MS1 |o ____    o| GND                              */
/*     MS2 |o|    |   o| 2B                               */
/*     MS3 |o|____|   o| 2A                               */
/*   RESET |o         o| 1A                               */
/*   SLEEP |o     _   o| 1B                               */
/*    STEP |o    |_|  o| VDD                              */
/*     DIR |o         o| GND                              */
/*         `-----------'                                  */
/*                                                        */
/*    ==========================================          */
/*     MS1 | MS2	|  MS3 |  Microstep resolution          */
/*    -----|------|------|----------------------          */
/*    Low	 | Low	| Low	 |  Full step                     */
/*    High | Low	| Low	 |  1/2 step                      */
/*    Low	 | High |	Low	 |  1/4 step                      */
/*    High | High |	Low	 |  1/8 step                      */
/*    High | High |	High |	1/16 step                     */
/*    =========================================           */
/*########################################################*/
#define dirPin 6 //2
#define stepPin 3
#define button_ForwardReverse 5
#define button_HaltStart 4
#define button_Microstep 2 //6

boolean M01,M02,M11,M12,M21,M22,
HaltStart,ForwardReverse,
P1,P2,P3 = LOW;

int Microstep = 3;
long debouncingTime = 15;
volatile unsigned long lastMusec;

int MS[3] = {9, 8, 7}; // pin microstepper MS1=MS[0]->9,
                       // MS2=MS[1]->8, MS3=MS[2]->7
boolean microstepArray[5][3] = {
 {LOW, LOW, LOW},
 {HIGH, LOW, LOW},
 {LOW, HIGH, LOW},
 {HIGH, HIGH, LOW},
 {HIGH, HIGH, HIGH} 
};

void setup() {
  Serial.begin(9600);

  for (int step = 0; step < 3; step++) {
    digitalWrite(MS[step],microstepArray[Microstep][step]);
  }
 
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(MS[1], OUTPUT);
  pinMode(MS[2], OUTPUT);
  pinMode(MS[3], OUTPUT);
  pinMode(button_ForwardReverse, INPUT);
  pinMode(button_HaltStart, INPUT);
  pinMode(button_Microstep, INPUT);
  attachInterrupt(0, debounce, RISING);
}


void loop() {
  P1 = digitalRead(button_HaltStart);
  M01 = P1 && (M01 || !HaltStart) && !M02;
  M02 = P1 && (M02 || HaltStart) && !M01;
  HaltStart = (M01 || HaltStart) && !M02;
  Serial.println(HaltStart);

  if (digitalRead(button_ForwardReverse)){
    setForwardReverse();
  }
  
  if (HaltStart) {
    digitalWrite(dirPin, ForwardReverse);
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(10);
  }
//  delay(1000);
}

void setForwardReverse(){
  digitalWrite(stepPin, LOW);
  delay(2000);
  M11 = HIGH && (M11 || !ForwardReverse) && !M12;
  M12 = HIGH && (M12 || ForwardReverse) && !M11;
  ForwardReverse = (M11 || ForwardReverse) && !M12;
  M11=0;
  M12=0;
  digitalWrite(stepPin, HIGH);
}

void debounce() {
  if((long)(micros() - lastMusec) >= debouncingTime*15000) {
    microstepInterrupt();
    lastMusec = micros();
  }
}

void microstepInterrupt() {
  if (digitalRead(button_Microstep)){
    Microstep++;
    Serial.println(Microstep);
    Microstep = Microstep%5;
    for (int step = 0; step < 3; step++) {
      digitalWrite(MS[step],microstepArray[Microstep][step]);
    }
  }
}