BRMIN-Prueba de barrido

Hoy vamos a probar el brazo robot completo con una prueba de barrido. Tenemos que estar seguros de que el programa va bien porque los servos del eje 2, que van juntos, deben moverse sincronizados.

Tomamos el programa que tenemos preparado y lo modificamos para mover ambos servos (para este proyecto son los servos 1 y 2) a la vez, grado a grado. Creamos una función, MueveServoDoble, que se va a encargar de eso. El programa queda:

// Hace un barrido de todos los ejes del brazo, en orden
#include <Servo.h>
 
Servo MiServo[7];
int PosServo[7]={90,89,90,90,90,0,90};
int MedServo[7]={90,89,90,90,90,0,90};
int MinServo[7]={0,0,178,0,0,0,0};
int MaxServo[7]={180,178,0,180,180,60,180};

int pos = 0;    // variable to store the servo position 
int ii;
 
void setup() 
{ 
  MiServo[0].attach(2);
  MiServo[1].attach(3);  
  MiServo[2].attach(4);
  MiServo[3].attach(6);
  MiServo[4].attach(7);
  MiServo[5].attach(5);
  MiServo[6].attach(8);
  
  for(ii=0;ii<6;ii++)
  {
   MiServo[ii].write(PosServo[ii]);
  }
   
   Serial.begin(115200);
  Serial.println("Conecte los servos");
  delay(5000);
  Serial.println("Robot preparado");

} 
 
void loop() 
{
  for(ii=0;ii<7;ii++)
  {
    switch(ii)
    {
     case 0:
     case 3:
     case 4:
     case 5:
     case 6: 
       MueveServo(ii,MinServo[ii],50);
       delay(1000);
       MueveServo(ii,MaxServo[ii],50);
       delay(1000);
       MueveServo(ii,MedServo[ii],50);
       break;
       
     case 1: 
       MueveServoDoble(1,2,MinServo[1],MinServo[2],50);
       delay(1000);
       MueveServoDoble(1,2,MaxServo[1],MaxServo[2],50);
       delay(1000);
       MueveServoDoble(1,2,MedServo[1],MedServo[2],50);
       break;

    }
  }
}

void MueveServo(int numservo,int posicion,int velocidad)
{
 int jj;
 
 if(posicion>PosServo[numservo])
 {
  for (jj=PosServo[numservo];jj<posicion;jj++)
  {
   MiServo[numservo].write(jj);
   delay(velocidad);
  }
 }
 else
 {
  for (jj=PosServo[numservo];jj>posicion;jj--)
  {
   MiServo[numservo].write(jj);
   delay(velocidad);
  }
 }
 PosServo[numservo]=posicion;
}

void MueveServoDoble(int numservo1,int numservo2,int posicion1,int posicion2,int velocidad)
{
 int jj,kk;
 
 if(posicion1>PosServo[numservo1])
 {
  for (jj=PosServo[numservo1],kk=PosServo[numservo2];jj<posicion1;jj++,kk--)
  {
   MiServo[numservo1].write(jj);
   MiServo[numservo2].write(kk);
   delay(velocidad);
  }
 }
 else
 {
  for (jj=PosServo[numservo1];jj>posicion1;jj--,kk++)
  {
   MiServo[numservo1].write(jj);
   MiServo[numservo2].write(kk);
   delay(velocidad);
  }
 }
 PosServo[numservo1]=posicion1;
 PosServo[numservo2]=posicion2;
}

Hemos puesto un retardo en el inicio para conectar los dos servos sincronizados, porque cuando se carga el programa en el Arduino, los servos se vuelven locos. Cuando el programa esté terminado lo quitaremos.

Conectamos todo y damos tensión, sujetando el brazo con las manos, por si se cae o tropieza algo en el movimiento. Al principio parece que va bien, pero empieza a dar ‘bandazos’. Los servos del eje 1 realmente tienen fuerza y parece que van a poder con el brazo.

Revisamos el programa y vemos un error: uno de los servos dobles no se inicializaba bien. Podía haberse roto algo. Lo corregimos y ahora se mueve todo bien. El defecto que tiene es que vibra bastante, y esoque se mueve despacio. La estructura es poco rígida, especialmente en el eje 1, ya que todo el brazo descansa sobre el pequeño eje del servo, y es demasiado pero eso no lo podemos arreglar ni fácilmente, ni ahora.

Lo siguiente que vamos a hacer es terminar de ajustar el movimiento con límites para cada servo y cambiar el programa de control para que sea más sencillo. Y también añadir el mando de PS3.