APÊNDICE 1 - PROGRAMA DE MANIPULAÇÃO AUTOMÁTICA
/*
Titulo: Braço Robótico 3 Eixos (RRR) - Programa de Movimentação
Automática
Autor: Luiz Gustavo
Data: Outubro de 2014
*/
// Bibliotecas auxiliares
#include <Servo.h> //biblioteca padrão do Arduino para a utilização de servo-
motores
// Definição de constantes
#define servoPinClaw 8 // Conectado a porta 8 do arduino
#define servoPinFist 9 // Conectado a porta 9 do arduino
#define servoPinElbow 10 // Conectado a porta 10 do arduino
#define servoPinShoulder 11 // Conectado a porta 11 do arduino
#define servoPinBase 12 // Conectado a porta 12 do arduino
#define mov_bot_1 2 // Conectado a porta 2 do arduino
#define mov_bot_2 3 // Conectado a porta 3 do arduino
#define mov_bot_3 4 // Conectado a porta 4 do arduino
#define led_mov_1 5 // Conectado a porta 5 do arduino
#define led_mov_2 6 // Conectado a porta 6 do arduino
#define led_mov_3 7 // Conectado a porta 7 do arduino
#define led_on_off 13 // Conectado a porta 13 do arduino
// ############# PROTÓTIPO DAS FUNÇÕES AUXILIARES #############
//
// Funções para movimento suave
void moveClawOpen(const int tempoDelay, int inicio, int fim); //função que
movimenta abre a garra
void moveClawClose(const int tempoDelay, int inicio, int fim); //função que
movimenta fecha a garra
void moveFistUp(const int tempoDelay, int inicio, int fim); //função que
movimenta o pulso para cima
void moveFistDown(const int tempoDelay, int inicio, int fim); //função que
movimenta o pulso para baixo
void moveElbowUp(const int tempoDelay, int inicio, int fim); //função que
movimenta o cotovelo para cima
void moveElbowDown(const int tempoDelay, int inicio, int fim); //função que
movimenta o cotovelo para baixo
void moveShoulderAhead(const int tempoDelay, int inicio, int fim); //função
que movimenta o ombro para frente
void moveShoulderBack(const int tempoDelay, int inicio, int fim); //função que
movimenta o ombro para trás
void moveBaseRight(const int tempoDelay, int inicio, int fim); //função que
movimenta a base para direita (sentido horário)
void moveBaseLeft(const int tempoDelay, int inicio, int fim); //função que
movimenta a base para esquerda (sentido antihorário)
// Funções de Manipulação
void armOrigin(unsigned char baseInit, unsigned char baseEnd, unsigned char
shoulderInit, unsigned char shoulderEnd, unsigned char elbowInit, unsigned
char elbowEnd, unsigned char fistInit, unsigned char fistEnd, unsigned char
clawInit, unsigned char clawEnd);
void armDestination(unsigned char baseInit, unsigned char baseEnd, unsigned
char shoulderInit, unsigned char shoulderEnd, unsigned char elbowInit,
unsigned char elbowEnd, unsigned char fistInit, unsigned char fistEnd,
unsigned char clawInit, unsigned char clawEnd);
// positionArm HELP:
// positionArm(BASE INICIAL, BASE FINAL, OMBRO INICIAL, OMBRO
FINAL, COTOVELO INICIAL, COTOVELO FINAL, PUNHO INICIAL, PUNHO
FINAL);
Servo servo_base, servo_shoulder, servo_elbow, servo_fist, servo_claw; //
Atribuição de objetos (servos do braço), descrimina todos os servos do
programa
void setup() // Configuração inicial do MCU, define as entradas e saídas do
programa
{
[Link](9600); // inicia a comunicação serial a 9600 bps
pinMode(servoPinClaw, OUTPUT); // define como saida o servo motor da
Garra
pinMode(servoPinFist, OUTPUT); // define como saida o servo motor do Pulso
pinMode(servoPinElbow, OUTPUT); // define como saida o servo motor do
Cotovelo
pinMode(servoPinShoulder, OUTPUT); // define como saida o servo motor do
Ombro
pinMode(servoPinBase, OUTPUT); // define como saida o servo motor da
Base
pinMode(mov_bot_1, INPUT_PULLUP); // define como entrada, fazendo o uso
da função INPUT_PULLUP
pinMode(mov_bot_2, INPUT_PULLUP); // define como entrada, fazendo o uso
da função INPUT_PULLUP
pinMode(mov_bot_3, INPUT_PULLUP); // define como entrada, fazendo o uso
da função INPUT_PULLUP
pinMode(led_mov_1, OUTPUT); // define como saida o led que indica
movimento 1 em execução
pinMode(led_mov_2, OUTPUT); // define como saida o led que indica
movimento 2 em execução
pinMode(led_mov_3, OUTPUT); // define como saida o led que indica
movimento 3 execução
pinMode(led_on_off,OUTPUT); // define como saida o led que indica que o
arduino está sendo alimentado
servo_claw.attach(8); // define o pino ao qual o servo atribuido esta linkado
servo_fist.attach(9); // define o pino ao qual o servo atribuido esta linkado
servo_elbow.attach(10); // define o pino ao qual o servo atribuido esta linkado
servo_shoulder.attach(11); // define o pino ao qual o servo atribuido esta
linkado
servo_base.attach(12); // define o pino ao qual o servo atribuido esta linkado
// Posicionamento inicial do Braço Robótico
servo_base.write(90); // Posicionamento inicial da Base
servo_shoulder.write(90); // Posicionamento inicial do Ombro
servo_elbow.write(90); // Posicionamento inicial do Cotovelo
servo_fist.write(90); // Posicionamento inicial do Pulso
servo_claw.write(90); // Posicionamento inicial da Garra
delay(10000);
} //Fim da configuração do MCU
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ LOOP INFINITO
$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ //
void loop() // Função a ser realiazada inumeras vezes
{
// Inicio do Movimento 1
int sensor_bot_1=digitalRead(2); // Cria a variavel de leitura da entrada
mov_bot_1, ou seja, BOTÃO 1 acionado
if (sensor_bot_1==LOW){ //Avalia a condição, botão 1 pressionado, para
iniciar o movimento
digitalWrite(5,HIGH); // Se o botão referente ao movimento 1 for acionado o
led correspondente acenderá
armOrigin(90,180, 90,180, 90,180, 90,180, 90,180); // Preencher
coordenada inicial e coordenada do objeto para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
armDestination(180,0, 180,0, 180,0, 180,0, 180,0); // Preencher cordenada
do objeto e coordenada intermediária para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
armOrigin(0,180, 0,180, 0,180, 0,180, 0,180); // Preencher cordenada
intermediária e coordenada final do objeto para todos os servos
delay(1000); // os valores das coordenadas são apens
para testes, o programa final terá valores diferentes
armDestination(180,0, 180,0, 180,0, 180,0, 180,0); // Preencher cordenada
final do objeto e coordenada inicial para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
// Comanda o Braço Robótico para a posição inicial
servo_base.write(90); // Posicionamento inicial da Base
servo_shoulder.write(90); // Posicionamento inicial do Ombro
servo_elbow.write(90); // Posicionamento inicial do Cotovelo
servo_fist.write(90); // Posicionamento inicial do Pulso
servo_claw.write(90); // Posicionamento inicial da Garra
delay(10000);
//Ao finalizar o movimento 1, o led correspondente piscará e apagará
digitalWrite(5,LOW);
delay(200);
digitalWrite(5,HIGH);
delay(200);
digitalWrite(5,LOW);
delay(200);
digitalWrite(5,HIGH);
delay(200);
digitalWrite(5,LOW);
delay(200);
digitalWrite(5,HIGH);
delay(200);
else{
digitalWrite(5,LOW); // Caso o botão referente ao movimento 1 não esteja
acionado, o led correspondente estará apagado
} // Fim do Movimento 1
// Inicio do Movimento 2
int sensor_bot_2=digitalRead(3); // Cria a variável de leitura da entrada
mov_bot_2, ou seja, BOTÃO 2 acionado
if (sensor_bot_2==LOW){ //Avalia a condição, botão 2 pressionado, para
iniciar o movimento
digitalWrite(6,HIGH); // Se o botão referente ao movimento 2 for acionado, o
led correspondente acenderá
armOrigin(90,180, 90,180, 90,180, 90,180, 90,180); // Preencher
coordenada inicial e coordenada do objeto para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
armDestination(180,0, 180,0, 180,0, 180,0, 180,0); // Preencher cordenada
do objeto e coordenada intermediária para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
armOrigin(0,180, 0,180, 0,180, 0,180, 0,180); // Preencher cordenada
intermediária e coordenada final do objeto para todos os servos
delay(1000); // os valores das coordenadas são apens
para testes, o programa final terá valores diferentes
armDestination(180,0, 180,0, 180,0, 180,0, 180,0); // Preencher cordenada
final do objeto e coordenada inicial para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
// Comanda o Braço Robótico para a posição inicial
servo_base.write(90); // Posicionamento inicial da Base
servo_shoulder.write(90); // Posicionamento inicial do Ombro
servo_elbow.write(90); // Posicionamento inicial do Cotovelo
servo_fist.write(90); // Posicionamento inicial do Pulso
servo_claw.write(90); // Posicionamento inicial da Garra
delay(10000);
//Ao finalizar o movimento 2, o led correspondente piscara e apagará
digitalWrite(6,LOW);
delay(200);
digitalWrite(6,HIGH);
delay(200);
digitalWrite(6,LOW);
delay(200);
digitalWrite(6,HIGH);
delay(200);
digitalWrite(6,LOW);
delay(200);
digitalWrite(6,HIGH);
delay(200);
else{
digitalWrite(3,LOW); // Caso o botão referente ao movimento 2 não esteja
acionado, o led correspondente estará apagado
} // Fim do Movimento 2
// Inicio do Movimento 3
int sensor_bot_3=digitalRead(4); // Cria a variável de leitura da entrada
mov_bot_3, ou seja, BOTÃO 3 acionado
if (sensor_bot_3==LOW){ //Avalia a condição, botão 3 pressionado, para
iniciar o movimento
digitalWrite(7,HIGH); // Se o botão referente ao movimento 3 for acionado, o
led correspondente acenderá
armOrigin(90,180, 90,180, 90,180, 90,180, 90,180); // Preencher
coordenada inicial e coordenada do objeto para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
armDestination(180,0, 180,0, 180,0, 180,0, 180,0); // Preencher cordenada
do objeto e coordenada intermediária para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
armOrigin(0,180, 0,180, 0,180, 0,180, 0,180); // Preencher cordenada
intermediária e coordenada final do objeto para todos os servos
delay(1000); // os valores das coordenadas são apens
para testes, o programa final terá valores diferentes
armDestination(180,0, 180,0, 180,0, 180,0, 180,0); // Preencher cordenada
final do objeto e coordenada inicial para todos os servos
delay(1000); // os valores das coordenadas são
apens para testes, o programa final terá valores diferentes
// Comanda o Braço Robótico para a posição inicial
servo_base.write(90); // Posicionamento inicial da Base
servo_shoulder.write(90); // Posicionamento inicial do Ombro
servo_elbow.write(90); // Posicionamento inicial do Cotovelo
servo_fist.write(90); // Posicionamento inicial do Pulso
servo_claw.write(90); // Posicionamento inicial da Garra
delay(10000);
//Ao finalizar o movimento 3, o led correspondente piscará e apagará
digitalWrite(7,LOW);
delay(200);
digitalWrite(7,HIGH);
delay(200);
digitalWrite(7,LOW);
delay(200);
digitalWrite(7,HIGH);
delay(200);
digitalWrite(7,LOW);
delay(200);
digitalWrite(7,HIGH);
delay(200);
else{
digitalWrite(7,LOW); // Caso o botão referente ao movimento 1 não esteja
acionado, o led correspondente estará apagado
} // Fim do Movimento 1
} //end void loop
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ LOOP INFINITO
$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ //
//
$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
$$$$$$$$$$$$$$$$$$$$$$$$$$ //
// ############# DESENVOLVIMENTO DAS FUNÇÕES DE MANIPULAÇÃO
############# //
/*Todas as funções de manipulação tem o mesmo principio de funcionalidade
Sendo assim elas recebem como Input as coordenadas iniciais e finais de
cada servo motor
e ao realizar uma comparção entre as entradas define qual função de
movimento suave utilizará
A função armOrigin realiza os movimentos dos servos, um de cada vez a
partir da base, ou seja,
o movimento segue a ordem - base - ombro - cotovelo - pulso - garra.
A função armDestination realiza os movimentos dos servos, um de cada vez
a partir da base, ou seja,
o movimento segue a ordem - garra -pulso - cotovelo - ombro - base.
*/
void armOrigin(unsigned char baseInit, unsigned char baseEnd, unsigned char
shoulderInit, unsigned char shoulderEnd, unsigned char elbowInit, unsigned
char elbowEnd, unsigned char fistInit, unsigned char fistEnd, unsigned char
clawInit, unsigned char clawEnd)
{
// Movimenta a base
if(baseInit < baseEnd)
{
moveBaseRight(30,baseInit,baseEnd);
delay(100);
}
else
{
moveBaseLeft(30,baseInit,baseEnd);
delay(100);
}
// Fim da movimentação da base
// Movimenta o ombro
if(shoulderInit < shoulderEnd)
{
moveShoulderAhead(10,shoulderInit,shoulderEnd);
delay(100);
}
else
{
moveShoulderBack(10,shoulderInit,shoulderEnd);
delay(100);
}
// Fim da movimentação do ombro
// Movimenta o cotovelo
if(elbowInit < elbowEnd)
{
moveElbowUp(10,elbowInit,elbowEnd);
delay(100);
}
else
{
moveElbowDown(10,elbowInit,elbowEnd);
delay(100);
}
// Fim da movimentação do cotovelo
//Movimenta o punho
if(fistInit < fistEnd)
{
moveFistUp(10,fistInit,fistEnd);
delay(100);
}
else
{
moveFistDown(10,fistInit,fistEnd);
delay(100);
}
// Fim da movimentação do punho
//Movimenta a Garra
if(clawInit < clawEnd)
{
moveClawOpen(10,fistInit,fistEnd);
delay(100);
}
else
{
moveClawClose(10,fistInit,fistEnd);
delay(100);
}
// Fim da movimentação da Garra
} //Fim da função armOrigin
void armDestination(unsigned char baseInit, unsigned char baseEnd, unsigned
char shoulderInit, unsigned char shoulderEnd, unsigned char elbowInit,
unsigned char elbowEnd, unsigned char fistInit, unsigned char fistEnd,
unsigned char clawInit, unsigned char clawEnd)
{
//Movimenta a Garra
if(clawInit < clawEnd)
{
moveClawOpen(10,fistInit,fistEnd);
delay(100);
}
else
{
moveClawClose(10,fistInit,fistEnd);
delay(100);
}
// Fim da movimentação da Garra
//Movimenta o punho
if(fistInit < fistEnd)
{
moveFistUp(10,fistInit,fistEnd);
delay(100);
}
else
{
moveFistDown(10,fistInit,fistEnd);
delay(100);
}
// Fim da movimentação do punho
// Movimenta o cotovelo
if(elbowInit < elbowEnd)
{
moveElbowUp(10,elbowInit,elbowEnd);
delay(100);
}
else
{
moveElbowDown(10,elbowInit,elbowEnd);
delay(100);
}
// Fim da movimentação do cotovelo
// Movimenta o ombro
if(shoulderInit < shoulderEnd)
{
moveShoulderAhead(10,shoulderInit,shoulderEnd);
delay(100);
}
else
{
moveShoulderBack(10,shoulderInit,shoulderEnd);
delay(100);
}
// Fim da movimentação do ombro
// Movimenta a base
if(baseInit < baseEnd)
{
moveBaseRight(30,baseInit,baseEnd);
delay(100);
}
else
{
moveBaseLeft(30,baseInit,baseEnd);
delay(100);
}
// Fim da movimentação da base
} //Fim da função armDestination
//
###############################################################
################# //
// ############# DESENVOLVIMENTO DAS FUNÇÕES DE
MOVIMENTAÇÃO SUAVE ############# //
/*As funções de movimento suave tem o mesmo principio de funcionalidade.
Inicialmente a função chama três variaveis (inicio, fim e tampoDelay),
depois é criado um laço "for" para que a variavel i, que inicialmente é igau a
variavel inicio,
seja incremnetada em uma unidade a cada período de tempo (tempoDelay),
até que a mesma seja igual a variavel fim.
Devido as condições iniciais das variaveis esse processo tambem pode ser de
decremento.
O preocesso se repete para cada um dos servos.*/
//Função for para desaceleração do movimento da Garra
void moveClawOpen(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i<fim; i++)
{
servo_claw.write(i);
delay(tempoDelay);
}
}
void moveClawClose(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i>fim; i--)
{
servo_claw.write(i);
delay(tempoDelay);
}
}
//Função for para desaceleração do movimento do Pulso
void moveFistUp(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i<fim; i++)
{
servo_fist.write(i);
delay(tempoDelay);
}
}
void moveFistDown(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i>fim; i--)
{
servo_fist.write(i);
delay(tempoDelay);
}
}
//Função for para desaceleração do movimento do Cotovelo
void moveElbowUp(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i<fim; i++)
{
servo_elbow.write(i);
delay(tempoDelay);
}
}
void moveElbowDown(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i>fim; i--)
{
servo_elbow.write(i);
delay(tempoDelay);
}
}
//Função for para desaceleração do movimento do Ombro
void moveShoulderAhead(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i<fim; i++)
{
servo_shoulder.write(i);
delay(tempoDelay);
}
}
void moveShoulderBack(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i>fim; i--)
{
servo_shoulder.write(i);
delay(tempoDelay);
}
}
//Função for para desaceleração do movimento da Base
void moveBaseRight(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i<fim; i++)
{
servo_base.write(i);
delay(tempoDelay);
}
}
void moveBaseLeft(const int tempoDelay, int inicio, int fim)
{
for(int i=inicio; i>fim; i--)
{
servo_base.write(i);
delay(tempoDelay);
}
}