[HardDrive]

Lets suppose I am getting feedback from a sensor. In this case, a sonic range finder. The results come back on a scale of 2-100cm. That output is then converted into an input value for a servo motor, and the servo has a needle attached, and sits in the middle of dial. A very simple range finder.

The problem I am having is that the range finder kicks out spurious results. Its right 95% of the time, but unless its sitting in a perfectly square environment, the less than perfect echos it gets back cause incorrect results. How could I calm the data so that the needle isn't hunting wildly?

Code is below. The servo operates on a scale of 100-600. The output from the range sensor is 2-100cm. The (rather crude) conversion is denoted by *************** in the code below.

Can anyone recommend a method of calming the results so that the spurious results don't cause the needle to hunt wildly?




#include
#include

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN 100 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)



// Pin number constants
const int triggerPin = 22;
const int echoPin = 31;
long int distanceServo = 100;
int servonum = 14;

void setup() {
// initialize serial communication:
Serial.begin(9600);
pwm.begin();
pwm.setPWMFreq(60);
}

void loop()
{
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, feet, inches, cm;

// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(triggerPin, OUTPUT);
digitalWrite(triggerPin, LOW);
delayMicroseconds(2);
digitalWrite(triggerPin, HIGH);
delayMicroseconds(5);
digitalWrite(triggerPin, LOW);

// The echo pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);

// convert the time into a distance
cm = microsecondsToCentimeters(duration);
Serial.print(cm);
Serial.print("cm");
Serial.println();
distanceServo = ((cm * 4) + 100);***************************************
updateServo(distanceServo);
delay(500);
}

void updateServo(int pulselen)
{

Serial.print("updateservo");
Serial.println();
pwm.setPWM(14, 0, pulselen);
Serial.print(pulselen);
Serial.println();
}

long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}