Ayuda con control de Matriz LED + Servos

Hola soy nuevo en el foro, así que me presento. Soy Nicolás y estoy haciendo un Robot sencillo como proyecto para terminar la secundaria. Apenas lo estoy empezando, lo único que hace por ahora es recibir comandos por serial, activar dos servos e ir en cierta dirección por 5 segundos. Recientemente intenté ponerle una matriz led de 8x6, con la intención de que mostrara cierta figura para cada uno de los movimientos(adelante, atras, derecha, izq) al mismo tiempo que se mueven los servos.

Mi problema está en que los servos utilizan una variable speed y la matriz otra, pero la figura en la matriz sólo aparece por menos de un segundo; y si aumento el valor de la speed en la matriz, la figura dura más tiempo pero el tiempo asignado de 5 segundos por cada movimiento de los servos también aumenta drásticamente. Me podrían dar una ayuda? :)...

Este es el código que llevo:

#include <stdlib.h>
#include <Servo.h>
//declaracion de variables

Servo izquierdo; 
Servo derecho; 
int speed = 100; // velocidad inicial servos
int speedMatrix = 5; // velocidad matriz, numero de veces que se repite cada fotograma
int pauseDelay = 1;    

int rowA[] = {9,8,7,6,5,4,3,2}; // pins filas matriz led        
                                         
int colA[] = {17,16,15,14,13,12};  // pins columnas matriz
byte data[] = {0,0,0,0,0,0,0,0};    



//--------------------------------------------------

//Inicio del programa de recepcion de comandos
//------------------------------------------------------------------------------------------------------------

void setup()
{
  iniciarServo();
  Serial.begin(9600);
  for(int i = 0; i <8; i++){ //establecer todos los pines de la matriz como OUTPUT
    pinMode(rowA[i], OUTPUT);
    pinMode(colA[i], OUTPUT);
  }
}

void loop()
{
   refrescarDatos();
  
}

void refrescarDatos()
{ 
  Parar();
  if (Serial.available() > 0) {
  char *datos = "";
  *datos = Serial.read();
  int comando = atol (datos);
  Serial.println(comando);
  
  if (comando == 1) {  
   irAdelante();
    
    data[0] = B00111100; //figura que aparece en la matriz
    data[1] = B01000010;
    data[2] = B10000001;
    data[3] = B01000001;
    data[4] = B00100010;
    data[5] = B00011100;
    
    showSprite(speedMatrix);
    delay(5000);
    Parar();
  }
  else if (comando == 2) {
    irAtras();
    
    data[0] = B00111100;
    data[1] = B01000010;
    data[2] = B10000001;
    data[3] = B01000001;
    data[4] = B00100010;
    data[5] = B00011100;
    showSprite(speed);
    
    delay(5000);
    Parar();
  }
  else if (comando == 3) {
    irDerecha();
    
    data[0] = B00111100;
    data[1] = B01000010;
    data[2] = B10000001;
    data[3] = B01000001;
    data[4] = B00100010;
    data[5] = B00011100;
    showSprite(speed);
    
    delay(5000);
    Parar();
  }
  else if (comando == 4) {
    irIzquierda();
    
    data[0] = B00111100;
    data[1] = B01000010;
    data[2] = B10000001;
    data[3] = B01000001;
    data[4] = B00100010;
    data[5] = B00011100;
    showSprite(speed);
    
    delay(5000);
    Parar();
  }
  else {
    irAdelante();
    showSprite(speed);
    delay(200);
    irAtras();
    delay(200);
   
    }
  }
}

//Planteamiento de funciones de movimiento

void iniciarServo()
{
  izquierdo.attach(11); 
  derecho.attach(10);  
}

void setSpeed(int nuevaVel)
{
  if(nuevaVel >= 100) {
  nuevaVel = 100;
  }
  if(nuevaVel <= 0) {
  nuevaVel = 0;
  }      
  speed = nuevaVel * 0.95;                   
}

void irAdelante()
{
 izquierdo.write(95 + speed);
 derecho.write(95 - speed);
}

void irAtras()
{
 izquierdo.write(95 - speed);
 derecho.write(95 + speed);
}
  
void irDerecha()
{
 izquierdo.write(95 + speed);
 derecho.write(95 + speed);
}

void irIzquierda()
{
 izquierdo.write(95 - speed);
 derecho.write(95 - speed);
}

void Parar()
{
 izquierdo.write(95);
 derecho.write(95);
} 

// Funcion de la matriz

//An array to store power values to act as bit masks
const int powers[] = {1,2,4,8,16,32,64,128};

//Runs a pattern where each LED is lit one after another

void showSprite(int speed2){
 for(int iii = 0; iii < speed2; iii++){                 //show the current frame speed2 times
  for(int column = 0; column < 8; column++){            //iterate through each column
   for(int i = 0; i < 8; i++){                          
       digitalWrite(rowA[i], LOW);                      //turn off all row pins  
   }
   for(int i = 0; i < 8; i++){ //Set only the one pin
     if(i == column){     digitalWrite(colA[i], LOW);}  //turns the current row on
     else{                digitalWrite(colA[i], HIGH); }//turns the rest of the rows off
   }

   for(int row = 0; row < 8; row++){                    //iterate through each pixel in the current column
    int bit = (data[column] >> row) & 1;
    if(bit == 1){ 
       digitalWrite(rowA[row], HIGH);                   //if the bit in the data array is set turn the LED on
    }

   }
   delay(pauseDelay);                       //leave the column on for pauseDelay microseconds (too high a delay causes flicker)
  } 
 }
}

...Otra idea que tengo es la de no poner un tiempo fijo a cada movimiento, sino que el movimiento se interrumpa al recibir un nuevo comando; pero creo que para poder hacer esto tengo que usar interrupciones de los timers y no entiendo mucho del tema... Alguna de idea de como lo podría hacer?. Es bienvenida toda sugerencia, explicación y corrección.

Muchas gracias :slight_smile: