Robot beta01 – Teste final

Quem tem acompanhado o meu blog já teve oportunidade de ler sobre o projecto do robot beta01. Penso que cheguei a uma fase, em que de acordo com os sensores disponíveis e a minha capacidade de programar, está a funcionar perto da “perfeição”.

Para além do sensor de proximidade IR, adicionei dois interruptores na frente para colmatar o “ângulo morto” e os obstáculos que não são facilmente detectados. Desta forma consegue ultrapassar a totalidade do que se lhe atravessa à frente sem ficar preso.

Como é possível verificar no video e na consulta da listagem em baixo, o programa do robot acaba pode ser bastante simples e por se revelar eficaz. O robot também consegue desviar-se de objectos revestidos em tecido e naqueles casos em que se aproxima de uma parede num ângulo perto do raso, neste caso os sensores no parachoques fazem o robot recuar e virar no sentido oposto.

No programa, além da lógica para evitar obstáculos, foram definidas funções para fazer andar em frente, virar ou recuar o robot com a velocidade escolhida para cada uma das situações, alta para andar em frente e mais lenta para as manobras. Existe também algum código para ajudar no debug em algumas situações de dúvida, como a leitura do sensor de proximidade mas que para a versão final não é necessário nem compilado.

Para quem percebe um pouco de Arduino e de programação, creio que o código é fácil de seguir mas se tiverem alguma dúvida podem usar os comentários. De igual forma, se tiverem sugestões, agradeço que as façam sob a forma de comentário.

Video

Nota: reparei depois que o robot colide com os garrafões em alguns casos. Isto ocorre porque o plástico e água não reflectem os infravermelhos como no caso da parede ou outro obstáculo opaco.

Programa

/*
 * Robot beta01 - this is a wall avoiding robot using an IR distance sensor from Sharp
 *
 * 20111230 Luis Sismeiro
 */

//#define DEBUG

#ifdef DEBUG
  #define DEBUG_PRINT(x) Serial.print(x)
  #define DEBUG_PRINTLN(x) Serial.println(x)
#else
  #define DEBUG_PRINT(x)
  #define DEBUG_PRINTLN(x)
#endif

#define HIGHSPEED 255 // motors high speed
#define LOWSPEED 128 // motors low speed

/* Pin assignment */
const int irSensor = A0; // IR sensor output
const int bumperSensorRight = 8; // Switch sensor right
const int bumperSensorLeft = 9; //Switch sensor left
const int motorRightEn = 7; // IC pin 9 
const int motorRight1 = 6; // IC pin 10
const int motorRight2 = 5; // IC pin 15
const int motorLeftEn = 2; // IC pin 1
const int motorLeft1 = 3; // IC pin 2
const int motorLeft2 = 4; // IC pin 7 

void setup()
{
  pinMode(bumperSensorRight, INPUT); // Configure bumper sensor input
  digitalWrite(bumperSensorRight, HIGH); // Configure pullup resistor
  pinMode(bumperSensorLeft, INPUT); // Configure bumper sensor input
  digitalWrite(bumperSensorLeft, HIGH); // Configure pullup resistor
  pinMode(motorRight1, OUTPUT);
  pinMode(motorRight2, OUTPUT);
  pinMode(motorRightEn, OUTPUT);
  pinMode(motorLeft1, OUTPUT);
  pinMode(motorLeft2, OUTPUT);
  pinMode(motorLeftEn, OUTPUT);
  #ifdef DEBUG
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  #endif
}

void loop()
{
  if (!digitalRead(bumperSensorRight)) {
    motorsBackwardLow();  
    delay(500);
    rotateLeft();
    delay(500);
  }

  if (!digitalRead(bumperSensorLeft)) {
    motorsBackwardLow();
    delay(500);
    rotateRight();
    delay(500);
  }

  if (readIrSensor() > 130) {
    rotateLeft();
    }
  else {
    motorsForwardHigh();
  }
}

/*
 * Function: read the value of the IR sensor using oversampling
 *
 * Return: average value of IR samples read
 */
int readIrSensor()
{
  int irsamples = 10; // number of IR read samples 
  int irOut = 0; // sample accumulator

  for (int i = 0; i < irsamples; i++) {
    irOut += analogRead(irSensor);
  }  
  return irOut / irsamples;
}

/*
 * Function: move forward high speed
 */
void motorsForwardHigh()
{
  /* motorRight forward */
  analogWrite(motorRightEn, HIGHSPEED);
  digitalWrite(motorRight1, LOW);
  digitalWrite(motorRight2, HIGH);

  /* motorLeft forward */
  analogWrite(motorLeftEn, HIGHSPEED);
  digitalWrite(motorLeft1, LOW);
  digitalWrite(motorLeft2, HIGH);
}

/*
 * Function: move backward low speed
 */
void motorsBackwardLow()
{
  /* motorRight forward */
  analogWrite(motorRightEn, LOWSPEED);
  digitalWrite(motorRight1, HIGH);
  digitalWrite(motorRight2, LOW);

  /* motorLeft forward */
  analogWrite(motorLeftEn, LOWSPEED);
  digitalWrite(motorLeft1, HIGH);
  digitalWrite(motorLeft2, LOW);
}

/*
 * Function: stop motors
 */
void motorsStop()
{
  /* stop motorRight */
  analogWrite(motorRightEn, 0);
  digitalWrite(motorRight1, LOW);
  digitalWrite(motorRight2, LOW);

  /* stop motorLeft */
  analogWrite(motorLeftEn, 0);
  digitalWrite(motorLeft1, LOW);
  digitalWrite(motorLeft2, LOW);
}

/*
 * Function: rotate right
 */
void rotateRight()
{
  /* rotate right back */
  analogWrite(motorRightEn, LOWSPEED);
  digitalWrite(motorRight1, HIGH);
  digitalWrite(motorRight2, LOW);

  /* rotate left front */
  analogWrite(motorLeftEn, LOWSPEED);
  digitalWrite(motorLeft1, LOW);
  digitalWrite(motorLeft2, HIGH);
}

/*
 * Function: rotate left
 */
void rotateLeft()
{
  /* rotate right back */
  analogWrite(motorRightEn, LOWSPEED);
  digitalWrite(motorRight1, LOW);
  digitalWrite(motorRight2, HIGH);

  /* rotate left front */
  analogWrite(motorLeftEn, LOWSPEED);
  digitalWrite(motorLeft1, HIGH);
  digitalWrite(motorLeft2, LOW);
}

Etiquetas: , , ,

2 Respostas to “Robot beta01 – Teste final”

  1. Diogo Says:

    boa! mas atrela uma vassoura nisso.

  2. sismeiro Says:

    Olha que essa da vassoura é uma boa ideia.🙂

Deixe uma Resposta

Preencha os seus detalhes abaixo ou clique num ícone para iniciar sessão:

Logótipo da WordPress.com

Está a comentar usando a sua conta WordPress.com Terminar Sessão / Alterar )

Imagem do Twitter

Está a comentar usando a sua conta Twitter Terminar Sessão / Alterar )

Facebook photo

Está a comentar usando a sua conta Facebook Terminar Sessão / Alterar )

Google+ photo

Está a comentar usando a sua conta Google+ Terminar Sessão / Alterar )

Connecting to %s


%d bloggers like this: