Servo gestito da un interruttore On-Off

[heagle81]

Ed eccomi qui a chiedere di nuovo i vostri preziosi consigli..
Questa volta mi sto cimentando nella programmazione del funzionamento di un servo e di un interruttore On - Off.
Le condizioni che voglio realizzare sono le seguenti:

1. interruttore ON ==> la lancetta del servo assume una posizione iniziale "minPos" (esempio 70° a sx) per poi avanzare verso dx in "x" secondi fino ad una posizione finale "maxPos" (es. 30° verso dx) e qui invertire il movimento con un tempo diverso rispetto a quello iniziale. Il ritorno però non sarà più nella posizione iniziale (minPos) ma si fermerà in una posizione intermedia (medPos) e da qui raggiungerà di nuovo la posizione finale. In sostanza solo all'accensione dell'interruttore la lancetta compirà il movimento completo da 70 ° a 30° mentre i successivi saranno da 50° a 30°;

2. con interruttore OFF e posizione diversa da quella iniziale (minPos) la lancetta deve andare sulla minPos rispettando i tempi che gli ho dato precedentemente;

Al momento sono riuscito a relizzare il caso nr. 1 e sto incontrando qualche difficoltà a scrivere il codice per il caso nr. 2.

Di seguito il codice che ho scritto (accetto consigli per eventuali correzioni che sono sicuro ci saranno):

#include <Servo.h>

Servo servoCpa;               // crea l'oggetto servo
int dt1 = 200;                // intervallo in millis
int dt2 = 100;                // intervallo in millis
int minPos = 70;              // angolo di partenza servo
int maxPos = 30;              // angolo di arrivo servo
int medPos = 50;              // angolo intermedio
int pos;                      // variabile stato pozizione servo
int statoBatterie;            // variabile stato pozizione batterie 
bool incr1 = true;            // aumenta  o dimnuisce l'angolo di 1
bool chiaveBatterie = false;
unsigned long t1 = 0; 
unsigned long t2 = 0;
unsigned long currentMillis;

void setup() {
  pinMode(3, INPUT);    // chiave batterie
  pos = minPos;         // reset posizione iniziale servo
  servoCpa.attach(5);   // indicazione pin servo
  servoCpa.write(pos);  // dico al servo di posizionarsi nella sua variabile 'pos'
}

void loop() {
  chiaveBatterie = digitalRead(3);
       
  if (chiaveBatterie && !statoBatterie) {
     statoBatterie = true;
	 }
  
  if (chiaveBatterie && statoBatterie) {
     currentMillis = millis();
     servo1();
 }
}
void servo1() {
 
     if (incr1 == false && currentMillis - t1 >= dt1) { 
        pos = pos + 1;
        servoCpa.write(pos); // dico al servo di andare nella posizione dello stato della variabile 'pos'
        t1 = currentMillis;
       
     if (pos == medPos) {
        incr1 = true;
    }
   }
  
  if (incr1 == true && currentMillis - t2 >= dt2) {
     pos = pos - 1;
     servoCpa.write(pos); // dico al servo di andare nella posizione dello stato della variabile 'pos'
     t2 = currentMillis;
    
  if (pos == maxPos) {
       incr1 = false;
   }
  }


  if (!chiaveBatterie && statoBatterie) {
      statoBatterie = false;
  }
  
  if (!chiaveBatterie && !statoBatterie) {
     currentMillis = millis();
     servo2();
 }
} 
  void servo2() {
    
    if (incr1 == false && currentMillis - t1 >= dt1) { 
        pos = pos + 1;
        servoCpa.write(pos); // dico al servo di andare nella posizione dello stato della variabile 'pos'
        t1 = currentMillis;
       
     if (pos == minPos) {
        incr1 = true;
    }
  }
 }

Grazie per la vostra attenzione.