I mounted GP2D12 IR range sensor on a servo, which is fixed on a moving car. To detect obstacles, I let Arduino UNO control the servo rotating between -35 to +35 degree in the forward direction and read range values for three times to get the minimal distance.
But even though I make the servo keep still, the range sensor tells unstable distances.
The following is the debug information printed on serial monitor.
servo step = 68, min_value = 37.13
servo step = 86, min_value = 36.64
servo step = 104, min_value = 74.01
servo step = 120, min_value = 37.90
min = 30.81, left = 30.81, right = 37.13, turn right
servo step = 102, min_value = 20.68
servo step = 84, min_value = 20.15
servo step = 66, min_value = 18.11
servo step = 50, min_value = 16.38
min = 16.38, left = 20.68, right = 28.32, go backward
servo step = 68, min_value = 32.10
servo step = 86, min_value = 32.10
servo step = 104, min_value = 38.42
servo step = 120, min_value = 36.89
min = 32.10, left = 38.69, right = 32.10, turn left
servo step = 102, min_value = 15.96
servo step = 84, min_value = 19.09
servo step = 66, min_value = 19.16
servo step = 50, min_value = 17.55
min = 15.96, left = 15.96, right = 20.95, go backward
servo step = 68, min_value = 43.13
servo step = 86, min_value = 43.46
servo step = 104, min_value = 30.63
servo step = 120, min_value = 39.23
min = 30.63, left = 38.42, right = 43.13, turn right
And my codes are listed here.
#include <Servo.h>
const int SERVO_ROTATE_CLOCKWISE = 1;
const int SERVO_ROTATE_COUNTERCLOCKWISE = 0;
const int SERVO_STEP_INTERVAL = 18;
const int SERVO_INIT_STEP = 85;
const int SERVO_MIN_STEP = 50;
const int SERVO_MAX_STEP = 120;
const int pinLeft = 3;
const int pinForward = 4;
const int pinBack = 7;
const int pinRight = 8;
const int pinServo = 5;
const int pinGP2D12 = 5;
const float RANGE_LOWER_LIMIT = 10;
const float RANGE_UPPER_LIMIT = 80;
const float DANGER_DISTANCE = 30;
const float SECURE_DISTANCE = 50;
Servo servo;
int servo_dir; /* current servo rotating direction */
int servo_step; /* current servo step */
float read_gp2d12_range(byte pin)
{
int tmp;
tmp = analogRead(pin);
if (tmp < 3)
{
return -1; // invalid value
}
return (6787.0 /((float)tmp - 3.0)) - 4.0;
}
void setup()
{
pinMode(pinLeft, OUTPUT);
pinMode(pinForward, OUTPUT);
pinMode(pinBack, OUTPUT);
pinMode(pinRight, OUTPUT);
cmdNeut();
servo.attach(pinServo);
servo.write(SERVO_INIT_STEP);
servo_dir = SERVO_ROTATE_CLOCKWISE;
servo_step = SERVO_INIT_STEP;
Serial.begin(9600);
Serial.println("Ready...");
}
void loop(void)
{
float min_dist;
float left_dist;
float right_dist;
while (true)
{
int ret = checkDistance(min_dist, left_dist, right_dist);
if (ret < 0)
{
Serial.println("check distance return invalid result");
delay(5000);
continue;
}
Serial.print("min = ");
Serial.print(min_dist);
Serial.print(", left = ");
Serial.print(left_dist);
Serial.print(", right = ");
Serial.print(right_dist);
if (min_dist >= DANGER_DISTANCE && min_dist < SECURE_DISTANCE)
{
if (left_dist > right_dist)
{
actionTurnLeft();
Serial.println(", turn left");
}
else
{
actionTurnRight();
Serial.println(", turn right");
}
}
else if (min_dist < DANGER_DISTANCE)
{
actionGoBackward();
Serial.println(", go backward");
}
else if (min_dist > SECURE_DISTANCE)
{
actionGoForward();
Serial.println(", go forward");
}
delay(500);
}
}
int checkDistance(float & min_distance, float & left_distance, float & right_distance)
{
min_distance = RANGE_UPPER_LIMIT;
left_distance = RANGE_LOWER_LIMIT;
right_distance = RANGE_LOWER_LIMIT;
if (servo_step >= SERVO_MAX_STEP)
{
servo_dir = SERVO_ROTATE_CLOCKWISE;
}
if (servo_step <= SERVO_MIN_STEP)
{
servo_dir = SERVO_ROTATE_COUNTERCLOCKWISE;
}
int servo_offset;
if (servo_dir == SERVO_ROTATE_CLOCKWISE)
{
servo_offset = -SERVO_STEP_INTERVAL;
}
else if (servo_dir == SERVO_ROTATE_COUNTERCLOCKWISE)
{
servo_offset = SERVO_STEP_INTERVAL;
}
else
{
Serial.println("Error: invalid servo_dir");
return -1;
}
while (true)
{
/* read 3 times and get valid minimal values */
float temp_dist1 = read_gp2d12_range(pinGP2D12);
float temp_dist2 = read_gp2d12_range(pinGP2D12);
float temp_dist3 = read_gp2d12_range(pinGP2D12);
float min_value = RANGE_UPPER_LIMIT;
if ((temp_dist1 < min_value) && (temp_dist1 >= RANGE_LOWER_LIMIT && temp_dist1 <= RANGE_UPPER_LIMIT))
{
min_value = temp_dist1;
}
if ((temp_dist2 < min_value) && (temp_dist2 >= RANGE_LOWER_LIMIT && temp_dist2 <= RANGE_UPPER_LIMIT))
{
min_value = temp_dist2;
}
if ((temp_dist3 < min_value) && (temp_dist3 >= RANGE_LOWER_LIMIT && temp_dist3 <= RANGE_UPPER_LIMIT))
{
min_value = temp_dist3;
}
if (min_distance >= min_value)
{
min_distance = min_value;
}
if (servo_step >= SERVO_MAX_STEP)
{
left_distance = min_value;
if (servo_dir == SERVO_ROTATE_COUNTERCLOCKWISE)
{
break;
}
}
else if (servo_step <= SERVO_MIN_STEP)
{
right_distance = min_value;
if (servo_dir == SERVO_ROTATE_CLOCKWISE)
{
break;
}
}
servo_step += servo_offset;
if (servo_dir == SERVO_ROTATE_COUNTERCLOCKWISE && servo_step >= SERVO_MAX_STEP)
{
servo_step = SERVO_MAX_STEP;
}
else if (servo_dir == SERVO_ROTATE_CLOCKWISE && servo_step <= SERVO_MIN_STEP)
{
servo_step = SERVO_MIN_STEP;
}
Serial.print("servo step = ");
Serial.print(servo_step);
Serial.print(", min_value = ");
Serial.println(min_value);
servo.write(servo_step);
}
return 0;
}
void cmdForward()
{
digitalWrite(pinForward, HIGH);
digitalWrite(pinBack, LOW);
}
void cmdBackward()
{
digitalWrite(pinForward, LOW);
digitalWrite(pinBack, HIGH);
}
void cmdRight()
{
digitalWrite(pinLeft, LOW);
digitalWrite(pinRight, HIGH);
}
void cmdLeft()
{
digitalWrite(pinLeft, HIGH);
digitalWrite(pinRight, LOW);
}
void cmdStop()
{
digitalWrite(pinForward, LOW);
digitalWrite(pinBack, LOW);
}
void cmdNeut()
{
digitalWrite(pinLeft, LOW);
digitalWrite(pinRight, LOW);
}
void actionTurnLeft()
{
cmdForward();
cmdLeft();
delay(1000);
}
void actionTurnRight()
{
cmdForward();
cmdRight();
delay(1000);
}
void actionGoForward()
{
cmdForward();
cmdNeut();
delay(1000);
}
void actionGoBackward()
{
cmdBackward();
cmdNeut();
delay(1000);
}
Can anyone give me some advise to get stable IR ranges?
Thank you!