mayo 7, 2015

BRMIN-Programa de movimiento por teclado

Written by

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

Category : BRAZO ROBOT MINI

Deja una respuesta

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *

Proudly powered by WordPress and Sweet Tech Theme