Control de robot con mando de la PS3

International Español Software
1

Hola, estoy manejando un hexapodo con un mando de la play 3, la libreria PS3BT y una shield de USB maestro para el dongle, el problema es que no consigo que mantenga un bucle sin tener que darle al boton cada "paso" completo que da, en este cacho del codigo:

if (PS3.getButtonClick(UP)) {
       //if ( PS3.getAnalogHat(LeftHatY) < 117 && PS3.getAnalogHat(LeftHatY) > 10){
        Serial.print(F("\r\nUp"));
        Numero = 1 ; 
             //gait
            if( Numero = 1){
              
  d.write(5);h.write(180);l.write(180); //levantando 2,4,6
  delay(150); //¿150?
  c.write(80);g.write(100);k.write(100); //moviendo 2,4,6 hacia delante
  a.write(100);e.write(100);i.write(80); //moviedo 1,3,5 para avanzar 
  delay(150); //250
  d.write(angI);h.write(angD);l.write(angD); //bajando 2,4,6
  delay(150);
  
  b.write(5);f.write(5);j.write(180); //levantando 1,3,5
  delay(150);
  a.write(80);e.write(80);i.write(100); //moviendo 1,3,5 hacia delante
  c.write(100);g.write(80);k.write(80); //moviendo 2,4,6 para avanzar
  delay(150);
  b.write(angI);f.write(angI);j.write(angD); //bajando 1,3,5
  delay(150); 
    
          
}
}

Si pongo while este pulsada la flecha hacia arriba no funciona, y con la condiciion que esta comentada (joystick hacia delante) repite el movimiento un par de veces independientemente de lo que hagas con el joystick una vez lo muevas. He probado todo y todas las combinaciones absurdas de codigo que se me han pasado por la cabeza, pero no consigo que mantenga el ciclo hasta recibir otra orden distinta, muchas gracias de antemano y un saludo.

2

Código completo:

#include <PS3BT.h>
#include <usbhub.h>

#ifdef dobogusinclude
#include <spi4teensy3.h>
#include <SPI.h>
#endif
#include <Servo.h>

Servo a;
Servo b;
Servo c;
Servo d;
Servo e;
Servo f;
Servo g;
Servo h;
Servo i;
Servo j;
Servo k;
Servo l;

USB Usb;


BTD Btd(&Usb); 
PS3BT PS3(&Btd);
bool printTemperature;
bool printAngle;
int Numero;
int angD;
int angI;
void setup() {
  Serial.begin(115200);
#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));
  
  a.attach(2);
  b.attach(3);
  c.attach(4);
  d.attach(34); //5
  e.attach(6);
  f.attach(7);
  g.attach(8);
  h.attach(36); //9
  i.attach(32); //10
  j.attach(28); //11
  k.attach(30); //12
  l.attach(13);
  
  a.write(90);
  b.write(90);
  c.write(90);
  d.write(90);
  e.write(90);
  f.write(90);
  g.write(90);
  h.write(90);
  i.write(90);
  j.write(90);
  k.write(90);
  l.write(90);
  
  delay(2000);
 
 Numero = 0;
 angD = 90;
 angI = 90;
 

}

void loop() {
  Usb.Task();
  
   if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
    if (PS3.getAnalogHat(LeftHatX) > 137 || PS3.getAnalogHat(LeftHatX) < 117 || PS3.getAnalogHat(LeftHatY) > 137 || PS3.getAnalogHat(LeftHatY) < 117 || PS3.getAnalogHat(RightHatX) > 137 || PS3.getAnalogHat(RightHatX) < 117 || PS3.getAnalogHat(RightHatY) > 137 || PS3.getAnalogHat(RightHatY) < 117) {
      Serial.print(F("\r\nLeftHatX: "));
      Serial.print(PS3.getAnalogHat(LeftHatX));
      Serial.print(F("\tLeftHatY: "));
      Serial.print(PS3.getAnalogHat(LeftHatY));
      
      if (PS3.PS3Connected) { // The Navigation controller only have one joystick
        Serial.print(F("\tRightHatX: "));
        Serial.print(PS3.getAnalogHat(RightHatX));
        Serial.print(F("\tRightHatY: "));
        Serial.print(PS3.getAnalogHat(RightHatY));
      }
    } 
    }
  
    // Analog button values can be read from almost all buttons
    if (PS3.getAnalogButton(L2) || PS3.getAnalogButton(R2)) {
      Serial.print(F("\r\nL2: "));
      Serial.print(PS3.getAnalogButton(L2));
      if (PS3.PS3Connected) {
        Serial.print(F("\tR2: "));
        Serial.print(PS3.getAnalogButton(R2));
      }
    }
    if (PS3.getButtonClick(PS)) {
      Serial.print(F("\r\nPS"));
      PS3.disconnect();
    }
    else {
      if (PS3.getButtonClick(TRIANGLE)){
        //Inclinandose hacia adelante
      //Azules 
      a.write(45);c.write(90);e.write(135);g.write(45);i.write(90);k.write(135);
      //Derecha desde arriba
      b.write(5);d.write(45);f.write(90);
      //Izquierda desde arriba
      h.write(90);j.write(135);l.write(180);
     }
       
      if (PS3.getButtonClick(CIRCLE)){
        //Incinandose hacia derech desde arriba
      //Azules 
      a.write(45);c.write(90);e.write(135);g.write(45);i.write(90);k.write(135);
      //Derecha desde arriba
      b.write(5);d.write(5);f.write(5);
      //Izquierda desde arriba
      h.write(90);j.write(90);l.write(90);
     }
      if (PS3.getButtonClick(CROSS)){
        //Inclinandose hacia  atras
      //Azules 
      a.write(45);c.write(90);e.write(135);g.write(45);i.write(90);k.write(135);
      //Derecha desde arriba
      b.write(90);d.write(45);f.write(5);
      //Izquierda desde arriba
      h.write(180);j.write(135);l.write(90);
     }
      if (PS3.getButtonClick(SQUARE)){
        //Inclinado hacia la izquierda desde arriba
      //Azules 
      a.write(45);c.write(90);e.write(135);g.write(45);i.write(90);k.write(135);
      //Derecha desde arriba
      b.write(90);d.write(90);f.write(90);
      //Izquierda desde arriba
      h.write(180);j.write(180);l.write(180);
     }
       

      
     if (PS3.getButtonClick(UP)) {
       //if ( PS3.getAnalogHat(LeftHatY) < 117 && PS3.getAnalogHat(LeftHatY) > 10){
        Serial.print(F("\r\nUp"));
        Numero = 1 ; 
             //gait
            if( Numero = 1){
              
  d.write(5);h.write(180);l.write(180); //levantando 2,4,6
  delay(150); //¿150?
  c.write(80);g.write(100);k.write(100); //moviendo 2,4,6 hacia delante
  a.write(100);e.write(100);i.write(80); //moviedo 1,3,5 para avanzar 
  delay(150); //250
  d.write(angI);h.write(angD);l.write(angD); //bajando 2,4,6
  delay(150);
  
  b.write(5);f.write(5);j.write(180); //levantando 1,3,5
  delay(150);
  a.write(80);e.write(80);i.write(100); //moviendo 1,3,5 hcia delante
  c.write(100);g.write(80);k.write(80); //moviendo 2,4,6 para avanzar
  delay(150);
  b.write(angI);f.write(angI);j.write(angD); //bajando 1,3,5
  delay(150); 
    
          
}
}
      
      
      
     
     if (PS3.getButtonClick(RIGHT)) {
        Serial.print(F("\r\nRight"));
        Numero = 2;
        if (Numero = 2){
         //horario
  d.write(5); h.write(1800); l.write(180); //levantando 2,4,6
  delay(150); //¿150?
  c.write(110); g.write(110); k.write(110); //moviendo 2,4,6 antihorario AUMENTANDO
  delay(150);
  d.write(angI);h.write(angD);l.write(angD); //bajando 2,4,6
  delay(150);
  b.write(5); f.write(5); j.write(180); //levantando 1,3,5
  delay(150);
  a.write(110); e.write(110); i.write(110); //moviendo 1,3,5 horario
  delay(150);
  b.write(angI);f.write(angI);j.write(angD); //bajando 1,3,5
  delay(150);
  a.write(90); c.write(90); e.write(90); g.write(90); i.write(90); k.write(90); // igualando cuerpo
  delay(150);
       
        }
      }
      
      if (PS3.getButtonClick(DOWN)) {
        d.write(5);h.write(180);l.write(180); //levantando 2,4,6
        delay(150); //¿150?
        c.write(100);g.write(80);k.write(80); //moviendo 2,4,6 hacia atras
        a.write(80);e.write(80);i.write(100); //moviedo 1,3,5 para retroceder 
        delay(150); //250
        d.write(angI);h.write(angD);l.write(angD); //bajando 2,4,6
        delay(150);
  
        b.write(5);f.write(5);j.write(180); //levantando 1,3,5
        delay(150);
        a.write(100);e.write(100);i.write(80); //moviendo 1,3,5 hcia atras
        c.write(80);g.write(100);k.write(100); //moviendo 2,4,6 para retroceder
        delay(150);
        b.write(angI);f.write(angI);j.write(angD); //bajando 1,3,5
        delay(150); 
     
      } 
      if (PS3.getButtonClick(LEFT)) {
        Serial.print(F("\r\nLeft"));
        Numero = 3;
        if(Numero = 3){
        d.write(5); h.write(1800); l.write(180); //levantando 2,4,6
  delay(150); //¿150?
  c.write(70); g.write(70); k.write(70); //moviendo 2,4,6 antihorario AUMENTANDO
  delay(150);
  d.write(angI);h.write(angD);l.write(angD); //bajando 2,4,6
  delay(150);
  b.write(5); f.write(5); j.write(180); //levantando 1,3,5
  delay(150);
  a.write(70); e.write(70); i.write(70); //moviendo 1,3,5 horario
  delay(150);
  b.write(angI);f.write(angI);j.write(angD); //bajando 1,3,5
  delay(150);
  a.write(90); c.write(90); e.write(90); g.write(90); i.write(90); k.write(90); // igualando cuerpo
  delay(150); } 
        //////////////////
       
      }

      if (PS3.getButtonClick(L1)) { // Subir
      if ( angD > 90 && angI < 90 ){
        angD = angD - 5;
        angI = angI + 5;
      // Azules
        a.write(90); c.write(90); e.write(90); g.write(90); i.write(90); k.write(90);
      // Derecha de frente  // 180
        h.write(angD); j.write(angD); l.write(angD);
      // Izquierda de frente // 5
        b.write(angI); d.write(angI); f.write(angI);
        }}
        
      if (PS3.getButtonClick(L3))
        Serial.print(F("\r\nL3"));
        
      if (PS3.getButtonClick(R1)){ // Bajar
      if ( angD < 180 && angI > 5 ){
        angD = angD + 5;
        angI = angI - 5;
      // Azules
        a.write(90); c.write(90); e.write(90); g.write(90); i.write(90); k.write(90);
      // Derecha de frente  // 180
        h.write(angD); j.write(angD); l.write(angD);
      // Izquierda de frente // 5
        b.write(angI); d.write(angI); f.write(angI);
        }}
        
      if (PS3.getButtonClick(R3))
        Serial.print(F("\r\nR3"));

      if (PS3.getButtonClick(SELECT)) {
        Serial.print(F("\r\nSelect - "));
        PS3.printStatusString();
      }
      if (PS3.getButtonClick(START)) {
        Serial.print(F("\r\nStart"));
        printAngle = !printAngle;
      }
   
  }

}