Projecto beta02 – Primeiros testes

O robot tem novo programa com todos os sensores e atuadores a trabalhar em conjunto. Já cumpre em grande parte o objectivo de evitar obstáculos como pode ser visto no video. Ainda é necessário afinar a velocidade quando está a recuar ou a desviar-se de um obstáculo, assim como acertar quanto o tempo em deverá fazer essas manobras.

É preciso agora encontrar um balanço entre a velocidade e o tempo por forma a que as manobras sejam o mais eficazes sem virar demais ou de menos como se vê no video. Isto leverá à alteração dos parâmetros e sucessivos testes até melhorar o comportamento de acordo com o esperado.

Video

Programa

/*
 * P012_ir_servo_decision_motors_2.pde - Wall avoider
 *   
 * 20120121 Luis Sismeiro
 */

#include <Servo.h>
#include <L29x.h>

#define SERVOMAXRIGHT 10
#define SERVOMAXLEFT 155
#define SERVOCENTER 75
#define IRSAMPLES 10
#define IRTHRESHOLD 170
#define FASTSPEED 255
#define SLOWSPEED 255
#define ROTATEDELAY 500

const byte irServoPin = 10;
const byte irSensorPin = A5;
const byte motorRightEn = 6; // L293 pin 15 PWM
const byte motorRight1 = 7; // L293 pin 10
const byte motorRight2 = 5; // L293 pin 15
const byte motorLeftEn = 3; // L293 pin 1 PWM
const byte motorLeft1 = 2; // L293 pin 2
const byte motorLeft2 = 4; // L293 pin 7
const byte bumperRightPin = 8;
const byte bumperLeftPin = 9;
const byte ledPin = 13;

Servo irServo; 

L29x motorRight(motorRightEn, motorRight1, motorRight2);
L29x motorLeft(motorLeftEn, motorLeft1, motorLeft2);

int rightIrValue = 0;
int leftIrValue = 0;

void setup()
{
  pinMode(bumperRightPin, INPUT);
  digitalWrite(bumperRightPin, HIGH); // activate pullup resistor
  pinMode(bumperLeftPin, INPUT);
  digitalWrite(bumperLeftPin, HIGH); // activate pullup resistor
  pinMode(ledPin, OUTPUT);

  irServo.attach(irServoPin);
  //Serial.begin(9600);
}

void loop()
{
  if (!digitalRead(bumperRightPin)) {
    //digitalWrite(ledPin, HIGH);
    motorRight.rotNeg(SLOWSPEED);
    motorLeft.rotNeg(SLOWSPEED);
    delay(500);
    motorRight.rotPos(SLOWSPEED);
    motorLeft.rotNeg(SLOWSPEED);
    delay(ROTATEDELAY);
  } else if (!digitalRead(bumperLeftPin)) {
    //digitalWrite(ledPin, HIGH);
    motorRight.rotNeg(SLOWSPEED);
    motorLeft.rotNeg(SLOWSPEED);
    delay(500);
    motorRight.rotNeg(SLOWSPEED);
    motorLeft.rotPos(SLOWSPEED);
    delay(ROTATEDELAY);
  } else {
    digitalWrite(ledPin, LOW);
  }
  if (irValueRead() > IRTHRESHOLD) {
    motorRight.stop();
    motorLeft.stop();
    irSideRead();
    //Serial.print("Right IR value: ");
    //Serial.println(rightIrValue);
    //Serial.print("Left IR value:" );
    //Serial.println(leftIrValue);
    if (rightIrValue > leftIrValue && leftIrValue < IRTHRESHOLD) {
      //Serial.println("Rotate left");
      motorRight.rotPos(SLOWSPEED);
      motorLeft.rotNeg(0);
      delay(ROTATEDELAY);
    } else if (leftIrValue > rightIrValue && rightIrValue < IRTHRESHOLD) {
      //Serial.println("Rotate right");
      motorRight.rotNeg(0);
      motorLeft.rotPos(SLOWSPEED);
      delay(ROTATEDELAY);
    } else {
      //Serial.println("Backup and rotate 180");
      motorRight.rotNeg(SLOWSPEED);
      motorLeft.rotNeg(SLOWSPEED);
      delay(500);
      motorRight.rotPos(SLOWSPEED);
      motorLeft.rotNeg(SLOWSPEED);
      delay(750);
    }
  } else {
    //Serial.println("Go forward");
    motorRight.rotPos(FASTSPEED);
    motorLeft.rotPos(FASTSPEED);
  }
}

int irValueRead()
{
  int irSum = 0;
  for (int i = 0; i < IRSAMPLES; i++)
    irSum += analogRead(irSensorPin);
  return irSum / IRSAMPLES;
}

void irSideRead()
{
  irServo.write(SERVOMAXRIGHT);
  delay(500);
  rightIrValue = irValueRead();
  irServo.write(SERVOMAXLEFT);
  delay(1000);
  leftIrValue = irValueRead();
  irServo.write(SERVOCENTER);
  delay(500);
}

Etiquetas: , ,

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: