BRMIN-Programa de movimiento por teclado

Durante la feria tenemos algunos problemas de funcionamiento. No se lleva el robot listo para demostración. El mando de PS3 no hemos conseguido tenerlo a punto, así que el profesor nos ayuda a desarrollar una solución alternativa. Hacemos un programa para mover el robot mediante códigos de teclado. Usamos 24 teclas, para mover los servos paso a paso individualmente y de 5 en 5 pasos. Así conseguimos demostrar el movimiento programado del robot.

El programa definitivo es:

// Mueve los ejes poco a poco hasta la posicion correcta
// Muestra la posición de los servos en pantalla
#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,148,180,180,180,65,30}; // Posiciones máximas de servos

char cad[20];

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;
 
  LeeCadena();
  switch(cad[0])
  {
    case 'P':
      Serial.print("Ejes: ");
      for(ii=0;ii<6;ii++)
      {
       Serial.print(PosServo[ii]);
       Serial.print(" / ");
      }
      Serial.println(" ");
      cad[0]=0;
      break;
      
    case '1':
      PosServo[0]+=5;
      MiServo[0].write(PosServo[0]);
      cad[0]=0;
      break;
      
    case 'Q':
      PosServo[0]++;
      MiServo[0].write(PosServo[0]);
      cad[0]=0;
      break;
      
    case 'A':
      PosServo[0]--;
      MiServo[0].write(PosServo[0]);
      cad[0]=0;
      break;
      
    case 'Z':
      PosServo[0]-=5;
      MiServo[0].write(PosServo[0]);
      cad[0]=0;
      break;

    case '3':
      PosServo[2]+=5;
      MiServo[2].write(PosServo[2]);
      cad[0]=0;
      break;

    case 'E':
      PosServo[2]++;
      MiServo[2].write(PosServo[2]);
      cad[0]=0;
      break;
      
    case 'D':
     PosServo[2]--;
     MiServo[2].write(PosServo[2]);
     cad[0]=0;
     break;
     
    case 'C':
      PosServo[2]-=5;
      MiServo[2].write(PosServo[2]);
      cad[0]=0;
      break;
      
    case '4':
      PosServo[3]+=5;
      MiServo[3].write(PosServo[3]);
      cad[0]=0;
      break;

    case 'R':
      PosServo[3]++;
      MiServo[3].write(PosServo[3]);
      cad[0]=0;
      break;

    case 'F':
      PosServo[3]--;
      MiServo[3].write(PosServo[3]);
      cad[0]=0;
      break;

    case 'V':
      PosServo[3]-=5;
      MiServo[3].write(PosServo[3]);
      cad[0]=0;
      break;

    case '5':
      PosServo[4]+=5;
      MiServo[4].write(PosServo[4]);
      cad[0]=0;
      break;

    case 'T':
      PosServo[4]++;
      MiServo[4].write(PosServo[4]);
      cad[0]=0;
      break;

    case 'G':
      PosServo[4]--;
      MiServo[4].write(PosServo[4]);
      cad[0]=0;
      break;

    case 'B':
      PosServo[4]-=5;
      MiServo[4].write(PosServo[4]);
      cad[0]=0;
      break;

    case '6':
      PosServo[5]+=5;
      MiServo[5].write(PosServo[5]);
      cad[0]=0;
      break;


    case 'Y':
      PosServo[5]++;
      MiServo[5].write(PosServo[5]);
      cad[0]=0;
      break;


    case 'H':
      PosServo[5]--;
      MiServo[5].write(PosServo[5]);
      cad[0]=0;
      break;


    case 'N':
      PosServo[5]-=5;
      MiServo[5].write(PosServo[5]);
      cad[0]=0;
      break;


    case '2':
      MueveServo(1,PosServo[1]+5,100);
      cad[0]=0;
      break;


    case 'W':
      MueveServo(1,PosServo[1]+1,100);
      cad[0]=0;
      break;

    case 'S':
      MueveServo(1,PosServo[1]-1,100);
      cad[0]=0;
      break;

    case 'X':
      MueveServo(1,PosServo[1]-5,100);
      cad[0]=0;
      break;
      
  }
  


  
}

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;
 }
}

void LeeCadena(void)
{
 int iiCad=0;
 char ch;
 
 if (Serial.available())
 {
   while(Serial.available() > 0) // Lee caracteres introducidos
   {
       if(iiCad<19)  // Comprueba que no se sale
       {
           ch=Serial.read();    // Lee un carácter
           cad[iiCad] = ch;
           iiCad++;
       }
   }
   cad[iiCad] = '\0';    // Termina la cadena con 0
 }  
}