Tabela de conteúdos

Relatório TP-2

Introdução

Este trabalho cosistiu na construção de sensores e seu uso no robô montado anteriormente.

Os sensores montadoes são, basicamente, sensíveis à luz no espéctro visível ou não(infra-vermelho). Entre as função dos sensores, encontra-se: identificar a cor de determinados objetos(blocos de isopor coloridos); guiar o robô em cima de uma linha preta em um piso branco; e orintá-lo em direção a luzes polarizadas.

O objetivo do robô para apresentação é andar em linhas, desviar de blocos amarelos e azuis, arrastar verdes e parar em frente a blocos vermelhos. Além disso o robo deve ser capaz de se orientar para uma luz polarizada especificada.

Construção dos Sensores

Os sensores criados foram 3: dois para seguir linha, um diferencial para orientar em direção à luz e um para identificar cores.

Segue Linha

Foram feitos utilizando um emissor e um receptor infra-vermelho. Eles foram colocados dentro de peças LEGO de forma que apontassem para baixo e que a distância entre receptor e emissor fosse suficientemente pequena.

A montagem utilizou resistores de 330Ω antes dos emissores de infra-vermelho.

Diferencial

Dois LDRs foram montados em peças LEGO, segundo a especificação abaixo:

Identificador de Cores

Para o identificador de cores foi utilizado um LDR e um LED RGB, antes do led foi utilizado um resistor de 330Ω

Eles também foram posicionados em peças LEGO, mas com o cuidado de utilizar o termo-retrátil ao redor do LDR para evitar interferência.

Implementação do Códigos

Segue Linha

Os sensores devem permanecer lendo o fundo branco(WHITE), caso o valor lido represente um fundo preto, deve haver uma correção, ou o robô deve fazer uma curva.

void linefollow(){
    int count;
 
    while(1){
        printf("%d %d\n", LINEFOLLOW_LEFT, LINEFOLLOW_RIGHT);
        if(LINEFOLLOW_LEFT == WHITE && LINEFOLLOW_RIGHT == WHITE){
            _pid_follow = start_process(fwd(INF, 0));
            while(LINEFOLLOW_LEFT==WHITE && LINEFOLLOW_RIGHT==WHITE){
                sleep(WAIT_SENSOR);
            }
            stopfollowline();
        }
 
        if(LINEFOLLOW_LEFT == BLACK && LINEFOLLOW_RIGHT == BLACK){
            _pid_follow = start_process(fwd(INF, 0));
            while(LINEFOLLOW_LEFT == BLACK && LINEFOLLOW_RIGHT == BLACK);
            stopfollowline();
            turn(90);
        }
 
        if(LINEFOLLOW_LEFT == WHITE && LINEFOLLOW_RIGHT == BLACK){
            _pid_follow = start_process(fwd(INF, -TURN_VELOCITY));
            count = 0;
            while(LINEFOLLOW_LEFT==WHITE && LINEFOLLOW_RIGHT==BLACK){
                sleep(WAIT_SENSOR);
                count++;
            }
            printf("%d\n", count);
            fwd(TURN_CORRECTION(count), TURN_VELOCITY);
            stopfollowline();
        }
 
        if(LINEFOLLOW_LEFT == BLACK && LINEFOLLOW_RIGHT == WHITE){
            _pid_follow = start_process(fwd(INF, TURN_VELOCITY));
            count = 0;
            while(LINEFOLLOW_LEFT == BLACK && LINEFOLLOW_RIGHT == WHITE){
                count++;
                sleep(WAIT_SENSOR);
            }
            printf("%d\n", count);
            fwd(TURN_CORRECTION(count), -TURN_VELOCITY);
            stopfollowline();
        }
    }
}
 
void stopfollowline(){
    kill_process(_pid_follow);
    stop();
}

Diferencial

O sensor diferencial para Seguir Luz, segue a lógica de ler todo o ambiente ao redor do robô (360°). De acordo com os respostas, ele deve, então, fazer um novo giro e apontar para o lado de maior resposta.

Para a luz ambiente não interferir na leitura dos sensores o LDR

void light_follow(int light){
    int leitura, diff, color, menor=DIFF_EYE+COLOR, maior=300+DIFF_EYE-COLOR;
 
    printf("Calibrating!\n");
 
    COUNTS_LEFT = COUNTS_RIGHT = 0;
 
    while(COUNTS_LEFT < 576 || COUNTS_RIGHT < 576){
        motor(MOTOR_LEFT, -30);
        motor(MOTOR_RIGHT, 30);
        sleep(0.5);
        off(MOTOR_LEFT);
        off(MOTOR_RIGHT);
        sleep(0.6);
        diff = DIFF_EYE;
        color = COLOR;
 
        if(diff+color < menor)
          menor = diff+color;
 
        if(300+diff-color > maior)
          maior = 300+diff-color;
 
    }    
 
    printf("%d %d\n", menor, maior);
 
    COUNTS_LEFT = COUNTS_RIGHT = 0;
 
    while(1){
        motor(MOTOR_LEFT, -30);
        motor(MOTOR_RIGHT, 30);
        sleep(0.5);
        off(MOTOR_LEFT);
        off(MOTOR_RIGHT);
        sleep(1.2);    
        diff = DIFF_EYE;
        color = COLOR;
 
        //Sensor diferencial tendera para zero
        //Sensor normal tendera para zero
        if(light == LIGHT_VERTICAL && diff+color <= menor+20)
          break;
 
        //Sensor diferencial tendera para 255
        //Sensor normal tendera para zero
        if(light == LIGHT_HORIZONTAL && 300+diff-color >= maior-20)
          break;
 
        printf("%d %d", diff+color, 300+diff-color);
    }
 
    sleep(1.0);
 
    //Fazer direcionamento diferencial
    fwd(100, 0);
}

Identificador de Cores

O robô permanece piscando o LED RGB nas 3 cores, a fim de identificar se há ou não um bloco na frente dele. Se houver, ele para.

void waitForObject(){
    int On, Off;
    do{ 
        COLOR_ALL;
        sleep(0.2);
        On = COLOR;
 
        COLOR_OFF;
        sleep(0.2);
        Off = COLOR;
 
        printf("%d\n", Off-On);
    }while((Off-On) < OBJECT_LIMIAR);
}

Quando ele encontra um objeto ele deve descobrir sua cor. Ele o faz emitindo a luz Vermelha, Verde e Azul, uma de cada vez, e mede a resposta do objeto para cada cor.

int getColor(){
    float val;
    float max = 0.0;
    float r, g, b;
    int color = NONE;
    
    //read color red
    COLOR_R;
    sleep(EX_TIME);
    r = (float) COLOR;
    
    //read color green
    COLOR_G;
    sleep(EX_TIME);
    g = (float) COLOR;
    
    //read color blue
    COLOR_B;
    sleep(EX_TIME);
    b = (float) COLOR;
    
    COLOR_OFF;
    
    //take unitary vector
    val = sqrt(r*r + g*g + b*b);
    r /= val;
    g /= val;
    b /= val;
    
    //compare unitary vector with each color
    val = r*Blue[0]  + g*Blue[1]  + b*Blue[2];
    if(max < val){ color = BLUE; max = val; }
    
    val = r*Red[0]  + g*Red[1]  + b*Red[2];
    if(max < val){ color = RED; max = val; }
    
    val = r*Green[0]  + g*Green[1]  + b*Green[2];
    if(max < val){ color = GREEN; max = val; }
    
    val = r*Yellow[0]  + g*Yellow[1]  + b*Yellow[2];
    if(max < val){ color = YELLOW; max = val; }
    
    return color;
}

Ele tem um conjunto de respostas diferentes para cada cor. Com isso, é possível definir um valor(val) que represente cada uma das cores de blocos (AMARELO, AZUL, VERDE e VERMELHO).

float Blue[3]   = {0.727910382, 0.579019622, 0.367263874};
float Red[3]    = {0.515920296, 0.654821915, 0.552299292};
float Green[3]  = {0.726601686, 0.403281319, 0.556250095};
float Yellow[3] = {0.558124347, 0.524040417, 0.643334171};

Caracterização dos Sensores

Os testes foram feitos com os blocos de cores AMARELO, AZUL, VERDE e VERMELHO. Alguns desses gráficos serão mostrados como exemplo.

A resposta para o Bloco Amarelo:

Sem influência de iluminação: E com iluminação:

E a influência dos motores.

Sem influência de iluminação: E com iluminação:

Enquanto para o Bloco Verde obtém-se a seguinte resposta:

Sem influência de iluminação: E com iluminação:

E a influência dos motores.

Sem influência de iluminação: E com iluminação:

Pelos gráficos é possível perceber que a luz interfere na percepção das cores e que a atuação dos motores também. O primeiro porque a presença de luz ambiente altera a leitura dos sensores, e o segundo porque o uso dos motores “puxa” muita corrente de handyboard.

Distância

E a resposta à medida que a distancia aumenta, 15mm, 25mm, 35mm:

Bloco Amarelo: Bloco Verde:

Nota-se que a resposta se torna muito menos precisa com a variação da distância entre o bloco e o robô/sensor.

Tarefa

A tarefa consiste em fazer algumas ações: além de seguir a luz polarizada e andar em cima da linha, o robô deveria também, uma vez identificada a cor do bloco a sua frente, desviar pela esquerda caso o bloco fosse azul, pela direita caso amarelo, empurrar o bloco se fosse verde e para se o bloco fosse vermelho.

void lf(float x){
 
    int color;
 
    COLOR_OFF;
 
    pid_fl = start_process(linefollow());
    waitForObject();
    kill_process(pid_fl);
    stopfollowline();
    color = getColor();
    switch(color){
 
        case BLUE:
          printf("blue\n");
        bkw(BACK_DIST, 0);
        fwd(TURN_DIST, -MOTOR_POWER); 
        _pfwd = start_process(fwd(INF, +ROUND_INTENSITY));
        while(LINEFOLLOW_LEFT == WHITE);
        while(LINEFOLLOW_LEFT == BLACK);
        kill_process(_pfwd);
        stop();
        lf(x);            
        break;
 
        case GREEN:
          printf("green\n");
        _pfwd = start_process(linefollow());
        sleep(x);
        kill_process(_pfwd);
        stopfollowline();
        break;
 
        case YELLOW:
          printf("yellow\n");
        bkw(BACK_DIST, 0);
        fwd(TURN_DIST, MOTOR_POWER); 
        _pfwd = start_process(fwd(INF, -ROUND_INTENSITY));
        while(LINEFOLLOW_RIGHT == WHITE);
        while(LINEFOLLOW_RIGHT == BLACK);
        kill_process(_pfwd);
        stop();
        lf(x);            
        break;
 
        case RED:
          printf("red\n");  
        break;
 
        case NONE:
          printf("none\n");
        break;
    }
}