cambio elettronico con arduino e servomotori

salve, sono nuovo del forum, e ne approfitto per salutarvi e ringraziarvi anticipatamente per le risposte. Se trovate errori e colpa del correttore automatico :blush: . Quello che sto tentando di realizzare è una modifica ad un cambio originale di un'auto, per renderlo sequenziale alla leva. Significa che alla leva del cambio si avrà un movimento sequenziale, ma nella scatola del cambio avverrà un movimento ( guiato da due servomotori ) analogo alla leva originale. Cioè quando io spingo la leva indietro i due motori si spostano e spingono i leveraggi del cambio nelle esatte posizioni di marcia. L'idea quindi è quella di utilizzare un programma che legga due pulsanti up/down e che muova a seconda dei casi i servi in più posizioni.
Es: Se siamo in prima: my servo1.write(20); my servo2.write(90); my servo1.write(180);
break.
ed il loop si ferma li senza ripetere niente.
se premo di nuovo up passa in seconda ed esegue questa funzione: my servo1.write(0); my servo2.write(180); my servo1.write(90);
sarebbe possibile? L'unica cosa che sono riuscito a fare è un programma che legge l'up/down

#define BUTTON_DOWN 11
#define BUTTON_UP 12

int ledPinA =  6;    // LED connected to digital pin 3
int ledPinB =  9;    // LED connected to digital pin 4
int ledPinC =  4;    // LED connected to digital pin 5
int ledPinD =  3;    // LED connected to digital pin 6
int ledPinE =  2;    // LED connected to digital pin 7
int ledPinF =  7;    // LED connected to digital pin 8
int ledPinG =  8;    // LED connected to digital pin 9
int ledPinDP =  5;   // LED connected to digital pin 10

int val_down = 0;
int old_val_down = 0;
int val_up = 0;
int old_val_up = 0;
int state = 0;
int gear = 1;

// The setup() method runs once, when the sketch starts

void setup()   {             
  pinMode(BUTTON_DOWN, INPUT);
  pinMode(BUTTON_UP, INPUT);
  
  // initialize the digital pin as an output:
  pinMode(ledPinA, OUTPUT);
  pinMode(ledPinB, OUTPUT);
  pinMode(ledPinC, OUTPUT);
  pinMode(ledPinD, OUTPUT);
  pinMode(ledPinE, OUTPUT);
  pinMode(ledPinF, OUTPUT);
  pinMode(ledPinG, OUTPUT);  
  pinMode(ledPinDP, OUTPUT);
  
  // initialize serial communication:
  Serial.begin(9600);
}

void led0()
{
  digitalWrite(ledPinA, HIGH);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, LOW);
}

void led1()
{
  digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, HIGH);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, LOW);
}

void led2()
{
  digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, HIGH);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, LOW);
}

void led3()
{
  digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, HIGH);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, LOW);
}

void led4()
{
 digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, HIGH);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, LOW);
} 

void led5()
{
  digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, HIGH);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, LOW);
} 

void led6()
{
  digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, HIGH); 
  digitalWrite(ledPinDP, LOW);
} 

void led7()
{
  digitalWrite(ledPinA, LOW);
  digitalWrite(ledPinB, LOW);
  digitalWrite(ledPinC, LOW);
  digitalWrite(ledPinD, LOW);
  digitalWrite(ledPinE, LOW);  
  digitalWrite(ledPinF, LOW);  
  digitalWrite(ledPinG, LOW); 
  digitalWrite(ledPinDP, HIGH);
} 



void setLed(int number)
{
  switch (number) {
    
    case 1:  
      led1();
      break;
    case 2:  
      led2();
      break;
    case 3:  
      led3();
      break;
    case 4:  
      led4();
      break;
    case 5:  
      led5();
      break;
    case 6:  
      led6();
      break;
    case 7:  
      led7();
      break;
      
     

  }
}
              
// the loop() method runs over and over again,
// as long as the Arduino has power

void oldloop()                     
{
                   // wait for a second
  led0();
  delay(1000);                  // wait for a second
  led1();
  delay(1000);                  // wait for a second
  led2();
  delay(1000);                  // wait for a second
  led3();
  delay(1000);                  // wait for a second
  led4();
  delay(1000);                  // wait for a second
  led5();
  delay(1000);                  // wait for a second
  led6();
  delay(1000);                  // wait for a second
  led7();
  delay(1000);                  // wait for a second
                   // wait for a second
}

void loop()
{
    val_up = digitalRead(BUTTON_UP);
    if ((val_up == HIGH) && (old_val_up == LOW)) {
      gear += 1;
      Serial.print("gear number:  ");
      Serial.println(gear, DEC);
      delay(100);
    }
    old_val_up = val_up;
 
    val_down = digitalRead(BUTTON_DOWN);
    if ((val_down == HIGH) && (old_val_down == LOW)) {
      gear -= 1;
      Serial.print("gear number:  ");
      Serial.println(gear, DEC);
      delay(100);
    }
    old_val_down = val_down;
    
    if (gear >= 6) {
      gear = 6;
    }
    if (gear <= 1) {
      gear = 1;
    }
    
    setLed(gear);
}

ed un programma che mi muove i servi:

#include <Servo.h> 
 
#define servoPinX 10
#define servoPinY 11
 
Servo myservoX;
Servo myservoY;
 
int buttX[]    = { 2,3,4 };
int gradX[]   = { 20,90,150 };
int lengthX = 3;
 
int buttY[]    = { 5,6,7,8 };
int gradY[]   = { 20,80,130,160 };
int lengthY = 4;
 
int servoPosX = 0;
int servoPosY = 0;
 
void setup()
{
  myservoX.attach( servoPinX );
  myservoX.write( servoPosX );
 
  myservoY.attach( servoPinY );
  myservoY.write( servoPosY );
 
  for (int pin=0; pin<lengthX; pin++)
  {
    pinMode( buttX[pin],INPUT );
  }
  for (int pin=0; pin<lengthY; pin++)
  {
    pinMode( buttY[pin],INPUT );
  }
 
} 
 
void loop()
{
  for (int pin=0; pin<lengthX; pin++)
  {
    if ( digitalRead(buttX[pin]) == HIGH ) { servoPosX = gradX[pin]; }
  }
 
  for (int pin=0; pin<lengthY; pin++)
  {
    if ( digitalRead(buttY[pin]) == HIGH ) { servoPosY = gradY[pin]; }
  }
 
  myservoX.write( servoPosX );
  myservoY.write( servoPosY );
  delay( 15 );
}

l'unica cosa che non riesco a fare è unirli e farli fare al servomotore più di un movimento alla volta..
so che è una cosa molto difficile però è davvero importante.
cordiali saluti a tutti quanti e vi ringrazio anticipatamente.. :expressionless: