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;
}
}