FWIW, I rewrote your sketch, since I plan to try it on my own robot, which has both
EZ1 and EZ4 - tomorrow, as I'm beat tonight. I need to change the I/O pins, etc.
It compiles ok.
The only thing I'm not too sure about is your holding the txpin high for the entire
duration of ALL sonar pings. The usual method is to pulse it low after triggering,
but I imagine the code will get around the loop ok, as it is, before the next
pulse comes out.
Also, thanks for the sort code, I needed that :-).
/* 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()
{
read_sonar( TriggerL, SonarL, rangevalueL, arraysizeL);
isort( rangevalueL, arraysizeL);
modEL = mode( rangevalueL, arraysizeL);
display_output("The mode/medianL is: ", modEL);
delay(100);
}
int distCalcR()
{
read_sonar( TriggerR, SonarR, rangevalueR, arraysizeR);
isort( rangevalueR, arraysizeR);
modER = mode( rangevalueR, arraysizeR);
display_output("The mode/medianR is: ", modER);
delay(100);
}
/*******************************************/
void display_output( char* msg, int mode)
{
Serial.print(msg);
Serial.print(mode);
Serial.println();
}
/**********************************************************/
void read_sonar(int txpin, int rxpin, int* ary, int asiz)
{
pinMode(rxpin, INPUT);
for(int i = 0; i < arraysizeL; i++) {
digitalWrite(txpin, HIGH);
ary[i] = pulseIn(rxpin, HIGH) / 147;
delay(10);
}
digitalWrite(txpin, LOW);
}
/*-----------Functions------------*/
//Function to print the arrays.
//void printArray(int *a, int n)
//{
// for (int i = 0; i < n; i++) {
// Serial.print(a[i], DEC);
// Serial.print(' ');
// }
// Serial.println();
//}
// sort function (Author: Bill Gentles, Nov. 12, 2010)
void isort(int *a, int n)
{
int i, j, k;
// *a is an array pointer function
for( i=1; i < n; ++i) {
j = a[i];
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 mode( int *x, int n )
{
int i=0, countL=0;
int prevCountL=0, maxCountL=0;
int modeL=0;
int bimodalL;
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;
}
}
