SHARE
TWEET

IR sensor smoothing http://darylrobotproject.wordpress.com

a guest Jan 25th, 2013 138 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. int getSmoothIRSensor(int sensorpin, char character){
  2.    
  3.    switch (character){
  4.     case 'X':
  5.       digitalWrite(leftSensorIr, HIGH);
  6.       break;
  7.     case 'Y':
  8.       digitalWrite(centerSensorIr, HIGH);
  9.       break;
  10.     case 'Z':
  11.       digitalWrite(rightSensorIr, HIGH);
  12.       break;
  13.     }
  14.  
  15.    delay(5);  
  16.    int returnval = 0;
  17.    returnval = irValueToCm(digitalRead(sensorpin));
  18.    
  19.    digitalWrite(leftSensorIr, LOW);
  20.    digitalWrite(centerSensorIr, LOW);
  21.    digitalWrite(rightSensorIr, LOW);
  22.    
  23.    int sortedValues[NB_LECTURE];
  24.    
  25.    for(int i=0;i<NB_LECTURE;i++){
  26.      
  27.      int value = irValueToCm(analogRead(sensorpin));
  28.      int j;
  29.      if(value<sortedValues[0] || i==0){
  30.        
  31.         j=0; //insert at first position
  32.        
  33.      }else{
  34.        
  35.        for(j=1;j<i;j++){
  36.          
  37.           if(sortedValues[j-1]<=value && sortedValues[j]>=value){
  38.             // j is insert position
  39.             break;
  40.           }
  41.        }
  42.      }
  43.      
  44.      for(int k=i;k>j;k--){
  45.        // move all values higher than current reading up one position
  46.        sortedValues[k]=sortedValues[k-1];
  47.      }
  48.      
  49.      sortedValues[j]=value; //insert current reading
  50.      
  51.    }
  52.    
  53.    //return scaled mode of NB_MEDIAN*2 values
  54.    for(int i=NB_LECTURE/2-NB_MEDIAN;i<(NB_LECTURE/2+NB_MEDIAN);i++){
  55.      
  56.      returnval +=sortedValues[i];
  57.      
  58.    }
  59.    returnval = returnval/(NB_MEDIAN*2);
  60.    return returnval;
  61. }
RAW Paste Data
Top