Ali Farfalla

#include <Servo.h>
#define WAIT 600

Servo Servo1;  // create servo object to control a servo
Servo Servo2;  // create servo object to control a servo

int StartButton = 2;
int Servo1Pos, oldServo1Pos;    // variable to read the value from the analog pin
int Servo2Pos, oldServo2Pos;
unsigned long StartTime, StopTime, WaitTime, pressTime;

const int DelayServo = 1000;
const int minPos = 5;
const int maxPos = 175;

const int offset1 = 7;
const int offset2 = -7;


bool flapping = false;

void setup() {
  pinMode(StartButton, INPUT_PULLUP);
  Servo1.attach(9);  // attaches the servo on pin 9 to the servo object
  Servo2.attach(10);
  Serial.begin(115200);
  Servo1.write(minPos);
  Servo2.write(minPos);
}

void loop() {
  /*
  Servo1Pos = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
  Servo1Pos = map(Servo1Posl, 0, 1020, 5, 110);     // scale it to use it with the servo (value between 0 and 180)
  Servo1Pos = constrain(Servo1Pos, 0, 180);
  myservo.write(Servo1Pos);                  // sets the servo position according to the scaled value
  */
  // Gestione pulsante
  pressTime = millis();
  checkButton();

  if( (flapping == true) && (millis() - WaitTime > WAIT) ){
    WaitTime = millis();
    SingleClick();
  }

  // Questa è la funzione che esegue il movimento vero e proprio
  runServo();

}



void runServo(){
  // Se il tempo trascorso è maggiore di DelayServo (1000 millisecondi) spengo il servomotore per risparmiare batteria
  if(millis() - StartTime > DelayServo){
    Servo1.detach();
    Servo2.detach();
    Serial.println("OFF");
  }
  else {
    // Altrimenti li "riaccendo" e sposto il motore
    if(!Servo1.attached()){
       Servo1.attach(9);
       Servo2.attach(10);
       Serial.println("ON");
    }
    Servo1.write(Servo1Pos + offset1);
    Servo2.write(Servo2Pos + offset2);
  }
  delay(5);
}


#define SHORT   50
#define MEDIUM  500
#define LONG    500

// Controlliamo se il pulsante è stato premuto brevemente o più a lungo
void checkButton() {
  static int pressType;
  static unsigned long pTime;
  if (digitalRead(StartButton) == LOW) {
    delay(50);
    pressType = 0;
    while (digitalRead(StartButton) == LOW) {
      pTime = millis() - pressTime;
      if (pTime > SHORT && pTime <= MEDIUM)
        pressType = 1;
      else if (pTime > MEDIUM && pTime <= LONG)
        pressType = 2;
      else if (pTime > LONG) {
        pressType = 3;
        break;
      }
    }
    switch (pressType) {
      case 0: break;
      case 1: SingleClick();  break;
      case 2: MediumClick();  break;
      case 3: LongClick(); delay(500); break;
    }
  }
}

void SingleClick(){
  // Memorizzo il momento di partenza dei servo
  StartTime = millis();
  if(Servo1Pos == maxPos){
    Servo1Pos = minPos;
    Servo2Pos = minPos;
  }
  else{
    Servo1Pos = maxPos;
    Servo2Pos = maxPos;
  }
}

void MediumClick(){
  return;
}

void LongClick(){
  flapping = !flapping;
  delay(1000);
}