Here is code for two Maxbotix sensors with a mode filter. It seems a little slow, and even with the mode filter the sensors read 2" to 4" short. Any input is appreciated.
/* Two LV Maxsonar EZ1 sensors with mode filter by Dan Shields
Adapted from Jason Lessels, Bruce Allen, and Bill Gentles.
*/
int button = 7;
int PIRleft = A3;
int LEDleft = 12;
int PIRright = A4;
int LEDright = 8;
int motorL = 9; //forward left
int motorR = 10; //forward right
int SonarR = 5;
int TriggerR = 2;
int SonarL = 3;
int TriggerL = 4;
int val; // variable for reading the pin status
int val2; // variable for reading the delayed status
int buttonState = 0; // variable to hold the button state
int robotMode = 0;
int arraysizeR = 9; //quantity of values to find the median (sample size). Needs to be an odd number
//declare an array to store the samples. not necessary to zero the array values here, it just makes the code clearer
int rangevalueR[] = {
0, 0, 0, 0, 0, 0, 0, 0, 0};
long pulseR;
int modER;
long valueR = 0;
int arraysizeL = 9; //quantity of values to find the median (sample size). Needs to be an odd number
//declare an array to store the samples. not necessary to zero the array values here, it just makes the code clearer
int rangevalueL[] = {
0, 0, 0, 0, 0, 0, 0, 0, 0};
long pulseL;
int modEL;
long valueL = 0;
void setup() {
pinMode(button, INPUT);
pinMode(LEDright, OUTPUT);
pinMode(LEDleft, OUTPUT);
pinMode(TriggerL, OUTPUT);
pinMode(TriggerR, OUTPUT);
pinMode(motorR, OUTPUT);
pinMode(motorL, OUTPUT);
Serial.begin(9600);
}
void loop() {
distCalcL();
distCalcR();
if (modEL >= 14 && modEL <= 36) {
digitalWrite(LEDleft, HIGH);
}
else if (modEL < 14 || modEL > 36) {
digitalWrite(LEDleft, LOW);
}
if (modER >= 14 && modER <= 36) {
digitalWrite(LEDright, HIGH);
}
else if (modER < 14 || modER > 36) {
digitalWrite(LEDright, LOW);
}
}
int distCalcL() {
pinMode(SonarL, INPUT);
for(int i = 0; i < arraysizeL; i++)
{
digitalWrite(TriggerL, HIGH);
pulseL = pulseIn(SonarL, HIGH);
rangevalueL[i] = pulseL/147;
delay(10);
}
// Serial.print("Unsorted: ");
// printArrayL(rangevalueL,arraysizeL);
isortL(rangevalueL,arraysizeL);
//Serial.print("Sorted: ");
// printArrayL(rangevalueL,arraysizeL);
modEL = modeL(rangevalueL,arraysizeL);
Serial.print("The mode/medianL is: ");
Serial.print(modEL);
Serial.println();
delay(100);
digitalWrite(TriggerL, LOW);
}
/*-----------Functions------------*///Function to print the arrays.
void printArrayL(int *a, int n) {
for (int i = 0; i < n; i++)
{
Serial.print(a[i], DEC);
Serial.print(' ');
}
Serial.println();
}
//Sorting function
// sort function (Author: Bill Gentles, Nov. 12, 2010)
void isortL(int *a, int n){
// *a is an array pointer function
for (int i = 1; i < n; ++i)
{
int j = a[i];
int k;
for (k = i - 1; (k >= 0) && (j < a[k]); k--)
{
a[k + 1] = a[k];
}
a[k + 1] = j;
}
}
//Mode function, returning the mode or median.
int modeL(int *x,int n){
int i = 0;
int countL = 0;
int maxCountL= 0;
int modeL = 0;
int bimodalL;
int prevCountL = 0;
while(i<(n-1)){
prevCountL=countL;
countL=0;
while(x[i]==x[i+1]){
countL++;
i++;
}
if(countL>prevCountL&countL>maxCountL){
modeL=x[i];
maxCountL=countL;
bimodalL=0;
}
if(countL==0){
i++;
}
if(countL==maxCountL){//If the dataset has 2 or more modes.
bimodalL=1;
}
if(modeL==0||bimodalL==1){//Return the median if there is no mode.
modeL=x[(n/2)];
}
return modeL;
}
}
int distCalcR() {
pinMode(SonarR, INPUT);
for(int i = 0; i < arraysizeR; i++)
{
digitalWrite(TriggerR, HIGH);
pulseR = pulseIn(SonarR, HIGH);
rangevalueR[i] = pulseR/147;
delay(10);
}
// Serial.print("Unsorted: ");
// printArrayR(rangevalueL,arraysizeL);
isortR(rangevalueR,arraysizeR);
//Serial.print("Sorted: ");
// printArrayR(rangevalueL,arraysizeL);
modER = modeR(rangevalueR,arraysizeR);
Serial.print("The mode/medianR is: ");
Serial.print(modER);
Serial.println();
delay(100);
digitalWrite(TriggerR, LOW);
}
/*-----------Functions------------*///Function to print the arrays.
void printArrayR(int *a, int n) {
for (int i = 0; i < n; i++)
{
Serial.print(a[i], DEC);
Serial.print(' ');
}
Serial.println();
}
//Sorting function
// sort function (Author: Bill Gentles, Nov. 12, 2010)
void isortR(int *a, int n){
// *a is an array pointer function
for (int i = 1; i < n; ++i)
{
int j = a[i];
int k;
for (k = i - 1; (k >= 0) && (j < a[k]); k--)
{
a[k + 1] = a[k];
}
a[k + 1] = j;
}
}
//Mode function, returning the mode or median.
int modeR(int *x,int n){
int i = 0;
int countR = 0;
int maxCountR= 0;
int modeR = 0;
int bimodalR;
int prevCountR = 0;
while(i<(n-1)){
prevCountR=countR;
countR=0;
while(x[i]==x[i+1]){
countR++;
i++;
}
if(countR>prevCountR&countR>maxCountR){
modeR=x[i];
maxCountR=countR;
bimodalR=0;
}
if(countR==0){
i++;
}
if(countR==maxCountR){//If the dataset has 2 or more modes.
bimodalR=1;
}
if(modeR==0||bimodalR==1){//Return the median if there is no mode.
modeR=x[(n/2)];
}
return modeR;
}
}