—-
* 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”).
/*************************** 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<x)) //Enquanto dist<x e nao pressionar stop { // PDControl(20,20,&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 } //Quando a distncia x for alcancada kill_process(pid); kill_process(pid1); 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 sleep(0.5); reset_encoder(0); reset_encoder(1); } void PDControl(int *encid1, int *encid2, int tipo) { int enc1=0, enc2=0; int erro1=0, erro2=0; int i, pterm1, pterm2, dterm1, dterm2; int erroant1=0, erroant2=0; int speed1=0,speed2=0; int curve1[9]={9,13,14,16,17,18,19,19,20}; int curve2[9]={10,14,16,18,19,20,20,20,20}; reset_encoder(0); reset_encoder(1); while(!stop_button()) { enc1 = read_encoder(0); enc1 = 5*enc1; //*5 = 30*enc1/6 enc2 = read_encoder(1); enc2 = 5*enc2; erro1 = (*encid1-enc1); erro2 = (*encid2-enc2); pterm1 = (int)((float)KP*(float)erro1+0.5); pterm2 = (int)((float)KP*(float)erro2+0.5); dterm1 = (int) ((float)KD*(float)(erro1-erroant1)+0.5); dterm2 = (int) ((float)KD*(float)(erro2-erroant2)+0.5); erroant1 = erro1; erroant2 = erro2; /* integ = integ+erro; deriv = (erro/tempo) - deriv; */ enc1 = *encid1+pterm1+dterm1; enc2 = *encid2+pterm2+dterm2; //relacao enc e speed for(i=0;i<9;i++) { if(enc1>=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(ldrmaior<ldrgreen-ldramb) ldrmaior=ldrgreen-ldramb; if (ldrmaior<ldrred-ldramb) ldrmaior=ldrred-ldramb; if (ldrmaior<ldrblue-ldramb) ldrmaior=ldrblue-ldramb; if(ldrred-ldramb==ldrmaior) { if((ldrred-ldrgreen)<15) { if(ldramb>120) 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<b) return(a); else return(b); } */ /* Return minimum of two floats */ float fmin(float a,float b) { if(a<b) return(a); else return(b); } /* Returns which button is depressed using definitions above. If both are pressed, start has precedence */ int chosen_button() { if(start_button()) return START_B; else if(stop_button()) return STOP_B; else return NEITHER_B; } /* wait until button is depressed(DOWN_B), released(UP_B), or both(CYCLE_B) and return which button if any activated the sequence */ int wait_button(int i) { if(i==DOWN_B){ while(!(start_button() || stop_button())); return chosen_button(); } else if (i==UP_B) { int b; b=chosen_button(); while(start_button() || stop_button()); return b; } else { int b; while(!(start_button() || stop_button())); b=chosen_button(); while(start_button() || stop_button()); return b; } } /********************* Knob to Number routines*****************************/ /* Returns an integer value from min_val to max_val based on the current position of the knob */ int knob_int_value(int min_val,int max_val) { int val, coarseness=(255)/(max_val-min_val),selection; val=min((knob())/coarseness+min_val,max_val); return min(val,max_val); } /* Returns an float value from min_val to max_val based on the current position of the knob */ float knob_float_value(float min_val,float max_val) { float val, coarseness=(255.)/(max_val-min_val),selection; val=fmin(((float)knob())/coarseness+min_val,max_val); return fmin(val,max_val); } /******************** Menu selection routines ****************************/ /* While waiting for a button press, display the string passed in and the val, the integer value betwen min_val and max_val for the knob. If the button pressed is the start button, returns the final value of val. If the button pressed is the stop button, returns -1. */ int select_int_value(char s[],int min_val,int max_val) { int val, button; printf("%s %d to %d\n",s,min_val, max_val); sleep(0.8); /* Wait until no button is pressed */ wait_button(UP_B); /* While no button is pressed, display the string passed in and the current value of val */ while((button = chosen_button())==NEITHER_B) { val=knob_int_value(min_val,max_val); printf("%s %d\n",s,val); msleep(100L); } /* 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(val); /* Stop not pressed, return val */ } /* While waiting for a button press, display the string passed in and the val, the float value betwen min_val and max_val for the knob. If the button pressed is the start button, returns the final value of val. If the button pressed is the stop button, returns -1. */ float select_float_value(char s[],float min_val,float max_val) { float val; int button; printf("%s %f to %f\n",s,min_val, max_val); sleep(0.8); /* Wait until no button is pressed */ wait_button(UP_B); /* While no button is pressed, display the string passed in and the current value of val */ while((button = chosen_button())==NEITHER_B) { val=knob_float_value(min_val,max_val); printf("%s %f\n",s,val); msleep(100L); } /* Wait until no button is pressed */ wait_button(UP_B); if(button==STOP_B) return(-1.0); /** -1 means stop pressed -- do not reset value **/ else return(val); /* Stop not pressed, return val */ } /* While waiting for a button press, display the string from the array of strings passed in which corresponds to the current position of the knob (see select_int_value). If the button pressed is the start button, returns the index of the string selected (0 to n-1). If the button pressed is the stop button, returns -1. */ int select_string(char choices[][],int n) { int selection,last_selection=-1,button; if(n>_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: */