4 wheel motor electric car

Hello everybody!!

I'm involved on a project that as been grinding my gears... I'm new to arduino, only bought my first one last year and my programming skills are bad.

I'm building a car with four electric motors, one on each wheel. The purpose of the project is to develop and compare different algorithms aiming to improve dynamic handling on certain situations.

Here is the list of components i'm using:

1x servo - For steering
4x ESC - Electronic speed control, one for each motor
1x Radio receiver 2.4Ghz
4x Hall sensors- one on each wheel to give me their RPM's
1x mpu6050- this give me the lateral and longitudinal acceleration and angular speeds
1x arduino MEGA 2560

I've successfully written the programs for each individual component, the car runs with all four esc's, servo and receiver, everything works as it's supposed to. My problem starts when i try to put every thing else together.

But first things first, do you think that the arduino mega can handle all this componentes at the same time?

Now, lets start with the hall sensors. Here is my code:

const int sensor =12;
long int pin= 0;
long int n;
float resto =0;
float inicialtime;
float finaltime;
float rpmtime;
int RPM;
int volta;
float x;
float resto1;
/*--------------------------__Setup__-------------------------------------------*/
void setup() {
  // put your setup code here, to run once:
  //pinMode(hepin, INPUT);
  Serial.begin(9600);
}

/*-------------------------------------------------------------__LOOP__---------------------------------------------------------*/
void loop() {
  
 
 if(!digitalRead(sensor)){
   
  pin++;
  x = pin;
  n = x/2;                
  
  int resto2 = resto;     
  resto = n % 3;         
   
  if (resto2 == resto){  
       RPMfunction();    
       }
   
    
   
  while(!digitalRead(sensor)){  
  //Serial.println("cenas");
    }
   
   }
}
/*----------------------------------------------------------------__funções__---------------------------------------------------*/    
int RPMfunction(){
  
  if (resto == 0 ){                               
    finaltime = millis()*1000;                     
    rpmtime = (finaltime - inicialtime)/1000000;  
    
    RPM = 60 * 3  / (rpmtime);                    
    
    Serial.print("RPM :  ");
    Serial.println(RPM);
    
    inicialtime = millis()*1000;                 
    finaltime = 0;                              
    
    }
    
  else{
    
    }
    
  }

I know it's really messy but than again i'm a very bad at programing.

In order to use more hall sensor's do i need to quadruplicate everything? Or can i some how use only one function that handles all four sensors?

I understand that this project is too much for someone with my skills but at least i can say that i'm Very motivated!!

Thanks,
João Palma Carpinteiro