Go Down

Topic: Sharp GP2D12 range is unstable when moving (Read 794 times) previous topic - next topic

achenal

Jun 03, 2012, 09:55 am Last Edit: Jun 03, 2012, 10:08 am by achenal Reason: 1
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: [Select]
#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!

achenal

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).

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy