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[10]; //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[9] = 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.