#include <AccelStepper.h>
#include <Servo.h>

AccelStepper motor_1(1,22,24);//step, dir
AccelStepper motor_2(1,28,30);
AccelStepper motor_3(1,34,36); 
AccelStepper motor_4(1,40,42);
AccelStepper motor_5(1,46,48);

String LEITURA, MOVIMENTO, Solucao;
int velocidade_Motor_1;
int velocidade_Motor_2;
int velocidade_Motor_3;
int velocidade_Motor_4;
int velocidade_Motor_5;

int Motor_1_go_to;
int Motor_2_go_to;
int Motor_3_go_to;
int Motor_4_go_to;
int Motor_5_go_to;

int mao_ESQ;
int mao_DIR;

Servo garra_ESQ;
Servo garra_DIR; 

void VAI();

void VAI_OMBRO_INTERNO_ESQUERDO_BAIXO();
void VAI_OMBRO_INTERNO_DIREITO_BAIXO();
void VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO();
void VAI_OMBRO_EXTERNO_DIREITO_BAIXO();

void CORREDOR_DIREITA();
void CORREDOR_ESQUERDA();

void VOLTA();

void setup()
{
  Serial.begin(9600); 
  
  pinMode(22, OUTPUT); pinMode(24, OUTPUT); pinMode(26, OUTPUT); digitalWrite(22, LOW); digitalWrite(24, LOW); digitalWrite(26, LOW); 
  pinMode(28, OUTPUT); pinMode(30, OUTPUT); pinMode(32, OUTPUT); digitalWrite(28, LOW); digitalWrite(30, LOW); digitalWrite(32, LOW); 
  pinMode(34, OUTPUT); pinMode(36, OUTPUT); pinMode(38, OUTPUT); digitalWrite(34, LOW); digitalWrite(36, LOW); digitalWrite(38, LOW); 
  pinMode(40, OUTPUT); pinMode(42, OUTPUT); pinMode(44, OUTPUT); digitalWrite(40, LOW); digitalWrite(42, LOW); digitalWrite(44, LOW); 
  pinMode(46, OUTPUT); pinMode(48, OUTPUT); pinMode(50, OUTPUT); digitalWrite(46, LOW); digitalWrite(48, LOW); digitalWrite(50, LOW); 
  
  motor_1.setCurrentPosition(0);
  motor_2.setCurrentPosition(0);
  motor_3.setCurrentPosition(0);  
  motor_4.setCurrentPosition(0);
  motor_5.setCurrentPosition(0);  

  garra_ESQ.attach(3);
  garra_DIR.attach(5);
  
  garra_ESQ.write(20);
  garra_DIR.write(20);

  velocidade_Motor_1 = 2000;
  velocidade_Motor_2 = 2000;
  velocidade_Motor_3 = 800;
  velocidade_Motor_4 = 2000;
  velocidade_Motor_5 = 2000;

  motor_1.setMaxSpeed(velocidade_Motor_1);
  motor_1.setSpeed(velocidade_Motor_1);
  motor_1.setAcceleration(velocidade_Motor_1*4);

  motor_2.setMaxSpeed(velocidade_Motor_2);
  motor_2.setSpeed(velocidade_Motor_2); 
  motor_2.setAcceleration(velocidade_Motor_2*4);
      
  motor_3.setMaxSpeed(velocidade_Motor_3);
  motor_3.setSpeed(velocidade_Motor_3);
  motor_3.setAcceleration(velocidade_Motor_3*4);

  motor_4.setMaxSpeed(velocidade_Motor_4);
  motor_4.setSpeed(velocidade_Motor_4);
  motor_4.setAcceleration(velocidade_Motor_4*4);

  motor_5.setMaxSpeed(velocidade_Motor_5);
  motor_5.setSpeed(velocidade_Motor_5);
  motor_5.setAcceleration(velocidade_Motor_5*4);

  mao_ESQ = 0;
  mao_DIR = 0;

}
void loop()
{
  while(Serial.available()==0);
  LEITURA = Serial.readStringUntil('\n');
  Serial.print ("Arduino Recebeu -  ");
  Serial.println (LEITURA);
  
      //Motor_1_go_to = 6000; //OMBRO INTERNO ESQUERDO    //pinos 22,24,26
      //Motor_2_go_to = 3000; //OMBRO EXTERNO ESQUERDO    //pinos 28,30,32
      //Motor_3_go_to = 1700; //CINTURA                   //pinos 34,36,38
      //Motor_4_go_to = 6000; //OMBRO INTERNO DIREITO     //pinos 40,42,44
      //Motor_5_go_to = 3000; //OMBRO EXTERNP DIREITO     //pinos 46,48,50
  
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  if (LEITURA =="1")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 6000;  
      Motor_2_go_to = 0;   
      Motor_3_go_to = 0;     
      Motor_4_go_to = 0;     
      Motor_5_go_to = 0;     
      VAI();
      delay(3500);
      CORREDOR_ESQUERDA();
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="2")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 6000;
      Motor_5_go_to = 0;
      VAI();
      delay(1000);
      CORREDOR_DIREITA();
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////    


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="3")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 6000;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3500);
      VAI_OMBRO_INTERNO_ESQUERDO_BAIXO();
      delay(3500);
      CORREDOR_ESQUERDA();
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="4")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 6000;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_INTERNO_DIREITO_BAIXO();
      delay(1000);
      CORREDOR_DIREITA();
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="5")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(1000);
      CORREDOR_ESQUERDA();
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="6")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(1000);
      CORREDOR_DIREITA();
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="7")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO();
      delay(1000);
      CORREDOR_ESQUERDA();
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="8")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO();
      delay(1000);
      CORREDOR_DIREITA();
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="9")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(1000);
      CORREDOR_ESQUERDA();
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="10")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(1000);
      CORREDOR_DIREITA();
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="11")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_DIREITO_BAIXO();
      delay(1000);
      CORREDOR_ESQUERDA();
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////  

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="12")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_DIREITO_BAIXO();
      delay(1000);
      CORREDOR_DIREITA();
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////  


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="13")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = -1700;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(1000);
      motor_3.moveTo(-1400);  
      while (motor_3.currentPosition() < -1400)
        {
          motor_3.run();
        } 

      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="14")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 1700;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(1000);
      motor_3.moveTo(1400);  
      while (motor_3.currentPosition() > 1400)
        {
          motor_3.run();
        } 
        
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////  

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="15")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = -1700;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO();
      delay(1000);
      motor_3.moveTo(-1400);  
      while (motor_3.currentPosition() < -1400)
        {
          motor_3.run();
        } 
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="16")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 1700;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;

      VAI();
      delay(7500);
      VAI_OMBRO_EXTERNO_DIREITO_BAIXO();
      delay(3500);
      motor_3.moveTo(1400);  
      while (motor_3.currentPosition() > 1400)
        {
          motor_3.run();
        } 
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  if (LEITURA =="17")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 6000;  
      Motor_2_go_to = 0;   
      Motor_3_go_to = 0;     
      Motor_4_go_to = 0;     
      Motor_5_go_to = 0;     
      VAI();
      delay(1000);
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="18")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 6000;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_INTERNO_ESQUERDO_BAIXO();
      delay(1000);
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="19")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(8000);
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="20")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO();
      delay(1000);
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="21")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(1000);
            mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="22")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 0;
      Motor_3_go_to = 0;
      Motor_4_go_to = 0;
      Motor_5_go_to = 3000;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_DIREITO_BAIXO();
      delay(1000);
      mao_DIR = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_DIR.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////  

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="23")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = -1700;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(1000);
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////   


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (LEITURA =="24")///////////////////////////////////////////////////////////////////////// VERIFICAR QUAL É O MOVIMENTO
    {
      Motor_1_go_to = 0;
      Motor_2_go_to = 3000;
      Motor_3_go_to = -1700;
      Motor_4_go_to = 0;
      Motor_5_go_to = 0;
      VAI();
      delay(3000);
      VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO();
      delay(1000);
      mao_ESQ = 1;//////////////////////////////////////////////////////////////////////////// verificar qual é a mão
      /*
      for (int M = 20; M <= 70; M = M + 1)
      {
        garra_ESQ.write(M);/////////////////////////////////////////////////////////////////// verificar qual é a mão
        delay(100);
      }
      */
      delay(3000);
      VOLTA();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 



    
}


void VAI()
{
      motor_1.moveTo(Motor_1_go_to);  
      motor_2.moveTo(Motor_2_go_to);   
      motor_3.moveTo(Motor_3_go_to);   
      motor_4.moveTo(Motor_4_go_to);  
      motor_5.moveTo(Motor_5_go_to); 
      if (Motor_3_go_to<0)
      {
        while ((motor_1.currentPosition() < Motor_1_go_to)||(motor_2.currentPosition() < Motor_2_go_to)||(motor_3.currentPosition() > Motor_3_go_to)||(motor_4.currentPosition() < Motor_4_go_to)||(motor_5.currentPosition() < Motor_5_go_to))
        {
          motor_1.run();
          motor_2.run();
          motor_3.run();
          motor_4.run();
          motor_5.run();
        } 
      }
      if (Motor_3_go_to>=0)
      {
        while ((motor_1.currentPosition() < Motor_1_go_to)||(motor_2.currentPosition() < Motor_2_go_to)||(motor_3.currentPosition() < Motor_3_go_to)||(motor_4.currentPosition() < Motor_4_go_to)||(motor_5.currentPosition() < Motor_5_go_to))
        {
          motor_1.run();
          motor_2.run();
          motor_3.run();
          motor_4.run();
          motor_5.run();
        } 
      }
}
void VOLTA()
{
      motor_1.moveTo(0);motor_2.moveTo(0);motor_3.moveTo(0);motor_4.moveTo(0);motor_5.moveTo(0);
      if (Motor_3_go_to<0)
      {
        while((motor_1.currentPosition()>0)||(motor_2.currentPosition()>0)||(motor_3.currentPosition()<0)||(motor_4.currentPosition()>0)||(motor_5.currentPosition()>0))
        {
          motor_1.run();motor_2.run();motor_3.run();motor_4.run();motor_5.run();
        }
      }
      if (Motor_3_go_to>=0)
      {
        while((motor_1.currentPosition()>0)||(motor_2.currentPosition()>0)||(motor_3.currentPosition()>0)||(motor_4.currentPosition()>0)||(motor_5.currentPosition()>0))
        {
          motor_1.run();motor_2.run();motor_3.run();motor_4.run();motor_5.run();
        }
      }
      if (mao_ESQ == 1)
      {
        /*
        for (int M = 70; M >= 20; M = M - 1)
        {
          garra_ESQ.write(M);
          delay(100);
        }
        */
      }
      if (mao_DIR == 1)
      {
       /*
        for (int M = 70; M >= 20; M = M - 1)
        {
          garra_DIR.write(M);
          delay(100);
        }
        */
      }
      mao_ESQ = 0;
      mao_DIR = 0;
      
      //Serial.write("ok");
      Serial.println("ok");
}


void VAI_OMBRO_INTERNO_ESQUERDO_BAIXO()
{
       motor_1.moveTo(4500);
       while(motor_1.currentPosition()>4500)
        {
          motor_1.run();
        }
} 
void VAI_OMBRO_INTERNO_DIREITO_BAIXO()
{
       motor_4.moveTo(4500);
       while(motor_4.currentPosition()>4500)
        {
          motor_4.run();
        }  
}

void VAI_OMBRO_EXTERNO_ESQUERDO_BAIXO()
{
       motor_2.moveTo(2000);
       while(motor_2.currentPosition()>2000)
        {
          motor_2.run();
        }
}

void VAI_OMBRO_EXTERNO_DIREITO_BAIXO()
{
       motor_5.moveTo(2000);
       while(motor_5.currentPosition()>2000)
        {
          motor_5.run();
        }
}

void CORREDOR_DIREITA()
{
      motor_3.moveTo(300);  
      while (motor_3.currentPosition() < 300)
        {
          motor_3.run();
        } 
      
}

void CORREDOR_ESQUERDA()
{
      motor_3.moveTo(-300);  
      while (motor_3.currentPosition() > -300)
        {
          motor_3.run();
        } 
}




Atividades recentes