Written by admin
Ahora procedemos a colocar estos dos servos en su lugar en el robot.
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:
Deja una respuesta