//pin logici
int full = 2; //sensore balla pronta
int limitSwitch = 6; //fine-corsa motore


//pin motore
int R_EN = 4; //enable rotazione destra motore
int L_EN = 10; //enable rotazione sinistra motore
int RPWM = 3; //velocità motore a destra
int LPWM = 5; //velocità motore a sinistra


//pin segnalazione o controllo
int attachLed = 7;
int startAttach = 8;
int buzzer = 9;


int maxSpeed = 255; //velocità massima motore
int minSpeed = 150; //velocità minima motore

const long fwTime = 5000; //tempo marcia avanti motore
const long fwTimeStop = 2000; //tempo fermo appena arrivato tutto avanti
const long rwTime = 2000; //tempo marcia indietro motore tratto finale
const long rwTimeStop = 3000; //tempo fermo arrivato al fine-corsa



void disableMotor(){
  digitalWrite(R_EN, LOW);
  digitalWrite(L_EN, LOW);
  analogWrite(RPWM, 0);
  analogWrite(LPWM, 0);
}


void checkIfFull(){
  bool sensorValue = !digitalRead(full);

  if (sensorValue == 1){
    Serial.println("pieno");
    while (!digitalRead(startAttach) == LOW){
      tone(buzzer, 1440);
    }

    noTone(buzzer);
    attach();
  }
}


void attach(){
  digitalWrite(attachLed, HIGH);
  disableMotor(); //assicurati che il motore sia fermo
  analogWrite(RPWM, maxSpeed); //motore avanti vel. massima
  digitalWrite(R_EN, HIGH); //fai partire il motore
  Serial.println("a");
  delay(fwTime);
  disableMotor();
  Serial.println("s1");
  delay(fwTimeStop);
  analogWrite(LPWM, minSpeed); //motore dietro vel. minima
  digitalWrite(L_EN, HIGH); //fai partire il motore
  Serial.println("fc");

  while (digitalRead(limitSwitch) == HIGH){// fai andare indietro il motore finche non arriva al fine-corsa
    delay(10);
  }

  disableMotor();
  Serial.println("s2");
  delay(rwTimeStop);
  analogWrite(LPWM, maxSpeed); //motore dietro vel. massima
  digitalWrite(L_EN, HIGH); //fai partire il motore
  Serial.println("f");
  delay(rwTime);
  disableMotor();
  Serial.println("stop");
  digitalWrite(attachLed, LOW);

  while (digitalRead(full) == LOW){ //non fare nulla finche non viene svuotato il rotoballa
    noTone(buzzer);
    delay(10);
  }

}

void setup() {
  Serial.begin(9600);
  pinMode(full, INPUT_PULLUP);
  pinMode(limitSwitch, INPUT_PULLUP);
  pinMode(startAttach, INPUT_PULLUP);
  pinMode(R_EN, OUTPUT);
  pinMode(L_EN, OUTPUT);
  pinMode(attachLed, OUTPUT);
  digitalWrite(attachLed, LOW);
  disableMotor();
}

void loop() {
  checkIfFull();
}