diciembre 23, 2019

4RAYA-Cambio de servos

Written by

Ahora procedemos a colocar estos dos servos en su lugar en el robot.

Servos de los ejes 1 y 6

Cargamos un pequeño programa de prueba, el posicionador de ejes, modificado para usar la nueva controladora de servos:

// BRAZO ROBOT MINI
// Versión3
//  Controladora Adafruit de servos
//  Eje2 con dos servos iguales controlados por la misma patilla
// Mueve los ejes poco a poco hasta la posicion correcta
// usando las teclas del teclado
// Muestra la posición de los servos en pantalla
//
// Teclas de servos: 
//  1: 1 - Q - A - Z
//  2: 2 - W - S - X
//  3: 3 - E - D - C
//  4: 4 - R - F - V
//  5: 5 - T - G - B
//  6: 6 - Y - H - N
//  P: muestra posición de servos en monitor serie

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// Definiciones de constantes
#define SERVOMIN  150       // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600       // This is the 'maximum' pulse length count (out of 4096)
#define OSC_FREQ 27000000   // The internal osc. is closer to 27MHz 
#define SERVO_FREQ 50       // Analog servos run at ~50 Hz updates

#define NUMSERVOS 6
#define interPeq 1          // Intervalo pequeño de movimiento
#define interGr 20          // Intervalo grande de movimiento
#define interDelay 20       // Pausa entre intervalos de movimiento

// Creación de la placa - dirección por defecto 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

uint16_t PosServo[NUMSERVOS]={(SERVOMAX+SERVOMIN)/2,             // Posiciones actuales de servos e iniciales
                         (SERVOMAX+SERVOMIN)/2,
                         (SERVOMAX+SERVOMIN)/2,
                         (SERVOMAX+SERVOMIN)/2,
                         (SERVOMAX+SERVOMIN)/2,
                         380};                      
uint16_t MinServo[NUMSERVOS]={150,150,150,150,150,340};          // Posiciones mínimas de servos
uint16_t MaxServo[NUMSERVOS]={600,600,600,600,600,393};          // Posiciones máximas de servos

char cad[20];     // Cadena para leer del puerto serie

/////////////////////////INICIALIZACIÓN///////////////////////////////
// Prepara la placa controladora para los servos
void setup() 
{
  uint8_t servonum;
  uint16_t pulselen;
  
  Serial.begin(9600);
  Serial.println("Prueba de posicionamiento de ejes");

  pwm.begin();
  pwm.setOscillatorFrequency(OSC_FREQ);    
  pwm.setPWMFreq(SERVO_FREQ);             // Analog servos run at ~50 Hz updates

  // Coloca los servos en la posición inicial
  for(servonum=0; servonum<NUMSERVOS ; servonum++)
  {
    pwm.setPWM(servonum, 0, PosServo[servonum]);
  } 
  
  Serial.println("Robot preparado");
  delay(1000);
}


/////////////////////// PROGRAMA PRINCIPAL //////////////////////
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(" ");
      break;
      
    case '1':
      MueveServo(0,PosServo[0]+interGr,interDelay);
      break;

    case 'Q':
      MueveServo(0,PosServo[0]+interPeq,interDelay);
      break;
      
    case 'A':
      MueveServo(0,PosServo[0]-interPeq,interDelay);
      break;
      
    case 'Z':
      MueveServo(0,PosServo[0]-interGr,interDelay);
      break;

    case '2':
      MueveServo(1,PosServo[1]+interGr,interDelay);
      break;

    case 'W':
      MueveServo(1,PosServo[1]+interPeq,interDelay);
      break;

    case 'S':
      MueveServo(1,PosServo[1]-interPeq,interDelay);
      break;

    case 'X':
      MueveServo(1,PosServo[1]-interGr,interDelay);
      break;

    case '3':
      MueveServo(2,PosServo[2]+interGr,interDelay);
      break;

    case 'E':
      MueveServo(2,PosServo[2]+interPeq,interDelay);
      break;
      
    case 'D':
     MueveServo(2,PosServo[2]-interPeq,interDelay);
     break;
     
    case 'C':
      MueveServo(2,PosServo[2]-interGr,interDelay);
      break;
      
    case '4':
      MueveServo(3,PosServo[3]+interGr,interDelay);
      break;

    case 'R':
      MueveServo(3,PosServo[3]+interPeq,interDelay);
      break;

    case 'F':
      MueveServo(3,PosServo[3]-interPeq,interDelay);
      break;

    case 'V':
      MueveServo(3,PosServo[3]-interGr,interDelay);
      break;

    case '5':
      MueveServo(4,PosServo[4]+interGr,interDelay);
      break;

    case 'T':
      MueveServo(4,PosServo[4]+interPeq,interDelay);
      break;

    case 'G':
      MueveServo(4,PosServo[4]-interPeq,interDelay);
      break;

    case 'B':
      MueveServo(4,PosServo[4]-interGr,interDelay);
      break;

    case '6':
      MueveServo(5,PosServo[5]+interGr,interDelay);
      break;

    case 'Y':
      MueveServo(5,PosServo[5]+interPeq,interDelay);
      break;

    case 'H':
      MueveServo(5,PosServo[5]-interPeq,interDelay);
      break;

    case 'N':
      MueveServo(5,PosServo[5]-interGr,interDelay);
      break;
  }
  cad[0]=0;
}

//----------------------------------------------------------------------------
// Mueve un servo de un eje de la posición actual a la final paso a paso
//  con un retardo entre pasos y comprueba si se pasa de los límites
void MueveServo(uint8_t numservo,uint16_t posicion,int velocidad)
{
 uint16_t jj,incr;

 // Comprueba los extremos
 if (posicion>MaxServo[numservo]) posicion=MaxServo[numservo];
 else if (posicion<MinServo[numservo]) posicion=MinServo[numservo];
 
 // Giro del servo
 incr=(posicion>PosServo[numservo])? 1 : -1;
 if (posicion!=PosServo[numservo])
 {
   jj=PosServo[numservo];
   while(jj!=posicion) 
   {
     pwm.setPWM(numservo, 0, jj);
     jj+=incr;
     delay(velocidad);
   }
   PosServo[numservo]=posicion;
 }
}
//************************************************************************************
// Lee una cadena de caracteres por el puerto serie
//************************************************************************************
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
 }  
}

Y este es el resultado:

Category : 4 EN RAYA ROBÓTICO

Deja un comentario

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