Let's Make Robots!

Arduino robot bored mode

 

Hi guys I have been programming a rover 5 tank the past few weeks and have successfully implemented a rc and autonomous mode that uses a ir compound eye to follow objects. I have been trying to add a "bored" function to my code but am not getting right. Below is the code, any help will be greatly appreciated.

#include "ioPins.h"

#include "constants.h"

#include <Servo.h>

#include "notes.h" 

 

// define Global Variables

long time;                                     // used for chasing the LEDs

int leftspeed;                                 // speed of left motor

int rightspeed;                                // speed of right motor

int pan=panCenter;                             // position of pan servo

int tilt=tiltCenter;                           // position of tilt servo

int distance;                                  // distance of object being tracked

int irLeftValue;                               // reading from Compound Eye left sensor

int irRightValue;                              // reading from Compound Eye right sensor

int irUpValue;                                 // reading from Compound Eye upper sensor

int irDownValue;                               // reading from Compound Eye lower sensor

int temp;

int noise;

int panscale;

int panadjust;

int tiltscale;

int tiltadjust;

  unsigned long startTime = 0;

byte boredom;

 

 

 

// define Servos

Servo panservo;                                // define panservo

Servo tiltservo;                               // define tiltservo

 

void setup()

{

  // initialize servos and configure pins

 

  panservo.attach(panServo);                   // configure pin as pan servo

  panservo.writeMicroseconds(panCenter);       // initialize neck pan servo  

  tiltservo.attach(tiltServo);                 // configure pin as tilt servo                      

  tiltservo.writeMicroseconds(tiltCenter);     // initialize neck tilt servo 

 

  pinMode (frontLeftMotorDirection,OUTPUT);    // configure Left Motor Direction pin as output

  pinMode (frontRightMotorDirection,OUTPUT);   // configure Right Motor Direction pin as output

  pinMode (backLeftMotorDirection,OUTPUT);     // configure Left Motor Direction pin as output

  pinMode (backRightMotorDirection,OUTPUT);    // configure Right Motor Direction pin as output

  pinMode (irLeds,OUTPUT);                     // configure Compound Eye IR pin for output

  pinMode (Speaker,OUTPUT);                    // configure speaker pin for output

 

  // play tune on powerup / reset

  int melody[] = {NOTE_C4,NOTE_G3,NOTE_G3,NOTE_A3,NOTE_G3,0,NOTE_B3,NOTE_C4};

  int noteDurations[] = {4,8,8,4,4,4,4,4};

  for (byte Note = 0; Note < 8; Note++)        // play eight notes

  {

    int noteDuration = 1000/noteDurations[Note];

    tone(Speaker,melody[Note],noteDuration);

    int pauseBetweenNotes = noteDuration * 1.30;

    delay(pauseBetweenNotes);

  }  

  Serial.begin(9600);

  Serial.println ("Begin Serial"); 

}

 

 

void loop()

{

 

  digitalWrite(frontLeftMotorDirection,(leftspeed>0));       // set Left Motor Direction to forward if speed>0

  analogWrite(frontLeftMotorSpeed,abs(leftspeed));           // set Left Motor Speed

  digitalWrite(frontRightMotorDirection,(rightspeed>0));     // set Right Motor Direction to forward if speed>0

  analogWrite(frontRightMotorSpeed,abs(rightspeed));         // set Left  Motor Speed

  digitalWrite(backLeftMotorDirection,(leftspeed>0));        // set Left Motor Direction to forward if speed>0

  analogWrite(backLeftMotorSpeed,abs(leftspeed));            // set Left Motor Speed

  digitalWrite(backRightMotorDirection,(rightspeed>0));      // set Right Motor Direction to forward if speed>0

  analogWrite(backRightMotorSpeed,abs(rightspeed));          // set Left  Motor Speed

 

  panservo.writeMicroseconds(pan);              // update pan servo  position

  tiltservo.writeMicroseconds(tilt);            // update tilt servo position

 

  switch(controlMode)

  {

  case 0:

    irEye();

    irFollow();

if(boredom>2)Bored();

break;

  case 1:

rcMode();

break;

  }

}

 

void irEye()

{//============================================================= READ IR COMPOUND EYE ================================================

 

  digitalWrite(irLeds,HIGH);                                   // turn on IR LEDs to read TOTAL IR LIGHT (ambient + reflected)

  delay(2);

  irLeftValue=analogRead(irLeft);                             // TOTAL IR = AMBIENT IR + LED IR REFLECTED FROM OBJECT

  irRightValue=analogRead(irRight);                           // TOTAL IR = AMBIENT IR + LED IR REFLECTED FROM OBJECT

  irUpValue=analogRead(irUp);                             // TOTAL IR = AMBIENT IR + LED IR REFLECTED FROM OBJECT

  irDownValue=analogRead(irDown);                             // TOTAL IR = AMBIENT IR + LED IR REFLECTED FROM OBJECT

 

  digitalWrite(irLeds,LOW);                                   // turn off IR LEDs to read AMBIENT IR LIGHT (IR from indoor lighting and sunlight)

  delay(2);

  irLeftValue=irLeftValue-analogRead(irLeft);                 // REFLECTED IR = TOTAL IR - AMBIENT IR

  irRightValue=irRightValue-analogRead(irRight);               // REFLECTED IR = TOTAL IR - AMBIENT IR

  irUpValue=irUpValue-analogRead(irUp);               // REFLECTED IR = TOTAL IR - AMBIENT IR

  irDownValue=irDownValue-analogRead(irDown);               // REFLECTED IR = TOTAL IR - AMBIENT IR

 

  panscale=(irLeftValue+irRightValue)/panScaleFact;

  tiltscale=(irUpValue+irDownValue)/tiltScaleFact;

 

  distance=(irLeftValue+irRightValue+irUpValue+irDownValue)/4; // distance of object is average of reflected IR

 

  if (panadjust < 6 ||  tiltadjust < 6)// reset bordom counter if object moves enough

  { 

        unsigned long loopTime = millis() - startTime; 

    if (loopTime > 5000)boredom=3;

 

  }

  else

  {

    startTime = millis();

  }

 

  noise++;

  if(noise>5)

  {

    tone(Speaker,distance*5+100,5);                             // produce sound every eighth loop - high pitch = close distance

    noise=0;

  }  

}

 

void irFollow()

{//============================================================= TRACK OBJECT WITH EYE ===============================================

 

  leftspeed=0;                                                  // start with motor speeds set to 0

  rightspeed=0;

 

  if (distance<distanceMax)                                     // if no object in range then center neck

  {

    if (pan>panCenter)pan--;

    if (pan<panCenter)pan++;

    if (tilt>tiltCenter)tilt--;

    if (tilt<tiltCenter)tilt=tilt++;

  }

  else

  {

    //----------------------------------------------------------Track object with head------------------------------------------------

    panscale=(irLeftValue+irRightValue)/panScaleFact;

    tiltscale=(irUpValue+irDownValue)/tiltScaleFact;

    if (irLeftValue>irRightValue)

    {

      panadjust=(irLeftValue-irRightValue)*5/panscale;

      pan=pan-panadjust;

    }

    if (irLeftValue<irRightValue)

    {

      panadjust=(irRightValue-irLeftValue)*5/panscale;

      pan=pan+panadjust;

    }

    if (irUpValue>irDownValue)

    {

      tiltadjust=(irUpValue-irDownValue)*5/tiltscale;

      tilt=tilt-tiltadjust;

    }

    if (irDownValue>irUpValue)

    {

      tiltadjust=(irDownValue-irUpValue)*5/tiltscale;

      tilt=tilt+tiltadjust;

    }

 

    constrain(pan,panMin,panMax);

    constrain(tilt,tiltMin,tiltMax); 

 

    //----------------------------------------------------------Turn body to follow object--------------------------------------------

 

    temp=pan-panCenter;                                         // how far offcenter is pan servo                  

    if (abs(temp)>panDeadband)                                  // if panservo is outside of deadband

    {

      leftspeed=  -temp/2;                                      // adjust left  motor speed to turn body toward object

      rightspeed= temp/2;                                       // adjust right motor speed to turn body toward object

    }

    //----------------------------------------------------------Move forward or backward to follow object------------------------------------

 

    temp=abs(distance-bestDistance);

 

    if (temp>disDeadband)

    {

      temp=(temp-disDeadband)/3;

      if (distance>bestDistance)

      {

        rightspeed=rightspeed-temp;

        leftspeed=leftspeed-temp;

      }

      else

      {

        rightspeed=rightspeed+temp;

        leftspeed=leftspeed+temp;

      }

    }

    constrain (leftspeed,-255,255);                           // limit speed for PWM

    constrain (rightspeed,-255,255);                          // limit speed for PWM

  }

}

 

void Bored()

{

  Serial.println ("bored");

}

 

void rcMode()

{  

  if (Serial.available() > 0)                    // check for serial data

    {

      int inByte = Serial.read();

 

      Serial.print("I received: "); // say what you got:

      Serial.println(inByte);

      delay(100); // delay 10 milliseconds to allow serial update time

 

      switch (inByte) 

      {

      case 119:    

        Serial.println ("Move Forward");

        leftspeed=255;

        rightspeed=255;

        break;

      case 115:    

        Serial.println ("Move Backward");

        leftspeed=-255;

        rightspeed=-255;

        break;

      case 97:    

        Serial.println ("Turn Left");

        leftspeed=-255;

        rightspeed=255;

        break;

      case 100:    

        Serial.println ("Turn Right");

        leftspeed=255;

        rightspeed=-255;

        break;

      }

   }

   else

   {

     rightspeed=0;

     leftspeed=0;

   }

 

}

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

You are not reseting your boredom variable, only the timer.

Ah thanks so much! I realised I hadnt set boredom to 0 on else either. School boy error ;-)

Sorry I dont think I needed to post all that code. I have got a time function going.

So my sensor is on a pan/tilt bracket so when robot turns its head to a follow object it should reset the boredom counter, if not it should count up to 30 and then run the bored function (print bored into serial).

but after the 30 seconds, even if the robot sees something its keeps printing bored.

switch(controlMode)
  {
  case 0:
    irEye();
    irFollow();
if(boredom>2)Bored();
break;

  case 1:
rcMode();
break;
  }
}

  if (panadjust < 6 ||  tiltadjust < 6)// reset bordom counter if object moves enough
  { 
        unsigned long loopTime = millis() - startTime; 
    if (loopTime > 30000)boredom=3;

  }
  else
  {
    startTime = millis();
  }

http://www.arduino.cc/playground/Code/Stopwatch

you could have a time based function that might be what your looking for

if elapsed time > interest threshold {look for something else } ?