Control de adelantamiento para velocistas

2 comentarios / 0 nuevos
Último envío
Imagen de thar
Offline
Visto: Hace 1 año 9 meses
Unido: 23/12/2015 - 12:00
Control de adelantamiento para velocistas

Como el tema de robots de carreras es bastante complicado para muchos, sobretodo porque aumenta la complejidad de la programación bastante, he preparado un "pequeño" template para que sirva como base para que cada uno pueda programarlo en su robot.

 

/* 
 * Máquina de estados para cambiar de carril
 * -----------------------------------------
 */

#define NUMERO_CARRILES 3

typedef enum
{
  ESPERA_OBSTACULO,
  GIRANDO_DERECHA,
  GIRANDO_IZQUIERDA,
  CAMBIANDO_CARRIL
} EstadoAdelantamiento;

typedef enum
{
  CAMBIO_DERECHA,
  CAMBIO_IZQUIERDA
} DireccionCambioCarril;

EstadoAdelantamiento estadoActual = ESPERA_OBSTACULO;

int carrilActual = 0; //Supongo como carril 0 el de mas a la izquierda, el carril preferido
bool adelantamientoPendiente = false;
bool isObstaculoFrente(); //TODO
bool isObstaculoDerecha(); //TODO
bool isObstaculoIzquierda(); //TODO
bool isLineaEncontrada(); //TODO: Si los sensores ven linea esto da true, false en caso contrario

/*
 *  TODO: No siempre se va a hacer el cambio de carril de forma correcta. Hay que comprobar
 *  que si hemos cambiado hacia la derecha la linea se encuentra por el sensor de mas a la 
 *  derecha o si hemos cambiado hacia la izquierda la linea se encuentra por el sensor de 
 *  mas a la izquierda.
 *  Devuelve: true si la line se encuentra por el lado correcto
 *  false en caso contrario
 */
bool isLineaEncontradaPorElLadoCorrecto(int medidaLinea, DireccionCambioCarril direccionCambio);

void procesarAdelantamiento(int medidaLinea)
{
  static DireccionCambioCarril direccionCambioCarril = CAMBIO_IZQUIERDA;
  switch (estadoActual)
  {
  case ESPERA_OBSTACULO:
    {
      if (isObstaculoFrente())
        adelantamientoPendiente = true;
      if (adelantamientoPendiente)
      {
        if (!isObstaculoIzquierda() && carril > 0)
        {
          direccionCambioCarril = CAMBIO_IZQUIERDA;
          estadoActual = GIRANDO_IZQUIERDA;
        }
        else if (!isObstaculoDerecha() && carril < (NUMERO_CARRILES-1) )
        {
          direccionCambioCarril = CAMBIO_DERECHA;
          estadoActual = GIRANDO_DERECHA;
        }
      }
    }
    break;
  case GIRANDO_DERECHA: case GIRANDO_IZQUIERDA:
    {
      if (!isLineaEncontrada())
        estadoActual = CAMBIANDO_CARRIL;
    }
    break;
  case CAMBIANDO_CARRIL:
    {
      if (isLineaEncontrada())
      {
        if (isLineaEncontradaPorElLadoCorrecto(medidaLinea, direccionCambioCarril))
        {
          switch (direccionCambioCarril)
          {
          case CAMBIO_IZQUIERDA:
            --carrilActual;
            break;
          case CAMBIO_DERECHA:
            ++carrilActual;
            break;
          }
        }
        estadoActual = ESPERA_OBSTACULO;
        adelantamientoPendiente = false;
      }
    }
    break;
  }
}

void controlLinea()
{
  shitch (estadoActual)
  {
  case ESPERA_OBSTACULO:
    {
      //TODO: PID de toda la vida de siguelineas
    }
    break;
  case GIRANDO_DERECHA:
    {
      //TODO: Colocar PWM para que el robot gire constante hacia la derecha
    }
    break;
  case GIRANDO_IZQUIERDA:
    {
      //TODO: Colocar PWM para que el robot gire constante hacia la izquierda
    }
    break;
  case CAMBIANDO_CARRIL:
    {
      //TODO: Colocar PWM para que el robot vaya recto
    }
    break;
  }
}

Si alguien lo prueba que lo ponga aquí y comparta su resultado!

Imagen de Luison
Offline
Visto: Hace 2 meses 6 días
Unido: 10/06/2015 - 23:58
UOO!!! Mil gracias!!! Seguro

UOO!!! Mil gracias!!! Seguro que ayuda a mucha gente! (y yo me incluyo).

love2