File content as of revision 1:8aece97d3c78:

#include "mbed.h"
PwmOut Left(p25);  //Definição do motor da esquerda
PwmOut Right(p23); //Definicao do motor da direita
DigitalIn Sensor1 (p17);// Sensor digital localizado na esquerda do robo
DigitalIn Sensor2 (p15);//Sensor digital localizado na diagonal esquerda do robo
DigitalIn Sensor3 (p13);//Sensor digital localizado na frente do robo
DigitalIn Sensor4 (p11); //Sensor digital localizado na diagonal direita do robo
DigitalIn Sensor5 (p9); //Sensor digital localizado na direita do robo


#define MIN 1260//1060 //velocidade maxima para tras
#define MID ((MAX+MIN)/2) //valor para parar
#define BUS 12 //velocidade de busca
#define MAX 1660//1860 //velocidade maxima para frente
#define r 2
#define e 3
#define d 4
#define p 9
#define b 5
#define PARAMETER 0.600


int i=MID;
int k=MID;
int total; //{right, right_diagonal, front, left_diagonal, left} valor atual da leitura
int measure[5]; // vetor que grava a leitura atual
int tot;
int in=20;
 
//*****************************  DEFINICOES - FINAL  *****************************
//################################################################################
//################################################################################
//******************************* FUNCOES - INICIO  ******************************
//Funcao que inicializa os motores
void Motors_Setup() 
{ 
    Right.period_ms(16);   
    Left.period_ms(16);
    Right.pulsewidth_us(MID);
    Left.pulsewidth_us(MID); 
    wait(0.1);
 }
//Função que desloca os motores de acordo com o sinal PPM recebido
void Drive(int maximo,int minimo,int controle) 
{
    switch(controle)
    {
        case 1:
            Left.pulsewidth_us(minimo);
            wait(0.02); 
            Right.pulsewidth_us(minimo);
            wait(0.02);
            break;
        case 2:
            //Right.period_ms(16);   
            //Left.period_ms(16);
            for (;k<=maximo && i<=maximo;k+=in,i+=in) 
            {
                Left.pulsewidth_us(k); 
                wait(0.02);
                Right.pulsewidth_us(i); 
                wait(0.02);
            if (k>MAX)
            {
                k=MAX;
                }
            if (i>MAX)
            {
                i=MAX;
                }
            if (k<MIN)
            {
                k=MIN;
                }
            if (i<MIN)
            {
                i=MIN;
                }
            }
            break;
        case 3:
            //Right.period_ms(16);   
            //Left.period_ms(16);
            for (;k>=minimo || i<=maximo;k-=in,i+=in) 
            {
                Left.pulsewidth_us(k); 
                wait(0.02);
                Right.pulsewidth_us(i); 
                wait(0.02);
            if (k>MAX)
            {
                k=MAX;
                }
            if (i>MAX)
            {
                i=MAX;
                }
            if (k<MIN)
            {
                k=MIN;
                }
            if (i<MIN)
            {
                i=MIN;
                }
            }
            
            break;
        case 4:
            //Right.period_ms(16);   
            //Left.period_ms(16);
            for (;k<=maximo || i>=minimo;k+=in,i-=in) 
            {
                Left.pulsewidth_us(k); 
                wait(0.02);
                Right.pulsewidth_us(i); 
                wait(0.02);
            if (k>MAX)
            {
                k=MAX;
                }
            if (i>MAX)
            {
                i=MAX;
                }
            if (k<MIN)
            {
                k=MIN;
                }
            if (i<MIN)
            {
                i=MIN;
                }
                }
            
            break;
        case 5:
            //Right.period_ms(16);   
            //Left.period_ms(16);
            for (;k>=minimo || i>=minimo;k-=in,i-=in) 
            {
                Left.pulsewidth_us(k); 
                wait(0.02);
                Right.pulsewidth_us(i); 
                wait(0.02);
            if (k>MAX)
            {
                k=MAX;
                }
            if (i>MAX)
            {
                i=MAX;
                }
            if (k<MIN)
            {
                k=MIN;
                }
            if (i<MIN)
            {
                i=MIN;
                }
                }
            
            break;
        default:
            Left.pulsewidth_us(MID); 
            Right.pulsewidth_us(MID);
            break;
    }
}

void Aceleration(int esq, int dir)
{
    Left.pulsewidth_us(esq);
    wait(0.02); 
    Right.pulsewidth_us(dir);
    wait(0.02);
}    

int Search ()
{
  measure[0] = Sensor1.read();
  measure[1] = Sensor2.read();
  measure[2] = Sensor3.read(); 
  measure[3] = Sensor4.read();
  measure[4] = Sensor5.read();

  total = measure[0]*10000 + measure[1]*1000 + measure[2]*100 + measure[3]*10 + measure[4]*1;
  return total;
}

int Action(int total)
{
  switch(total)
  {
     case 1: //vira para a direita 
        Drive(MAX,MIN,d);
        Aceleration(MAX,MIN);
        break;
     
     case 10: //vira para a direita 
        Drive(MAX,MIN,d);
        Aceleration(MAX,MIN);  
        break;
     
     case 11: //vira para a direita
        Drive(MAX,MIN,d);
        Aceleration(MAX,MIN);
        break;
    
     case 100: //Vai reto
        Drive(MAX,MIN,r);
        Aceleration(MAX,MAX);
        break;
     
     case 110: //direita com leiruta do sensor da diagonal direita
        Drive(MAX,MIN,d);
        Aceleration(MAX,MAX);
        break;
    
     case 1000: //vai para a esquerda 
        Drive(MAX,MIN,e);
        Aceleration(MIN,MAX);
        break;
     
     case 1100: //esquerda com leitura do sensor da diagonal esquerda
        Drive(MAX,MIN,e);
        Aceleration(MIN,MAX);
        break;
    
     case 1110: //vai reto com a leitura dos sensores diagonais e o da frente 
        Drive(MAX,MIN,r);
        Aceleration(MAX,MAX);
        break;
    
     case 10000: //vai para a esquerda
        Drive(MAX,MIN,e);
        Aceleration(MIN,MAX);
        break;
     
     case 11000: //vai para a esquerda 
        Drive(MAX,MIN,e);
        Aceleration(MIN,MAX);
        break;
    
     case 11111: //leu todos os sensores ao mesmo tempo
        Drive(MAX,MIN,p);
        Aceleration(MID,MID);
        break; 
     
   //todos os outros casos que são impossíveis
     default:// Para  
        Drive(MAX,MIN,0); 
        break; 
  }
  return 0;
}

int main()
{
    while(1)
    {
    tot=Search();
    Action(tot);
    }
    return 0;
}