BRMIN-Modificar programa de control

Vamos a cambiar el programa de barrido, de manera que sea más lógico, y los servos se muevan todos con una única función. Además nos servirá para hacer los movimientos de forma más sencilla, independientemente del eje que se mueva.

// Hace un barrido de todos los ejes del brazo, en orden
// Versión 2 con numeración alternativa y continua de servos 
//  y función única de movimiento
#include <Servo.h>

#define dServoSec 6      // Número de servo secundario en eje 2
 
Servo MiServo[7];
  // Patillas de los servos según ejes
int PinServo[7]={2,    // Eje 1
                 3,    // Eje 2
                 6,    // Eje 3
                 7,    // Eje 4
                 8,    // Eje 5
                 5,    // Eje 6 = garra
                 4};   // Eje 2 secundario
int PosServo[7]={90,89,90,90,90,30,90};      // Posiciones actuales de servos
int MedServo[7]={90,89,90,90,90,30,90};      // Posiciones medias de servos
int MinServo[7]={0,0,0,0,0,17,178};          // Posiciones mínimas de servos
int MaxServo[7]={180,178,180,180,180,65,0}; // Posiciones máximas de servos

void setup() 
{
  int ii;
  
  for(ii=0;ii<=6;ii++)
  {
   MiServo[ii].attach(PinServo[ii]);
   MiServo[ii].write(PosServo[ii]);
  }
   
  Serial.begin(115200);
  Serial.println("Robot preparado");
  delay(3000);
} 
 
void loop() 
{
  int ii;
  
  for(ii=0;ii<6;ii++)
  {
   MueveServo(ii,MinServo[ii],50);
   delay(100);
   MueveServo(ii,MaxServo[ii],50);
   delay(100);
   MueveServo(ii,MedServo[ii],50);
  }
}

void MueveServo(int numservo,int posicion,int velocidad)
{
 int jj,kk;
 
   // Giro a derecha
 if (posicion>PosServo[numservo])
 {
   // Caso especial eje 2
  if (numservo==1)
  {
   for (jj=PosServo[1],kk=PosServo[dServoSec];jj<posicion;jj++,kk--)
   {
    MiServo[1].write(jj);
    MiServo[dServoSec].write(kk);
    delay(velocidad);
   }
  }
  else
  {
   for (jj=PosServo[numservo];jj<posicion;jj++)
   {
    MiServo[numservo].write(jj);
    delay(velocidad);
   }
  } 
 }
   // Giro a izquierda
 else
 {
   // Caso especial eje 2
  if (numservo==1)
  {
   for (jj=PosServo[1],kk=PosServo[dServoSec];jj>posicion;jj--,kk++)
   {
    MiServo[1].write(jj);
    MiServo[dServoSec].write(kk);
    delay(velocidad);
   }
  }
  else
  {
   for (jj=PosServo[numservo];jj>posicion;jj--)
   {
    MiServo[numservo].write(jj);
    delay(velocidad);
   }
  } 
 }
 PosServo[numservo]=posicion;
 if (numservo==1)
 {
  PosServo[dServoSec]=kk;
 }
}