Pages: [1]   Go Down
Author Topic: Sharp GP2D12 range is unstable when moving  (Read 678 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

Code:
#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!
« Last Edit: June 03, 2012, 03:08:48 am by achenal » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I found the reason. It is because the servo is still rotating when making another IR range reading. The problem can be fixed by adding delay(200) after servo.write(servo_step).
Logged

Pages: [1]   Go Up
Jump to: