===== PRÉVIA & COMPETIÇÃO =====
== ==
{{:cursos:introrobotica:2011-2:grupo05:welcome.gif|}}
* Nós, do grupo MIBR, no intuito de finalmente participar da competição de robôs ao final do segundo semestre de 2011, que foi realizada dia 12/12/2011, dedicamos boa parte do nosso tempo a fim de tentar concretizar as estratégias traçadas.
* A competição para a qual nos preparamos consistia em:
- Orientar-se, ao inicio da partida, no campo através da luz polarizada;
- Seguir em direção ao campo adversário;
- Derrubar a torre dos inimigos;
- Atravessar a ponte que liga os dois campos e passa sobre um "absmo";
- Capturar o rei, seus soldados e trazer para a prisão em seu campo.
* Após ter implementado sensores optico ativos, diferencial e optico reflexivos nos trabalhos anteriores, optamos por não utilizar o diferencial na competição. Tal decisão vem do fato de a qualidade da localização da luz cair com o quadrado da distância. Assim, realizamos a identificação da luz através do lado mais iluminado, feita pelo LDR utilizado no sensor optico ativo.
* Outra decisão importante foi relacionada a catapulta. Esta, implementada no TP3, seria de grande utilidade para derrubar as torres inimigas de longa distância, mas optamos por um meio de capturar o rei em detrimento desta estratégia. Nos próximos tópicos descrevemos mais detalhadamente cada passo, e a estratégia adotada.
== ==
{{:cursos:introrobotica:2011-2:grupo05:design.gif|}}
* Como design final, continuamos por optar posicionar a handy-board em cima do eixo das rodas, a fim de, como já discutido em outros trabalhos, minimizar os erros obtidos na movimentação do robô (por exemplo, girar 90º em cima do eixo).
{{ :cursos:introrobotica:2011-2:grupo05:Imagem1.jpg?500 }}
* Adotamos um reforço estrutural nas laterais a fim de interligar todas as peças do robô. Podemos visualizar esse reforço na figura a seguir.
{{ :cursos:introrobotica:2011-2:grupo05:Imagem2.jpg?500 }}
* Sensores de toque foram implementados junto a peças lego (diferentes do KIT Lego fornecido) na traseira do robô a fim de normalizar a sua direção e sentido para atingir a estratégia que será citada posteriormente. Visualizamos então na figura a seguir o novo sensor implementado.
{{ :cursos:introrobotica:2011-2:grupo05:Imagem3.jpg?500 }}
* Um sensor óptico-reflexivo, com emissor e receptor, foi implementado para uso de detecção da emissão de luz de partida para a competição, como podemos ver a seguir. É importante ressaltar que este sensor foi utilizado através das entradas analógicas da handy-board e se localizava embaixo do robô.
{{ :cursos:introrobotica:2011-2:grupo05:Imagem4.jpg?500 }}
* Para coletar o rei inimigo, bloco amarelo ou vermelho, tivemos inicialmente a idéia de construir um braço simples que travaria o bloco analisado ao robô. Porém, ao evoluir do robô para o preparativo da competição, encontramos uma forma de com apenas um motor, construir uma garra que inicialmente deveria fechar e prender o bloco ao robô, como podemos ver a seguir.
{{ :cursos:introrobotica:2011-2:grupo05:Imagem6.jpg?500 }}
* Então, quando a garra travasse, uma série de engrenagens travaria na parte posterior do seu “braço” e movimentaria a peça como um todo, como podemos ter uma idéia pela próxima figura.
{{ :cursos:introrobotica:2011-2:grupo05:Imagem5.jpg?500 }}
{{:cursos:introrobotica:2011-2:grupo05:estrategia.gif|}}
=== Descrição ===
* Inicialmente a estratégia foi discutida em cima dos pontos. De forma bem "simples" e bolamos as seguintes idéias iniciais.
{{ :cursos:introrobotica:2011-2:grupo05:est_mibr.png?500 }}
- Obter referências para encontrar o Norte. (Como? Luz polarizada? Um par emissor receptor em cada lado do robô seria 100%)
- Capturar o próprio Rei. Guardá-lo preso ao robô. Evita que o adversário o capture e ganhe 8 a 10 pontos. Não tem problema derrubar as próprias torres depois da captura.
- Continua reto, até que os dois sensores de linha vejam linha preta. Andar reto com precisão é fundamental e já conseguimos fazer.
- Seguir e entrar no algoritmo segue-linha padrão até que os dois sensores vejam linha preta novamente. Marcar a distância pra saber se algo deu errado.
- No fim da ponte (sabe que chegou no fim pela distância e pelos dois sensores no preto) continua reto até a posição do Rei inimigo (também pela distância).
- Checa cor e captura o Rei inimigo. (+8). Com 8 pontos já é provável ganhar.
- Derruba torres adversárias. (+2 x4) Com 16 pontos, é quase impossível perder.
- Procura, na linha, por blocos inimigos (pela cor) até o tempo acabar. É importante não sair da linha para não correr o risco de cair no abismo. Com mais de 16 pontos é matematicamente o vencedor (pq o inimigo não pode capturar o nosso Rei).
* Porém, ao longo do desenvolvimento final do robô, e entre seus contratempos, tivemos de modificar a discussão inicial e bolar uma estratégia viável a tempo para a competição.
* A estratégia desenvolvida pela equipe focava de forma bem simples, realizar as tarefas como: detectar início da partida, se localizar, atravessar o campo sem derrubar as próprias torres, identificar a linha da ponte para seguir, seguir linha, identificar entrada do campo inimigo, descer a garra para sua utilização, alinhamento com as torres inimigas devido a posição fixa, derrubar as torres (direita e esquerda) e finalmente procurar através de movimentos aleatórios limitados pelas linhas pretas o rei-inimigo.
{{ :cursos:introrobotica:2011-2:grupo05:imagem8.png?500 }}
=== Problemas Enfrentados e Resultado ===
* No inicio da preparação para a competição nos deparamos com um problema, a handy board estava corrompendo o código constantemente. Tal defeito, após consulta ao monitor, foi identificado como possivel falha de código. Durante o periodo de implementação, ele diminuiu e ocorria somente quando havia sobre corrente. Entretanto, no dia da competição o defeito se tornou critico e identificamos que ao ligar o LDR do sensor optico ativo, se este fosse acionado, o código era corrompido.
* Conseguimos, faltando duas horas para a competição, uma outra HB que se demonstrou complentamente recepitiva ao código. Entretanto não tivemos tempo de recondificar para as novas adversidades de hardware, o que acarretou em um comportamento completamente errôneo na competição, levando o Catcher a cair no absmo e se auto destruir!
=== ===
----
== ==
{{:cursos:introrobotica:2011-2:grupo05:fotos.gif|}}
== Fotos do Robô "CATCHER" #COMPETICAO – Versão Definitiva ==
* O robô então ficou com seu design final bem robusto, prático e eficiente.
{{ :cursos:introrobotica:2011-2:grupo05:Imagem7.jpg?500 }}
----
== ==
{{:cursos:introrobotica:2011-2:grupo05:codigo.gif|}}
* Foram implementados códigos bem modulados. Os arquivos utilizados são: color.ic, Comp.ic, directions_sensor.ic, hbmenu.ic, MIBR_TP3.ic, e wf_movements.ic.
* color.ic
/*
Introducao a Robotica - TPs 1, 2 e 3
Grupo MIBR (Made in Brasil)
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
Modulo responsavel pelo detector de cores!
*/
#define LED_RED 4
#define LED_GREEN 2
#define LED_BLUE 3
#define STOP_LIMIT 195 // Limiar de parada ao encontrar o bloco
#define RED_LIMIT 80 // Limiar de valor do LDR para o bloco vermelho
#define GREEN_LIMIT 70 // Limiar de valor do LDR para o bloco verde
#define BLUE_LIMIT 110 // Limiar de valor do LDR para o bloco azul
//#define STARTLIGHT 8 // Limiar de detecao da luz de partida do campo (ja foi calibrado)
#define BRANCO 5
#define AZUL 4
#define VERDE 2
#define AMARELO 3
#define VERMELHO 1
#define PRETO 0
#use "directions_sensor.ic"
//Global variables
//int stop=0;
int count=0;
//Descobre qual a cor do bloco a frente do sensor
int DetectColor() //ldramb = valor do ldr apenas com luz ambiente
{ //Uma variavel para cada aquisicao do LDR com uma cor
int ldramb, ldrall, ldrblue, ldrgreen,ldryellow,ldrred,ldrblack, ldrmaior=0;
int cor=0;
LedOff(LED_BLUE);
LedOff(LED_RED);
LedOff(LED_GREEN);
//Aquisita valor atual do LDR
sleep(0.2);
ldramb=light(2);
printf("LDR: %d\n", ldramb);
//sleep(2.0);
//Ascende cada led e aquisita o valor do LDR
LedOn(LED_RED);//bit_set(0x1008, 0b00010000);
sleep(0.2);
ldrred=light(2);
//printf("Led vermelho! %d\n",ldrred-ldramb);
//sleep(2.0);
LedOff(LED_RED);//bit_clear(0x1008, 0b00010000);
LedOn(LED_GREEN);//bit_set(0x1008, 0b00000100);
sleep(0.2);
ldrgreen=light(2);
//printf("Led verde! %d\n",ldrgreen-ldramb);
//sleep(2.0);
LedOff(LED_GREEN);//bit_clear(0x1008, 0b00000100);
LedOn(LED_BLUE);//bit_set(0x1008, 0b00001000);
sleep(0.2);
ldrblue=light(2);
//printf("Led azul! %d\n",ldrblue-ldramb);
//sleep(2.0);
LedOff(LED_BLUE);//bit_clear(0x1008, 0b00001000);
LedOn(LED_BLUE);
LedOn(LED_RED);
LedOn(LED_GREEN);
sleep(0.2);
ldrall=light(2);
//printf("Led todos! %d\n",ldrall-ldramb);
//sleep(0.2);
LedOff(LED_BLUE);
LedOff(LED_RED);
LedOff(LED_GREEN);
//Calcula cor do bloco
//maior
if (ldrmaior120)
{ cor=3;//amarelo
if(ldramb>220) cor=5; //branco
}
else cor=0; //preto
}
else cor=cor+1; //vermelho
}
if(ldrmaior==ldrgreen-ldramb){
// if((ldrgreen-ldrred)<15) {
// //if(ldramb>120) cor=cor+3; //amarelo
// cor=0; //preto
// }
// else
cor=cor+2; //verde
}
if(ldrblue-ldramb==ldrmaior) cor=cor+4; //azul
//printf("LDR maior: %d\n",ldrmaior);
//sleep(1.0);
switch (cor) {
// Verifica todas as combinacoes possiveis do calculo acima
case VERMELHO: //vermelho
printf("Bloco vermelho!\n");
sleep(2.0);
break;
case VERDE: //verde
printf("Bloco verde!\n");
sleep(2.0);
break;
case AMARELO: //vermelho+verde = amarelo
printf("Bloco amarelo!\n");
sleep(2.0);
break;
case AZUL: //azul
printf("Bloco azul!\n");
sleep(2.0);
break;
case BRANCO: //azul + vermelho = roxo
printf("Bloco branco!\n");
sleep(2.0);
break;
case PRETO: //preto
printf("Bloco preto!\n");
sleep(2.0);
break;
default: cor=5;//Cor desconhecida = branco
break;
}
return cor;
}
//Acende o led da porta D2,D3,D4,D5 ou D4/D5
void LedOn(int led)
{
switch(led)
{
case 2: //Porta D2
bit_set(0x1008, 0b00000100);
break;
case 3: //Porta D3
bit_set(0x1008, 0b00001000);
break;
case 4: //Porta D4
bit_set(0x1008, 0b00010000);
break;
case 5: //Porta D5
bit_set(0x1008, 0b00100000);
break;
case 6: //Porta D4 e D5
bit_set(0x1008, 0b00010000);
bit_set(0x1008, 0b00100000);
break;
default:
printf("Led invalido!\n");
break;
}
}
//Apaga o led da porta D2,D3,D4,D5 ou D4/D5
void LedOff(int led)
{
switch(led)
{
case 2: //Porta D2
bit_clear(0x1008, 0b00000100);
break;
case 3: //Porta D3
bit_clear(0x1008, 0b00001000);
break;
case 4: //Porta D4
bit_clear(0x1008, 0b00010000);
break;
case 5: //Porta D5
bit_clear(0x1008, 0b00100000);
break;
case 6: //Porta D4 e D5
bit_clear(0x1008, 0b00010000);
bit_clear(0x1008, 0b00100000);
break;
default:
printf("Led invalido!\n");
break;
}
}
* Comp.ic
#use "color.ic"
#use "wf_movements.ic"
#use "directions_sensor.ic"
#define MOTORC 0
int stop=0; //global
int rest = 0;
void main()
{
int start=0;
int pidgo=0, pidgob=1, pidbridge=2, pid_time = 3;
int campo;
int cor;
int lamb;
int not_done=1;
int blockcolor=5; //inicia como branco
int timgo=0;
int e1 = 20;
int e2 = 20;
enable_encoder(0);
enable_encoder(1);
poke(0x1009, 0x3c);
LedOff(2);
LedOff(3);
LedOff(4);
campo = SetPolarized();
printf("Press start or turn lights on!\n");
sleep(1.0);
//printf("READY!\n");
//sleep(1.0);
//Enqunto Luz nao acesa, espera...
while(!start_button()){
if(StartLight()) break;
}
pid_time=start_process(LifeTime());
PolarizedDiffSensor(campo);
sleep(0.1);
//Direciona para frente
Bumper();
sleep(0.1);
//Segue direto monitorando portas digitais 10 e 11 (sensores de seguir linha) -> poucos cm <10cm
pidgo = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (1a vez)
kill_process(pidgo);
Go(100.0); //espessura da linha
printf("1a linha!");
//captura o prprio rei
sleep(0.2);
printf("Captura Rei!");
sleep(1.0);
//detecta ambos em zero (2a vez)
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
Go(50.0); //espessura da linha
printf("2a linha!");
//ativa o segue linha e atravessa a ponte seguindo linha
pidbridge = start_process(FollowLine());
BlackLineDetect(); //detecta ambos em zero (3a vez)
Go(50.0); //espessura da linha
printf("\nPonte!");
StopMotors();
//anda reto ate detectar ambos em zero(4a vez)
pidgob = start_process(PDControl2(&e1, &e2, MOVE));
pidgo = start_process(Discharge());
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
Go(50.0); //espessura da linha
printf("4a linha!");
Gira(4.0, RIGHT);
sleep(0.1);
Go(200.0);
Gira(4.0, RIGHT);
sleep(0.1);
Gira(4.0, LEFT);
sleep(0.1);
Go(700.0);
kill_process(pidgo);
pidgo = start_process(SearchKing());
while(1)
{
if(light(2)<(int)(0.58*(float)lamb))
{
break;
}
}
kill_process(pidgo);
cor = DetectColor();
if(cor==VERDE || cor==AZUL || cor==AMARELO || cor==VERMELHO)
Charge();
}
//pega o bloco e desliga
void Charge()
{
motor(MOTORC,-70);
sleep(2.0);
motor(MOTORC,0);
}
//solta o bloco e desliga
void Discharge()
{
motor(MOTORC,70);
sleep(2.0);
motor(MOTORC,0);
}
void SearchKing()
{
int pidgob = 0;
int e1 = 20;
int e2 = 20;
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
Gira(4.0, LEFT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
Gira(4.0, LEFT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
//1
Go(200.0);
Gira(4.0, LEFT);
Go(200.0);
Gira(4.0, LEFT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
//2
Gira(4.0, RIGHT);
Go(200.0);
Gira(4.0, RIGHT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
//3
Gira(4.0, LEFT);
Go(200.0);
Gira(4.0, LEFT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
//4
Gira(4.0, RIGHT);
Go(200.0);
Gira(4.0, RIGHT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
//5
Gira(4.0, LEFT);
Go(150.0);
Gira(4.0, LEFT);
pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
BlackLineDetect(); //detecta ambos em zero (2a vez)
kill_process(pidgob);
Gira(4.0, RIGHT);
pidgob = start_process(FollowLine());
BlackLineDetect(); //detecta ambos em zero (3a vez)
}
* directions_sensor.ic
/*
Introducao a Robotica - TPs 1, 2 e 3
Grupo MIBR (Made in Brasil)
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
Modulo responsavel pelos sensores de direcao (Infra-vermelhos que mantem o robo na linha,
e o diferencial que direciona para luz polarizada vertical e horizontal)!
*/
#define DIFF 3
#define VERTICAL 1
#define HORIZONTAL 2
#define STARTLIGHT 12 // Limiar de detecao da luz de partida do campo (ja foi calibrado)
#use "wf_movements.ic"
#use "Comp.ic"
//Inverte o sentido da leitura de LDR, torna-a mais logica.
//Quanto maior o valor de light, maior o valor.
int light(int port)
{
return 255 - analog(port); //Nao utilizar portas 0 e 1
}
//Testa valores de LDR
//Mais iluminado, maior o valor.
void LDR_test(/**/)
{
while(!stop_button()) {
printf("LDR: %d\n", light(2));
msleep(100L);
}
}
//Seta direcao onde o melhor valor da luz foi encontrado
void SetDirection(int iteracao)
{
int i = 0;
int oitavo = 0;
int direct;
float dist=0.0;
printf("%d\n", iteracao); //Imprime Status
reset_encoder(0);
//avalia em qual sentido corrigira(horario, anti-horario)
if(iteracao ==0 )
{
direct = LEFT;
iteracao++;
}//Liga os motores e gira para o mesmo lado q o polarizeddiff girou
else if(iteracao ==2 )
{
direct = RIGHT;
iteracao = 1;
}
else if(iteracao ==3 )
{
direct = RIGHT;
iteracao = 2;
}
printf("%d\n", iteracao);
msleep(1000L);
for(i=0; i< iteracao; i++)
Gira(4.0, direct);
}
int SetPolarized()
{
if(1==getKnob("Direcao[1-Verde, 2-Azul: ",1, 1))
return VERTICAL;
else
return HORIZONTAL;
}
//Funcao controladora do sensor diferencial polarizado
void PolarizedDiffSensor(int polo)
{
//variaveis de controle
int iteracao=0;
int encid1, encid2;
int pid = 0, pid1 = 1;
int direction = 0, correct = 0;
int i = 0;
float results[4];
i=0;
//Gira e capta medias de valores
printf("%d\n", (light(2)));
msleep(1000L);
results[i] = (float)((light(2)));
for(i=1; i<4; i++)
{
Gira(4.0, RIGHT);
printf("%d\n", (light(2)));
msleep(1000L);
results[i] = (float)((light(2)));
}
// Gira(4.0, RIGHT);
//faz avaliao da melhor direo, de acordo com a luz desejada
for(i=1; i<4; i++)
{
if(/*(polo==VERTICAL && results[0] > results[i]) ||(polo==HORIZONTAL && */results[0] < results[i])
{
iteracao = i;
results[0] = results[i];
}
}
//faz apurao da direo, para corrigir o erro natural(robo gira mais q o angulo pedido)
if(iteracao!=1)
{
printf("%f - ", results[0]);
SetDirection(iteracao);
}
}
//segue a linha atraves dos chaves opticas-reflexivas
void FollowLine()
{
int pid=4, pid1=5;
int encid1, encid2;
encid1 = 20;
encid2 = 20;
//faz controle Proporcional Direcional em paralelo
pid = start_process(PDControl2(&encid1, &encid2, MOVE));
pid1 = start_process(le_sensores());
while(!stop_button() && stop == 0) {
//caso um o sensor da direita esteja sobre a linha, corrige para direita
if(!digital(10))
correct_right_side(&encid1, &encid2);
else if(!digital(11)) //oposto do de cima
correct_left_side(&encid1, &encid2);
//caso um bloco seja encontrado, para
sleep(0.3);
if(!digital(10)&&!digital(11)) break;
}
kill_process(pid);
kill_process(pid1);
StopMotors();
}
//le os dados colhidos pelos sensores ir
void le_sensores(){
while(!stop_button()) {
//if(digital(10) && digital(11))
printf("IR: D-%d E-%d\n", digital(10), digital(11));
msleep(100L);
}
}
//corrige para direita
void correct_right_side(int *encid1, int *encid2)
{
*encid1 = 16;
*encid2 = 20;
while(!digital(10));
*encid1 = 20;
*encid2 = 20;
}
//corrige para esquerda
void correct_left_side(int *encid1, int *encid2)
{
*encid1 = 20;
*encid2 = 16;
while(!digital(11));
*encid1 = 20;
*encid2 = 20;
}
//Reconhece acendimento da luz de partida
int StartLight()
{ int start=0;
start=analog(6);
printf("READY! - IR:%d\n", start);
msleep(10L);
if(start0)
//while(1)
{
if(timergo>100)
{
printf("TimeUP");
break;
}
timergo++;
msleep(100L);
}
alloff();
}
*/
void BlackLineDetect()
{
int timergo=0;
int line=2, get=0;
//line=(digital(10)+digital(11));
//while(timergo<100)
while(1)
{ if(!digital(10)&&!digital(11)) break;
/* line=(digital(10)+digital(11));
if(line<1)get++;
else line=2;
msleep(10L);
line=(digital(10)+digital(11));
if(line<1)get++;
else line=2;
msleep(10L);
line=(digital(10)+digital(11));
if(line<1)get++;
else line=2;
msleep(10L);
if(get>2) break;
timergo++;
msleep(70L);*/
}
alloff();
}
* hbmenu.ic
/****************************** hbmenu.c ********************************/
/* Menu functions which also allow variables to be set via the knob
and selection buttons
Version 1.0
Written for MIT 6.270 contest by Anne Wright 11/14/1991
Version 2.0
Converted for Handy Board for Botball by Anne Wright 1/13/2001
*/
/* abstractions for chosen_button */
#define NEITHER_B 0
#define START_B 1
#define STOP_B 2
/* abstractions for wait_button */
#define UP_B 3
#define DOWN_B 4
#define CYCLE_B 5
int min(int x, int y)
{ if (x > y)
return y;
else
return x;
}
/*****************************button routines*************************/
/* Return minimum of two integers */
/* defined in cmucam3.ic which is loaded by this file -dpm 1/03
int min(int a,int b)
{
if(a_array_size(choices))
n=_array_size(choices);
/* Wait until no button is pressed */
wait_button(UP_B);
/* While no button is pressed, display the string from the array
of strings passed in which corresponds to the current position
of the knob */
while((button = chosen_button())==NEITHER_B) {
selection=knob_int_value(0,n-1);
if(selection!=last_selection) {
printf("%s\n",choices[selection]);
msleep(150L);
last_selection=selection;
}
}
/* Wait until no button is pressed */
wait_button(UP_B);
if(button==STOP_B)
return(-1); /** -1 means stop pressed -- do not reset value **/
else
return(selection); /* Stop not pressed, return val */
}
/*
* Local variables:
* comment-column: 40
* c-indent-level: 4
* c-basic-offset: 4
* End:
*/
* MIBR_TP3.ic
/*
Introducao a Robotica - TP 3
Grupo MIBR (Made in Brasil)
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
Main Module
*/
#use "wf_movements.ic"
//#use "color.ic"
//"color.ic" includes glogal stop variable
void main()
{
float results[8];
int direcao,cat;
int start=0;
int pid_time;
// Habilita encoder do canal 0
enable_encoder(0);
// Habilita encoder do canal 1
enable_encoder(1);
// Habilita saidas digitais
poke(0x1009, 0x3c);
//LedOff(2);
//LedOff(3);
//LedOff(4);
printf("Equipe MIBR\n");
sleep(1.0);
/*
cat=escolhecatapulta();
if(cat>0)
PolarizedDiffSensor(VERTICAL);
//Gira(1.0, RIGHT);
else {
escolhePosicao();
escolheMapa();
printf("Comecar em [%d %d], alvo: [%d %d]\n",
pose_inicial[0], pose_inicial[1],
pose_desejada[0], pose_desejada[1]);
calculaWaveFront();
//printMap();
printf("Press start or turn lights on!\n");
sleep(1.0);
printf("READY!\n");
sleep(1.0);
//Enqunto Luz nao acesa, espera...
while(!start_button() && start==0) {
start=StartLight();
}
//start process count 60 seconds
pid_time=start_process(LifeTime());
*/
while(1) {
Go(MM900);
while(!start_button());
}
printf("Desligue!\n");
/*
while (!stop) {
direcao = calculaDirecao();
if (direcao == ESQUERDA)
Gira(4.0, LEFT);
if (direcao == DIREITA)
Gira(4.0, RIGHT);
Go(300.0);
printf("Estou em [%d %d]\n", pose_atual[0], pose_atual[1]);
msleep(1000L);
//Verifica se ja chegou no alvo
if (pose_atual[0] == pose_desejada[0] &&
pose_atual[1] == pose_desejada[1])
stop = 1;
}
kill_process(pid_time);
}*/
}
//Contador de vida do robo (60 seg no TP3)
void LifeTime()
{
sleep(60.0);
alloff();
stop=1;
printf("TIME OUT!\n");
beep();
msleep(10L);
beep();
msleep(10L);
beep();
}
* wf_movements.ic
/*
Introducao a Robotica - TPs 1, 2 e 3
Grupo MIBR (Made in Brasil)
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
Modulo responsavel pelos movimentos do robo!
*/
#use "color.ic"
//"color.ic" includes glogal stop variable
//Parmetros modificveis
#define RAIO_RODA 40.0 //Raio efetivo da roda+pneu em milmetros [mm]
//#define PULSOS_VOLTA 12.0*24.0 //De acordo com o disco do shaft encoder->6*2=12
#define PULSOS_VOLTA 12.0*20.0 //De acordo com o disco do shaft encoder->6*2=12 (reduo de 20)
//Parametros no-modificveis
#define PI 3.1416 //Constante PI
#define REDUCAO 20.0 //Reducao do motor ate a roda devido as engrenagens
#define VOLTA (2.0*PI*(float)RAIO_RODA) //Perimetro da roda [mm]
#define DX VOLTA/(PULSOS_VOLTA) //Menor delta de deslocamento [mm]
#define BITOLA 235.0 //Distancia entre rodas [mm] (medir)
#define MM300 320.0 //Ajuste de 300mm
#define MM900 3.0*MM300 //Ajuste de 900mm
#define MOTOR_RPS (int)(140/(REDUCAO*60)) //Rotacoes por segundo do motor utilizado [rps]
#define MOTOR_1 2 // Amarelo lado visor LCD/Laranja (Direito)
#define MOTOR_2 3 // Roxo lado visor LCD/Cinza (Esquerdo)
#define ENCODER_1 0
#define ENCODER_2 1
#define RPSID1 18 //Velocidade do motor1 em RPS (default: 18)
#define RPSID2 18 //Velocidade do motor2 em RPS
#define MOVE 0 //Vai para frente ou faz curva
#define RIGHT 1 //Go to right
#define LEFT 2 //Go to left
#define KP 2.0 //Parmetro de controle Kp
#define KD 0.01 //Parametro de controle Kd
#define ROUNDTIME 5000 //Tempo de 360 graus em milisegundos
//#use "wavefront.c"
//#use "hbmenu.ic"
/* Le um valor do knob variando de 0 at intervalo*/
int getKnob(char texto[], int intervalo, int set_camp) {
int valor;
while (!start_button()) {
if(set_camp) valor = ((intervalo * knob()) / 255) + 1;
else valor = ((intervalo * knob()) / 255);
printf("%s %d\n", texto, valor);
msleep(100L);
}
while (start_button());
return valor;
}
//Desliga os motores
void StopMotors()
{
motor(MOTOR_1,0);
motor(MOTOR_2,0);
alloff();
}
//Funo de ramp up e ramp down para motores
//Obs.: Aumenta/Diminui gradativamente a velocidade do motor em 1s
void StartMotors(int speed1, int speed2)
{
int i;
int power1, power2;
power1 = (speed1/10);
power2 = (speed2/10);
for (i=0;i<=10;i++)
{
motor(MOTOR_1,i*power1); //Gira motor 1 com i*int(speed2/10) de velocidade
motor(MOTOR_2,i*power2); //Gira motor 2 com i*int(speed2/10) de velocidade
msleep(200L); //Aguarda movimento mecnico
}
}
//Desloca para frente x unidades de distncia [mm]
void Go(float x)
{
int enc1=0, enc2=0, speed1, speed2;
float dist=0.0;
int erroant1=0, erroant2=0,encsum=0;
// Habilita encoder do canal 0
reset_encoder(0);
reset_encoder(1);
if(stop==0)
{
if(x>=0.0)
{ printf("Indo pra frente!\n"); //Imprime Status
//StartMotors(100,80); //Liga os motores
motor(MOTOR_1,100);
motor(MOTOR_2,80);
}
else{
printf("Indo pra Tras!\n"); //Imprime Status
StartMotors(-100,-80); //Liga os motores
}
while ((dist=curve1[i])
speed1=i*10+10;
if(enc2>=curve2[i])
speed2=i*10+10;
}
//Saturacao dos valores (manter por seguranca)
if (speed1<-100)
speed1=-100;
if(speed1>100)
speed1=100;
if(speed2<-100)
speed2=-100;
if(speed2>100)
speed2=100;
switch(tipo)
{
case 0: //Frente ou curva
motor(MOTOR_1,speed1);
motor(MOTOR_2,speed2);
break;
case 1: //Direita
motor(MOTOR_1,speed1);
motor(MOTOR_2,-speed2);
break;
case 2: //Esquerda
motor(MOTOR_1,-speed1);
motor(MOTOR_2,speed2);
break;
default: {
printf("Switch error!\n");
break;
}
}
}
/*
//Desloca para frente x unidades de distncia [mm]
void Go(float x)
{
int enc1=0, enc2=0, speed1, speed2;
float dist=0.0;
int erroant1=0, erroant2=0,encsum=0;
int pid=0, pid1=1;
int encid1, encid2;
// Habilita encoder do canal 0
reset_encoder(0);
reset_encoder(1);
if(x>=0.0)
{ //printf("Indo pra frente!\n"); //Imprime Status
//StartMotors(100,80); //Liga os motores
motor(MOTOR_1,80);
motor(MOTOR_2,100);
encid1 = 20;
encid2 = 20;
pid = start_process(PDControl(&encid1, &encid2, MOVE));
}
else{
//printf("Indo pra Tras!\n"); //Imprime Status
StartMotors(-100,-80); //Liga os motores
x = -x;
}
// while (!stop_button()) //Enquanto dist=curve1[i])
speed1=i*10+20;
if(enc2>=curve2[i])
speed2=i*10+20;
}
//Saturacao dos valores (manter por seguranca)
if (speed1<-100)
speed1=-100;
if(speed1>100)
speed1=100;
if(speed2<-100)
speed2=-100;
if(speed2>100)
speed2=100;
switch(tipo)
{
case 0: //Frente ou curva
motor(MOTOR_1,speed1);
motor(MOTOR_2,speed2);
break;
case 1: //Direita
motor(MOTOR_1,-speed1);
motor(MOTOR_2,speed2);
break;
case 2: //Esquerda
motor(MOTOR_1,speed1);
motor(MOTOR_2,-speed2);
break;
default: {
printf("Switch error!\n");
break;
}
}
reset_encoder(0);
reset_encoder(1);
//msleep(33L);
msleep(30L);
}
StopMotors();
}
void ControlTime(long time, int *round)
{
*round = 0;
msleep(time);
StopMotors();
*round = 1;
}
//Gira para a direcao determinada 360/arc graus
void Gira(float arc, int direction)
{
//variveis controle de angulo e pd dos motores
int encid1, encid2;
int pid = 4, pid1=5;
float dist1=0.0, dist2=0.0;
int erroant1=0, erroant2=0,encsum1=0,encsum2=0;
int round = 0;
if(stop==0)
{
printf("Gira %f para ", 360.0/arc); //Imprime Status
//inicializao data para deslocamento
reset_encoder(0);
reset_encoder(1);
encid1 = 18;
encid2 = 18;
if (direction==RIGHT)
{
/* pose_atual[2] -= (int)(360.0/arc);
if (pose_atual[2] == -(int)(360.0/arc))
pose_atual[2]= 360-(int)(360.0/arc);
*/
printf("Direita!\n"); //Imprime Status
pid = start_process(PDControl2(&encid1, &encid2, RIGHT));
}
else if(direction==LEFT)
{
/* pose_atual[2] += (int)(360.0/arc);
if (pose_atual[2] == 360)
pose_atual[2]= 0;
*/
printf("Esquerda!\n"); //Imprime Status
pid = start_process(PDControl2(&encid1, &encid2, LEFT));
}
pid1 = start_process(ControlTime((long)(ROUNDTIME/(int)(arc)), &round));
msleep(30L);
//controle da distancia percorrida
while(!stop_button() && !round) //volta determinada pela proporso pedida
{
//Caso for pedido 360, ele far a avaliao da luz ambiente a cada 45 graus
//contabiliza deslocamento
encsum1 = encsum1 + read_encoder(0);
encsum2 = encsum2 + read_encoder(1);
//reseta encoder para contabilizao apartir daquele tempo
reset_encoder(0);
reset_encoder(1);
dist1 = ((float)(encsum1))*DX; //Calcula a distancia percorrida
dist2 = ((float)(encsum2))*DX;
printf("Dist1.: %f mm Dist2.: %f mm\n",dist1,dist2); //Imprime distancia percorrida
msleep(30L);
//msleep(100L);
}
kill_process(pid);
kill_process(pid1);
StopMotors(); //Desliga os motores
}
}
----
----