Codigo de Robot2

//
###############################################
###############################################
####################################
// #
LIBRERIAS
#
//
###############################################
###############################################
####################################
#include <stdlib.h> // Funciones para gestin de memoria dinmica, control
de procesos y otras
#include <Servo.h> // Funciones predefinidas para trabajar con servos
//
###############################################
###############################################
####################################
// #
DECLARACIONES
#
//
###############################################
###############################################
####################################
//#define CALIBRACION // Para calibrar el robot
// LONGITUD DE LOS BRAZOS DEL ROBOT (en milimetros)

#define L3 13.2
#define L2 60
#define L1 35
// CONEXIONES DE LOS SERVOS A LOS PINES

#define SERVOELEVADORPIN 5 // Servo de Elevacion -> Pin 5 del Arduino
(cable NARANJA del servo)
#define SERVOIZQUIERDOPIN 6 // Servo Izquierdo

(cable NARANJA del servo)
#define SERVODERECHOPIN 7 // Servo Derecho
NARANJA del servo)
-> Pin 6 del Arduino

-> Pin 7 del Arduino (cable
// VALORES DE LOS SERVOS (Usados y optimizados en la fase de CALIBRACION)

#define AMPLITUDSERVOIZQ 1850
#define AMPLITUDSERVODER 800
#define RANGOSERVOS 600
// POSICIONES DEL SERVO ELEVADOR

#define ACTIVARELEVADOR 890 // Cambio de letras
#define DESACTIVARELEVADOR 1400 // Escribiendo
#define VELOCIDADELEVADOR 1500 // Velocidad con la que levantamos o
bajamos
// PUNTOS ORIGEN SERVOS (I/D)

#define O1X 22
#define O1Y -25
#define O2X 47
#define O2Y -25
volatile float coordX = 3; // Limite minimo donde empezamos a escribir en el

eje X
volatile double ultimaY = 47.5; // Limite maximo en el eje Y
volatile double ultimaX = 75; // Limite maximo en el eje X
Servo servoElevador; // Crea un objeto servo para controlar el servo ELEVADOR

Servo servoIzquierdo; // Crea un objeto servo para controlar el servo
IZQUIERDO
Servo servoDerecho; // Crea un objeto servo para controlar el servo DERECHO
String palabra=""; // Palabra que el robot escribira

float escala = 0.75; // Escala de la fuente con la que escribe el robot
int posElevador = 1500; // Posicion inicial del servo elevador
//
###############################################
###############################################
####################################
// #
FUNCIONES
#
//
###############################################
###############################################
####################################
void setup(){
servoElevador.attach(SERVOELEVADORPIN); // Asocia el servo al pin 5
servoIzquierdo.attach(SERVOIZQUIERDOPIN); // Asocia el servo al pin 6
servoDerecho.attach(SERVODERECHOPIN); // Asocia el servo al pin 7
ir_a(75.2, 47);
moverElevador(1);
delay(1000); // Esperamos un poco
}
void loop() {
if (servoElevador.attached() && servoIzquierdo.attached() &&
servoDerecho.attached()) { // Comprobamos que los SERVOS esten asociados a
los PINES
#ifdef CALIBRACION
calibrar(); // LLamada a la funcion de calibracion
#else
escribirPalabra(); // Llamada a la funcion que escribe la palabra
#endif
} else {
printf("\nERROR: Al menos un servomotor no esta conectado. \n");
}
// Desactivamos los servos
servoElevador.detach();
servoIzquierdo.detach();
servoDerecho.detach();
}
void escribirPalabra() {
int posicionCaracter; // Posicion que ocupa el caracter dentro de la
palabra
int coordY; // Varia dependiendo de la posicion de la letra, debido a que las
ultimas tienen una desviacion que hay que corregir mediante software
for (posicionCaracter = 0 ; posicionCaracter < palabra.length() ;
posicionCaracter++) {
if(posicionCaracter<4){
coordY = 25;
}else{
coordY = 28;
}
escribirCaracter(coordX, coordY, palabra[posicionCaracter], escala); //
Llamada a la funcion que dibuja la letra
coordX+=10; // La siguiente letra debe escribirse en la siguiente
posicion
}
}
void calibrar() {
// Esta funcion se utiliza para calibrar los brazos del robot.
// Los brazos de los servos tienen que tener 90 grados entre los movimientos
que hacemos
ir_a(-3, 29.2);
delay(500);
ir_a(74.1, 28);
delay(500);
}
void escribirCaracter(float coordX, float coordY, char letra, float escala) {

// El eje en el que dibujamos cada letra quedaria asi expresado
graficamente:
/*
EJE Y
MAX 20
|
|
|
|
|
15
|
|
|
|
|
10
|
|
|
|
|
|
|
|
|
|
---------------------------------------------------------------- EJE X
(0,0)
12 MAX
*/
// Las letras se empiezan en el 2 porque sino al dibujarlas se solapararian
switch (letra) {
case 'A':
ir_a(coordX + 2 * escala, coordY + 0 * escala);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'B':
moverElevador(0);
moverElevador(1);
break;
case 'C':
moverElevador(0);
moverElevador(1);
break;
case 'D':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'E':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'F':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'G':
moverElevador(0);

moverElevador(1);
break;
case 'H':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'I':
moverElevador(0);
moverElevador(1);
moverElevador(0);

moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'J':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'K':
ir_a(coordX + 10+ escala, coordY + 18 * escala);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'L':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'M':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'N':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'O':
moverElevador(0);
moverElevador(1);
break;
case 'P':
moverElevador(0);
moverElevador(1);
moverElevador(0);

moverElevador(1);
break;
case 'Q':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'R':
moverElevador(0);
moverElevador(1);
moverElevador(0);

moverElevador(1);
moverElevador(0);
ir_a(coordX + 12*escala, coordY + 0*escala);
moverElevador(1);
break;
case 'S':
moverElevador(0);
moverElevador(1);
break;
case 'T':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'U':

moverElevador(0);
moverElevador(1);
break;
case 'V':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'W':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'X':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'Y':
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
case 'Z':
moverElevador(0);

moverElevador(1);
moverElevador(0);
moverElevador(1);
break;
}
}
void moverElevador(int senial) {

// Empleamos las siguientes funciones de la libreria Servo.h :
// "writeMicroseconds(int)" --> Manda un pulso de un ancho de tantos
microsegundos como le especifiquemos en el parntesis, de esta forma el
servo se colocar en un determinado punto. 1500-Centro, 1000-Izq y 2000-Der
// "delayMicroseconds(int)" --> Hace una pausa en el programa con la cantidad
de tiempo pasada en microsegundos
switch (senial) {
case 0: // Escribir
if (posElevador >= DESACTIVARELEVADOR) {
while (posElevador >= DESACTIVARELEVADOR) {
posElevador--;
servoElevador.writeMicroseconds(posElevador);
delayMicroseconds(VELOCIDADELEVADOR);
}
} else {
while (posElevador <= DESACTIVARELEVADOR) {
posElevador++;
}
}
break;
case 1: // Movimiento entre letras

if (posElevador >= ACTIVARELEVADOR) {
while (posElevador >= ACTIVARELEVADOR) {
posElevador--;
}
} else {
while (posElevador <= ACTIVARELEVADOR) {
posElevador++;
}
}
break;
}
}
void ir_a(double posicionX, double posicionY) {

double distanciaX, distanciaY, r;
distanciaY = posicionY - ultimaY;
distanciaX = posicionX - ultimaX;
r = floor(4 * sqrt(distanciaX * distanciaX + distanciaY * distanciaY));
if (r < 1)
r = 1;
int i;
for (i = 0; i <= r; i++) {
// Dibujamos la linea punto a punto hasta la distancia calculada
generarMovimientosParaEscribirEn(ultimaX + (i * distanciaX / r), ultimaY
+ (i * distanciaY / r));
}
// Actualizamos la variable que guarda la ultima posicion escrita
ultimaX = posicionX;
ultimaY = posicionY;
}
double calcularAngulo (double ladoA, double ladoB, double ladoC) {

return acos((ladoA * ladoA + ladoC * ladoC - ladoB * ladoB) / (2 * ladoA *
ladoC)); // Aplicando la Ley de Cosenos, asi podemos calcular el angulo que
queremos
}
void generarMovimientosParaEscribirEn(double x, double y) {

delay(1);
double distanciaX, distanciaY, s, angulo1, angulo2, hX, hY;
/*
M_PI = Numero PI en la libreia Math.h
Hay que recordar:
LONGITUD DE LOS BRAZOS DEL ROBOT
#define L3 13.2
#define L2 60
#define L1 35
Usamos la formula matematica de la "longitud de arco" o "rectificacin de

una curva", que es la medida de la distancia a lo largo de una curva o
dimensin lineal.
BREVE TEORIA:
Suponiendo que se tiene una curva rectificable cualquiera,
determinada por una funcin "f", y suponiendo que se quiere aproximar la
longitud del arco de curva "s" que va desde un punto "a" a uno b.
Con la formula de la longitud del arco es posible disear una
serie de tringulos rectngulos cuyas hipotenusas concatenadas cubran el
arco de curva elegido.
Para calcular un pequeo segmento de la curva, podemos
aproximar la formula mencionada, bajo condiciones especiales, con la formula
del Teorema de Pitagoras, que en nuestro codigo seria: s =
raizCuadrada(distanciaX^2 + distanciaY^2)
*/
// ------------------------------------------> PARA EL BRAZO IZQUIERDO

<--------------------------------------distanciaX = x - O1X;
distanciaY = y - O1Y;
// Para calcular el triangulo rectangulo mencionado en la teoria,
calculamos el triangulo entre el servoIzq, el brazo y el rotulador:
s = sqrt(distanciaX * distanciaX + distanciaY * distanciaY);
angulo1 = atan2(distanciaY, distanciaX);
angulo2 = calcularAngulo(L1, L2, s);
// Movemos el servo a donde queremos
servoIzquierdo.writeMicroseconds(floor(((angulo2 + angulo1 - M_PI) *
RANGOSERVOS) + AMPLITUDSERVOIZQ)); // "writeMicroseconds(int)" --> Manda
un pulso de un ancho de tantos microsegundos como le especifiquemos en el
parntesis, de esta forma el servo se colocar en un determinado punto.
1500-Centro, 1000-Izq y 2000-Der
// ------------------------------------------> PARA EL BRAZO DERECHO

<--------------------------------------angulo2 = calcularAngulo(L2, L1, s); // Invertimos los lados y calculamos
hX = x + L3 * cos((angulo1 - angulo2 + 0.621) + M_PI); // 36,5 ->

0.621
hY = y + L3 * sin((angulo1 - angulo2 + 0.621) + M_PI);
distanciaX = hX - O2X;
distanciaY = hY - O2Y;
// Para calcular el triangulo rectangulo mencionado en la teoria,
calculamos el triangulo entre el servoDer, el brazo y el rotulador:
s = sqrt(distanciaX * distanciaX + distanciaY * distanciaY);
angulo1 = atan2(distanciaY, distanciaX);
angulo2 = calcularAngulo(L1, (L2 - L3), s);
// Movemos el servo a donde queremos
servoDerecho.writeMicroseconds(floor(((angulo1 - angulo2) *
RANGOSERVOS) + AMPLITUDSERVODER)); // "writeMicroseconds(int)" -->
Manda un pulso de un ancho de tantos microsegundos como le especifiquemos
en el parntesis, de esta forma el servo se colocar en un determinado
punto. 1500-Centro, 1000-Izq y 2000-Der
}

Codigo de Robot2

Transféré par

Informations du document

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Codigo de Robot2

Transféré par

Droits d'auteur :

Formats disponibles

//

// LONGITUD DE LOS BRAZOS DEL ROBOT (en milimetros)

// CONEXIONES DE LOS SERVOS A LOS PINES

#define SERVOIZQUIERDOPIN 6 // Servo Izquierdo

-> Pin 6 del Arduino

// VALORES DE LOS SERVOS (Usados y optimizados en la fase de CALIBRACION)

// POSICIONES DEL SERVO ELEVADOR

// PUNTOS ORIGEN SERVOS (I/D)

volatile float coordX = 3; // Limite minimo donde empezamos a escribir en el

Servo servoElevador; // Crea un objeto servo para controlar el servo ELEVADOR

String palabra=""; // Palabra que el robot escribira

void escribirCaracter(float coordX, float coordY, char letra, float escala) {

ir_a(coordX + 2 * escala, coordY + 0 * escala);

ir_a(coordX + 7 * escala, coordY + 20 * escala);

ir_a(coordX + 2 * escala, coordY + 10 * escala);

ir_a(coordX + 2 * escala, coordY + 10 * escala);

ir_a(coordX + 4 * escala, coordY + 20 * escala);

ir_a(coordX + 12 * escala, coordY + 20 * escala);

void moverElevador(int senial) {

case 1: // Movimiento entre letras

void ir_a(double posicionX, double posicionY) {

double calcularAngulo (double ladoA, double ladoB, double ladoC) {

void generarMovimientosParaEscribirEn(double x, double y) {

Usamos la formula matematica de la "longitud de arco" o "rectificacin de

// ------------------------------------------> PARA EL BRAZO IZQUIERDO

// ------------------------------------------> PARA EL BRAZO DERECHO

hX = x + L3 * cos((angulo1 - angulo2 + 0.621) + M_PI); // 36,5 ->

Vous aimerez peut-être aussi