Trabalho Prático 3

Neste trabalho, o robô construído foi adaptado para executar três novas tarefas: (1) o lançamento de uma bola leve em direção a uma fonte de luz polarizada; (2) o deslocamento através de um campo conhecido utilizando-se um algoritmo wavefront; (3) simultâneas a essas tarefas, a pausa total das atividades após 60 segundos de execução. Para a execução da tarefa três, em especial, foram utilizadas técnicas de programação paralela, e um novo motor foi acrescentado ao robô para a tarefa um. Os trabalhos foram feitos com sucesso, cumprindo os objetivos propostos, e as maiores dificuldades encontradas foram acerca da correta execução do algoritmo wavefront.

Diversas mudanças estruturais foram feitas no robô, para garantir o comprimento das tarefas. A catapulta, descrita adiante, foi colocada, e a parte responsável por detectar a presença e a cor de blocos foi removida. Diversas alterações estruturais gerais foram feitas no robô, para que ele comportasse a nova estrutura da catapulta.

As rodas do robô também foram substituídas por rodas maiores, que dão mais velocidade.

Uma vez que o mecanismo de localização da fonte de luz polarizada já foi criado e implementado com sucesso no trabalho passado, bastou criar a catapulta e o algoritmo para fazer o lançamento da bola na direção correta. Para tal, acrescentou-se ao robô um novo motor que, através do uso de três reduções, movimentava uma alavanca que, por sua vez, movimentava o braço da catapulta, preso à estrutura do robô por elásticos em sua parte superior, e através de conexões de lego em sua parte inferior. Tal alavanca, ao continuar seu giro, acabava por soltar o braço da catapulta tensionado, de modo a retornar a sua posição inicial rapidamente, movimento que dava à bola o impulso necessário para chegar à lâmpada polarizada.

Como resultado, obteve-se uma catapulta que conseguia atirar a bola com força e precisão razoáveis, fazendo com que esta alcançasse uma boa distância.

O robô, para esta tarefa, precisava se deslocar por um campo conhecido de maneira autônoma, determinando o caminho mais curto do ponto de partida até o alvo, para alcançá-lo posteriormente. Para a execução da tarefa, utilizou-se o algoritmo wavefront, “comprimento de onda”, que divide o ambiente em um grid, ou seja, mapeia-o em uma matriz, onde cada posição da matriz representa um quadrado do campo de deslocamento. Neste caso, o quadrado representado tinha 30cm de lado. Após isso, atribui-se a cada posição onde existam obstáculos, e nas bordas do campo, valores numéricos extremamente altos. Em seguida, a partir do ponto de chegada, atribuem-se valores crescentes às casas adjacentes a ela, progressivamente, até alcançarem o robô ou algum obstáculo: se existem 3 casas entre o robô e o alvo, elas serão numeradas 1, 2 e 3, respectivamente, e a saída será 0. Se entre a saída e o robô, acima da casa de número 2, existe um caminho livre, cercado de obstáculos, ele também será numerado 3.

O robô, então, move-se sempre para a casa adjacente a ele que possua o menor número possível, até alcançar o alvo. Além disso, o algoritmo trata uma situação importante, em que o robô começa cercado por todos os lados, menos pelas costas, por obstáculos. Nesse caso, o robô irá detectar a situação, girar 180 graus e se movimentar normalmente. Parte do algoritmo aqui explicado compõe o algoritmo de controle deliberativo, disponível na especificação do trabalho. O algoritmo que permite ao robô se deslocar em linha reta e girar 90 graus utilizado foi o mesmo dos trabalhos anteriores.

Para que esta tarefa fosse executada independente do que o robô estivesse fazendo, surgiu a necessidade de executá-la em paralelo em relação ao restante do algoritmo do robô. Isso foi feito utilizando-se a biblioteca pthreads. Como resultado, a trava dos sessenta segundos foi implementada com sucesso, funcionando independentemente do estado atual do robô.

#include sencdr4.icb
 
#include sencdr1.icb
 
 
 
#define ESQUERDA 1
 
#define FRENTE 0
 
#define DIREITA -1
 
#define ATRAS -2
 
 
 
#define LESTE 0
 
#define NORTE 90
 
#define OESTE 180
 
#define SUL 270
 
 
 
#define X_DIM 7
 
#define Y_DIM 6
 
 
 
#define MAX 255
 
 
 
#define SEN_START 14
 
 
 
#define MOTOR_L 0
 
#define MOTOR_R 2
 
#define MOTOR_C 3
 
#define SENS_DIF 6
 
#define SENS_CDR 2
 
 
 
#define TURN_ANGLE 145
 
 
 
/* Mapa contendo os obstaculos do ambiente */
 
int map[X_DIM][Y_DIM];
 
 
 
/* Pose = posicao (x y) + orientacao */
 
int pose_atual[3];    // posicao (x,y) atual + orientacao (N / S / L / O)
 
int pose_inicial[3];  // posicao inicial
 
int pose_desejada[3]; // posicao final desejada (ignorar a orientacao)
 
 
 
/* Variaveis extras*/
 
int fifo[100][2];
 
int indice = 0;
 
int indice2 = 0;
 
 
 
void catapulta(){
 
    motor(MOTOR_C, 100);
 
    msleep(10000L);
 
    off(MOTOR_C);
 
}
 
 
 
 
 
//void motor(int m, int p) {}
 
//void ao() {}
 
//void msleep(long time) {}
 
 
 
void segue_luz_vertical(){
 
    int light, polarizada, min=1000, value,valuec, count;
 
 
 
    motor(MOTOR_R, -30);
 
    motor(MOTOR_L, 30);
 
    count = 0;
 
    while(count < 300){
 
        value = analog(SENS_DIF);
 
        valuec = analog(SENS_CDR);
 
        if(min > value+valuec){
 
            min = value+valuec;
 
        }
 
        msleep(50L);
 
        count++;
 
        printf("%d %d\n",min,count);
 
    }
 
 
 
    printf("saiiiii\n");
 
    count = 0;
 
    min = min + 10;
 
 
 
    while(1){
 
        value=analog(SENS_DIF);
 
        valuec=analog(SENS_CDR);
 
        printf("%d %d\n",min,value);
 
        if( value+valuec< min){
 
            ao();
 
            msleep(50L);
 
            catapulta();
 
            break;
 
        }
 
 
 
        msleep(50L);
 
    }
 
 
 
    return;
 
}
 
 
 
void segue_luz_horizontal(){
 
    int light, polarizada, min=0, value, valuec, count;
 
 
 
    motor(MOTOR_R, -30);
 
    motor(MOTOR_L, 30);
 
    count = 0;
 
    while(count < 300){
 
        value = analog(SENS_DIF);
 
        valuec = analog(SENS_CDR);
 
        if(min < 1000 + value - valuec){
 
            min = 1000 + value - valuec;
 
        }
 
        msleep(50L);
 
        count++;
 
        printf("%d %d\n",min,count);
 
    }
 
 
 
    printf("saiiiii\n");
 
    count = 0;
 
    min = min - 10;
 
    while(1){
 
        value=analog(SENS_DIF);
 
        valuec = analog(SENS_CDR);
 
        printf("%d %d\n",min,value);
 
        if(1000 + value - valuec > min){
 
            ao();
 
            msleep(50L);
 
            catapulta();
 
            break; 
 
        }
 
        msleep(50L);
 
    }
 
    return;    
 
}
 
 
 
/* Esta funcao deve fazer o seu robo andar 1 unidade de distancia para  
 
 * frente e tambem deve atualizar a variavel pose_atual, de acordo
 
 * com a orientacao atual
 
 */
 
void goAhead() {    
 
    if (pose_atual[2] == LESTE) 
 
      pose_atual[1]++;    
 
    if (pose_atual[2] == NORTE)
 
      pose_atual[0]--;
 
    if (pose_atual[2] == OESTE)
 
      pose_atual[1]--;
 
    if (pose_atual[2] == SUL)
 
      pose_atual[0]++;
 
 
 
    andaPraFrente(320);
 
    //msleep(300L);
 
    off(MOTOR_L);
 
    off(MOTOR_R);
 
}
 
 
 
void giraRobo(int direction){
 
    int i, j;
 
 
 
    int rodou0;
 
    int rodou1;
 
    int app=0;
 
 
 
    int furos;
 
    int speed;
 
    int VMOTOR_0;
 
    int VMOTOR_1;
 
    int inv0;
 
    int inv1;
 
 
 
    inv0 = 0;
 
    inv1 = 0;
 
    rodou0 = 0;
 
    rodou1 = 0;
 
 
 
    furos = 3;
 
    speed = 50;
 
    encoder4_counts = 0;
 
    encoder1_counts = 0;
 
 
 
    VMOTOR_0 = speed;
 
    VMOTOR_1 = speed;
 
 
 
    i = 0;
 
 
 
 
 
    if(direction){
 
        inv1=1;
 
    } else {
 
        inv0=1;
 
    }
 
 
 
    if (inv0) VMOTOR_0 = -speed; 
 
    if (inv1) VMOTOR_1 = -speed;
 
 
 
    while(rodou0 < TURN_ANGLE ){
 
        i++;
 
 
 
        motor(MOTOR_L,VMOTOR_0);
 
        motor(MOTOR_R,VMOTOR_1);
 
 
 
        rodou0 += encoder4_counts;
 
        rodou1 += encoder1_counts;
 
 
 
        if (encoder4_counts < furos){
 
            if(inv0) VMOTOR_0 -= 15;
 
            else VMOTOR_0 += 15;
 
        } else if (encoder4_counts > furos){
 
            if(inv0)VMOTOR_0 += 15;
 
            else VMOTOR_0 -= 15;
 
        }
 
 
 
        if (encoder1_counts < furos){
 
            if(inv1) VMOTOR_1 -= 15;
 
            else VMOTOR_1 += 15;
 
        } else if (encoder1_counts > furos){
 
            if(inv1)VMOTOR_1 += 15;
 
            else VMOTOR_1 -= 15;
 
        }
 
 
 
        encoder4_counts = 0;
 
        encoder1_counts = 0;
 
 
 
        msleep(25L);
 
    }
 
 
 
    off(MOTOR_L);
 
    off(MOTOR_R);
 
    msleep(200L);
 
}
 
 
 
void andaPraFrente(int counts){
 
 
 
 
 
 
 
    int i, j;
 
 
 
    int rodou0;
 
    int rodou1;
 
    int app=0;
 
 
 
    int furos;
 
    int speed;
 
    int VMOTOR_0;
 
    int VMOTOR_1;
 
    int inv0;
 
    int inv1;
 
 
 
    inv0 = 0;
 
    inv1 = 0;
 
    rodou0 = 0;
 
    rodou1 = 0;
 
 
 
    furos = 6;
 
    speed = 80;
 
    encoder4_counts = 0;
 
    encoder1_counts = 0;
 
 
 
    VMOTOR_0 = speed;
 
    VMOTOR_1 = speed;
 
 
 
    if (inv0) VMOTOR_0 = -speed; 
 
    if (inv1) VMOTOR_1 = -speed;
 
 
 
    i = 0;
 
    while(rodou0<counts){
 
        i++;
 
 
 
        rodou0 += encoder4_counts;
 
        rodou1 += encoder1_counts;
 
 
 
        if (encoder4_counts < furos){
 
            if(inv0) VMOTOR_0 -= 4;
 
            else VMOTOR_0 += 4;
 
        } else if (encoder4_counts > furos){
 
            if(inv0)VMOTOR_0 += 4;
 
            else VMOTOR_0 -= 4;
 
        }
 
 
 
        if (encoder1_counts < furos){
 
            if(inv1) VMOTOR_1 -= 4;
 
            else VMOTOR_1 += 4;
 
        } else if (encoder1_counts > furos){
 
            if(inv1)VMOTOR_1 += 4;
 
            else VMOTOR_1 -= 4;
 
        }
 
 
 
        encoder4_counts = 0;
 
        encoder1_counts = 0;
 
 
 
        motor(MOTOR_L,VMOTOR_0);
 
        motor(MOTOR_R,VMOTOR_1);
 
 
 
        msleep(50L);
 
    }
 
    off(MOTOR_L);
 
    off(MOTOR_R);
 
 
 
    msleep(200L);
 
}
 
 
 
/* Esta funcao deve fazer o seu robo girar 90 graus para a esquerda e
 
 * tambem deve atualizar a variavel pose_atual 
 
 */
 
void turnLeft() {
 
    pose_atual[2] += 90;
 
    if (pose_atual[2] == 360)
 
      pose_atual[2]= 0;
 
 
 
    giraRobo(0);
 
    //msleep(300L);
 
    off(MOTOR_L);
 
    off(MOTOR_R);
 
}
 
 
 
/* Esta funcao deve fazer o seu robo girar 90 graus para a direita e
 
 * tambem deve atualizar a varivael pose_atual 
 
 */
 
void turnRight() {
 
    pose_atual[2] -= 90;
 
    if (pose_atual[2] == -90)
 
      pose_atual[2]= 270;
 
 
 
    giraRobo(1);
 
    //msleep(300L);
 
    off(MOTOR_L);
 
    off(MOTOR_R);
 
}
 
 
 
void push(int x, int y, int valor) {
 
    if (x >= 0 && x < X_DIM && y >=0 && y < Y_DIM && map[x][y] == 0) {
 
        //printf("fifo[%d] <= [%d %d]\n", indice, x, y);
 
        map[x][y] = valor;
 
        fifo[indice][0] = x;
 
        fifo[indice][1] = y;
 
        indice++;
 
    }
 
}
 
 
 
void pop(int *x, int *y) {
 
    *x = fifo[indice2][0];
 
    *y = fifo[indice2][1];
 
    indice2++;
 
    //printf("fifo[%d] => [%d %d]\n", indice2, *x, *y);
 
}
 
 
 
 
 
/* Esta funcao deve calcular o mapa de distancias usando o algoritmo
 
 * wavefront.
 
 * A variavel map deve conter as distancias entre o ponto final e inicial
 
 * ao final da sua execucao
 
 */
 
void calculaWaveFront() {
 
    int x,y;
 
    int i,j;
 
    int stop = 0;
 
 
 
    for (i = 0; i < X_DIM; i++) {
 
        map[i][0] = MAX;
 
        map[i][Y_DIM - 1] = MAX;
 
    }
 
    for (j = 0; j < Y_DIM; j++) {
 
        map[0][j] = MAX;
 
        map[X_DIM- 1][j] = MAX;
 
    }
 
 
 
    push(pose_desejada[0], pose_desejada[1], 1);
 
    x = pose_desejada[0], y = pose_desejada[1];
 
    while (!stop) {
 
        pop(&x , &y);
 
        push(x, y - 1, map[x][y] + 1);
 
        push(x, y + 1, map[x][y] + 1);
 
        push(x + 1, y, map[x][y] + 1);
 
        push(x - 1, y, map[x][y] + 1);   
 
        if (x == pose_inicial[0] && y == pose_inicial[1])
 
          stop = 1;
 
    }
 
 
 
    for (j = 0; j < Y_DIM; j++)
 
      for (i = 0; i < X_DIM; i++)
 
        if (map[i][j] == 0) map[i][j] = MAX;   
 
}
 
 
 
/* Esta funcao deve retornar a direcao que o robo deve seguir para sair
 
 * do quadrante atual para chegar ate o proximo quadrante, obedecendo
 
 * o mapa criado pelo algoritmo wavefront
 
 */
 
int calculaDirecao() {
 
    int x, y, xa, ya, xl, yl, xr, yr,yb,xb;
 
 
 
    x = pose_atual[0];
 
    y = pose_atual[1];
 
    xa = x;    ya = y;
 
    xr = x;    yr = y;
 
    xl = x;    yl = y;
 
    xb = x;    yb = y;
 
 
 
    if (pose_atual[2] == SUL) {
 
        xa = x + 1;
 
        xb = x - 1;
 
        yl = y + 1;
 
        yr = y - 1;
 
    }
 
    if (pose_atual[2] == LESTE) {
 
        ya = y + 1;
 
        yb = y - 1;
 
        xl = x - 1;
 
        xr = x + 1;
 
    }
 
    if (pose_atual[2] == NORTE) {
 
        xa = x - 1;
 
        xb = x + 1;
 
        yl = y - 1;
 
        yr = y + 1;
 
    }
 
    if (pose_atual[2] == OESTE) {
 
        ya = y - 1;
 
        yb = y + 1;
 
        xl = x + 1;
 
        xr = x - 1;
 
    }
 
 
 
 
 
    if ((xa > 0 && xa < (X_DIM - 1) && ya > 0 && ya < (Y_DIM - 1)) && map[xa][ya] < map[x][y])
 
      return FRENTE;
 
    if ((xl > 0 && xl < (X_DIM - 1) && yl > 0 && yl < (Y_DIM - 1)) && map[xl][yl] < map[x][y])
 
      return ESQUERDA;
 
    if ((xr > 0 && xr < (X_DIM - 1) && yr > 0 && yr < (Y_DIM - 1)) && map[xr][yr] < map[x][y])
 
      return DIREITA;
 
    if ((xb > 0 && xb < (X_DIM - 1) && yb > 0 && yb < (Y_DIM - 1)) && map[xb][yb] < map[x][y])
 
      return ATRAS;
 
}
 
 
 
/* Imprime o mapa na tela. util para realizar a depuracao (so funciona no PC).
 
 */
 
void printMap() {
 
    int i, j;
 
    for (j = 0; j < Y_DIM; j++) {
 
        for (i = 0; i < X_DIM; i++) {
 
            printf("%3d ", map[i][j]);       
 
        }
 
        printf("\n");
 
    }
 
}
 
 
 
/* Le um valor do knob variando de 0 ate intervalo*/
 
int getKnob(char texto[], int intervalo) {
 
    int valor;
 
    while (!start_button()) {
 
        valor = ((intervalo - 2) * knob()) / 255;
 
        valor += 1;
 
        printf("%s %d\n", texto, valor);
 
        msleep(100L);
 
    }
 
    while (start_button());
 
    return valor;
 
}
 
 
 
int getKnobString(char texto[][], int intervalo) {
 
    int valor;
 
    while (!start_button()) {
 
        valor = ((intervalo - 1) * knob()) / 255;        
 
        printf("%s\n", texto[valor]);
 
        msleep(100L);
 
    }
 
    while (start_button());
 
    return valor;
 
}
 
 
 
void escolhePosicao() {
 
    int posicao;
 
    pose_inicial[0] = getKnob("X inicial: ", (X_DIM - 1));
 
    pose_inicial[1] = getKnob("Y inicial: ", (Y_DIM - 1));
 
    pose_inicial[2] = getKnob("Direcao inicial LNOS: ", 5);
 
    posicao = pose_inicial[2];
 
 
 
    switch(posicao){
 
        case 1:
 
          pose_inicial[2] = LESTE;
 
        break;
 
        case 2:
 
          pose_inicial[2] = NORTE;
 
        break;
 
        case 3: 
 
          pose_inicial[2] = OESTE;
 
        break;
 
        case 4:
 
          pose_inicial[2] = SUL;
 
        break;
 
    }
 
 
 
 
 
    pose_atual[0] = pose_inicial[0];
 
    pose_atual[1] = pose_inicial[1];
 
    pose_atual[2] = pose_inicial[2];
 
    pose_desejada[0] = getKnob("X desejado: ",(X_DIM-1));
 
    pose_desejada[1] = getKnob("Y desejado: ",(Y_DIM-1));
 
}
 
 
 
void escolheMapa() {
 
    int x, y;
 
    int n, i;
 
    n = getKnob("Numero de obstaculos: ", 20);
 
    for (i = 0; i < n; i++) {
 
        x = getKnob("X obstaculo: ",(X_DIM-1));
 
        y = getKnob("Y obstaculo: ",(Y_DIM-1));
 
        map[x][y] = MAX;
 
    }
 
}
 
 
 
int waveFront(){
 
    int direcao, i, j;
 
    int stop = 0;
 
 
 
    calculaWaveFront();
 
    //printMap();
 
 
 
 
 
    while (!stop) {
 
        direcao = calculaDirecao();
 
 
 
        if (direcao == ESQUERDA){
 
            printf("esquerda: %d", direcao);
 
            //sleep(1.0);
 
            turnLeft();
 
        }
 
 
 
        else if (direcao == DIREITA){ 
 
            printf("direita: %d", direcao);
 
            //sleep(1.0);
 
            turnRight();
 
        }
 
 
 
        else if (direcao == FRENTE){ 
 
            printf("frente: %d", direcao);
 
            //sleep(1.0);
 
            goAhead();
 
        }
 
 
 
        else if (direcao == ATRAS){
 
            printf("atras: %d", direcao);
 
            //sleep(1.0);
 
            turnRight();
 
            turnRight();
 
        }
 
 
 
        printf("Estou em [%d %d]\n", pose_atual[0], pose_atual[1]);
 
 
 
        /* Verifica se ja chegou no alvo*/
 
        if (pose_atual[0] == pose_desejada[0] && pose_atual[1] == pose_desejada[1]){
 
            stop = 1;
 
            off(MOTOR_L);
 
            off(MOTOR_R);
 
        }
 
    }    
 
    return 0;
 
}
 
 
 
int main() {
 
    int i, j, th, selecao;
 
    char  string[2][40] = {"Executar        Catapulta", "Executar        Wavefront"};
 
    char  string2[2][30] = {"Polarizacao     Horizontal", "Polarizacao     Vertical"};
 
    //  motor(MOTOR_C, 100);
 
    selecao = getKnobString(string, 2);
 
 
 
    if (selecao == 0){
 
        selecao = getKnobString(string2, 2);
 
 
 
        printf("Esperando Luz\n");
 
        //while(!digital(SEN_START));
 
 
 
        if (selecao == 0) {
 
            th = start_process(segue_luz_horizontal());
 
            sleep(120.0);
 
            kill_process(th);
 
        } else {
 
            th = start_process(segue_luz_vertical());
 
            sleep(120.0);
 
            kill_process(th);
 
        }
 
 
 
        return 0;
 
 
 
    } else {
 
 
 
        for(i=0; i<X_DIM; i++){
 
            for(j=0; j<Y_DIM; j++){
 
                map[i][j] = 0;
 
            }
 
        }
 
 
 
        printf("Wavefront\n");
 
        msleep(1000L);
 
 
 
        escolhePosicao();
 
        escolheMapa();
 
 
 
        printf("Aperte start\n");
 
        while(!start_button());
 
 
 
        printf("Comecando em [%d %d], alvo: [%d %d]\n", 
 
               pose_inicial[0], pose_inicial[1],
 
               pose_desejada[0], pose_desejada[1]);
 
 
 
        while(!digital(SEN_START));
 
        printf("aq\n");
 
        th = start_process(waveFront());
 
 
 
 
 
        sleep(60.0);
 
        kill_process(th);
 
        printf("Acabou o tempo\n");
 
        off(MOTOR_L);
 
        off(MOTOR_R);
 
        return 0;
 
    }
 
}

Neste trabalho o robô previamente construído foi adaptado para a execução de três tarefas: o lançamento de uma bola em direção a uma luz polarizada previamente localizada, o deslocamento autônomo em um ambiente conhecido com obstáculos, e a criação de um mecanismo de trava que para o robô após sessenta segundos de execução. As maiores dificuldades encontradas estavam relacionadas com o algoritmo de deslocamento autônomo wavefront, utilizado neste trabalho. A trava dos sessenta segundos foi implementada com o uso de programação paralela. Todas as tarefas propostas foram executadas com sucesso.

MARTIN, Fred G. Interactive C User's Guide MIT Media Laboratory. Manual Edition 0.9. 1997.
MARTIN, Fred G. Robotic Explorations: An Introduction to Engineering Through Design. Prentice Hall; ISBN: 0-130-89568-7.
MARTIN, Fred G. The Handy Board Technical Reference. at http://handyboard.com/oldhb/techdocs/hbmanual.pdf
MARTIN, Fred G. The Art of LEGO Design. Journal for Robot Builders, volume 1, number 2. March 15, 1995