Trabalho Prático 1
Introdução
Para o trabalho, testamos diversas montagens do robô, iniciando pela sugerida no livro. Paralelamente á montagem, elaboramos o programa do Shaft Encoder bem como sua localização no robô. Quando definimos uma montagem mais específica, fizemos a programação do controle PD pra, depois, fazer a programação do quadrado e do círculo.
Montagem
A primeira montagem testada foi a sugerida pelo livro. Porém, além de algumas peças não coincidirem, a implementação do Shaft Encoder e da caneta se mostraram inviáveis com essa montagem. O alto número de reduções no motor também não eram necessárias, uma vez que o motor disponibilizado já tinha um certo nível de redução.
A primeira montagem testada foi a indicada no livro.
- colocar foto do livro -
Então, com a necessidade de mais espaço para encaixar o sensor break-beam e vendo que as reduções estavam em excesso, um modelo mais espaçoso foi montado, sem reduções.
Percebemos que ainda assim alguma redução era necessária. Além disso os sensores não estavam fixados com firmeza e o robô não era robusto. Então, os motores foram colocados orientados para frente e mais redução foi acrescentada. Além disso, uma adaptação para os sensores foi feita de modo que eles ficassem mais firmes.
Essa montagem se mostrou muito eficiente, porém uma das polias estava encostando no sensor e, depois de começar a programar, percebemos que as reduções estavam exageradas. Com isso tiramos uma engrenagem e substituímos a polia
Finamente fizemos a adaptação da caneta, com alguns eixos e 2 elásticos, e fixamos a Handyboard. Essa é a montagem final do nosso robô.
Estrutura do Shaft Encoder
Programação
- Linha de Comando
Após o download do Interactive C e dos Drives da Handyboard, começamos a nos familiarizar com os comandos pela linha de comando do IC, começando pelos comandos de ligar o motor e capturar os dados do sensores, bem como os comandos para imprimir dados na tela da Handyboard. Com esses comandos já começamos a testar os sensores para implementação do Shaft Encoder.
- Download
Depois de aprender os comandos, fizemos os primeiros programas para Download na Handyboard. No primeiro download o robô já andava em linha reta, com um simples controle PD que, como ensinado do livro, fazia um cálculo de quantos “furinhos” um motor estava á frente de outro e, de posse desse número, aumentava a potência do outro motor em 1% vezes o número de furinhos de diferença, e diminuia a do motor adiantado em 1% vezes o número de furinhos de diferença.
- Quadrado e círculo
O cálculo do trajeto do quadrado se deu por distância. Calculou-se a correspondência entre furinhos lidos pelo “Shaft Encoder” e a distância percorrida pelo robô, tanto na translação quanto rotação. Porém o quadrando não estava saindo corretamente, e vários ajustes empíricos tiveram que ser realizados. Nesse ponto o controle PD sofreu algumas alterações pois não estava funcionando bem na rotação. A função usada foi a encoderx_velocity, onde “x” era o motor correspondente. Com essa função uma queda de potência até quase desligamento total dos motores foi observada. O círculo não ofereceu grandes problemas para o grupo.
- Problema de “zig-zag”
Outro problema notado pelo grupo é que o PD estava causando um andar em zig-zag por parte do robô. Com isso, implementamos uma programação em paralelo, onde a atuação nos 2 motores se dava ao mesmo tempo, e não em sequência.
Testes
Código
#include fencdr6.icb
#include sencdr4.icb
#define MOTOR_ESQ 1
#define MOTOR_DIR 2
#define NUM_VOLTA 3
#define VEL_RETA 12
#define VEL_ESQ 7
#define VEL_DIR 12
#define C_CIRC 875
#define DIST_ANGULO 83
#define DIST_RETA 221
void main()
{
int pot_esq = 65, pot_dir=71;
printf("Start->Quadrado Stop -> Circulo.");
//O sleep foi apenas para dar tempo de liberar da pilha comandos anteriores de outros programas
sleep(2.5);
while(1)
{
if (stop_button())
{
printf("Desenhando o Circulo.\n");
pot_esq = 48;
pot_dir = 80;
circulo(&pot_dir,&pot_esq);
motor(MOTOR_DIR,0);
motor(MOTOR_ESQ,0);
}
if(start_button())
{
printf("Desenhando o Quadrado.\n");
pot_esq = 65;
pot_dir = 71;
motor(MOTOR_ESQ,pot_esq);
motor(MOTOR_DIR,pot_dir);
quadrado(&pot_dir, &pot_esq);
motor(MOTOR_ESQ,0);
motor(MOTOR_DIR,0);
}
}
}
//Funcao que desenha o circulo
void circulo(int *p_dir, int *p_esq)
{
int j, m_esq, m_dir;
//Desenhando o circulo
for(j=0;j<NUM_VOLTA;j++)
{
encoder4_counts = 0;
m_esq = start_process(regula_curva_esq(p_esq));
m_dir = start_process(regula_curva_dir(p_dir));
while(encoder4_counts < C_CIRC)
{}
kill_process(m_dir);
kill_process(m_esq);
}
}
void regula_curva_esq(int *potencia)
{
while(1)
{
if(encoder4_velocity > VEL_ESQ)
{
*potencia--;
motor(MOTOR_ESQ,*potencia);
}
else if(encoder4_velocity < VEL_ESQ)
{
*potencia++;
motor(MOTOR_ESQ,*potencia);
}
}
}
void regula_curva_dir(int *potencia)
{
while(1)
{
if(encoder6_velocity > VEL_DIR)
{
*potencia--;
motor(MOTOR_DIR,*potencia);
}
else if(encoder6_velocity < VEL_DIR)
{
*potencia++;;
motor(MOTOR_DIR,*potencia);
}
}
}//Desenha o Quadrado completo 3 vezes
int quadrado(int *p_dir, int *p_esq)
{
int i;
for(i =0;i<(NUM_VOLTA);i++)
{
reta(p_dir, p_esq);
angulo_reto(p_dir, p_esq);
reta(p_dir, p_esq);
angulo_reto(p_dir, p_esq);
reta(p_dir, p_esq);
angulo_reto(p_dir, p_esq);
reta(p_dir, p_esq);
angulo_reto(p_dir, p_esq);
}
motor(MOTOR_ESQ,0);
motor(MOTOR_DIR,0);
return 0;
}
//Desenha a reta do quadrado
void reta(int *pot_dir, int *pot_esq)
{
int sub=0;
encoder6_counts = 0;
//Estou considerando apenas um encoder, pois se considerar os dois pode haver conflito de valores
//O valor 628 primeiramente foi calculado com base na mecanica do robo e depois realizado experimentos para adequar
//Este ajuste seria interessante melhorar-lo, mas esta bem proximo, com esta condicao no while que ele anda os 30cm
while(encoder6_counts<DIST_RETA)
{
sub = encoder4_velocity - encoder6_velocity;
//ecnoder 6 velocidade da direita
/*
Realiza o controle da velocidade dos motores para andar em linha reta, este controle ficou bem simples.
Tentei fazer com outras formulas para chegar ao valor otimo mas rapido, mas elas funcionavam bem em um primeiro instante e depois falhavam.
Este metodo ficou simples mas funcional.
*/
if(sub > 0)
{
if(*pot_dir == 100)
{
*pot_esq--;
}
else if(*pot_esq==1)
{
*pot_dir++;
}
else
{
*pot_dir++;
*pot_esq--;
}
}
else if(sub < 0)
{
if(*pot_esq == 100)
{
*pot_dir--;
}
else if(*pot_dir == 1)
{
*pot_esq++;
}
else
{
*pot_dir--;
*pot_esq++;
}
}
if(encoder6_velocity > VEL_RETA)
{
*pot_dir--;
}
else if(encoder6_velocity < VEL_RETA)
{
*pot_dir++;
}
if(encoder4_velocity > VEL_RETA)
{
*pot_esq--;
}
else if(encoder4_velocity < VEL_RETA)
{
*pot_esq++;
}
//Realimenta o motor com os novos valores de potencia calculados
motor(MOTOR_ESQ,*pot_esq);
motor(MOTOR_DIR,*pot_dir);
}
}
//Muda a direcao para o quadrado no angulo reto
void angulo_reto(int *p_dir, int *p_esq)
{
motor(MOTOR_ESQ,0);
motor(MOTOR_DIR,0);
//Desligo os motores e dou um sleep, pois percebi que sem fazer isto os motores as vezes desligavam ou davam um tranco muito forte
//Antes e depois de "realizar" o angulo reto desligo os motores e dou um sleep para evitar deslizamentos e trancos desnecessarios que atrapalhavam o desenho
sleep(0.5);
encoder6_counts = 0;
motor(MOTOR_ESQ,-76);
motor(MOTOR_DIR, 80);
while(encoder6_counts<DIST_ANGULO)
{}
motor(MOTOR_ESQ,0);
motor(MOTOR_DIR,0);
sleep(0.5);
motor(MOTOR_ESQ,*p_esq);
motor(MOTOR_DIR,*p_dir);
}