===== TRABALHO PRÁTICO 2 ===== == == {{:cursos:introrobotica:2011-2:grupo05:welcome.gif|}} * No intuito de participar de uma competição de robôs ao final do segundo semestre de 2011, nós, alunos de Engenharia de Controle & Automação e Ciência da Computação, construimos nosso primeiro robô! * A partir dele foram feitas algumas adaptações e novas versões, para que no final do segundo trabalho da disciplina, conseguíssemos atingir os objetivos desejados. * O trabalho prático 2, necessitou de medidas auxiliares para que o robô pudesse realizar suas atividades. Introduziu-se o uso de sensores ópticos para captar sinais do ambiente em que devia-se trabalhar. Além disso, foram necessários para a conclusão da tarefa a utilização dos recursos de multithread da HandyBoard de forma a otimizar a resposta do sistema de controle pois o há uma significativa melhora no uso de processos concorrentes. * O robô então, deveria utilizar os sinais obtidos do: sensor óptico ativo, sensor diferencial, e sensores infra-vermelhos, sendo os dois primeiros baseados no uso de sensores LDR (ou, Light-Depend-Resistor – Resistor Dependente de Luz) e o último em emissor e receptor de luz infra-vermelha. * De um modo geral, as tarefas realizadas foram seguir-linha (1), localizar-se (2) e identificar cores (3) com ação de resposta as mesmas de modo interativo no menu da HandyBoard. Todas estas foram bem desenvolvidas, apesar das limitações como veremos adiante. ---- == == {{:cursos:introrobotica:2011-2:grupo05:design.gif|}} * A mesma plataforma de mecanismos que foi utilizada no desenvolvimento do primeiro trabalho prático também foi utilizada no desenvolvimento do segundo. Porém, houveram algumas modificações nos demais aspectos. * Como não haveria mais a necessidade de se utilizar a caneta, a mesma foi retirada. No lugar dela, abriu-se espaço para a melhor acomodação da HandyBoard pois sua estrutura agora se apoiaria sobre o eixo de tração das rodas, dando maior estabilidade e melhor resposta nas movimentações do robô. * Foram criados novos espaços, essencialmente frontais, de modo que conseguisse alocar todos os sensores que nele deveriam estar presentes. O melhor posicionamento dos sensores foi pensado para que interagissem da forma mais eficiente possível junto ao código desenvolvido. {{:cursos:introrobotica:2011-2:grupo05:nova_base_mibr.jpg|}} == == {{:cursos:introrobotica:2011-2:grupo05:estrategia.gif|}} * Na primeira tarefa, o robô deveria seguir uma linha preta, pré-determinada em uma espécie de mesa branca no laboratório de robótica, tal que de forma autônoma houvesse o controle de direção e sentido sobre a tarja preta. Para realizar o mesmo foram utilizados dois sensores infra-vermelho reflexivos, os quais se encontraram como a melhor solução para o problema. Tais sensores foram construídos de modo a se conectar à HandyBoard devolvendo valores binários de captação de luz-refletida. * Para a segunda foi utilizado um sensor pré-determinado, o diferencial. Este foi construído com base em dois outros, os LDR, de modo que houvesse uma captação da luz por um par de filtro polarizador. Estes são orientados de maneira ortogonal entre si. * A terceira e última tarefa foi desenvolvida com um sensor óptico ativo do tipo LED/LDR. Este, composto por um LDR e 4 LEDs complementando o padrão RGB, funciona como uma espécie de detector de cores onde, os leds, emitem luz nos objetos e a luz refletida é medida por um LDR. Como os blocos de cores diferentes absorvem e emitem cores diferentes, pode-se então capturar o valor diferenciado para a resistência envolvida no LDR, e assim através de uma padronização identificar qual a cor que desejamos tratar. ===Sensor Óptico-Ativo=== * O sensor óptico-ativo, como antes citado, foi usado para a identificação de cores. Basicamente, o elemento foi montado em uma placa de circuito impresso, onde haviam 4 leds ao todo, compartilhando o mesmo terra disponível da seção SI Expansion Header. Dois leds verdes ligados em mesmo ponto positivo, no caso a saída digital D2 da seção SI, outro led, este azul, ligado a saída digital D3 e outro vermelho, ligado a saída digital D4. Houve uma padronização neste aspecto para facilitar o uso dos mesmos, mas o que não impossibilita o usuário rever a ordem de uso dos mesmo quando bem entender (ou seja, é necessário haver uma certa cautela para que o programa esteja compatível com a montagem, até que se faça uma solda em uma placa de circuito impresso para a respectiva parte do sensor – pinos de conexão na seção SI Expansion Header). A figura a seguir mostra como ficou o circuito onde os leds foram acoplados: {{:cursos:introrobotica:2011-2:grupo05:sensor_optico_ativo.jpg|}} * O dispositivo foi montado desta maneira, porém, é fácil perceber que esta possivelmente não era a melhor opção, já que não teremos o mesmo brilho para todos os leds ativando-os um de cada vez. Para que isso ocorresse, teríamos de alocar um resistor para cada ramo de entrada, sendo o resistor do ramo 1 o dobro dos demais, de forma que a corrente transmitida a todos fossem iguais. A implementação dos leds na placa de circuito impresso, para dar robustez a montagem do sensor, pode ser vista na imagem a seguir: {{:cursos:introrobotica:2011-2:grupo05:montagem_soa.jpg|}} * Apesar disso, o sensor respondeu de forma aceitável. Esta configuração foi exigida de forma que o LDR, alocado no centro dos leds, captasse separadamente o sinal de luz refletida por uma das cores ativas. O sinal se propaga até encontrar um objeto-alvo e assim, é refletido para o LDR, variando o valor do mesmo (como já comentamos). A figura a seguir mostra como ficou o sensor óptico ativo: {{:cursos:introrobotica:2011-2:grupo05:final_soa.jpg|}} * Observe que há um envoltório de estrutura lego sobre o sensor, de modo que o sensor LDR receba o mínimo possível de luzes ambiente já que estas poderiam variar o valor do sensor e dificultar a padronização dos sinais recebidos pelo mesmo. Na caracterização do sensor óptico ativo obtivemos os resultados obtidos abaixo, tanto para sem luz (cobrindo a montagem robô+objeto com uma caixa de papelão) quanto para a luz ambiente, retirando a respectiva caixa, calculando para ambos os casos (com motores ligados ou não) o seus respectivos valores médios (“VM”) e os seus desvios padrões (“DP”). {{ :cursos:introrobotica:2011-2:grupo05:b15mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:b7mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:b_30mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:blue15mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:blue7mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:blue_30mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:g15mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:g7mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:g_30mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:r7mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:r15mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:y7mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:y_15mm.png?500 }} {{ :cursos:introrobotica:2011-2:grupo05:y_30mm.png?500 }} === Sensor Óptico-Reflexivo Infra-Vermelho === * Foram feitos testes dos motores a fim de gerar uma tabela relacionando potência a velocidade (em rps). O teste foPara a primeira tarefa, como já citado, foi utilizado o sensor infra-vermelho. O sensor é baseado em um conjunto. Duas chaves óticas PHCR359 são então utilizadas no fundo do robô de forma que ambas consigam captar a reflexão ou não de luz próximo ao exterior da linha preta, de modo que quando um dos lados entrem para dentro tendendo a uma curva, um sinal antes emitido pare de ser enviado, possibilitando a identificação de estados. Cada chave ótica é composta por 2 transmissores infra-vermelhos e 2 receptores infra-vermelhos de mesma freqüência, e ambos estão alojados a um bloco opaco de forma que não consigam enviar informações um para o outro pelas laterais. * As figuras abaixo ilustram a chave ótica utilizada, assim como a montagem realizada pela equipe. {{:cursos:introrobotica:2011-2:grupo05:infra_red.jpg|}} {{:cursos:introrobotica:2011-2:grupo05:infra_red1.jpg|}} * Então, cada conjunto durante o deslocamento monitora um lado extremo (fora) da faixa preta. Caso o robô entre na faixa em algum lado, esta variação é detectada pelo programa desenvolvido, e assim uma ação, atuada no motor, pode ser dada de forma a corrigir esse movimento indevido, fazendo-o retornar a linha desejada. {{:cursos:introrobotica:2011-2:grupo05:infra_red2.jpg|}} === Sensor Diferencial === * O ultimo sensor é o diferencial, que consiste em dois LDR's soldados de maneira a juntar duas de suas pernas quaisquer do sensor. Com isso temos que a perna livre de um é ligada no ground e a do outro no positivo, já as interligadas são conectadas na entrada signal da HB, o que caracteriza a ligação em paralelo dos dois (figura ). {{:cursos:introrobotica:2011-2:grupo05:sdife.png|}} * Tal característica faz com que a resistência de um LDR varia de [0,x], e a do outro de [x,y], com x = y/2 e y sendo o maior valor captado pela Handy Board. Tendo isto em vista, ao cobrirmos cada um com um filtro polarizador, vertical e horizontal respectivamente, sabe-se que se o valor lido pelo sensor é menor ou igual a x, temos uma luz vertical na proximidade, caso contrário a horizontal. Para ajudar nas medições, utilizamos o LDR do sensor Óptico-Ativo para medir a luz ambiente, a qual é subtraída do valor calculado, restando somente as componentes vertical e horizontal. === === {{:cursos:introrobotica:2011-2:grupo05:codigo.gif|}} /*************************** madeinbrasil.c ********************************/ /* Test program for Handy Board version 1.7 1/05 testmotors() updated to reduce strain on HB motor drivers version 1.6 1/04 CMUcam test updated to use new library and serial handlers version 1.5 5/03 Expanded by Dan Gates to add the CMUcam comm test. Reqires serial.icb to be in the same directory. version 1.4 1/02 Changed by Randy Sargent for IC 4.0 and to include srf04 sonar test. version 1.3 1/01 Changed by Anne Wright for Botball to allow selecting which test using the knob. Program previously called hb-expbd-test.c. Also includes hbmenu.c. version 1.2 11/00 tests IR emitter -dpm version 1.1 7/98 Expanded by Dave Miller 7/13/98 dmiller@kipr.org to include servo and to handle menuing. version 1.0 -- 26 nov 95 Fred G. Martin (fredm@media.mit.edu) */ /* Parametros medidos: - Diametro da Roda: 43 [mm] - Distancia mais curta entre o Eixo-Motor ate sua respectiva roda: 35 [mm] - Distancia entre as duas rodas: 105 [mm] Extras: 1) Voltas necessarias para alcancar 30cm: 300/(2*PI*RAIO_RODA) [mm] 2) Num. Voltas x 24 dentes x 6 = Num. Passos necessarios */ //Parmetros modificveis #define RAIO_RODA 21.5 //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 //Parametros no-modificveis #define PI 3.1416 //Constante PI #define REDUCAO 24.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 118.0 //Distancia entre rodas [mm] #define MM300 300.0 #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 1 //Parmetro de controle Kp #define KD 0.01 //Parametro de controle Kd // Programa exemplo #define TESTNUM 8 #define DIFF 3 #define VERTICAL 1 #define HORIZONTAL 2 #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 #use "cmucamlib.ic" int data[100]; void main() { int sel; char a[TESTNUM][16]={"Menu: Frente","Menu: Horizont","Menu: Vertical", "Menu: LDR_test", "Menu: 360", "Menu: Line", "Menu: Light LED", "Menu: IR Value"}; printf("Grupo MIB: Aperte START! %f ** %f",DX,PULSOS_VOLTA); while(!start_button()); printf("Gire o KNOB para selecionar \n"); sleep(1.0); // 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); while(1) { printf("Gire o KNOB para selecionar \n"); sleep(0.5); sel = select_string(a,TESTNUM); if(sel==0) Go(300.0); else if(sel==1)PolarizedDiffSensor(2); else if(sel==2) PolarizedDiffSensor(2); else if(sel==3) SalvaDados(); else if(sel==4) LDR_test(); else if(sel==5) FollowLine(); else if(sel==6) light_led_test(); else if(sel==7) IR_test(); else if(sel==8) break; } printf("Pronto!\n"); } //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 } } void Left() { int encid1, encid2; int pid = 0; float dist1=0.0, dist2=0.0; int erroant1=0, erroant2=0,encsum1=0,encsum2=0; printf("Esquerda!\n"); //Imprime Status reset_encoder(0); reset_encoder(1); encid1 = 20; encid2 = 20; pid = start_process(PDControl(&encid1, &encid2, RIGHT)); while((dist1 < (((float)BITOLA*0.94*(float)(PI))/4.0)) || (dist2 <(((float)BITOLA*0.94*(float)(PI))/4.0))) //Meia volta = 90 graus { encsum1 = encsum1 + read_encoder(0); encsum2 = encsum2 + read_encoder(1); //soma acumulativa dos encoders 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); //Aguarda movimento mecanico } kill_process(pid); //Quando a distncia x for alcancada StopMotors(); //Desliga os motores sleep(0.5); //Aguarda movimento mecnico dist1 = ((float)(encsum1))*DX; //Calcula a distancia percorrida dist2 = ((float)(encsum2))*DX; printf("D1.: %f mm D2.: %f mm\n",dist1,dist2); //Imprime distancia percorrida //sleep(0.5); reset_encoder(0); reset_encoder(1); //printf("Parado.\n"); //Imprime Status //while(!stop_button()); } //Gira para a direita void Right() { float dist=0.0; int erroant1=0, erroant2=0,encsum=0; /*teste*/ int enc1sum=0,enc2sum=0; int encid1, encid2; int pid = 0; encid1 = 20; encid2 = 20; // Habilita encoder do canal 0 reset_encoder(0); reset_encoder(1); printf("Direita!\n"); //Imprime Status pid = start_process(PDControl(&encid1, &encid2, LEFT)); while((dist <= (((float)BITOLA*(float)(PI))/4.0) )) //Meia volta = 90 graus { // PDControl2(18,18,&erroant1,&erroant2,LEFT); //encsum = encsum + read_encoder(0) + read_encoder(1); //soma acumulativa dos encoders enc1sum = enc1sum + read_encoder(0); enc2sum = enc1sum + read_encoder(1); reset_encoder(0); reset_encoder(1); dist = (float)((enc1sum + enc2sum)/2)*DX; //Calcula a distancia percorrida printf("Dist.: %f mm\n",dist); //Imprime distancia percorrida msleep(30L); //Aguarda movimento mecanico } //Gira at completar meia volta en cada roda //Quando a distncia x for alcancada kill_process(pid); StopMotors(); //Desliga os motores sleep(1.0); //Aguarda movimento mecnico //encsum = encsum + read_encoder(0) + read_encoder(1); //soma acumulativa dos encoders enc1sum = enc1sum + read_encoder(0); enc2sum = enc1sum + read_encoder(1); dist = (float)((enc1sum+enc2sum)/2)*DX; //Calcula a distancia percorrida printf("Dist.: %f mm\n",dist); //Imprime distancia percorrida reset_encoder(0); reset_encoder(1); } //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,100); motor(MOTOR_2,80); encid1 = 20; encid2 = 20; pid = start_process(PDControl(&encid1, &encid2, MOVE)); pid1 = start_process(le_motor()); } else{ printf("Indo pra Tras!\n"); //Imprime Status StartMotors(-100,-80); //Liga os motores x = -x; } while ((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); } StopMotors(); } //Descobre qual a cor do bloco a frente do sensor void 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(1.0); ldramb=light(2); printf("LDR: %d\n", ldramb); sleep(3.0); //Ascende cada led e aquisita o valor do LDR (falta ajustar qual canal ascende cada LED) LedOn(LED_RED);//bit_set(0x1008, 0b00010000); sleep(1.0); ldrred=light(2); printf("Led vermelho! %d\n",ldrred-ldramb); sleep(1.0); LedOff(LED_RED);//bit_clear(0x1008, 0b00010000); LedOn(LED_GREEN);//bit_set(0x1008, 0b00000100); sleep(1.0); ldrgreen=light(2); printf("Led verde! %d\n",ldrgreen-ldramb); sleep(1.0); LedOff(LED_GREEN);//bit_clear(0x1008, 0b00000100); LedOn(LED_BLUE);//bit_set(0x1008, 0b00001000); sleep(1.0); ldrblue=light(2); printf("Led azul! %d\n",ldrblue-ldramb); sleep(1.0); LedOff(LED_BLUE);//bit_clear(0x1008, 0b00001000); LedOn(LED_BLUE); LedOn(LED_RED); LedOn(LED_GREEN); sleep(1.0); ldrall=light(2); printf("Led todos! %d\n",ldrall-ldramb); sleep(1.0); LedOff(LED_BLUE); LedOff(LED_RED); LedOff(LED_GREEN); //Calcula cor do bloco //maior if(ldrmaior120) cor=cor+3; //amarelo 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 1: //vermelho printf("Bloco vermelho!\n"); ColorAction(); break; case 2: //verde printf("Bloco verde!\n"); ColorAction(); break; case 3: //vermelho+verde = amarelo printf("Bloco amarelo!\n"); ColorAction(); break; case 4: //azul printf("Bloco azul!\n"); ColorAction(); break; case 5: //azul + vermelho = roxo printf("Bloco roxo!\n"); ColorAction(); break; case 6: //verde+azul = anil/ciano (azul claro) printf("Bloco azul claro?\n"); ColorAction(); break; case 7: //azul+vermelho+verde = branco printf("Bloco Amarelo!\n"); ColorAction(); break; case 0: //preto printf("Bloco preto!\n"); ColorAction(); break; default: //Cor desconhecida break; } } //Selecao da acao void ColorAction() { int sel,sair=0; char a[5][16]={"Parar","Contornar Dir.","Contornar Esq.", "Empurrar","Sair"}; //Espera o LCD mostrar a cor identificada sleep(5.0); printf("Gire o KNOB para selecionar\n"); sleep(1.0); while(sair==0) { printf("Gire o KNOB para selecionar\n"); sleep(0.5); sel = select_string(a,5); if(sel==0) alloff(); //Para todos os motores else if(sel==1) ColorTurnRight(); //Contorna pela direita else if(sel==2) ColorTurnLeft(); //Contorna pela esquerda else if(sel==3) ColorPushBlock(); //Empurra por x segundos else if(sel==4) sair=1; else if(sel==5) break; } } void ColorTurnLeft() { int i; //Volta alguns centimetros //Go(-50.0); motor(MOTOR_1,-100); motor(MOTOR_2,-100); sleep(2.5); //Contorna Left(); Go(200.0); Right(); Go(500.0); Right(); Go(200.0); Left(); } void ColorTurnRight() { int i; //Volta alguns centimetros //Go(-50.0); motor(MOTOR_1,-100); motor(MOTOR_2,-100); sleep(2.5); //Contorna Right(); Go(200.0); Left(); Go(500.0); Left(); Go(200.0); Right(); } void ColorPushBlock() { int seg; while(!start_button()) { seg=knob_int_value(0,10); printf("Empurrar por: %d seg\n",seg); msleep(500L); } motor(MOTOR_1,100); motor(MOTOR_2,80); sleep((float)seg); StopMotors(); } //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; } } //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 } void SalvaDados ()/* variavel apontador, int data[100]; na func principal*/ { //- Memoria Limitada da HB: coleta pouco mais de 2500 inteiros no vetor //- Lembrar de inicializar o vetor //- Para retirar os dados: Tools -> Upload Array -> Seleciona nome do vetor // e onde quer salvar , save to CSV (excell) int i; beep(); sleep(3.0); for (i = 0; i< 100; i++) { data[i] = light(2); msleep(100L); } beep(); } //Testa valores de LDR //Mais iluminado, maior o valor. void LDR_test(/**/) { while(!stop_button()) { printf("LDR: %d\n", light(2)); msleep(100L); } } //Gira para a esquerda 360 graus void Gira360(float results[8]) { int i = 0; int oitavo = 0; float dist=0.0; printf("Gira 360!\n"); //Imprime Status reset_encoder(0); results[oitavo] = 0.0; for(i=0; i<20; i++) { results[oitavo] = results[oitavo] + (float)(light(2)-light(DIFF)); } results[oitavo] = results[oitavo]/20.0; oitavo++; StartMotors(50,-50); //Liga os motores e gira para a direita while(dist <=(BITOLA*PI*0.94)) //uma volta = 360 graus { if(dist >=(float)oitavo*(BITOLA*PI*0.94)/8.0) { printf("captando %d\n", light(2)-light(DIFF)); StopMotors(); msleep(30L); results[oitavo] = 0.0; for(i=0; i<20; i++) { results[oitavo] = results[oitavo] + (float)(light(2)-light(DIFF)); } results[oitavo] = results[oitavo]/20.0; StartMotors(50,-50); //Liga os motores e gira para a direita oitavo++; } if(stop_button()) { break;} dist = (float)read_encoder(0); dist = (dist*DX); //Calcula a distancia percorrida printf("Dist.: %f mm\n",dist); msleep(100L); } StopMotors(); //Desliga os motores if(oitavo<8) { results[oitavo] = 0.0; for(i=0; i<20; i++) { results[oitavo] = results[oitavo] + (float)(light(2)-light(DIFF)); } results[oitavo] = results[oitavo]/20.0; } } //Seta direcao onde o melhor valor da luz foi encontrado void SetDirection(int iteracao) { int i = 0; int oitavo = 0; float dist=0.0; printf("%d\n", iteracao); //Imprime Status reset_encoder(0); //avalia em qual sentido corrigira(horario, anti-horario) if(iteracao <5) StartMotors(50,-50); //Liga os motores e gira para a esquerda else { StartMotors(-50,50); iteracao = 8 - iteracao; } while(dist <=(float)(iteracao)*(BITOLA*PI*0.94)/8.0) //uma volta = 360 graus { if(stop_button()) { break;} dist = (float)read_encoder(0); dist = (dist*DX); //Calcula a distancia percorrida printf("Dist.: %f mm\n",dist); msleep(100L); } StopMotors(); //Desliga os motores } //corrige direcao, caso a setagem nao seja boa void CorrectDirection(float results[8], int *correct, int *direction, int polo) { float dist = 0.0; int i; switch(polo){ case VERTICAL: if(results[0]<(float)(2*(light(2)-light(DIFF)))) {*correct = 1; //define direcao a girar if(*direction==1) StartMotors(50,-50); //Liga os motores e gira para a esquerda else { StartMotors(-50,50); *direction = 2; } while(dist <=(BITOLA*PI*0.94)/8.0) //uma volta = 360 graus { if(stop_button()) { break;} dist = (float)read_encoder(0); dist = (dist*DX); //Calcula a distancia percorrida // printf("Dist.: %f mm\n",dist); msleep(100L); } StopMotors(); //Desliga os motores results[1] = 0.0; for(i=0; i<5; i++) { results[1] = results[1] + (float)(light(2)-light(DIFF)); } results[1] = results[1]/5.0; results[0] = results[1]; } break; case HORIZONTAL: if(results[0]>(float)(2*(light(2)-light(DIFF)))) {*correct = 1; if(*direction==1) StartMotors(50,-50); //Liga os motores e gira para a esquerda else { StartMotors(-50,50); *direction = 2; } while(dist <=(BITOLA*PI*0.94)/8.0) //uma volta = 360 graus { if(stop_button()) { break;} dist = (float)read_encoder(0); dist = (dist*DX); //Calcula a distancia percorrida // printf("Dist.: %f mm\n",dist); msleep(100L); } StopMotors(); //Desliga os motores results[1] = 0.0; for(i=0; i<5; i++) { results[1] = results[1] + (float)(light(2)-light(DIFF)); } results[1] = results[1]/5.0; results[0] = results[1]; } break; } } //Funcao controladora do sensor diferencial polarizado void PolarizedDiffSensor(int polo) { int iteracao=0; int encid1, encid2; int pid = 0, pid1 = 1; int direction = 0, correct = 0; int i = 0; float results[8], dist; //Gira e capta medias de valores Gira360(results); //define qual luz vai ser localizada //parte do codigo esta comentado devido a um overflow switch(polo) { case VERTICAL: /* for(i=1; i<8; i++) { if(results[0] > results[i]){ iteracao = i; results[0] = results[i]; } } if(iteracao>0) { SetDirection(iteracao); // pid1 = start_process(LDR_test()); // if(results[0]>(float)(light(2)-light(DIFF))) if(iteracao >=5) { direction = 1; } // CorrectDirection(results, &correct, &direction, VERTICAL); } if(correct) { if(direction==2) { encid1 = 14; encid2 = 20; direction = 1; } else { encid1 = 20; encid2 = 14; direction = 2; } } else { encid1 = 20; encid2 = 20; printf("Reto!\n"); sleep(2.0); results[1] = 0.0; for(i=0; i<20; i++) { results[1] = results[1] + (float)(light(2)-light(DIFF)); } results[1] = results[1]/20.0; results[0] = results[1]; direction = 2; } pid = start_process(PDControl(&encid1, &encid2, MOVE)); pid1 = start_process(LDR_test()); i=0; while(!stop_button()/* && (light(2) > 225)*) { if(i==3) { results[1] = 0.0; StopMotors(); kill_process(pid); sleep(2.0); for(i=0; i<20; i++) { results[1] = results[1] + (float)(light(2)-light(DIFF)); } results[1] = results[1]/20.0; if((results[0]*0.99)<=results[1]) { if(direction==2) { encid1 = 14; encid2 = 20; pid = start_process(PDControl(&encid1, &encid2, MOVE)); direction = 1; } else { encid1 = 20; encid2 = 14; pid = start_process(PDControl(&encid1, &encid2, MOVE)); direction = 2; } } else { pid = start_process(PDControl(&encid1, &encid2, MOVE)); encid1 = 20; encid2 = 20; if(direction==2) direction = 1; else direction = 2; } results[0] = results[1]; i=0; } sleep(1.0); i++; } kill_process(pid1); kill_process(pid); StopMotors(); */ break; case HORIZONTAL: //avalia melhor direcao for(i=1; i<8; i++) { if(results[0] < results[i]){ iteracao = i; results[0] = results[i]; } } if(iteracao>0) { SetDirection(iteracao); // pid1 = start_process(LDR_test()); // if(results[0]>(float)(light(2)-light(DIFF))) if(iteracao >=5) { direction = 1; } // CorrectDirection(results, &correct, &direction, HORIZONTAL); } //faz correcao se necessario if(correct) { if(direction==2) { encid1 = 14; encid2 = 20; direction = 1; } else { encid1 = 20; encid2 = 14; direction = 2; } } //inicia movimento em linha reta (correcao no realizada) else { encid1 = 20; encid2 = 20; printf("Reto!\n"); sleep(2.0); results[1] = 0.0; for(i=0; i<20; i++) { results[1] = results[1] + (float)(light(2)-light(DIFF)); } results[1] = results[1]/20.0; results[0] = results[1]; direction = 2; } pid = start_process(PDControl(&encid1, &encid2, MOVE)); pid1 = start_process(LDR_test()); i=0; while(!stop_button()/* && (light(2) > 225)*/) { //avalia se o sentido esta correto if(i==3) { results[1] = 0.0; StopMotors(); kill_process(pid); sleep(2.0); for(i=0; i<20; i++) { results[1] = results[1] + (float)(light(2)-light(DIFF)); } results[1] = results[1]/20.0; //avalia se o sinal captado, corresponde ao esperado, e corrige caso contrario. if((results[0]*0.99)>=results[1]) { if(direction==2) { encid1 = 14; encid2 = 20; pid = start_process(PDControl(&encid1, &encid2, MOVE)); direction = 1; } else { encid1 = 20; encid2 = 14; pid = start_process(PDControl(&encid1, &encid2, MOVE)); direction = 2; } } else { pid = start_process(PDControl(&encid1, &encid2, MOVE)); encid1 = 20; encid2 = 20; if(direction==2) direction = 1; else direction = 2; } results[0] = results[1]; i=0; } sleep(1.0); i++; } kill_process(pid1); kill_process(pid); StopMotors(); break; default: printf("Polo inexistente!\n"); } StopMotors(); //Desliga os motores } //segue a linha atraves dos chaves opticas-reflexivas void FollowLine() { int pid=0, pid1=1; int encid1, encid2,lamb; encid1 = 20; encid2 = 20; lamb=light(2); //faz controle Proporcional Direcional em paralelo pid = start_process(PDControl(&encid1, &encid2, MOVE)); pid1 = start_process(le_sensores()); while(!stop_button()) { //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 if(light(2)<(int)(0.58*(float)lamb)) break; } kill_process(pid); kill_process(pid1); StopMotors(); //detecta cor dos blocos DetectColor(); } //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)) printf("correct right\n"); *encid1 = 20; *encid2 = 20; } //corrige para esquerda void correct_left_side(int *encid1, int *encid2) { *encid1 = 20; *encid2 = 16; while(!digital(11)) printf("correct left\n"); *encid1 = 20; *encid2 = 20; } /****************************** 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 /*****************************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: */ ---- ----