• Nós, do grupo MIBR, no intuito de finalmente participar da competição de robôs ao final do segundo semestre de 2011, que foi realizada dia 12/12/2011, dedicamos boa parte do nosso tempo a fim de tentar concretizar as estratégias traçadas.
• A competição para a qual nos preparamos consistia em:

1. Orientar-se, ao inicio da partida, no campo através da luz polarizada;
2. Seguir em direção ao campo adversário;
3. Derrubar a torre dos inimigos;
4. Atravessar a ponte que liga os dois campos e passa sobre um “absmo”;
5. Capturar o rei, seus soldados e trazer para a prisão em seu campo.

• Após ter implementado sensores optico ativos, diferencial e optico reflexivos nos trabalhos anteriores, optamos por não utilizar o diferencial na competição. Tal decisão vem do fato de a qualidade da localização da luz cair com o quadrado da distância. Assim, realizamos a identificação da luz através do lado mais iluminado, feita pelo LDR utilizado no sensor optico ativo.
• Outra decisão importante foi relacionada a catapulta. Esta, implementada no TP3, seria de grande utilidade para derrubar as torres inimigas de longa distância, mas optamos por um meio de capturar o rei em detrimento desta estratégia. Nos próximos tópicos descrevemos mais detalhadamente cada passo, e a estratégia adotada.

• Como design final, continuamos por optar posicionar a handy-board em cima do eixo das rodas, a fim de, como já discutido em outros trabalhos, minimizar os erros obtidos na movimentação do robô (por exemplo, girar 90º em cima do eixo).

• Adotamos um reforço estrutural nas laterais a fim de interligar todas as peças do robô. Podemos visualizar esse reforço na figura a seguir.

• Sensores de toque foram implementados junto a peças lego (diferentes do KIT Lego fornecido) na traseira do robô a fim de normalizar a sua direção e sentido para atingir a estratégia que será citada posteriormente. Visualizamos então na figura a seguir o novo sensor implementado.

• Um sensor óptico-reflexivo, com emissor e receptor, foi implementado para uso de detecção da emissão de luz de partida para a competição, como podemos ver a seguir. É importante ressaltar que este sensor foi utilizado através das entradas analógicas da handy-board e se localizava embaixo do robô.

• Para coletar o rei inimigo, bloco amarelo ou vermelho, tivemos inicialmente a idéia de construir um braço simples que travaria o bloco analisado ao robô. Porém, ao evoluir do robô para o preparativo da competição, encontramos uma forma de com apenas um motor, construir uma garra que inicialmente deveria fechar e prender o bloco ao robô, como podemos ver a seguir.

• Então, quando a garra travasse, uma série de engrenagens travaria na parte posterior do seu “braço” e movimentaria a peça como um todo, como podemos ter uma idéia pela próxima figura.

• Inicialmente a estratégia foi discutida em cima dos pontos. De forma bem “simples” e bolamos as seguintes idéias iniciais.

1. Obter referências para encontrar o Norte. (Como? Luz polarizada? Um par emissor receptor em cada lado do robô seria 100%)
2. Capturar o próprio Rei. Guardá-lo preso ao robô. Evita que o adversário o capture e ganhe 8 a 10 pontos. Não tem problema derrubar as próprias torres depois da captura.
3. Continua reto, até que os dois sensores de linha vejam linha preta. Andar reto com precisão é fundamental e já conseguimos fazer.
4. Seguir e entrar no algoritmo segue-linha padrão até que os dois sensores vejam linha preta novamente. Marcar a distância pra saber se algo deu errado.
5. No fim da ponte (sabe que chegou no fim pela distância e pelos dois sensores no preto) continua reto até a posição do Rei inimigo (também pela distância).
6. Checa cor e captura o Rei inimigo. (+8). Com 8 pontos já é provável ganhar.
7. Derruba torres adversárias. (+2 x4) Com 16 pontos, é quase impossível perder.
8. Procura, na linha, por blocos inimigos (pela cor) até o tempo acabar. É importante não sair da linha para não correr o risco de cair no abismo. Com mais de 16 pontos é matematicamente o vencedor (pq o inimigo não pode capturar o nosso Rei).

• Porém, ao longo do desenvolvimento final do robô, e entre seus contratempos, tivemos de modificar a discussão inicial e bolar uma estratégia viável a tempo para a competição.

• A estratégia desenvolvida pela equipe focava de forma bem simples, realizar as tarefas como: detectar início da partida, se localizar, atravessar o campo sem derrubar as próprias torres, identificar a linha da ponte para seguir, seguir linha, identificar entrada do campo inimigo, descer a garra para sua utilização, alinhamento com as torres inimigas devido a posição fixa, derrubar as torres (direita e esquerda) e finalmente procurar através de movimentos aleatórios limitados pelas linhas pretas o rei-inimigo.

• No inicio da preparação para a competição nos deparamos com um problema, a handy board estava corrompendo o código constantemente. Tal defeito, após consulta ao monitor, foi identificado como possivel falha de código. Durante o periodo de implementação, ele diminuiu e ocorria somente quando havia sobre corrente. Entretanto, no dia da competição o defeito se tornou critico e identificamos que ao ligar o LDR do sensor optico ativo, se este fosse acionado, o código era corrompido.
• Conseguimos, faltando duas horas para a competição, uma outra HB que se demonstrou complentamente recepitiva ao código. Entretanto não tivemos tempo de recondificar para as novas adversidades de hardware, o que acarretou em um comportamento completamente errôneo na competição, levando o Catcher a cair no absmo e se auto destruir!

• O robô então ficou com seu design final bem robusto, prático e eficiente.

• Foram implementados códigos bem modulados. Os arquivos utilizados são: color.ic, Comp.ic, directions_sensor.ic, hbmenu.ic, MIBR_TP3.ic, e wf_movements.ic.

• 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)
#define BRANCO   5
#define AZUL     4
#define VERDE    2
#define AMARELO  3
#define VERMELHO 1
#define PRETO    0
 
#use "directions_sensor.ic"
 
//Global variables
//int stop=0;
int count=0;
 
 
//Descobre qual a cor do bloco a frente do sensor
int 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(0.2);
    ldramb=light(2);
    printf("LDR: %d\n", ldramb);
    //sleep(2.0);
 
    //Ascende cada led e aquisita o valor do LDR 
    LedOn(LED_RED);//bit_set(0x1008, 0b00010000);
    sleep(0.2);
    ldrred=light(2);
    //printf("Led vermelho! %d\n",ldrred-ldramb);
    //sleep(2.0);
    LedOff(LED_RED);//bit_clear(0x1008, 0b00010000);
 
    LedOn(LED_GREEN);//bit_set(0x1008, 0b00000100);
    sleep(0.2);
    ldrgreen=light(2);
    //printf("Led verde! %d\n",ldrgreen-ldramb);
    //sleep(2.0);
    LedOff(LED_GREEN);//bit_clear(0x1008, 0b00000100);
 
    LedOn(LED_BLUE);//bit_set(0x1008, 0b00001000);
    sleep(0.2);
    ldrblue=light(2);
    //printf("Led azul! %d\n",ldrblue-ldramb);
    //sleep(2.0);
    LedOff(LED_BLUE);//bit_clear(0x1008, 0b00001000);
 
    LedOn(LED_BLUE);
    LedOn(LED_RED);
    LedOn(LED_GREEN);
    sleep(0.2);
    ldrall=light(2);
    //printf("Led todos! %d\n",ldrall-ldramb);
    //sleep(0.2);
    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=3;//amarelo
                if(ldramb>220) cor=5; //branco
              }
                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 VERMELHO: //vermelho
          printf("Bloco vermelho!\n");
        sleep(2.0);
        break;
        case VERDE: //verde
          printf("Bloco verde!\n");
        sleep(2.0);
        break;
        case AMARELO: //vermelho+verde = amarelo
          printf("Bloco amarelo!\n");
        sleep(2.0);
        break;
        case AZUL: //azul
          printf("Bloco azul!\n");
        sleep(2.0);
        break;
        case BRANCO: //azul + vermelho = roxo
          printf("Bloco branco!\n");
        sleep(2.0);
        break;
        case PRETO: //preto
          printf("Bloco preto!\n");
        sleep(2.0);
        break;
        default: cor=5;//Cor desconhecida = branco
          break;
    }
    return cor;
}
//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;
    }
 
}

• Comp.ic

#use "color.ic"
#use "wf_movements.ic"
#use "directions_sensor.ic"

#define MOTORC 0

int stop=0; //global
int rest = 0;

void main()
{
    int start=0;
    int pidgo=0, pidgob=1, pidbridge=2, pid_time = 3;
    int campo;
    int cor;
    int lamb;
    int not_done=1;
    int blockcolor=5;  //inicia  como branco
    int timgo=0;
    int e1 = 20;
    int e2 = 20;
    enable_encoder(0);
    enable_encoder(1);
    poke(0x1009, 0x3c);
    LedOff(2);
    LedOff(3);
    LedOff(4);
    
    
    campo = SetPolarized();
    
    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()){
        if(StartLight()) break;
    }
    
    pid_time=start_process(LifeTime());
    
    PolarizedDiffSensor(campo);
    
    sleep(0.1);
    //Direciona para frente
    
    Bumper();
    
    sleep(0.1);
    
    
    //Segue direto monitorando portas digitais 10 e 11 (sensores de seguir linha) -> poucos cm <10cm
    pidgo = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (1a vez)
    kill_process(pidgo);
    Go(100.0); //espessura da linha
    printf("1a linha!");
    
    //captura o prprio rei
    sleep(0.2);
    printf("Captura Rei!");
    sleep(1.0);
    
    //detecta ambos em zero (2a vez)
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    Go(50.0); //espessura da linha
    printf("2a linha!");
    
    //ativa o segue linha e atravessa a ponte seguindo linha
    pidbridge = start_process(FollowLine());
    BlackLineDetect();                        //detecta ambos em zero (3a vez)
    Go(50.0); //espessura da linha
    printf("\nPonte!");
    StopMotors();
    
    //anda reto ate detectar ambos em zero(4a vez)
    pidgob = start_process(PDControl2(&e1, &e2, MOVE));
    pidgo = start_process(Discharge());
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    Go(50.0); //espessura da linha
    printf("4a linha!");
    
    Gira(4.0, RIGHT);
    sleep(0.1);
    Go(200.0);
    Gira(4.0, RIGHT);
    sleep(0.1);
    Gira(4.0, LEFT);
    sleep(0.1);
    Go(700.0);
    
    kill_process(pidgo);
    pidgo = start_process(SearchKing());
    
    while(1)
      {
        if(light(2)<(int)(0.58*(float)lamb))
          {
            break;   
        }
    }
    
    kill_process(pidgo);
    
    cor = DetectColor();
    
    if(cor==VERDE || cor==AZUL || cor==AMARELO || cor==VERMELHO)
      Charge();
    
}

//pega o bloco e desliga
void Charge()
{
    motor(MOTORC,-70);
    sleep(2.0);
    motor(MOTORC,0);
}

//solta o bloco e desliga
void Discharge()
{
    motor(MOTORC,70);
    sleep(2.0);
    motor(MOTORC,0);
}


void SearchKing()
{
    int pidgob = 0;
    int e1 = 20;
    int e2 = 20;    
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    Gira(4.0, LEFT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    Gira(4.0, LEFT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    //1
    Go(200.0);
    Gira(4.0, LEFT);
    Go(200.0);
    Gira(4.0, LEFT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    //2
    Gira(4.0, RIGHT);
    Go(200.0);
    Gira(4.0, RIGHT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    //3
    Gira(4.0, LEFT);
    Go(200.0);
    Gira(4.0, LEFT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    //4
    Gira(4.0, RIGHT);
    Go(200.0);
    Gira(4.0, RIGHT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    //5
    Gira(4.0, LEFT);
    Go(150.0);
    Gira(4.0, LEFT);
    pidgob = start_process(PDControl2(&e1, &e2, MOVE)/*Go(500.0)*/);
    BlackLineDetect();                        //detecta ambos em zero (2a vez)
    kill_process(pidgob);
    Gira(4.0, RIGHT);
    pidgob = start_process(FollowLine());
    BlackLineDetect();                        //detecta ambos em zero (3a vez)
}

• 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

#define STARTLIGHT   12                  // Limiar de detecao da luz de partida do campo (ja foi calibrado)

#use "wf_movements.ic"
#use "Comp.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;
    int direct;
    float dist=0.0;
    printf("%d\n", iteracao);    //Imprime Status
    reset_encoder(0);
    //avalia em qual sentido corrigira(horario, anti-horario)
    if(iteracao ==0 )
      {
        direct = LEFT;
        iteracao++;
    }//Liga os motores e gira para o mesmo lado q o polarizeddiff girou
    else if(iteracao ==2 ) 
      {
        direct = RIGHT;
        iteracao = 1;
    }
    else if(iteracao ==3 ) 
      {
        direct = RIGHT;
        iteracao = 2;
    }
    printf("%d\n", iteracao);
    msleep(1000L);
    for(i=0; i< iteracao; i++)
      Gira(4.0, direct);
}    
int SetPolarized()
{
    if(1==getKnob("Direcao[1-Verde, 2-Azul: ",1, 1))
      return VERTICAL;
    else
      return HORIZONTAL;
}


//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[4];
    i=0;
    //Gira e capta medias de valores   
    printf("%d\n", (light(2)));
    msleep(1000L);
    results[i] = (float)((light(2)));
    for(i=1; i<4; i++)
      {
        Gira(4.0, RIGHT);
        printf("%d\n", (light(2)));
        msleep(1000L);
        results[i] = (float)((light(2)));
        
    }
    // Gira(4.0, RIGHT); 
    //faz avaliao da melhor direo, de acordo com a luz desejada
    for(i=1; i<4; 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!=1)
      {
        printf("%f - ", results[0]);
        SetDirection(iteracao);    
    }
    
    
}
//segue a linha atraves dos chaves opticas-reflexivas
void FollowLine()
{
    int pid=4, pid1=5;
    int encid1, encid2;
    
    encid1 = 20;
    encid2 = 20;
    
    //faz controle Proporcional Direcional em paralelo
    pid = start_process(PDControl2(&encid1, &encid2, MOVE));
    pid1 = start_process(le_sensores());
    while(!stop_button() && stop == 0) {
        
        //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        
        sleep(0.3);
        if(!digital(10)&&!digital(11)) break;
    }
    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));
    *encid1 = 20;
    *encid2 = 20;
    
}
//corrige para esquerda
void correct_left_side(int *encid1, int *encid2)
{
    *encid1 = 20;
    *encid2 = 16;
    while(!digital(11));
    *encid1 = 20;
    *encid2 = 20;
}

//Reconhece acendimento da luz de partida
int StartLight()
{   int start=0;
    start=analog(6);
    printf("READY! - IR:%d\n", start);
    msleep(10L);
    if(start<STARTLIGHT) return 1;
    else return 0;
}

void LifeTime()
{
    sleep(60.0);
    stop = 1;
    rest = 1;
    alloff();
    printf("60sec GAME OVER!");
    //stop=1;
    
}

void walk()
{
    int moto = 3;
    int moto2 = 2;
    
    while(1)
      {
        printf("%d - %d\n", digital(14), digital(15));
        motor(moto, -100);
        motor(moto2, -70);
        
        msleep(100L);
        motor(moto, 0);
        motor(moto2, 0);
             
        if(moto==3){ moto = 2; moto2 = 3;}
        else if(moto==2){ moto =3; moto2 = 2;}
    }    
}

void Bumper()
{
    //StartMotors(-100,-100);
    int pid = 4;
    
    pid = start_process(walk());
    
    while(1)
      {
        if(digital(14) || digital(15)) { StopMotors(); kill_process(pid); break;}
        
      }
    
    if(digital(14))
      {
        motor(2, 100);
        motor(3, 30);
        sleep(1.0);
        StopMotors();
      }
    else if(digital(15))
      {
        motor(3, 100);
        motor(2, 30);
        sleep(1.0);
        StopMotors();
      }
    
    pid = start_process(walk());
    
    while(1)
      {
        if(digital(14) || digital(15)) { StopMotors(); kill_process(pid); break;}
        
      }
    
    StopMotors();
    
}
/*
void BlackLineDetect()
{
    int timergo=0;
    int line=0;
    line=(digital(10)+digital(11));
    while(line>0)
    //while(1)
    {
        if(timergo>100)
         {
            printf("TimeUP");
            break;
         }
        timergo++;
        msleep(100L);
    }
    alloff();
}
*/
void BlackLineDetect()
{
    int timergo=0;
    int line=2, get=0;
    //line=(digital(10)+digital(11));
    //while(timergo<100)
      while(1)
      { if(!digital(10)&&!digital(11)) break;
/*        line=(digital(10)+digital(11));
        if(line<1)get++;
        else line=2;
        msleep(10L);
        
        line=(digital(10)+digital(11));
        if(line<1)get++;
        else line=2;
        msleep(10L);
        
        line=(digital(10)+digital(11));
        if(line<1)get++;
        else line=2;
        msleep(10L);
        
        if(get>2) break;
        timergo++;
        msleep(70L);*/
    }
    alloff();
}

• 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:
 */

• 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 "wf_movements.ic"
//#use "color.ic"
//"color.ic" includes glogal stop variable

void main()
{
    float results[8];
    int direcao,cat;
    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);
/*    
    cat=escolhecatapulta();
    if(cat>0)
      PolarizedDiffSensor(VERTICAL);
    //Gira(1.0, RIGHT);
    else {
        
        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(1) {
    Go(MM900);
    while(!start_button());
    }
    printf("Desligue!\n");
    
 /*       
        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);
    }*/
}
//Contador de vida do robo (60 seg no TP3)
void LifeTime()
{   
    sleep(60.0);
    alloff();
    stop=1;
    printf("TIME OUT!\n");
    beep();
    msleep(10L);
    beep();
    msleep(10L);
    beep();
}

• 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!

*/

#use "color.ic"
//"color.ic" includes glogal stop variable

//Parmetros modificveis
#define RAIO_RODA    40.0                      //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
#define PULSOS_VOLTA 12.0*20.0                 //De acordo com o disco do shaft encoder->6*2=12 (reduo de 20)

//Parametros no-modificveis
#define PI        3.1416                      //Constante PI
#define REDUCAO   20.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    235.0                       //Distancia entre rodas [mm] (medir)
#define MM300     320.0                       //Ajuste de 300mm
#define MM900     3.0*MM300                   //Ajuste de 900mm

#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 2.0                                   //Parmetro de controle Kp
#define KD 0.01                                //Parametro de controle Kd

#define ROUNDTIME 5000                         //Tempo de 360 graus em milisegundos

//#use "wavefront.c"
//#use "hbmenu.ic"

/* 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;
}

//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;
    
    // Habilita encoder do canal 0
    reset_encoder(0);
    reset_encoder(1);
    if(stop==0)
      {
    if(x>=0.0)
      { printf("Indo pra frente!\n");  //Imprime Status
        //StartMotors(100,80);          //Liga os motores
        motor(MOTOR_1,100);
        motor(MOTOR_2,80);
    }
    else{
        printf("Indo pra Tras!\n");    //Imprime Status
        StartMotors(-100,-80);        //Liga os motores   
    }
    
    while ((dist<x))       //Enquanto dist<x e nao pressionar stop
      { 
        PDControl(20,20,&erroant1,&erroant2,MOVE); 
        
        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
        
    }
    //Quando a distncia x for alcancada
    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
    //while(!stop_button());
    reset_encoder(0);
    reset_encoder(1);
    //printf("Parado.\n");                  //Imprime Status
    //while(!stop_button());
        
    }
}

void PDControl(int encid1, int encid2, int *erroant1, int *erroant2, int tipo)
{
    int enc1=0, enc2=0;
    int erro1=0, erro2=0;
    int i, pterm1, pterm2, dterm1, dterm2;
    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,19,20,20,20};
    
    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+10;
        if(enc2>=curve2[i])
          speed2=i*10+10;
    }
    
    //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;
        }
    }
    
}

/*
//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 PDControl2(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 curve1[9]={10,14,16,18,19,20,20,20,20};
    //int curve2[9]={10,14,16,18,19,20,20,20,20};
    int curve1[9]={9,13,14,15,16,18,19,19,20};  //novos parametros 10/12
    int curve2[9]={10,14,16,18,19,19,20,21,21}; //novos parametros 10/12
    reset_encoder(0);
    reset_encoder(1);
    
    
    while(!stop_button() && stop==0)
      {
        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);
        msleep(30L);
    }
    StopMotors();
}

void ControlTime(long time, int *round)
{
    *round = 0;
    msleep(time);
    StopMotors();
    *round = 1;
}

//Gira para a direcao determinada 360/arc graus
void Gira(float arc, int direction)
{
    //variveis controle de angulo e pd dos motores
    int encid1, encid2;
    int pid = 4, pid1=5;
    float dist1=0.0, dist2=0.0;
    int  erroant1=0, erroant2=0,encsum1=0,encsum2=0;
    int round = 0;
    
    if(stop==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(PDControl2(&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(PDControl2(&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
    }    
}