# help with simple algorithm IR sensor and servo

Hi, I want to control a servo motor with an IR sharp sensor.

Now, the main issue is that I don’t want to pilot the servo with the raw data from the sensor because I need it to be stable; so what I did is creating an array and calculate the average.
What I also want to do is that if the avg value is in a range -+3 from the previous one, the servo just won’t update itself (so that a small change in the values wont affect the servo position).
Another thing that I did is that every too big/small value coming in from the sensor, will be approximated to the max/min value inside the array, before being added to it. (to avoid glitches or potential wrong values to be considered)
Finally, only the values in a specific range from the sensor (6 to 80, assuming that the sensor won’t work between 0 and 5 cm). Everything above 80 should just be ignored and then 80 should be the default incoming signal when nothing interfere with the sensor beam.

I’m not sure everything is working properly and I think I might have done too much stuff to accomplish my task so I can’t find the problem. Any hint appreciated ``````#include <Servo.h>
#include <SharpIR.h>

#define SERVO 9
#define SHARP A0

Servo myServo;  //servo obj
SharpIR sharp(SHARP,25,93,1080); //using IR library so the distance coming in is in cm

int pos = 0;    //servo position
int val = 0;    //sharp input val
int servoUpd = 0;
float avg = 0; //average

int myValues; //array of values from the IR

void setup()
{
myServo.attach(SERVO);  // attaches the servo on pin 9 to the servo object
myServo.write(0); //init servo
pinMode(SHARP, INPUT);

Serial.begin(9600);

//array init
int i;
for(i=0;i<10;i++)
myValues[i] = 0;
}

void loop()
{
int int_avg;
int i=0;

int dis = sharp.distance(); //distance (cm)

if(dis > 80) //80 è is the sensor max limit
dis = 80;

//  calculate average

for(;i<10;i++)
avg += myValues[i]/10;

int_avg = (int)avg+10; //add avg dist top_head-ear needed for the project, just adding some more distance to the average value from the IR

if(dis > int_avg+3 || dis < int_avg-3){ //servo internal limit - update the servo only if dis is in range -+3 from the average
servoUpd = map(int_avg, 10, 80, 0, 90);
myServo.write(servoUpd);
delay(15);
}

//get rid of glitches: if the value read from the sensor is too big/small, it will be reduced to the max/min of the array

if(dis > int_avg+20){  //max
int massimo = 0;
for(i=0;i<10;i++){
if(myValues[i] > massimo)
massimo = myValues[i];
}
dis = massimo;
}else if(dis < int_avg-20){ //min
int minimo = 9999;
for(i=0;i<10;i++){
if(myValues[i] < minimo)
minimo = myValues[i];
}
dis = minimo;
}

//update the array by shifting its values and adding the last read value from the sensor
pointer_shift(myValues, 10);
myValues = dis;

avg = 0; //reset the average
delay(100);

}

//function to shift the values into the array

void pointer_shift(int *a, int n) {
int i;
for(i=0;i!=n-1;i++){
*(a+i)=*(a+i+1);
}
}
``````

particularly I’m a bit lost about where I should put all the ‘limits’…

thank you!

EDIT: I found that eliminating the -+ 3 range actually the servo works fine wen there are object in the range.

``````  for(;i<10;i++)
avg += myValues[i]/10;
``````

Adding the values and then dividing, once, will give a better average.

``````//get rid of glitches
``````

After you have added them to the array, averaged them, and moved the servo. Seems useless at this point…

pointer_shift is a lousy name for a function that is moving elements in an array.

PaulS:

``````//get rid of glitches
``````

After you have added them to the array, averaged them, and moved the servo. Seems useless at this point...

in fact it is done before adding the value to the array..

ok so I changed a few things…

to update the servo angle the average of the values must be different from the previous average. as the angle value given to the servo is an integer, very small changes of decimal numbers in the avg are ignored

``````#include <Servo.h>
#include <SharpIR.h>

#define SERVO 9
#define SHARP A0

Servo myServo;  //servo obj
SharpIR sharp(SHARP,25,93,1080);

int pos = 0;    //servo position
int val = 0;    //sharp input val
int servoUpd = 0;
float avg = 0; //media
int oldAvg = 0;

int myValues;

void setup()
{
myServo.attach(SERVO);  // attaches the servo on pin 9 to the servo object
myServo.write(0); //init servo
pinMode(SHARP, INPUT);

Serial.begin(9600);

//array init
int i;
for(i=0;i<10;i++)
myValues[i] = 0;
}

void loop()
{
int int_avg;
int i=0;

buttState = digitalRead(BUTT); //button per schermo
int dis = sharp.distance();//distance (cm)
dis+=2; //library seems 2cm short than the real measurement

//  calc avg
for(;i<10;i++)
avg += myValues[i];

int_avg = (int)avg/10;

/** needed for the project **/
if(int_avg > 30)
int_avg+=12;
else if(int_avg < 30 && int_avg > 30)
int_avg+=13;
else
int_avg+=14;

//servo internal check - update only if the avg has changed

if(int_avg > oldAvg || int_avg < oldAvg){

servoUpd = calc_angle(int_avg+n);
myServo.write(servoUpd);
delay(15);
}

//get rid of glitches*/
if(dis > int_avg+20){ // > than max
int massimo = 0;
for(i=0;i<10;i++){
if(myValues[i] > massimo)
massimo = myValues[i];
}
dis = massimo;
}else if(dis < int_avg-20){ // < than min
int minimo = 9999;
for(i=0;i<10;i++){
if(myValues[i] < minimo)
minimo = myValues[i];
}
dis = minimo;
}

//update array
arr_shift(myValues, 10);
myValues = dis;

oldAvg = int_avg;  //update the old average
avg = 0; //reset

}

int calc_angle(int val){

float ipoten = sqrt(sq(45)+sq(val));
float cosAlpha = 45/ipoten;
int angle = 180 - (cosAlpha*4068)/71; //the angle needed will be between 180 and 90 but, as all the other values are in range 0-90, subtracting the value from 180 will do the trick

return (int)angle;
}

void arr_shift(int *a, int n) {
int i;
for(i=0;i!=n-1;i++){
*(a+i)=*(a+i+1);
}
}
``````