BRMIN-Prueba de movimiento con el mando

Ya nos ha dado tiempo de adaptar el programa del robot para usar el mando PS3. Aprovechamos la magnífica librería para usar el mando PS· por Bluetooth creada por Kristian Lauszus. El código es el que adjuntamos:

/*
 Example sketch for the PS3 Bluetooth library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or 
 send me an e-mail:  kristianl@tkjelectronics.com
 */
#include <Servo.h>
#include <PS3BT.h>

#define dServoSec 6      // Número de servo secundario en eje 2
 
Servo servo1,servo2,servo3,servo4;
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 MandoPS3[7]={0,0,0,0,0,0,0};             // Movimientos del mando PS3
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
int kk=0;

USB Usb;
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
/* You can create the instance of the class in two ways */
//PS3BT PS3(&Btd); // This will just create the instance
PS3BT PS3(&Btd,0x00,0x1f,0x81,0x00,0x08,0x30); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch

boolean printTemperature;
boolean printAngle;
uint8_t state = 0;
char cad[100];          // Cadena de comando por teclado

//·············································································
void setup() 
{
  int ii;

  Serial.begin(115200);
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while(1); //halt
  }
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));

  for(ii=0;ii<=6;ii++)
  {
   MiServo[ii].attach(PinServo[ii]);
   MiServo[ii].write(PosServo[ii]);
  }
//  servop.attach(7);
// servop.write(5);
 
  Serial.println("\r\nRobot preparado");
  delay(1000);

}
//·············································································
void loop() 
{
 int ii;

  // Lee los botones del mando PS3
 LeeMandoPS3();

/* if (MandoPS3[0]==1) 
   servop.write(kk++);
 if (MandoPS3[0]==-1) 
   servop.write(kk--);
 MandoPS3[0]=0; */
   // Comprueba los movimientos en cada eje
   // vigilando no pasarse en máximo y mínimo

if(PS3.PS3Connected)
{
 Serial.print("Mando: ");
 for(ii=0;ii<6;ii++)
 {
  Serial.print(MandoPS3[ii]);
  Serial.print(", ");
 }
 Serial.println("");
  Serial.print("Ejes : ");
  for(ii=0;ii<6;ii++)
  {
   Serial.print(PosServo[ii]);
   Serial.print(", ");
  }
  Serial.println(" ");



 for(ii=0;ii<6;ii++)
 {
  if (MandoPS3[ii]==1)
  {
   if (PosServo[ii]<MaxServo[ii])
   {
    PosServo[ii]++;
    MueveServo(ii,PosServo[ii],10);
   } 
   MandoPS3[ii]=0;
  }
  else if (MandoPS3[ii]==-1)
  {
   if (PosServo[ii]>MinServo[ii])
   {
    PosServo[ii]--;
    MueveServo(ii,PosServo[ii],10);
   } 
   MandoPS3[ii]=0;
  }
 }
}

   // Lee un comando por teclado
/* LeeCadena();
 if (cad[0]=='P')
 {
  Serial.print("Posicion: ");
  for(ii=0;ii<6;ii++)
  {
   Serial.print("/Eje ");
   Serial.print(ii);
   Serial.print(": ");
   Serial.print(PosServo[ii]);
   Serial.print(" ");
  }
  Serial.println(" ");
  cad[0]=0;
 }
*/
delay(200); 
}
//·············································································
void LeeCadena(void)
{
 int ii;

 if (Serial.available() > 0)
 {
  ii=0;
  while (Serial.available() > 0)
  {
   cad[ii]=Serial.read(); 
   ii++;
  }
  cad[ii]=0;
 } 
}
//·············································································
void LeeMandoPS3(void)
{
 uint8_t joystick;
 
 Usb.Task();

  if(PS3.PS3Connected || PS3.PS3NavigationConnected) 
  {
   joystick=PS3.getAnalogHat(LeftHatX);
   if (joystick > 157)
     MandoPS3[0]=1;
   else if (joystick < 107)
     MandoPS3[0]=-1;

   joystick=PS3.getAnalogHat(LeftHatY);
   if (joystick > 157)
     MandoPS3[1]=1;
   else if (joystick < 107)
     MandoPS3[1]=-1;
    
   joystick=PS3.getAnalogHat(RightHatX);
   if (joystick > 157)
     MandoPS3[2]=1;
   else if (joystick < 107)
     MandoPS3[2]=-1;
  
   joystick=PS3.getAnalogHat(RightHatY);
   if (joystick > 157)
     MandoPS3[3]=1;
   else if (joystick < 107)
     MandoPS3[3]=-1;

    if(PS3.getButtonClick(UP))
      MandoPS3[4]=1;
    if(PS3.getButtonClick(RIGHT))
      MandoPS3[5]=1;
    if(PS3.getButtonClick(DOWN))
      MandoPS3[4]=-1;
    if(PS3.getButtonClick(LEFT))
      MandoPS3[5]=-1;
  }
}
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;
 }
}

El programa permite mover varios ejes simultáneamente. Le hemos tenido que bajar mucho la velocidad de movimiento de los servos porque de dejarla sin pausas el zarandeo era enorme y podía desencajarse.

Hacemos una primera prueba para comprobar el estado de movimiento. Adjuntamos un pequeño vídeo para muestra.