====== Relatório TP-3 ======

===== Introdução =====

Este trabalho possui 3 partes. Implementar um código de //WaveFront//, em que se passa um mapa para o robô e ele deve definir uma rota, de um ponto inicial a um final.

A implementação de uma função //startup// e a construção do sensor necessário. Pois quando uma luz acende na mesa, abaixo do robô, ele deve funcionar durante 60 segundos.

E a montagem de uma catapulta capaz de se armar automaticamente e que consiga arremessar um projétil ao outro lado do campo.


===== Construção da Catapulta =====
Nossa catapulta foi implementada como um estilingue. Para a sua construção foi utilizado um motor adicional, mas para que ele consiga "puxar" o elástico foi feita uma grande redução.

O motor gira uma alavanca e ela puxa o elástico pelas duas extremidades, ou seja, a cada 180° girados pela alavanca.

Para a agregação da catapulta ao robô não foram feitas grandes alterações em sua estrutura. Além do posicionamento da catapulta, foram modificados:
  * A posição dos sensores para a luz polarizada, para que não fosse escondido pela catapulta.
  * A base da //HandBoard// para diminuir o espaço utilizado a fim de caber a catapulta.
  * A substituição da roda pivô, por um //roll-on// pois o peso em cima da roda devido à catapulta se tornou muito grande e a roda começou a arrastar.

==== Redução ====
Foram feitas 4 reduções de 40/8.

===== WaveFront =====
<code c>
#define ESQUERDA 1
#define FRENTE 0
#define DIREITA -1

#define LESTE 0
#define NORTE 90
#define OESTE 180
#define SUL 270

#define X_DIM 10
#define Y_DIM 10

#define MAX 255

/* 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 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;
    
    x = pose_atual[0];
    y = pose_atual[1];
    xa = x;    xl = x;
    xr = x;    ya = y;
    yl = y;    yr = y;
    
    if (pose_atual[2] == LESTE) {
        xa = x + 1;
        yl = y + 1;
        yr = y - 1;
    }
    if (pose_atual[2] == NORTE) {
        ya = y + 1;
        xl = x - 1;
        xr = x + 1;
    }
    if (pose_atual[2] == OESTE) {
        xa = x - 1;
        yl = y - 1;
        yr = y + 1;
    }
    if (pose_atual[2] == SUL) {
        ya = y - 1;
        xl = x + 1;
        xr = x - 1;
    }
    if (map[xa][ya] < map[x][y])
      return FRENTE;
    if (map[xl][yl] < map[x][y])
      return ESQUERDA;
    if (map[xr][yr] < map[x][y])
      return DIREITA;
}

/* 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 at intervalo*/
int getKnob(char texto[], int intervalo) {
    int valor;
    while (!start_button()) {
        valor = (intervalo * knob()) / 255;
        printf("%s %d\n", texto, valor);
        msleep(100L);
    }
    while (start_button());
    return valor;
}

void escolhePosicao() {
    int posicao;
    pose_inicial[0] = getKnob("X inicial: ", X_DIM);
    pose_inicial[1] = getKnob("Y inicial: ", Y_DIM);
    
    pose_atual[0] = pose_inicial[0];
    pose_atual[1] = pose_inicial[1];
    
    pose_desejada[0] = getKnob("X desejado: ",X_DIM);
    pose_desejada[1] = getKnob("Y desejado: ",Y_DIM);
}

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);
        y = getKnob("Y obstaculo: ",Y_DIM);
        map[x][y] = MAX;
    }
}
</code>
===== StartUp =====

==== Sensor ====
Um LDR foi colocado em um peça Lego, que foi presa na parte inferior do robô.

==== Função ====

Enquanto espera pela luz de partida

<code c>
while(STARTUP_SENSOR > STARTUP_LIMIAR){
                    sleep(0.1);
                    printf("wating for start light! %d\n", STARTUP_SENSOR);
                }                
                printf("go\n");
                stopstartup();
                _pid_startup = start_process(startup());

<\code>

Depois que a luz é ativada:

<code c>
void startup(){
    sleep(60.0);
    stopmenu();
    _pid_menu = start_process(menu());
}
<\code>