===== TRABALHO PRÁTICO 1 ===== == == {{: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ências da Computação, construimos nosso primeiro robô! * Para a primeira tarefa, teríamos de construir um robô que iria competir apenas com si próprio. Utilizando uma quantidade limitada de sensores, materiais como um kit LEGO DACTA, uma HandyBoard (sistema microcontrolador 6811), solda, cola quente, e outras coisas mais, deveríamos montá-lo e programá-lo para a tarefa exigida. Sua missão seria então percorrer 3 vezes consecutivas um quadrado de aresta de 30cm e 3 vezes consecutivas um círculo de raio igual a 30cm, delimitando a sua trajetória com uma caneta em uma superfície plana revestida de papel kraft. * O resultado foi que conseguimos realizar a tarefa! Construimos um robô com precisão bastante aceitável, tendo em vista algumas limitações obtidas, que através do controle PD, podia "contar seus passos" e definir sua trajetória, direção e sentido, para concretizar o objetivo final! ---- == == {{:cursos:introrobotica:2011-2:grupo05:design.gif|}} * O MIBr (Made in Brasil) foi baseado na HandyBug similar ao 9645, porém foram feitas algumas modificações no intento de adaptar a montagem às peças existentes no kit e demais fatores que explicitamos adiante. * Adotamos a idéia de reforçar a estrutura colocando peças na diagonal presas por pinos LEGO e entrecruzamos as peças de forma que mais peças presas entre si. {{ :cursos:introrobotica:2011-2:grupo05:00070.jpg?400 }} * Os motores utilizados não são LEGOS por natureza (foram adaptados para serem utilizados como tal). Dessa forma, existem algumas braçadeiras que impossibilitam o encaixe perfeito entre as peças do kit. * Como sensor, foi utilizada uma roda vazada de 6 furos em cada eixo do motor e um par emissor-receptor óptico. Assim foi montado um encoder a fim de medir a velocidade angular do eixo do motor e dessa forma, de cada roda. {{ :cursos:introrobotica:2011-2:grupo05:00069.jpg?400 }} * A princípio utilizamos duas rodas traseiras médias e duas rodas dianteiras pequenas e lisas para que não influenciassem muito no movimento. Porém, percebemos que o atrito gerao pelas rodas dianteiras era bem considerável e decidimos optar pelo uso de uma rodinha “roll-on”. Abaixo seguem as montagens com a primeira e a segunda estrutura das rodas dianteiras. {{ :cursos:introrobotica:2011-2:grupo05:00071.jpg?400 }} * Para verificarmos a trajetória do robô, fixamos um pincel alinhado com o eixo do MIBR. Deixamos de forma que não encostasse muito na superfície (apenas tocasse) para não interferir no movimento correto. == == {{:cursos:introrobotica:2011-2:grupo05:estrategia.gif|}} * Utilizamos 2 motores que acionam cada uma das rodas traseiras e um encoder medindo diretamente a velocidade de cada motor. Dessa forma, é possível controlar cada motor separadamente (os motores têm parâmetros diferentes e portanto respostas diferentes para uma mesma entrada. Precisam ser trabalhados de maneira isolada, pois uma mesma potência gera velocidades diferentes.). No item relacionado a análises, segue o estudo feito para relacionar algumas velocidades às potências exigidas por cada um dos motores. {{ :cursos:introrobotica:2011-2:grupo05:00063.jpg }} * Para garantir melhor mobilidade para o robô, utilizamos uma roda do tipo “roll-on”, cujo atrito é bem menos que as rodinhas tradicionais. * Foi feita a redução do motor para as rodas por meio de uma engrenagem de 24 dentes e utilizamos rosca sem fim com eixo bi-apoiado. Apesar de ela ser ineficiente por desgastar as peças mais facilmente, no nosso projeto foi necessária. * Optamos pela técnica de controle PD, em que um ganho é proporcional ao erro da distância percorrida em relação à referência, assim como um outro ganho (ganho derivativo) é proporcional ao erro relativo à velocidade. ===Sensor e medição de distância percorrida e velocidade=== * Como foi dito, o sensor utilizado foi o shaft encoder. Consiste de um círculo com furos espaçados igualmente ao longo do comprimento e um par de emissor-receptor óptico. O círculo e o par é alinhado de forma que o emissor emite luz continuamente, mas o receptor só recebe a luz quando passa um furo do círculo entre ambos. A figura abaixo explica melhor esse mecanismo: {{ :cursos:introrobotica:2011-2:grupo05:encoder_esquema.jpg }} * O centro do círculo fica preso ao eixo do motor, dessa forma, gira com a mesma velocidade angular do mesmo. Para saber a distância percorrida, basta saber que cada 6 pulsos equivalem a uma volta, cuja distância correspondente percorrida é dada pelo comprimento do círculo (2*pi*raio). Para encontrar a velocidade, basta saber quantos pulsos foram percebidos pelo emissor em determnado intervalo de tempo. === Análise === * Foram feitos testes dos motores a fim de gerar uma tabela relacionando potência a velocidade (em rps). O teste foi feito para cada um dos motores separadamente, pois como já foi dito, os rendimentos são diferentes. * Assim obteve-se os seguintes dados: * {{ :cursos:introrobotica:2011-2:grupo05:pote-rota-motor1.jpg?700 }} {{ :cursos:introrobotica:2011-2:grupo05:pote-rota-motor2.jpg?700 }} * Também foram realizados testes para saber o erro entre a distância desjada e a obtida para alguns valores de velocidade requeridos: {{ :cursos:introrobotica:2011-2:grupo05:translacao_v_20.jpg?700 }} {{ :cursos:introrobotica:2011-2:grupo05:translacao_v_60.jpg?700 }} {{ :cursos:introrobotica:2011-2:grupo05:translacao_v_100.jpg?700 }} === Controle PD === * O controle é bem dinâmico. Essa tabela foi passada à memória da handyboard, de forma que a cada velocidade desejada, é aplicada a respectiva potência exigida. Assim, através do valor do erro, temos o quanto desejamos modificar para atingirmos o valor de referência. === Conclusão === * Foram tomados os devidos cuidados em relação às características do ambiente (por exemplo, o chão do robô não pode ter buraco que impeça sua trajetória). Assim, ao fim desse projeto inicial, nosso robô fez 3 círculos e 3 quadrados com precisão considerável. O erro não foi desprezível, mas o resultado foi considerado aceitável para essa primeira fase. * Nos próximos pretendemos aperfeiçoar o controle e a estrutura para reduzir esse erro. Além de introduzir novos sensores aumentando sua funcionalidade. === === ---- == == {{:cursos:introrobotica:2011-2:grupo05:fotos.gif|}} == Fotos do MIBR #ALFA - Primeira Tentativa == {{ :cursos:introrobotica:2011-2:grupo05:00073.jpg }} {{ :cursos:introrobotica:2011-2:grupo05:00074.jpg }} {{ :cursos:introrobotica:2011-2:grupo05:00072.jpg }} == Fotos do MIBR #BETA - Versão Definitiva == {{ :cursos:introrobotica:2011-2:grupo05:00078.jpg }} {{ :cursos:introrobotica:2011-2:grupo05:img-20110908-00077.jpg }} ---- == == {{:cursos:introrobotica:2011-2:grupo05:codigo.gif|}} * Foram implementadas as funções básicas de girar para direita, girar para a esquerda, dar ré e seguir em linha reta. Além dessas, também existem as funções para que o nosso robô faça círculo e quadrado. Existe um trecho dedicado ao controle PD. * O quadrado é realizado com a chamada da função de seguir em linha reta seguida da função de girar para a direita. No caso do círculo, são feitas mais algumas contas, pois a roda de dentro deve percorrer uma distância menor que a roda de fora. Portanto, a roda de dentro deve ter uma velocidade menor que a de fora. Finalmente, a potência dirigida deve ser proporcional a cada uma das velocidades desejadas para cada roda, respeitando as relações estabelecidas e explicitadas nos itens anteriores. /*************************** 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 121.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 #define MOTOR_2 3 // Roxo lado visor LCD/Cinza #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 #use "cmucamlib.ic" char buff[11]; void main() { char a[TESTNUM][16]={"Menu: Frente","Menu: Quadrado","Menu: Circulo", "Menu: Direita", "Menu: Esquerda", "Menu: Re", "Menu: Mission", "Menu: RPSxPower"}; 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); while(1) { int sel; printf("Gire o KNOB para selecionar \n"); sleep(0.5); sel = select_string(a,TESTNUM); if(sel==0) Go(MM300); else if(sel==1) RightSquare(MM300); else if(sel==2) Circle(MM300); else if(sel==3) Right(); else if(sel==4) Left(); else if(sel==5) Go(-MM300); else if(sel==6) missionImpossible(); else if(sel==7) mapcalibration(); 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 } } //Gira para a direita void Gira360() { float dist=0.0; printf("Gira 360!\n"); //Imprime Status reset_encoder(0); StartMotors(50,-50); //Liga os motores e gira para a direita while(dist <=(BITOLA*PI)) //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 } //Gira para a direita void Right() { float dist1=0.0, dist2=0.0; int erroant1=0, erroant2=0,encsum1=0,encsum2=0; printf("Direita!\n"); //Imprime Status reset_encoder(0); reset_encoder(1); //StartMotors(100,-80); //Liga os motores e gira para a direita //motor(MOTOR_1,100); //motor(MOTOR_2,-80); while((dist1 < (((float)BITOLA*0.94*(float)(PI))/4.0)) || (dist2 <(((float)BITOLA*0.94*(float)(PI))/4.0))) //Meia volta = 90 graus { PDControl(20,20,&erroant1,&erroant2,RIGHT); 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 } //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 esquerda void Left() { float dist=0.0; int erroant1=0, erroant2=0,encsum=0; /*teste*/ int enc1sum=0,enc2sum=0; printf("Esquerda!\n"); //Imprime Status reset_encoder(0); reset_encoder(1); //StartMotors(-100,80); //Liga os motores e gira para a esquerda //motor(MOTOR_1,-100); //motor(MOTOR_2,100); while((dist <= (((float)BITOLA*(float)(PI))/4.0) )) //Meia volta = 90 graus { PDControl(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 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); //printf("Parado.\n"); //Imprime Status //while(!stop_button()); } //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(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+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; } } } //Desenha um quadrado de lado x mm girando para a direita void RightSquare(float x) { int i=1; while((i<13)) { Go(x); //Desloca x mm pra frente Right(); //Gira 90 graus para a direita if(stop_button()) break; i++; } } //Desenha um crculo de raio x mm void Circle(float x) { float dist=0.0; float L1=0.0,L2=0.0; int encsum=0,erroant1=0,erroant2=0; int pid=0; pid = start_process(missionImpossible()); L1 = x-(BITOLA/2.0); //Comprimento do arco da roda interna [mm] L2 = x+(BITOLA/2.0); //Comprimento do arco da roda EXTERNA [mm] while((dist<=3.0*(2.0*PI*x))) { PDControl((20),((int)(20.0*(L1/L2)+0.5)),&erroant1,&erroant2,MOVE); encsum = encsum + read_encoder(0) + read_encoder(1); //soma acumulativa dos encoders reset_encoder(0); reset_encoder(1); dist = (float) ((encsum)/2)*DX; //Calcula a distancia percorrida printf("Dist.: %f mm\n",dist); //Imprime distancia percorrida msleep(30L); //Aguarda movimento mecanico if(stop_button()) break; } //Quando a distncia x for alcancada StopMotors(); //Desliga os motores sleep(1.0); //Aguarda movimento mecnico encsum = encsum + read_encoder(0) + read_encoder(1); //soma acumulativa dos encoders dist = (float)(encsum/2)*DX; //Calcula a distancia percorrida printf("Dist.: %f mm\n",dist); //Imprime distancia percorrida reset_encoder(0); reset_encoder(1); kill_process(pid); //printf("Parado.\n"); //Imprime Status //while(!stop_button()); } void testmotors(void) { int pulsos1,rps1; int pulsos2,rps2; int aux = 0; int cont=2; printf("Testando motor, HoldSTOP para finalizar. \n"); for(cont=1;cont<11;cont++) { motor(MOTOR_1,cont*10); motor(MOTOR_2,cont*10); while ((!stop_button())||(aux<=10)) { reset_encoder(0); reset_encoder(1); msleep(500L); // Le o numero de pulsos pulsos1 = read_encoder(0); rps1 = 2*pulsos1/6; pulsos2 = read_encoder(1); rps2 = 2*pulsos2/6; aux = aux+1; printf("RPS1: %d RPS2: %d \n",rps1,rps2); } StopMotors(); beep(); msleep(500L); } StopMotors(); beep(); } //Mapeamento da tabela de controle void mapcalibration() { // float rps1=0.0,rps2=0.0; int pulsos1=0, pulsos2=0; int i=0,k=0; for(i=10;i<101;i++) { while(k<(2*3)) // k < 2*[N segundos] { //Limpa contadores de encoder reset_encoder(0); reset_encoder(1); //define potencia do motor motor(MOTOR_1,i); motor(MOTOR_2,i); msleep(500L); // Le o numero de pulsos pulsos1 = read_encoder(0); rps1 = (float)(pulsos1)/3.0; pulsos2 = read_encoder(1); rps2 = (float)(pulsos2)/3.0; printf("S1: %f S2: %f *i: %d\n",rps1,rps2,i); beep(); k++; } k=0; } } void missionImpossible() { float B = 493.88; float Bb= 466.16; float E = 659.25; float G = 783.99; float G_Grave = 392.00; float A = 880.00; float A_Grave = 440.00; float D = 587.33; float Eb = 622.25; int i=0; for(i=0;i<5;i++) { if(stop_button()) break; //1 tone(E,0.5); tone(E,0.5); sleep(0.05); tone(G,0.3); tone(A,0.3); tone(E,0.5); tone(E,0.5); sleep(0.05); tone(D,0.3); tone(Eb,0.3); //2 tone(E,0.5); tone(E,0.5); sleep(0.05); tone(G,0.3); tone(A,0.3); tone(E,0.5); tone(E,0.5); sleep(0.05); tone(D,0.3); tone(Eb,0.3); //3 tone(G,0.2); tone(E,0.2); tone(B,1.0); sleep(0.25); //4 tone(G,0.2); tone(E,0.2); tone(Bb,1.0); sleep(0.25); //5 tone(G,0.2); tone(E,0.2); tone(A_Grave,1.0); sleep(0.25); tone(G_Grave,0.2); tone(A_Grave,0.2); sleep(0.25); } } void hb_ir_transmit_on() { bit_set(0x1000,0b01000000); } /****************************** 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: */ ---- ----