cursos:introrobotica:2011-2:grupo05:tp3

TRABALHO PRÁTICO 3

O trabalho prático 3, necessitou de medidas auxiliares para que o robô pudesse realizar suas atividades. Além de seguir-linha (1), localizar-se (2) e identificar cores (3), o robô deve ser capaz de lançar objetos esféricos por meio de uma catapulta (4), realizar um controle deliberativo para desviar de obstáculos (5), reconhecer o sinal luminoso de partida (6) e utilizar um processo que interromperá o seu funcionamento após 60 segundos (7). Todas estas foram bem desenvolvidas, apesar das limitações como veremos adiante.

—-

A plataforma de mecanismos utilizada no desenvolvimento dos dois primeiros trabalhos práticos foi modificada com o objetivo de reduzir massa e dimensões totais. Essa modificação foi essencial para economizar peças do kit LEGO que estavam se tornando escassas.

A primeira modificação foi deslocar a HandyBoard para uma posição de menor altura e simetricamente acima do eixo de tração. Dessa forma, deslocamos o C.G. do robô para mais próximo do chão e do eixo de tração, o que contribuiu para aumento da estabilidade e redução dos desvios da odometria.

A segunda modificação foi remover a estrutura frontal, e refazê-la adicionando reforços estruturais de forma mais eficiênte e econômica.

A terceira modificação foi desnvolver uma plataforma de lançamento modular, que pudesse ser facilmente montada e removida e que lançasse prejeteis para o campo adversário. Algumas peças economizadas com as modificações anteriores foram fundamentais nesta fase.

O resultado foi uma economia significativa de peças, redução das dimensões e do peso (sem catapulta). Entretanto, as mudanças de design afetaram os parâmetros do Controle PD, que deverão ser ajustados.

—-

Catapulta

A primeira tarefa foi construir uma catapulta modular, de fácil montagem e desmontagem, capaz de lançar objetos no campo adversário e se rearmar automaticamente. Depois de avaliar diversos modelos e formas de implementação, optamos pelo modelo de rotação contínua. Neste método, duas rodas giram em sentidos opostos para comprimir o projétil e lançá-lo.
Essa escolha ocorreu por dois motivos principais. Capacidade de ajuste do ângulo de lançamento e capacidade de ajuste da força de lançamento.
A primeira tentativa utilizava 1 motor conectado aos eixos de rotação para lançamento por correntes. Entretanto, essa montagem se mostrou pouco eficiente, 1 único motor não era capaz de girar a montagem completa.

A montagem sob plataforma possibilitou que o ângulo de lançamento pudesse ser escolhido antes de qualquer operação, o que permite atingir tanto objetos próximos ao solo, quanto objetos mais altos.

A força de lançamento é proporcional à potência dos motores, e pode ser alterada por software em tempo de execussão.

A exatidão dos lançamentos é relativamente boa para um alvo de cerca de 30 cm de diâmetro. A precisão não é boa, mas é aceitável, diante das limitações.
A exatidão dos lançamentos foi considerada boa em comparação com outros grupos da disciplina.
Em um benchmarking durante a apresentação do TP3, verificamos que a catapulta desenvolvida garantiu a segunda maior distância de lançamento, além de ser a única com regulagem rápida do ângulo de lançamento.

Partida e Finalização

Para identificar a luz de partida, utilizou-se um terceiro par de emissor/receptor de luz. O mesmo utilizado com sucesso no algoritmo de seguir linha.
Ao invés de conectar o novo sensor à entrada digital, utilizou-se a entrada analógica. A variação obtida entre o piso branco (12) e a luz acesa (5) foi de 7 unidades de fundo de escala da conversão AD, mais que o suficiente para identificar o acendimento da luz, ja que a operação funcionou em 29 de 30 testes realizados. A única falha ocorreu devido ao travamento de outra parte do código e já foi corrigida.
As figuras abaixo ilustram a chave ótica utilizada, assim como a montagem realizada pela equipe.

Para a finalização e imobilidade em 60 segundos, foi inserido um novo processo contador que encerra todas as atividades após o referido tempo.

Wavefront

O controle deliberativo foi realizado a partir do algoritmo Wavefront, com algumas modificações.
A matriz de campo foi dividida em quadrados de 30 cm. O entorno do campo foi preenchido com obstácuos, representados pelo número 255. O espaço disponível para o cálculo ficou então restrito às dimensões físicas reais.

Definido o espaço de atuação, o próximo passo foi gravar a posição inicial (Amarelo), a desejada (Verde) e os demais obstáculos (255), recebidos pelo menu à vontade do usuário..

A partir daí, calcula-se o caminho numerando as células do destino até o ponto de início. Os pontos não preenchidos são preenchidos com obstáculos. O caminho a seguir é sempre para o menor número visinho, até chegar ao destino.

Conclusão

As novas implementações cumpriram com os requisitos e provavelmente serão de grande ajuda na competição.
A reestruturação do robô permitiu economia de peças, reforço estrutural e deslocamento do C.G. para mais próximo do eixo e do chão, o que gerou ganho de estabilidade e redução de peso. Porém, essas alterações culminaram na necessidade de recalibração do Controle PD para linha reta.
O controle deliberativo foi corrigido e implementado com sucesso.
A catapulta foi capas de lançar bolinhas à boas distâncias, além de permitir controle de ângulo e forca de lançamento.
As taferas de partida luminosa e finalização em 60 segundos também foram implementadas com sucesso.

hbmenu.ic

/****************************** 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
 
int min(int x, int y)
{ if (x > y)
      return y;
    else
      return x;
}
 
/*****************************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:
 */

wavefront.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 4
#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 motor(int m, int p) {}
//void ao() {}
//void msleep(long time) {}
 
 
 
void push(int x, int y, int valor) {
    if (x >= 0 && x < X_DIM && y >=0 && y < Y_DIM && map[x][y] == 0 && indice < 100) {
        //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) {
        if (indice2 < 100) {
    *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;
 
    printf("Calc Wave\n");
    msleep(1000L);
 
/*        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], 2);
    x = pose_desejada[0], y = pose_desejada[1];
    while (!stop) {
        pop(&x , &y);
        if (x < X_DIM-1 && y < Y_DIM-1)// (-2) pois sera somado 1 em cada
          { 
            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; 
 
    printf("Out Wave\n");
    msleep(1000L);
}
 
/* 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;
        if(yr>0) yr = y - 1;
    }
    if (pose_atual[2] == NORTE) {
        ya = y + 1;
        if(xr>0) xr = x - 1;
        xl = x + 1;
    }
    if (pose_atual[2] == OESTE) {
        if(xa>0) xa = x - 1;
        if(yl>0) yl = y - 1;
        yr = y + 1;
    }
    if (pose_atual[2] == SUL) {
        if(ya>0) ya = y - 1;
        xl = x + 1;
        if(xr>0) xr = x - 1;
    }
    printf("Calc direc\n");
    msleep(1000L);
 
    if(xa>=X_DIM)
      {
        xa = X_DIM - 1;
    }
    if(xl>=X_DIM)
      {
        xl = X_DIM - 1;
    }
    if(xr>=X_DIM)
      {
        xr = X_DIM - 1;
    }
    if(ya>=Y_DIM)
      {
        ya = Y_DIM - 1;
    }
    if(yr>=Y_DIM)
      {
        yr = Y_DIM - 1;
    }
    if(yl>=Y_DIM)
      {
        yl = Y_DIM - 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;
}
 
/* Le um valor do knob variando de 0 at intervalo*/
int getKnob(char texto[], int intervalo, int set_camp) {
    int valor;
    while (!start_button()) {
        if(set_camp) valor = ((intervalo * knob()) / 255) + 1;
        else 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-2, 0);
    pose_inicial[1] = getKnob("Y inicial: ", Y_DIM-2, 0);
    posicao = getKnob("Direcao inicial: ", 3, 0);
 
    if(posicao == 0)
      pose_atual[2] = LESTE;
    else if(posicao == 1)
      pose_atual[2] = NORTE;
    else if(posicao == 2)
      pose_atual[2] = OESTE;
    else if(posicao == 3)
      pose_atual[2] = SUL;
 
    pose_atual[0] = pose_inicial[0];
    pose_atual[1] = pose_inicial[1];
 
    pose_desejada[0] = getKnob("X desejado: ",X_DIM-2, 0);
    pose_desejada[1] = getKnob("Y desejado: ",Y_DIM-2, 0);
}
 
void escolheMapa() {
    int x, y;
    int n, i, j;
 
    for (j = 0; j < Y_DIM; j++)
      for (i = 0; i < X_DIM; i++)
        map[i][j] == 0; 
 
    n = getKnob("Numero de obstaculos: ", 20, 0);
    for (i = 0; i < n; i++) {
        x = getKnob("X obstaculo: ",X_DIM-2, 0);
        y = getKnob("Y obstaculo: ",Y_DIM-2, 0);
        map[x][y] = MAX;
    }
}

wf_movements.ic

/*
Introducao a Robotica - TPs 1, 2 e 3
Grupo MIBR (Made in Brasil)
 
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
 
Modulo responsavel pelos movimentos do robo!
 
*/
 
//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
 
#define ROUNDTIME 7468                         //Tempo de 360 graus em milisegundos
 
#use "wavefront.c"
//#use "hbmenu.ic"
 
//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
    }
}
 
//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,80);
        motor(MOTOR_2,100);
        encid1 = 20;
        encid2 = 20;    
 
        pid = start_process(PDControl(&encid1, &encid2, MOVE));
    }
    else{
        printf("Indo pra Tras!\n");    //Imprime Status
        StartMotors(-100,-80);         //Liga os motores   
        x = -x;
    }
 
    //    while (!stop_button())                   //Enquanto dist<x e nao pressionar stop
    //  { 
    if (pose_atual[2] == LESTE) {printf("Lest\n");sleep(1.0);
        pose_atual[0]--;    }
    if (pose_atual[2] == NORTE){printf("North\n");sleep(1.0);
        pose_atual[1]++;}
    if (pose_atual[2] == OESTE){printf("West\n");sleep(1.0);
        pose_atual[0]++;}
    if (pose_atual[2] == SUL){printf("South\n");sleep(1.0);
        pose_atual[1]--;}
    while ((dist<x))                   //Enquanto dist<x e nao pressionar stop
      { 
        //        PDControl(20,20,&erroant1,&erroant2,MOVE); 
     if(stop_button()) break;   
        encsum = encsum + read_encoder(0) + read_encoder(1);         //soma acumulativa dos encoders
 
        //        reset_encoder(0);
        //      reset_encoder(1);
 
        dist = (float) ((encsum)/2)*DX;  
        //O tempo deve ser ajustado neste ponto de maneira a proporcionar a correta contagem do pose_atual
        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();
}
 
void ControlTime(long time, int *round)
{
    *round = 0;
    msleep(time);
    StopMotors();
    *round = 1;
}
 
//Gira para a direo determinada 360/arc graus
void Gira(float arc, int direction)
{
    //variveis controle de angulo e pd dos motores
    int encid1, encid2;
    int pid = 0, pid1=1;
    float dist1=0.0, dist2=0.0;
    int  erroant1=0, erroant2=0,encsum1=0,encsum2=0;
    int round = 0;
 
    printf("Gira %f para ", 360.0/arc);    //Imprime Status
 
    //inicializao data para deslocamento
    reset_encoder(0);
    reset_encoder(1);
    encid1 = 18;
    encid2 = 18;
 
    if (direction==RIGHT)
      {
        pose_atual[2] -= (int)(360.0/arc);
        if (pose_atual[2] == -(int)(360.0/arc))
          pose_atual[2]= 360-(int)(360.0/arc);    
 
        printf("Direita!\n");     //Imprime Status
        pid = start_process(PDControl(&encid1, &encid2, RIGHT));
    }
    else if(direction==LEFT)
      {
        pose_atual[2] += (int)(360.0/arc);
        if (pose_atual[2] == 360)
          pose_atual[2]= 0;    
 
        printf("Esquerda!\n");     //Imprime Status
        pid = start_process(PDControl(&encid1, &encid2, LEFT));
    }
    pid1 = start_process(ControlTime((long)(ROUNDTIME/(int)(arc)), &round));
    msleep(30L);
    //controle da distancia percorrida
    while(!stop_button() && !round)    //volta determinada pela proporso pedida
      {         
        //Caso for pedido 360, ele far a avaliao da luz ambiente a cada 45 graus
 
        //contabiliza deslocamento
        encsum1 = encsum1 + read_encoder(0);
        encsum2 = encsum2 + read_encoder(1);
 
        //reseta encoder para contabilizao apartir daquele tempo
        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);
        //msleep(100L);
    }
    kill_process(pid);
    kill_process(pid1);
 
    StopMotors();             //Desliga os motores
 
}
 
//Faz circulo ou arco, proporcional a arc, de raio x
void Circle(float x, float n_rounds, float arc, int direction)
{
    float dist=0.0;
    float L1=0.0,L2=0.0;
    int encsum=0,erroant1=0,erroant2=0;
    int encid1, encid2;
    int pid=0;
    //    pid = start_process(missionImpossible());
 
    L1 = x-(BITOLA/2.0);   //Comprimento do arco da roda interna [mm]
    L2 = x+(BITOLA/2.0);   //Comprimento do arco da roda EXTERNA [mm]
 
    //inicializao data para deslocamento
    reset_encoder(0);
    reset_encoder(1);
    if(direction==LEFT)
      {
        encid1 = 20;
        encid2 = (int)(20.0*(L1/L2)+0.5);
    }
    else
      {
        encid1 = (int)(20.0*(L1/L2)+0.5);
        encid2 = 20;
    }
    printf("%d %d", encid1, encid2); sleep(2.0);
    pid = start_process(PDControl(&encid1, &encid2, MOVE));
 
    while((dist<=(n_rounds*(2.0*PI*x))/arc))
      { 
 
        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
 
        if(stop_button())
          break;
 
    }
    //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
    dist = (float)(encsum/2)*DX;    //Calcula a distancia percorrida
    printf("Dist.: %f mm\n",dist);       //Imprime distancia percorrida
 
    reset_encoder(0);
    reset_encoder(1);
 
}

directions_sensor.ic

/*
Introducao a Robotica - TPs 1, 2 e 3
Grupo MIBR (Made in Brasil)
 
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
 
Modulo responsavel pelos sensores de direcao (Infra-vermelhos que mantem o robo na linha, 
e o diferencial que direciona para luz polarizada vertical e horizontal)!
 
*/
 
 
#define DIFF 3
#define VERTICAL 1
#define HORIZONTAL 2
 
#use "wf_movements.ic"
 
//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
}
 
//Testa valores de LDR
//Mais iluminado, maior o valor.
void LDR_test(/**/)
{
    while(!stop_button()) {
        printf("LDR: %d\n", light(2)); 
        msleep(100L);
    }
}
 
 
//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)
{
    //variaveis de controle
    int iteracao=0;
    int encid1, encid2;
    int pid = 0, pid1 = 1;
    int direction = 0, correct = 0;
    int i = 0;
    float results[8];
 
    //Gira e capta medias de valores   
    Gira(1.0, RIGHT);
 
    //faz avaliao da melhor direo, de acordo com a luz desejada
    for(i=1; i<8; i++)
      {
        if((polo==VERTICAL && results[0] > results[i]) ||(polo==HORIZONTAL && results[0] < results[i]))
          {
            iteracao = i;
            results[0] = results[i];
        }
    }
 
    //faz apurao da direo, para corrigir o erro natural(robo gira mais q o angulo pedido)
    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)// se houve correo...
      {
        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());
 
    //anda em direo  luz, enquanto stop no  precionado
    i=0;
    while(!stop_button())
      {
        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 a direao esta sendo correta, a cada 3s, caso contrario corrige
            if((polo==VERTICAL&&(results[0]*0.99)<=results[1]) ||(polo==HORIZONTAL &&(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();
 
}
 
//segue a linha atraves dos chaves opticas-reflexivas
void FollowLine()
{
    int pid=0, pid1=1;
    int encid1, encid2;
 
    encid1 = 20;
    encid2 = 20;
 
    //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        
 
    }
    kill_process(pid);
    kill_process(pid1);
    StopMotors();
}
//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;
}

color.ic

/*
Introducao a Robotica - TPs 1, 2 e 3
Grupo MIBR (Made in Brasil)
 
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
 
Modulo responsavel pelo detector de cores!
 
*/
 
#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
 
#define STARTLIGHT   8                  // Limiar de detecao da luz de partida do campo (ja foi calibrado)
 
#use "directions_sensor.ic"
 
//Global variables
int stop=0;
int count=0;
 
/*
void LineDetectColor()
{
    int pid=0;
    int lamb;
 
    lamb=light(2);
 
    pid = start_process(FollowLine());
 
 
 
    while(1)
      {
        if(light(2)<(int)(0.58*(float)lamb))
          {
            StopMotors();
            kill_process(pid);
            break;   
        }
    }
 
    //detecta cor dos blocos
    DetectColor();
}
 
//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<ldrred-ldramb)
      ldrmaior=ldrred-ldramb;
    if(ldrmaior<ldrgreen-ldramb)
      ldrmaior=ldrgreen-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
    Gira(4.0, LEFT);
    Go(200.0);
    Gira(4.0, RIGHT);
    Go(500.0);
    Gira(4.0, RIGHT);
    Go(200.0);
    Gira(4.0, LEFT);
}
 
void ColorTurnRight()
{
    int i;
    //Volta alguns centimetros
    //Go(-50.0);
    motor(MOTOR_1,-100);
    motor(MOTOR_2,-100);
    sleep(2.5);
    //Contorna
    Gira(4.0, RIGHT);
    Go(200.0);
    Gira(4.0, LEFT);
    Go(500.0);
    Gira(4.0, LEFT);
    Go(200.0);
    Gira(4.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;
    }
 
}
 
//Reconhece acendimento da luz de partida
int StartLight()
{   int start=0;
    start=analog(3);
    //printf("READY! - IR:%d\n", start);
    msleep(10L);
    if(start<STARTLIGHT) return 1;
    else return 0;
}
 
//Contador de vida do robo (60 seg no TP3)
void LifeTime()
{ 
    sleep(60.0);
    alloff();
    stop=1;    
}

MIBR_TP3.ic

/*
Introducao a Robotica - TP 3
Grupo MIBR (Made in Brasil)
 
Filipe Silva Silveira
Igor Monteiro
Rafael Mizerani C. Moreira
 
Main Module
 
*/
 
#use "color.ic"
//"color.ic" includes glogal stop variable
 
void main()
{
    float results[8];
    int direcao;
    int start=0;
    int pid_time;
 
    // 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);  
 
    printf("Equipe MIBR\n");
    sleep(1.0);
 
    escolhePosicao();
    escolheMapa();
 
    printf("Comecar em [%d %d], alvo: [%d %d]\n", 
           pose_inicial[0], pose_inicial[1],
           pose_desejada[0], pose_desejada[1]);
 
    calculaWaveFront();
    //printMap();
 
    printf("Press start or turn lights on!\n");
    sleep(1.0);
    printf("READY!\n");
    sleep(1.0);
 
    //Enqunto Luz nao acesa, espera...
    while(!start_button() && start==0) {
        start=StartLight();
    }
    //start process count 60 seconds
    pid_time=start_process(LifeTime());
 
    while (!stop) {
        direcao = calculaDirecao();
        if (direcao == ESQUERDA)
          Gira(4.0, LEFT);
        if (direcao == DIREITA) 
          Gira(4.0, RIGHT);    
        Go(300.0);
 
        printf("Estou em [%d %d]\n", pose_atual[0], pose_atual[1]);
        msleep(1000L);
        //Verifica se ja chegou no alvo
        if (pose_atual[0] == pose_desejada[0] && 
            pose_atual[1] == pose_desejada[1])
          stop = 1;
    }
    kill_process(pid_time);
 
}