Projet Bac programmation carte arduino

Bonjour, je suis en terminal S option Si ( science de l'ingénieur ).
J'ai un projet à faire pour le bac, c'est un sous marin à évitement d'obstacle instantanée.

Je vais utiliser 2 cartes arduino, il y a 2 manettes pour piloter le sous marin :
une carte pour les entrées avancer, reculer, gauche et droite(Av,Re,Ga,Dr) et les sorties M1 et M2 ( chaque moteur à sens positif et negatif ).

une carte pour les entrées plonger, remonter et obstacle (Pl,REM,OB) et les sorties Mp1 et Mp2 ( c'est deux pompes pour la plonger et la remonter du système)

pour la premiére carte :

J'ai réalisé ça comme programmation ( je n'ai pas encore pu essayé ), cependant j'aimerais faire par étape, réussir a ce que le sous marin puisse aller à gauche , droite, avancer et reculer.

" 

int Av = 4;        //Bouton avancer connectée à la broche 04
int Re = 5;        //Bouton reculer connectée à la broche 05
int Ga = 6;       //Bouton gauche connectée à la broche 06
int Dr =  7;      //Bouton droite connectée à la broche 07
int MotA =  12;      //Moteur A connectée à la broche 12
int MotB =  13;     //Moteur B connectée à la broche 13
boolean M1p = true;
boolean M1m = false;
boolean M2p = true;
boolean M2m = false;


const int vitesseMotA=3; // Constante pour la broche 3
const int sensMotA=12; // Constante pour la broche 12
const int intensiteMotA=A0; // intensité du moteur 0

const int vitesseMotB=11; // Constante pour la broche 11
const int sensMotB=13; // Constante pour la broche 13
const int intensiteMotB=A1; // intensité du moteur 1

void setup ()
{
  pinMode (Av, INPUT);  // Av configuré comme une entrée
  pinMode (Re, INPUT);
  pinMode (Ga, INPUT);
  pinMode (Dr, INPUT);
  
  pinMode (vitesseMotA,OUTPUT); // Broche vitesseMotA configurée en sortie
  pinMode (vitesseMotB,OUTPUT);
//  pinMode (freinMotA,OUTPUT); // Broche freinMotA configurée en sortie
//  pinMode (freinMotB,OUTPUT);
  pinMode (sensMotA,OUTPUT); // Broche sensMotA configurée en sortie
  pinMode (sensMotB,OUTPUT);
  
}


void loop()
{
  digitalRead(Av);      // lire l'entrée Av
  digitalRead(Re);      // lire l'entrée Re
  digitalRead(Ga);      // lire l'entrée Ga
  digitalRead(Dr);      // lire l'entrée Dr
  
  // appliquer les équations logiques
  M1p = Dr && !Re && !Av || !Dr && !Ga && Av;  //dr./re./av + /dr./ga.av
  M1m = Ga && !Re && !Av || !Dr && !Ga && Re;  //ga./re./av+ /dr./ga.re
  M2p = Ga && !Re && !Av || !Dr && !Ga && Av;  //ga./re./av + /dr./ga.av
  M2m = Dr && !Re && !Av || !Dr && !Ga && Re;  //dr./re./av + /dr./ga.re

"

ensuite pour la seconde carte :

pour la seconde carte, je ne sais pas du tout si il faut que je réalise des équations booléenne car je ne vais pas demander à mon sous marin de plonger et remonté en même temps, car notre système de plonger et une poche qui se rempli d'eau a l'aide de 2 pompes, ainsi je ne peux pas monter et descendre en même temps, ensuite pour la détection d'obstacle, j'aimerais mettre une tempo de 2 à 4 secondes, je m'explique : un obstacle est détecté, le sous marin va à gauche pendant 4 secondes par exemple pour qu'il ne détecte pu d'obstacle.

"
int Pl =           //Bouton plonger connectée à la broche 9
int Rem =          //Bouton remonter connectée à la broche 8
int Ob =           //Capteur connectée à la broche 7
int Mp1 =          //Moteur pompe 1 connectée à la broche 4
int Mp2 =          //Moteur pompe 2 connectée à la broche 3

const int vitesseMotA=3; // Constante pour la broche 3
const int sensMotA=12; // Constante pour la broche 12
const int intensiteMotA=A0; // intensité du moteur 0

const int vitesseMotB=11; // Constante pour la broche 11
const int sensMotB=13; // Constante pour la broche 13
const int intensiteMotB=A1; // intensité du moteur 1

void setup ()
{
  pinMode (Pl, INPUT);  // Pl configuré comme une entrée
  pinMode (Rem, INPUT);
  pinMode (Ob, INPUT);
  
  pinMode (vitesseMotA,OUTPUT); // Broche vitesseMotA configurée en sortie
  pinMode (vitesseMotB,OUTPUT);
//  pinMode (freinMotA,OUTPUT); // Broche freinMotA configurée en sortie
//  pinMode (freinMotB,OUTPUT);
  pinMode (sensMotA,OUTPUT); // Broche sensMotA configurée en sortie
  pinMode (sensMotB,OUTPUT);
 
}
  
  void loop()
{
  digitalRead(Pl);      // lire l'entrée Av
  digitalWrite(sensMotA,HIGH); // sens 1
  digitalWrite(vitesseMotA, HIGH); // vitesse maximale
  digitalWrite(sensMotB,HIGH); // sens 2
  digitalWrite(vitesseMotB, HIGH); // vitesse maximale
  
  digitalRead(Rem);      // lire l'entrée Re
  digitalWrite(sensMotA,LOW); // sens 1
  digitalWrite(vitesseMotA, HIGH); // vitesse maximale
  digitalWrite(sensMotB,LOW); // sens 2
  digitalWrite(vitesseMotB, HIGH); // vitesse maximale
  
  digitalRead(Ob);      // lire l'entrée Ga
  digitalWrite(sensMotA,HIGH); // sens 1
  digitalWrite(vitesseMotA, LOW); // vitesse maximale
  digitalWrite(sensMotB,HIGH); // sens 2
  digitalWrite(vitesseMotB, HIGH); // vitesse maximale
}
"

J'aimerais vos avis et votre aide car mon professeur ne peut pu m'aider car il est absent pour cause de blessure ...
Je vous remercie d'avance de m'aider car je suis vraiment pas doué sur l'arduino.

Cordialement.