— Professor: Mário Fernando Montenegro Campos
— 2° semestre de 2011
— Leandro Henrique Costa Andrade 2011/09/04 12:16
— Guilherme Silva Terra 2011/09/04 12:16
— Daniel Salviano Araújo 2011/09/04 12:16
— Paulo Eduardo 2011/09/04 12:16
competicao
Introdução
O robô foi montado de várias formas diferentes, com mudanças no modelo, pontos de apoio, rodas, entre outros aspectos, até chegarmos a um consenso de qual seria o robô com maior robustez, estabilidade e curvas de torque/velocidade aceitáveis.
Para o primeiro trabalho prático, montamos um robô com duas rodas dianteiras com trações diferentes em cada uma delas (um motor para cada roda) e uma peça de roll-on que sustenta a parte traseira do mesmo. Na parte superior, foi projetado um teto coberto, onde se encontra a handy-board quando o robô está em operação. Foi anexado também, próximo ao eixo dianteiro, uma caneta de forma que a ponta da mesma sempre tenha contato com o chão. Assim, o robô pode desenhar a sua trajetória encima de uma cartolina ou papel kraft.
Comprimento: 20 centímetros.
Largura: 17 centímetros.
Altura: 13 centímetros.
Raio das rodas dianteiras: 2 centímetros.
Distância do eixo dianteiro: 15 centímetros
Controle PD(Proporcional e Derivativo)
O Controle PD implementado no robô tem o objetivo de garantir que o mesmo percorra uma distância pré-determinada. Para tal, utilizamos sensores break-beam(encoder), que são sensores foto-elétricos em junção com emissores de luz, que geram pulsos quando se quebra ou libera a passagem de luz entre um e outro. Assim, foram ligados dois sensores break-beam no robô, um para cada roda, o que permitiu receber dados em tempo real de posição angular relativa do eixo e até mesmo velocidades relativas. Tal forma de se usar os sensores citados anteriormente, é chamada de shaft-encoder.
A referência passada para a malha é um número pré-determinado de posição angular que deverá ser percorrida. O shaft-encoder foi montado em uma polia que apresenta 6 furos, logo em uma revolução o sensor captará 12 pulsos. A montagem do robô manteve na roda a mesma velocidade angular dessa polia, assim o número de rotações desenvolvido pela roda será visto sem escala pelo sensor na polia. O raio da roda utilizada mede 2 centímetros, de modo que a distância percorrida em uma rotação é 2πr, ou seja, em torno de 12.56 centímetros. Portanto, a cada 12.56 centímetros percorridos o shaft-encoder capta 12 pulsos, ou seja, um pulso a cada 0.955 centímetros.
O controle PD implementado aplica no motor uma potência inicialmente alta, já que o erro entre a posição desejada e a atual é grande. A medida que o caminho é percorrido essa diferença cai assim como a potência aplicada até se tornar nula, quando a referência é atingida. Assim, o robô inicia sua trajetória com uma velocidade alta e, a medida que a trajetória é percorrida a velocidade diminui até se tornar nula (ponto de chegada).
Afim de melhor ajustar os parâmetros do controlador coletamos os dados referentes a potência fornecida e o número de pulsos apontados pelo shaft-encoder para cada roda. O controlador foi ajustado para três valores diferentes de ganho e proporcional e ganho derivativo e o número de interações. Segue abaixo para cada configuração do controlador o número de iterações necessário para que a potência se estabilize em zero e o número de pulsos na referência de 75 pulsos.
O melhor resultado foi obtido para ganho proporcional Gp = 10 e ganho derivativo Gd = 5. Com essa configuração a potência e o número de pulsos demorou apenas 18 iterações para se estabilizarem. Segue abaixo o gráfico com os valores coletados para essa configuração.
Análise dos erros de translação e rotação
Erro de Translação:
Atuamos no motor por um tempo constante de 10s e anotamos as seguintes distâncias percorridas para as potências especificadas:
Para V = 20 ⇒ 17 centímetros
Para V = 60 ⇒ 29 centímetros
Para V = 100 ⇒ 39 centímetros
Erro de Rotação:
A rotação de 90º é obtida com as rodas são atuadas em sentidos opostos. Como o eixo dianteiro do robô mede 15cm as duas rodas devem percorrer uma distância referente a um quarto do círculo cujo diâmetro é o eixo sobre o qual ocorrerá a rotação. Assim, a distância percorrida pelas rodas deve ser (π/2)*r, ou seja, 11.78 centímetros em sentidos opostos. Calibramos os tempos necessários para cada potência especificada e medimos o erro de rotação, conforme mostrado abaixo.
Tarefas pedidas
A primeira tarefa do robô é percorrer um quadrado de 30cm de lado três vezes consecutivas. Para isso, atuamos em cada roda para uma referência em pulsos do shaft-encoder correspondente aos 30 centímetros, de acordo com a conversão apresentada, através de um controlador PD. Além disso, foi feito um ajuste de alinhamento de modo que caso uma roda, em alguma iteração, tenha percorrido o número maior de pulsos do que a outra o motor dessa última atua com uma valor maior considerando essa diferença de alinhamento. A rotação de 90° foi feita com as rodas percorrendo, em sentido contrário, um quarto da circunferência cujo diâmetro é o eixo dianteiro, sobre o qual ocorre a rotação.
A segunda tarefa é percorrer uma circunferência de 30cm de raio três vezes consecutivas. Para isso o controle PD foi feito para atuar em apenas na roda que percorreria o raio maior, com a referência em pulsos adequada para a distância percorrida por ela. Já para a outra roda foi aplicada no motor durante o percurso uma potência proporcional àquela aplicada na primeira roda, de acordo com razão entre os raios percorridos. Assim, garantimos que as duas rodas apresentem a mesma velocidade angular e, consequentemente, que a trajetória seja uma circunferência.
A interação com o robô ocorre através de uma simples interface que permite ao usuário escolher qual tarefa será realizada. Assim, ao ligar a handy-board o usuário pode apertar Start para que o robô execute o quadrado ou ainda pressionar Stop para que a circunferência seja percorrida. A princípio tivemos algumas dificuldades iniciais em locais com atritos diferentes, mas em uma cartolina sobre um local plano obtivemos o resultado esperado.
Código IC
#use fencdr6.icb
#use fencdr5.icb
// Pensamento: "Wer hat davon spricht der Mund, was er will" - Jesus, Samuel de
float raio_ponto = 0.977; // Valor percorrido pela roda a cada contagem do Encoder
// Quadrado de 30 cm de lado
float dist_square = 30.0;
int ref_square = (int) (dist_square/raio_ponto);
float giro90 = 50.57/4.0; // Valor percorrido pela roda para girar 90 graus
int giro90_ref = (int) (giro90/raio_ponto);
// Circunferencia de 30 cm de raio
float dist_circle0 = 238.64;
int ref_circle0 = (int) (dist_circle0/raio_ponto);
float dist_circle1 = 28.6368;
int ref_circle1 = (int) (dist_circle1/raio_ponto);
int i, power0, power1, alinhamento;
void main(){
while (1){
printf("Start --> squareStop --> circle");
if(start_button()){
for(i = 0; i < 12; i++){
sleep (0.5);
power0 = 0;
power1 = 0;
encoder6_counts = 0;
encoder5_counts = 0;
alinhamento = 0;
while (1) {
alinhamento = encoder6_counts - encoder5_counts;
power0 = 10 * (ref_square - encoder6_counts ) - 5 * encoder6_velocity;
power1 = 10 * (ref_square - encoder5_counts ) - 5 * encoder5_velocity;
if (alinhamento > 0) power1 + 3 * alinhamento;
if (alinhamento < 0) power0 - 3 * alinhamento;
motor(0, power0);
motor(1, power1);
if(power0 == 0 && power1 == 0) break;
}
sleep (0.5);
power0 = 0;
power1 = 0;
encoder6_counts = 0;
encoder5_counts = 0;
while (1) {
power0 = 10 * (giro90_ref - encoder6_counts ) - 5 * encoder6_velocity;
power1 = 10 * (giro90_ref - encoder5_counts ) - 5 * encoder5_velocity;
motor(0, power0);
motor(1, -power1);
if(power0 ==0 && power1 == 0) break;
}
}
sleep (0.5);
beep();
off(0);
off(1);
beep();
}
if(stop_button()){
for (i = 0; i < 3; i++){
power0 = 0;
power1 = 0;
encoder6_counts = 0;
encoder5_counts = 0;
while (1) {
power0 = 10 *( ref_circle0 - encoder6_counts ) - 5 * encoder6_velocity;
if (power0 > 100) power0 = 100;
power1 = (int) ( (float) power0 * ( dist_circle1/dist_circle0 ) );
motor(0, power0);
motor(1, power1);
if(power0 == 0 && power1 == 0) break;
}
sleep (0.5);
}
beep();
off(0);
off(1);
beep();
}
}
}
Introdução
Para esse trabalho prático, foi necessário comprar LEDs coloridos, de luz e RGB, LDRs, sensores IF, fios, pinos, resistências, entre outros objetos.
A estrutura básica do robô continuou a mesma, tendo pequenas modificações para acomodar os novos sensores que serão mais bem explicados a seguir.
Sensores seguidores de linha
Inicialmente tentamos implementar os sensores (um par), responsáveis por auxiliar o robô a seguir linha, através de pares de emissores e receptores de infra-vermelhos. Porém, devido a problemas nos emissores e receptores comprados, implementamos esses sensores através de um LED de luz vermelha e um LED de luz branca, ambos relacionados a um LDR. Assim, usando saídas analógicas, foi possível calibrar valores feitos pelos LDRs quando estivessem encima da linha preta e fora da mesma (chão branco). Por fim, implementamos um código em IC que permitiu ao robô seguir a linha preta.
Sensores de proximidade e reconhecedores de luz
As funções de reconhecer a proximidade de um obstáculo (blocos) e reconhecer cores (vermelho, azul, verde, amarelo) foram implementadas através de dois LEDs de luz branca, um LED RGB e um LDR com uma blindagem lateral (como um binóculo) entre os mesmos. Os valores utilizados através do LDR são analógicos.
Caso no menu seja escolhido a função de seguir linha reta, o robô usa os sensores seguidores de linha, como explicados anteriormente, e ao mesmo tempo, ativa os LEDs frontais de luz branca (como um farol).
Os LEDs de luz branca ficam ligados até o sensor reconhecer um objeto próximo, já que após isso o LED RGB entrará em ação e a luz branca interfereria nos valores recebidos pelo LDR. O reconhecimento de objetos próximos é feito através de uma análise de diferencial que ocorre quando o objeto faz sombra sobre o LDR. Pois, quando ocorre a aproximação do robô de um bloco, o LDR captura a projeção dos LEDs de luz branca no mesmo, logo ao aproximar, os valores analógicos diminuem aos poucos. Porém, quando o LDR se torna muito próximo do bloco, o mesmo faz sombra sobre ele e os valores analógicos do LDR se tornam muito altos de uma forma abrupta, já que a sombra não permite a entrada de luz no tubo que leva ao LDR. Assim, podemos reconhecer a proximidade de blocos.
Logo após o robô reconhecer um bloco, os LEDs de luz branca são desligados e o robô dá uma pequena ré para que possa capturar melhor a recepção de luz colorida. Assim, com o bloco a aproximadamente 3 cm do robô, os LEDs coloridos (RGB) são emitidos, um de cada vez e em sequência, e os valores médios analógicos capturados pelo LDR são armazenados em variáveis no programa. Por fim, através de um código de análise de erros, com valores calibrados anteriormente, implementamos o reconhecimento da cor. As ações feitas após o reconhecimento da cor são definidas no início do programa através do menu implementado no display.
Sensores de luz polarizada
Como foi pedido no trabalho, montamos também dois sensores receptores de luz polarizada, através de dois LDRs, blindagens laterais e uma tela polarizadora. Foram usadas duas saídas analógicas para receberem os valores dos LDRs sensíveis a luzes polarizadas.
Os sensores de luz polarizada foram montados na forma de dois olhos de um binóculo, um do lado do outro, onde um deles é sensível a luz polarizada horizontalmente e o outro a luz polarizada verticalmente.
Inicialmente, o operador do robô deve escolher qual polarização o robô deve seguir no menu inicial.Após escolher, um dos LDRs será desativado e o outro será usado para capturar os valores necessários. Apesar da distância do robô das luzes interferir nos valores obtidos pelos LDRs, notamos que a luz polarizada entra com maior intensidade nos sensores polarizados da mesma forma que a mesma. Assim, fizemos com que o robô desse uma volta (360 graus) e capturasse os valores analógicos do LDR em questão. Como quanto maior a intensidade da luz, menor é o valor capturado pelo LDR, obtivemos um mínimo durante a volta feita pelo robô. Logo, armazenamos o mínimo em uma variável no programa e mandamos o robô dar outra volta, porém agora, quando o robô se deparar novamente com o valor mínimo capturado anteriormente, o mesmo saberá que estará de frente para a luz que deve seguir e irá em sua direção.
Interface
Implementamos também um menu inicial que realiza a interface usuário/máquina. Onde o botão 'Start' significa aceitar e o botão 'Stop' significa rejeitar. Abaixo se encontra um diagrama explicativo do menu implementado.
As ações, caso o robô se depare com um obstáculo, são parar, contornar pela direita, contornar pela esquerda e empurrar o bloco por uma determinada distância, que pode ser determinada pelo potenciômetro que se encontra na lateral da HandyBoard.
Caracterização dos sensores e influência de distâncias e cores de objetos
Gráficos sobre a influência da luz ambiente e do motor.
- Desvio padrão -
Luz azul: 0,59
Luz vermelha : 0,86
Luz verde: 0,43
- Valor Médio -
Luz azul: 103
Luz vermelha : 193
Luz verde: 139
- Desvio padrão -
Luz azul: 0,75
Luz vermelha : 0,83
Luz verde: 0,6
- Valor Médio -
Luz azul: 102
Luz vermelha : 191
Luz verde: 139
Podemos notar pelo gráfico que o ambiente afeta a medição de cor dos LRDs, alterando os valores, geralmente para mais, e dessa forma, aumentando o desvio padrão dos valores.
- Desvio padrão -
Luz azul: 2,4
Luz vermelha : 1,9
Luz verde: 1,7
- Valor Médio -
Luz azul: 108
Luz vermelha : 194
Luz verde: 144
O motor estar ligado influencia na medição de luz refletida no bloco, devido ao fato de mudar a angulação do emissor de luz e do receptor, fazendo com que o desvio padrão fique elevado.
Reflexão da luz no bloco para diferentes distâncias
- Códigos
#use fencdr5.icb
#use fencdr6.icb
int i = 0, u = 0, sum = 0, option, init, power0, power1;
int startButton, stopButton, lineFollowID, waitBlockID, alignPolarizedLightID, showDistanceID, showValuesLineID;
float ratio_point = 0.977; Valor percorrido pela roda a cada contagem do Encoder
float d_rot = 15.0; Diametro do eixo
float grad_point =1)
int rot_ref = (int) (90.0*grad_point); Conuts necessrios para rotacionar 90 graus
float goOn_dist1 = 15.0; Distancia percorrida perpendicular a linha
float goOn_dist2 = 30.0; Distancia percorrida paralela a linha
int dist = knob();
int MOTOR_LEFT = 0; Encoder 5
int MOTOR_RIGHT = 1; Encoder 6
int LINE_LEFT_LIMIT = 96; > preto
int LINE_RIGHT_LIMIT = 55; > preto
int LINE_SENSOR_LEFT = 3;
int LINE_SENSOR_RIGHT = 2;
int PROX_SENSOR = 4;
int LIGHT_HORIZONTALLY = 3; Esquerda (Mesa) → Torta
int LIGHT_VERTICALLY = 2; Direita (Armario)
int YELLOW = 3001;
int GREEN = 3002;
int BLUE = 3003;
int RED = 3004;
default
int GO_AROUND_LEFT = YELLOW;
int GO_AROUND_RIGHT = GREEN;
int PUSH_BLOCK = BLUE;
int STOP = RED;
int BLOCK_DETECTED = 0;
float RED_red_default = 115.0;
float RED_green_default = 155.0;
float RED_blue_default = 144.0;
float GREEN_red_default = 137.0;
float GREEN_green_default = 80.0;
float GREEN_blue_default = 95.0;
float YELLOW_red_default = 80.0;
float YELLOW_green_default = 70.0;
float YELLOW_blue_default = 83.0;
float BLUE_red_default = 190.0;
float BLUE_green_default = 140.0;
float BLUE_blue_default = 100.0;
float vermelho;
float verde;
float azul;
float errovermelho;
float erroverde;
float erroamarelo;
float erroazul;
float modulo (float n){
if (n < 0.0) return -n; else return n;} int color = 0; void showValuesLine () {
while(1){
printf ("\nEsq: %d Dir: %d Frente: %d", analog(LINE_SENSOR_LEFT), analog(LINE_SENSOR_RIGHT), analog(PROX_SENSOR));
sleep(0.1);
}
}
void lineFollow (){
int first = 0;
motor(MOTOR_LEFT, 50);
motor(MOTOR_RIGHT, 50);
motor(2,50);
motor(3,50);
showValuesLineID = start_process(showValuesLine());
while (1){
if (analog(LINE_SENSOR_LEFT) < LINE_LEFT_LIMIT && analog(LINE_SENSOR_RIGHT) > LINE_RIGHT_LIMIT) {
goRight();
if (first == 0) first = LINE_SENSOR_RIGHT;
}
else if (analog(LINE_SENSOR_LEFT) > LINE_LEFT_LIMIT && analog(LINE_SENSOR_RIGHT) < LINE_RIGHT_LIMIT) {
goLeft();
if (first == 0) first = LINE_SENSOR_LEFT;
}
else if (analog(LINE_SENSOR_LEFT) > LINE_LEFT_LIMIT && analog(LINE_SENSOR_RIGHT) > LINE_RIGHT_LIMIT) {
if (first == LINE_SENSOR_RIGHT) {
power0 = -30;
power1 = 30;
}
else {
power0 = 30;
power1 = -30;
}
motor(MOTOR_LEFT, power0);
motor(MOTOR_RIGHT, power1);
while (analog(LINE_SENSOR_LEFT) > LINE_LEFT_LIMIT && analog(LINE_SENSOR_RIGHT) > LINE_RIGHT_LIMIT);
if (analog(LINE_SENSOR_LEFT) < LINE_LEFT_LIMIT) first = LINE_SENSOR_RIGHT;
else first = LINE_SENSOR_LEFT;
}
else goAhead();
}
}
void waitBlock (){
while(1) {
int erro = 25, atual;
lineFollowID = start_process(lineFollow());
while (1){
atual = analog(PROX_SENSOR);
sleep(0.1);
if(analog(PROX_SENSOR) > atual + erro){
motor(MOTOR_RIGHT, 0);
motor(MOTOR_LEFT, 0);
break;
}
}
kill_process(lineFollowID);
motor(MOTOR_LEFT, -50);
motor(MOTOR_RIGHT, -50);
sleep(0.2);
color = lookColor();
if (color == GO_AROUND_LEFT) goAroundLeft();
else if (color == GO_AROUND_RIGHT) goAroundRight();
else if (color == PUSH_BLOCK) pushBlock (dist);
}
}
int lookColor() { alloff();
sleep (0.2);
kill_process (showValuesLineID);
poke(0x1009, 0x3c);
bit_set(0x1008, 0x4c);
sleep(0.3);
for (i = 0; i<10;i++){
sleep(0.3);
vermelho=vermelho+(float)analog(PROX_SENSOR);
printf("\nVERMELHO: %d", analog(PROX_SENSOR));
}
sleep(0.3);
bit_clear(0x1008, 0x4c);
bit_set(0x1008, 0x3c);
bit_clear(0x1008, 0x1c);
sleep(0.3);
for (i = 0; i<10;i++){
sleep(0.3);
verde=verde+(float)analog(PROX_SENSOR);
printf("\nVERDE: %d", analog(PROX_SENSOR));
}
sleep(0.3);
bit_clear(0x1008, 0x3c);
bit_set(0x1026, 0x80);
bit_set(0x1000, 0x80);
sleep(0.3);
for (i = 0; i<10;i++){
sleep(0.3);
azul=azul+(float)analog(PROX_SENSOR);
printf("\nAZUL: %d", analog(PROX_SENSOR));
}
sleep(0.3);
bit_clear(0x1000,0x80);
errovermelho=modulo(vermelho/(vermelho+verde+azul)- RED_red_default/(RED_red_default + RED_green_default + RED_blue_default));
errovermelho=errovermelho+modulo(verde/(vermelho+verde+azul)- RED_green_default/(RED_red_default + RED_green_default + RED_blue_default));
errovermelho=errovermelho+modulo(azul/(vermelho+verde+azul)- RED_blue_default/(RED_red_default + RED_green_default + RED_blue_default));
erroverde=modulo(vermelho/(vermelho+verde+azul)- GREEN_red_default/(GREEN_red_default + GREEN_green_default + GREEN_blue_default));
erroverde=erroverde+modulo(verde/(vermelho+verde+azul)- GREEN_green_default/(GREEN_red_default + GREEN_green_default + GREEN_blue_default));
erroverde=erroverde+modulo(azul/(vermelho+verde+azul)- GREEN_blue_default/(GREEN_red_default + GREEN_green_default + GREEN_blue_default));
erroamarelo=modulo(vermelho/(vermelho+verde+azul)- YELLOW_red_default/(YELLOW_red_default + YELLOW_green_default + YELLOW_blue_default));
erroamarelo=erroamarelo+modulo(verde/(vermelho+verde+azul)- YELLOW_green_default/(YELLOW_red_default + YELLOW_green_default + YELLOW_blue_default));
erroamarelo=erroamarelo+modulo(azul/(vermelho+verde+azul)- YELLOW_blue_default/(YELLOW_red_default + YELLOW_green_default + YELLOW_blue_default));
erroazul=modulo(vermelho/(vermelho+verde+azul)- BLUE_red_default/(BLUE_red_default + BLUE_green_default + BLUE_blue_default));
erroazul=erroazul+modulo(verde/(vermelho+verde+azul)- BLUE_green_default/(BLUE_red_default + BLUE_green_default + BLUE_blue_default));
erroazul=erroazul+modulo(azul/(vermelho+verde+azul)- BLUE_blue_default/(BLUE_red_default + BLUE_green_default + BLUE_blue_default));
if(erroamarelo<erroazul&&erroamarelo<erroverde&&erroamarelo<errovermelho){
printf("\nAMARELO");
sleep(2.0);
return YELLOW;
}
if(erroverde<erroazul&&erroverde<erroamarelo&&erroverde<errovermelho){
printf("\nVERDE");
sleep(2.0);
return GREEN;
}
if(errovermelho<erroazul&&errovermelho<erroverde&&errovermelho<erroamarelo){
printf("\nVERMELHO");
sleep(2.0);
return RED;
}
if(erroazul<erroverde&&erroazul<errovermelho&&erroazul<erroamarelo){
printf("\nAZUL");
sleep(2.0);
return BLUE;
}
}
void goRight(){motor(MOTOR_LEFT, 40); motor(MOTOR_RIGHT, 0);} void goLeft(){
motor(MOTOR_RIGHT, 40); motor(MOTOR_LEFT, 0);} void goAhead(){
motor(MOTOR_RIGHT, 50); motor(MOTOR_LEFT, 50);} void stop(){
motor(MOTOR_RIGHT, 0); motor(MOTOR_LEFT, 0); sleep(5.0);} void goOn(float ref){
power0 = 0;
power1 = 0;
encoder5_counts = 0;
encoder6_counts = 0;
do {
power0 = 10 * ((int) (ref/ratio_point) - encoder6_counts);
power1 = 10 * ((int) (ref/ratio_point) - encoder5_counts );
motor(MOTOR_RIGHT, power0);
motor(MOTOR_LEFT, power1);
}while (power0 > 0 && power1 > 0);
}
void turnLeft(){ power0 = 0;
power1 = 0;
encoder5_counts = 0;
encoder6_counts = 0;
do {
power0 = 10 * (rot_ref - encoder6_counts );
power1 = 10 * (rot_ref - encoder5_counts );
motor(MOTOR_RIGHT, power0);
motor(MOTOR_LEFT, -power1);
}while (power0 > 0 && power1 > 0);
}
void turnRight(){ power0 = 0;
power1 = 0;
encoder5_counts = 0;
encoder6_counts = 0;
do {
power0 = 10 * (rot_ref - encoder6_counts );
power1 = 10 * (rot_ref - encoder5_counts );
motor(MOTOR_RIGHT, -power0);
motor(MOTOR_LEFT, power1);
}while (power0 > 0 && power1 > 0);
}
void goAroundLeft(){ turnLeft(); sleep(0.2); goOn(goOn_dist1); sleep(0.2); turnRight(); sleep(0.2); goOn(goOn_dist2); sleep(0.2); turnRight(); sleep(0.2); goOn(goOn_dist1); sleep(0.2); turnLeft(); sleep(0.2); alloff();} void goAroundRight(){
turnRight(); sleep(0.2); goOn(goOn_dist1); sleep(0.2); turnLeft(); sleep(0.2); goOn(goOn_dist2); sleep(0.2); turnLeft(); sleep(0.2); goOn(goOn_dist1); sleep(0.2); turnRight(); sleep(0.2); alloff();} void pushBlock(int dist){
int ref_dist = (int) ((float)dist/ratio_point);
power0 = 0;
power1 = 0;
encoder5_counts = 0;
encoder6_counts = 0;
do {
power0 = 10 * (ref_dist - encoder6_counts );
power1 = 10 * (ref_dist - encoder5_counts );
motor(MOTOR_RIGHT, power0);
motor(MOTOR_LEFT, power1);
}while (power0 > 0 && power1 > 0);
}
void alignPolarizedLight (int light_sensor){
float angle = 360.0;
int erro = 10;
int minimo = 300;
int value = 0;
power0 = 0;
power1 = 0;
encoder5_counts = 0;
encoder6_counts = 0;
do {
power0 = 10 * ((int)(angle*grad_point) - encoder6_counts );
power1 = 10 * ((int)(angle*grad_point) - encoder5_counts );
motor(MOTOR_RIGHT, power0);
motor(MOTOR_LEFT, -power1);
value = analog(light_sensor);
if (value < minimo) minimo = value;
}while (power0 > 0 && power1 > 0);
motor (MOTOR_RIGHT, -40);
motor (MOTOR_LEFT, 40);
while(1){
sum = 0;
for (u = 0; u < 5; u++){
sum = sum + analog(light_sensor);
sleep (0.1);
}
value = sum/5;
if (value < minimo + erro && value > minimo - erro){
if (light_sensor == LIGHT_VERTICALLY) {
angle = 30.0;
power0 = 0;
power1 = 0;
encoder5_counts = 0;
encoder6_counts = 0;
do {
power0 = 10 * ((int)(angle*grad_point) - encoder6_counts );
power1 = 10 * ((int)(angle*grad_point) - encoder5_counts );
motor(MOTOR_RIGHT, power0);
motor(MOTOR_LEFT, -power1);
}while (power0 > 0 && power1 > 0);
}
printf ("\nACHOU LUZ");
goAhead();
sleep(3.0);
}
}
}
void showDistance(){ while (1){
sleep(0.5);
printf ("\nConfirme a distancia: %d", knob());
}
}
void waitStop (){stop_press(); option = -1;} void waitStart (){
start_press(); option = 1;} int wait(){
option = 0; startButton = start_process(waitStart()); stopButton = start_process(waitStop()); while (option == 0)sleep(0.1); if (option == -1) kill_process(startButton); if (option == 1) kill_process(stopButton); return option;} void initialize(){
init = 0; motor (0,0); motor (1,0); motor (2,0); motor (3,0); bit_clear(0x1008, 0x4c); bit_clear(0x1008, 0x1c); bit_clear(0x1008, 0x3c); bit_clear(0x1000, 0x80);} void main(){
while (1) {
initialize();
printf ("\nSeguir Linha ?");
if (wait() == 1){
int choice_AROUND_LEFT = 0;
int choice_AROUND_RIGHT = 0;
int choice_STOP = 0;
int choice_PUSH = 0;
// Yellow
while (choice_AROUND_LEFT + choice_AROUND_RIGHT + choice_STOP + choice_PUSH == 0) {
if (choice_AROUND_LEFT == 0) {
printf("\nAMARELO->Contornar a esquerda ?");
if (wait() == 1) {
choice_AROUND_LEFT++;
GO_AROUND_LEFT = YELLOW;
break;
}
}
if (choice_AROUND_RIGHT == 0) {
printf("\nAMARELO->Contornar a direita ?");
if (wait() == 1) {
choice_AROUND_RIGHT++;
GO_AROUND_RIGHT = YELLOW;
break;
}
}
if (choice_STOP == 0) {
printf("\nAMARELO->Parar ?");
if (wait() == 1) {
choice_STOP++;
STOP = YELLOW;
break;
}
}
if (choice_PUSH == 0) {
printf("\nAMARELO->Empurrar bloco ?");
if (wait() == 1) {
choice_PUSH++;
PUSH_BLOCK = YELLOW;
showDistanceID = start_process (showDistance());
while (wait() != 1);
kill_process(showDistanceID);
dist = knob();
}
}
}
// Blue
while (choice_AROUND_LEFT + choice_AROUND_RIGHT + choice_STOP + choice_PUSH == 1) {
if (choice_AROUND_LEFT == 0) {
printf("\nAZUL->Contornar a esquerda ?");
if (wait() == 1) {
choice_AROUND_LEFT++;
GO_AROUND_LEFT = BLUE;
break;
}
}
if (choice_AROUND_RIGHT == 0) {
printf("\nAZUL->Contornar a direita ?");
if (wait() == 1) {
choice_AROUND_RIGHT++;
GO_AROUND_RIGHT = BLUE;
break;
}
}
if (choice_STOP == 0) {
printf("\nAZUL->Parar ?");
if (wait() == 1) {
choice_STOP++;
STOP = BLUE;
break;
}
}
if (choice_PUSH == 0) {
printf("\nAZUL->Empurrar Bloco ?");
if (wait() == 1) {
choice_PUSH++;
PUSH_BLOCK = BLUE;
showDistanceID = start_process (showDistance());
while (wait() != 1);
kill_process(showDistanceID);
dist = knob();
}
}
}
// Red
while (choice_AROUND_LEFT + choice_AROUND_RIGHT + choice_STOP + choice_PUSH == 2) {
if (choice_AROUND_LEFT == 0) {
printf("\nVERMELHO->Contornar a esquerda ?");
if (wait() == 1) {
choice_AROUND_LEFT++;
GO_AROUND_LEFT = RED;
break;
}
}
if (choice_AROUND_RIGHT == 0) {
printf("\nVERMELHO->Contornar a direita ?");
if (wait() == 1) {
choice_AROUND_RIGHT++;
GO_AROUND_RIGHT = RED;
break;
}
}
if (choice_STOP == 0) {
printf("\nVERMELHO->Parar ?");
if (wait() == 1) {
choice_STOP++;
STOP = RED;
break;
}
}
if (choice_PUSH == 0) {
printf("\nVERMELHO->Empurrar bloco ?");
if (wait() == 1) {
choice_PUSH++;
PUSH_BLOCK = RED;
showDistanceID = start_process (showDistance());
while (wait() != 1);
kill_process(showDistanceID);
dist = knob();
}
}
}
// Green
while (choice_AROUND_LEFT + choice_AROUND_RIGHT + choice_STOP + choice_PUSH == 3) {
if (choice_AROUND_LEFT == 0) {
printf("\nVERDE->Contornar a esquerda ?");
if (wait() == 1) {
choice_AROUND_LEFT++;
GO_AROUND_LEFT = GREEN;
break;
}
}
if (choice_AROUND_RIGHT == 0) {
printf("\nVERDE->Contornar a direita ?");
if (wait() == 1) {
choice_AROUND_RIGHT++;
GO_AROUND_RIGHT = GREEN;
break;
}
}
if (choice_STOP == 0) {
printf("\nVERDE->Parar ?");
if (wait() == 1) {
choice_STOP++;
STOP = GREEN;
break;
}
}
if (choice_PUSH == 0) {
printf("\nVERDE->Empurrar bloco ?");
if (wait() == 1) {
choice_PUSH++;
PUSH_BLOCK = GREEN;
showDistanceID = start_process (showDistance());
while (wait() != 1);
kill_process(showDistanceID);
dist = knob();
}
}
}
waitBlock();
init = 1;
}
if (init == 0) {
printf ("\nSeguir luz polarizada ?");
if (wait() == 1){
printf ("\nAlinhar com luz vertical ?");
if (wait() == 1) {
alignPolarizedLight (LIGHT_VERTICALLY);
init = 1;
}
if (init == 0) {
printf ("\nAlinhar com luz horizontal ?");
if (wait() == 1) {
alignPolarizedLight (LIGHT_HORIZONTALLY);
}
}
}
sleep(0.1);
}
}
}
Funções implementadas
1) Arremesso de uma bolinha em direção a uma fonte de luz polarizada, através de um sistema de catapulta ou lançamento.
2) Interrupção de todas as ações do robo em 60 segundos, contados a partir do acionamento da luz na parde de baixo do campo.
3) Movimentação através do campo com um mapa previamente conhecido, utilizando um algoritmo wavefront
*
Primeiramente foi projetado a construção de uma catapulta que seria inserida ao lado da estrutura do robô, sendo que uma haste ligada ao motor empurraria o braço da catapulta para baixo, tensionando um elástico preso a ela. Quando essa haste liberasse esse braço, o elástico puxaria a catapulta para frente lançando a bolinha. Porém a idéia não se mostrou eficaz, uma vez que no momento dos testes, a haste não coseguiu vencer a resistência da catapulta com o elástico e forçou o motor até soltá-lo da estrutura do robô.
Foi feito um segundo projeto, dessa vez implementamos um atirador em vez da catapulta. Esse atirador consistira em um motor que rodava uma engrenagem com um arco de 180º liso, sem os dentes. Quando a parte dentada estava para baixo, puxava o lançador para trás, esticando o elástico preso a ele. Quando a parte lisa ficava para baixo, o lançador escapava e com a força do elástico empurrava a bolinha para frente.
Com isso conseguimos construir um lançador de bolinhas que atirava na direção da luz polarizada com eficiência.
*
Para começar a tarefa, a luz inferior do campo seria ligada e o sensor LDR do robô apontado para baixo seria responsável por captar esse sinal para iniciar a contagem do tempo.
Foi utilizado uma rotina em paralelo as outras atividades do robô para garantir o desligamento no momento certo. Assim que a luz era ligada, uma thread era iniciada por 60 segundos e quando terminava, travava o robô independente do estado atual.
*
Nessa tarefa foi implementado o controle deliberativo que tinha como objetivo mapear o campo e levar o robô do ponto de partida até o alvo. O campo foi modularizado em quadrados de 30cm e o alvo e os obstáculos eram informados ao robô logo antes do teste. O algoritmo definia o ponto do alvo como valor zero e seus vizinhos como valor um. Os pontos adjacentes eram adicionados de uma unidade e assim sucessivamente até completar todo o campo. Os obstáculos e paredes eram definidos como valores bem alto. O algoritmo informava ao robô para se deslocar para o quadrado adjacente com o menor valor, assim ele se movimentaria em direção ao objetivo sem colidor com os obstáculos, ou com a borda do campo. Esse passo era repetido até chegar ao alvo seguindo o menor caminho. O controle P D, implementado anteriormente, foi incorporado ao algoritmo wavefront para otimizar o deslocamento do robô.
*
Nosso robô apresentou alguns problemas, como a quebra da catapulta, desencaixe do motor com as engrenagens e a falta de espaço para as peças e os fios. Assim o grupo resolveu reconstruir nosso robô.
Nossa base foi criada para dar mais estabilidade para o resto dos equipamentos e ser mais firme
Além disso, as engrenagens, barras e os motores foram ligados de uma forma que evitasse o desprendimento dessas peças, o que comprometeria o desempenho em todos os teste e na competição
Para evitar o problema do excesso de fios soltos, implementamos uma canaleta que conduzia todos os fios até a handybord. Além disso a rampa que serve de base para o lançador de bolinhas foi construida para que se levantesse com facilidade para realizar os reparos necessários nos elementos da base
O lançador de bolinhas foi colocado em cima dessa rampa para efetuar um lançamento com uma boa distancia.
No final, nosso todas as outras partes já implementadas foram colocadas no robô para que ele ficasse completo para a competição. Todas as funções foram testadas e funcionaram dentro do esperado, corrigindo as falhas anteriores.
