help with simple algorithm IR sensor and servo

Using Arduino Programming Questions
#1

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 :slight_smile:

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

#2 
  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.

#3

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

#4

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[10];

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[9] = 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);
   }
}
#5