I'm having problems with a robot I am making. I am making a follower robot using an Arduino Mega 2560, 2 DC brush motors for drive motors, a PING))), and a servo. I have a sketch written, but when I go to run it I seem to be getting massive processing lag or something because the servo will stop moving and whatever the last PWM values were are sustained until the robot kind of Lucy and Ethal's itself by like trying to run through as many commands that it missed as fast as possible until it gets stuck again. I'm not a programmer/ CS major or anything, so my code may be highly unoptimized (I'm told many if statements and nested if statements are very processing intensive). Does anyone have any advice?
As kind of a run down with what I'm trying to do, I'm trying to limit the 40ish degree FOV of the PING))) by taking 2 readings that are 10-15 degrees (still experimenting) plus and minus the direction of the person the robot is to follow. The way I have my sensor mounted is good; it sees what I want it to see and gets readings. What I want from the 2 readings is just a binary output to give the robot an idea of where the person is (ie 11 right in front - go forward, 10 to the left - turn left, 01 to the right - turn right, and 00 no where in sight - stop and wait for person). I'm not super worried about my steering stuff at the moment, I can hammer that out with experimentation, and don't worry about the base stuff, that's a contrivance I've added to slowly speed up the robot instead of just lyrching it to full blast (again I can hammer that out with experimentation). I'm also not worried about my void setup(){}, the stuff commented out is from an older code I was working with that I may, but probably will not keep. The problem I have is with the main loop. If I just turn on the robot and don't stand in front of it (so the readings come back 00) the robot is fine, the servo goes back and forth from 75-105 degrees at regular speed. The second I stand in front of it it sees me from the left, the servo turn sees me on the right, pwm value are written and the thing pretty much stays like that, even if i walk out of its sight (I have the robot on a crate since right now it just likes to crash into walls) it will sustain this for a couple of seconds and then go back into its happy no detect state. It seems like controller is getting bogged down somewhere, and I know I have a TON of if statements. I've tried adding some delays here and there because i notice the Arduino seems to process its backlogged stuff during delays, but I haven't had much success.
O and another little note: As part of my experimentation I noticed the PING))) didn't always detect on the first attempt at the ranged I am looking to do (~2-7 feet), so I had a for loop to give it a few "chances."
#include <Servo.h>
Servo servo;
//Servo Position and Steering Value values
int servoPos = 90;
int steerVal = 0;
// Motor control values (PWM values)
int rMot = 0;
int lMot = 0;
// Sensor reading and accepted viewing range. Viewing range in
// milliseconds. Sensor measures the time for an ultrasonic pulse
// to travel from the sensor to the nearest object. Sound travels
// at approximately 74 microseconds per inch, so a sensor reading
// 1776 microseconds corresponds to a distance of 1 foot away.
// "inc" in the increment for servo steps, "n" is a counter.
int sensorRead = 0;
int farthest = 9000;
int shortest = 5000;
int n;
boolean detectR;
boolean detectL;
int scan = 15;
// Base values for PWM values. Base values used to gradually speed
// up and slow down robot to make robot motion less jumpy and to
// draw current more eficiently when speeding the robot up.
int base = 50;
int baseInc = 20;
int baseMax = 220;
int baseMin = 180;
// Pin Values
int sensorPin = 15;
int servoPin = 14;
int rMotPin = 2;
int lMotPin = 3;
int hBdg1 = 16;
int hBdg2 = 17;
int hBdg3 = 18;
int hBdg4 = 19;
void setup()
{
servo.attach(servoPin);
servo.write(servoPos);
pinMode(rMotPin, OUTPUT);
pinMode(lMotPin, OUTPUT);
pinMode(hBdg1, OUTPUT);
pinMode(hBdg2, OUTPUT);
pinMode(hBdg3, OUTPUT);
pinMode(hBdg4, OUTPUT);
forward();
base = baseMin;
// Initially locates to runner before juping into the main loop.
/* getRange();
if(sensorRead > shortest && sensorRead < farthest)
{
detectR = true;
}
else
{
detectR = false;
}
while(detectR == false)
{
servoPos += inc;
servo.write(servoPos);
getRange();
if(servoPos <= servoMin || servoPos >= servoMax)
{
inc *= -1;
}
delay(100);
}*/
}
// **Main Loop**
void loop()
{
// if((!detectR && detectL) || (detectR && !detectL))
// {
// delay(100);
// }
servoPos += scan;
if(servoPos > 180)
{
servoPos = 180;
}
servo.write(servoPos);
delay(300);
detectR = getRange();
servoPos -= 2 * scan;
if(servoPos < 0)
{
servoPos = 0;
}
servo.write(servoPos);
delay(300);
detectL = getRange();
servoPos += scan;
delay(100);
if(detectR && detectL)
{
steerVal = (servoPos - 90) / 2;
if(steerVal > 0)
{
rMot = base - steerVal;
lMot = base;
}
else
{
rMot = base;
lMot = base - steerVal;
}
base += baseInc;
if(base > baseMax)
{
base = baseMax;
}
}
if(detectR && !detectL)
{
servoPos += scan;
if(servoPos > 180)
{
servoPos = 180;
}
steerVal = (servoPos - 90) / 2;
if(steerVal > 0)
{
rMot = base - steerVal;
lMot = base;
}
else
{
rMot = base;
lMot = base - steerVal;
}
}
if(!detectR && detectL)
{
servoPos -= scan;
if(servoPos < 0)
{
servoPos = 0;
}
steerVal = (servoPos - 90) / 2;
if(steerVal > 0)
{
rMot = base - steerVal;
lMot = base;
}
else
{
rMot = base;
lMot = base - steerVal;
}
}
if(!detectR && !detectL)
{
servoPos = 90;
rMot = 0;
lMot = 0;
base = baseMin;
}
rMot -= 10;
if(rMot < 0)
{
rMot = 0;
}
analogWrite(rMotPin, rMot);
analogWrite(lMotPin, lMot);
delay(300);
}
// **Subroutines**
// Sensor detection process The sensor uses the same pin to
// recieve intiation pulse and output result. The sensor is
// initiated with a 5 microsecond digital high pulse and outputs a
// short digital high pulse after recieving its echo.
// Experimentation showed the process does not always yield a
// value so the process is looped 10 times or until a value is
// obtained to weed out false negatives.
boolean getRange()
{
for(n = 1; n < 5; n++)
{
pinMode(sensorPin, OUTPUT);
digitalWrite(sensorPin, LOW);
delayMicroseconds(2);
digitalWrite(sensorPin, HIGH);
delayMicroseconds(5);
digitalWrite(sensorPin, LOW);
pinMode(sensorPin, INPUT);
sensorRead = pulseIn(sensorPin, HIGH);
delay(100);
if(sensorRead >= shortest && sensorRead <= farthest)
{
return true;
break;
}
}
return false;
}
// While this program does not use it the robot's circuity does
// include H-bridges that allow the wheel motors to spin
// backwards and forwards with proper digital inputs. Each motor
// has 2 pins (right has hBdg3 and hBdg4, left has hBdg1 and
// Hbdg2), and the high low for those pins (in respective order)
// is forward, low high is backwards, high high is documented as a
// "fast stop" which will draw a lot of current and risks damaging
// the H-bridges so should be used with caution, and low low is a
// free spin stop.
void forward()
{
digitalWrite(hBdg1, HIGH);
digitalWrite(hBdg2, LOW);
digitalWrite(hBdg3, HIGH);
digitalWrite(hBdg4, LOW);
}
void reverse()
{
digitalWrite(hBdg1, LOW);
digitalWrite(hBdg2, HIGH);
digitalWrite(hBdg3, LOW);
digitalWrite(hBdg4, HIGH);
}
Besides, it's easy to write.