XRAD'S How to find a SET of GROUPED PEAK values in an array?

Projects Programming
1

I have an US sensor HC-sr04 which detects a range of distance values while sweeping on a servo. Servo angle ranges from 10 to 170. I can record the distance and angles to two arrays. I need to find the greatest (most distant) set of distance values for say an angle range of 15 degrees. this will then be used to drive a robot to those degrees. I can find the greatest distance, but I do not know how to approach finding a SET of greatest distance values located in a ~15- 20 degree sweep (15 or maybe 20 values). I would then drive the robot towards the middle angle value of the SET of longest distance readings. This is to avoid a spurious LONG distance finding. Anyone with suggestions? Help much appreciated!

and BTW, I looked at scipy.signal find_peaks and semi ported it to C++, but it was beyond my understanding... (code below)...

basic code:

#include <Ultrasonic.h>
#include <Servo.h>

Ultrasonic ultrasonic(2, 20);
Servo servo;

int angle = 0;
const int led1 = 13;


int distanceUltrasound[160];
int arrayIndexUS = 0;
int distanceUS = 0;

int distanceAngle[160];
int arrayIndexAngle = 0;
int distanceAng = 0;


void setup() {
  Serial.begin(9600);
  servo.attach(11);
  servo.write(angle);
}

void loop() {
  // scan from 0 to 180 degrees
  for (angle = 10; angle < 170; angle++) {
    distanceUS = ultrasonic.distanceRead();
    Serial.print("Distance in cm: ");
    Serial.println(distanceUS);
    distanceUltrasound[arrayIndexUS++] = distanceUS;
    distanceAngle[arrayIndexAngle++] = angle;
    servo.write(angle);
    delay(10);
  }
//for debugging
  if (arrayIndexUS = 160) {
    for (int i = 0; i < arrayIndexUS; i++) {
      Serial.println(distanceUltrasound[i]);
      delay(10);
    }
    arrayIndexUS = 0;
  }
//for debugging
if (arrayIndexAngle = 160) {
    for (int i = 0; i < arrayIndexAngle; i++) {
      Serial.println(distanceAngle[i]);
      delay(10);
    }
    arrayIndexAngle = 0;
  }

  // now scan back from 180 to 0 degrees
  for (angle = 170; angle > 10; angle--) {
    Serial.print("Distance in cm: ");
    Serial.println(ultrasonic.distanceRead());
    servo.write(angle);
    delay(10);
  }
}

scipy C++ conversion???

#include <vector>
#include <algorithm>
#include <cmath>




struct Peak {
    int index;
    double height;
};

std::vector<Peak> findPeaks(const std::vector<double>& data, double minHeight = -INFINITY, int minDistance = 1) {
    std::vector<Peak> peaks;
    for (size_t i = 1; i < data.size() - 1; ++i) {
        if (data[i] > data[i - 1] && data[i] > data[i + 1] && data[i] >= minHeight) {
            bool tooClose = false;
            for (const auto& peak : peaks) {
                if (std::abs(static_cast<int>(i) - peak.index) < minDistance) {
                    tooClose = true;
                    break;
                }
            }
            if (!tooClose) {
               peaks.push_back({static_cast<int>(i), data[i]});
            }
        }
    }
    return peaks;
}
2

What is the response angle of your ultrasonic receiver? The one I have is almost omnidirectional and that will not work well for this project.

3

This array seems pointless. It will always hold the angles generated by the for-loop. It's a waste of memory to store them when you can calculate the angles from the array index.

The two index variables always have the same value. They both start at zero and get incremented together. It's pointless having two index variables when only one is needed.

This code takes readings and stores them in the array for the wrong angles. The servo should be moved to the angle, and given sufficient time to reach it, before the distance is read. At best, they will all be out by 1 degree. For the first reading taken, who knows what angle that was read at?

What is that? Why don't you post a link?

How much did you alter it when you ported it? Because from the little of it I have read through so far, it's possible that it was beyond the original author's understanding too. I would not recommend using it, it's full of errors and inefficiencies.

4

I would calculate a moving average with each sweep. No need to store anything but the average, the current maximum average, and the angle at which the max occurred.

5

The problem I see with this idea is that you don't know what is a spurious long or short distance reading, or a genuine long or short reading, because the servo moves angles, in small steps, very frequently, with only one reading taken at each angle. Perhaps it would be better to move in 5 or 10 degree steps and take more readings at each step. Then the readings taken at an angle could be filtered for spurious readings before taking an average.

I agree with @herbschwarz. Unless your sensor has a reasonably narrow beamwidth, sweeping it with a servo is pointless.

6

THANKS for replies (EDIT SP) !! I took your advice on multiple points. Changed to VL53L1x laser. Increased servo pos change to 4 degrees. Changed to single array as I already know the angles where the reads were obtained based on their array position and which way the servo was turning. Getting great nonblocking reads with the laser using code below. next step, I will try to find a way to get the SET of longest grouped reads (not just a single read, not an average) and then drive robot in that direction. see picture for my idea... but this is easily changed for a better method.

(note: servo sweep was originlly 45 degree off center, now 80)

dgm laser
dgm laser1224×1632 41.7 KB

this is the link to signal processing at SciPy. It seems to be close to what I need. I will try to work with it some more...

 
#include <Servo.h>
#include <Wire.h>
#include <VL53L1X.h>


VL53L1X sensor;
 
Servo servo;

int angle = 0;
const int led1 = 13;


int distanceLaserArray[40];//only 40 reads per scan
int arrayIndexLaser = 0;
int distanceLaser = 0;
 


void setup() {
  Serial.begin(9600);

  Wire.begin();
  Wire.setClock(400000);  // use 400 kHz I2C serial1
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1)
      ;
  }
  sensor.setDistanceMode(VL53L1X::Short);
  //sensor.setPresetMode(&dev, VL53L1X, PRESETMODE_AUTONOMOUS);
 // sensor.setMeasurementTimingBudget(500000);  //not needed w/dataReady()
  sensor.startContinuous(10);

  servo.attach(11);
  servo.write(angle);
}

void loop() {
  // scan from 0 to 180 degrees
  for (angle = 10; angle <= 170; angle = angle +4) {
    sensor.dataReady();
    distanceLaser = (sensor.read(1));
    Serial.print("Distance in mm: ");
    Serial.println(distanceLaser);
    distanceLaserArray[arrayIndexLaser++] = distanceLaser; 
    //angle=angle+5;
    servo.write(angle);
    delay(10);
  }

 arrayIndexLaser = 0;
/*
  if (arrayIndexLaser = 40) {
    for (int i = 0; i < arrayIndexLaser; i++) {
      Serial.println(distanceLaserArray[i]);
      delay(10);
    }
    arrayIndexLaser = 0;
  }
*/

  // now scan back from 180 to 0 degrees
  for (angle = 170; angle >= 10; angle= angle-4) { 
    sensor.dataReady();
    distanceLaser = (sensor.read(1));
    Serial.print("Distance in mm: ");
    Serial.println(distanceLaser);
    distanceLaserArray[arrayIndexLaser++] = distanceLaser; 
    servo.write(angle);
    delay(10);
  }
  
   arrayIndexLaser = 0;
 /* 
    if (arrayIndexLaser = 40) {
    for (int i = 0; i < arrayIndexLaser; i++) {
      Serial.println(distanceLaserArray[i]);
      delay(10);
    }
    arrayIndexLaser = 0;
  }
  */
}
7

I think I have a plan now. I can find the PEAK distance value with code below, then I will find one or two values to either side of the PEAK. If these 3-5 values fall within a 10-20 degree range and are within say 5-10% of the PEAK value, then proceed in that direction.....direction easily calculated from the index in the array since the angle of servo and array location coincide (160 degree sweep, 40 elements in array, 4 degree per measurement)...depending on sweep direction..

ex data:
The max value (%d) is: 407
is at index %d: 38


  int max_v = INT_MIN;
	int max_i = 0;

	for ( int i = 0; i < sizeof(distanceLaserArray)/sizeof(distanceLaserArray[0]); i++ )
	{
		if ( distanceLaserArray[i] > max_v )
		{
			max_v = distanceLaserArray[i];
			max_i = i;
		}
	}
	
  Serial.print("The max value (%d) is: ");
  Serial.println(max_v);
  Serial.print("is at index %d: ");
  Serial.println( max_i);
8

Is your vehicle pointing in only one direction as it moves? You seem to have neglected bearing.

9

If you are sampling every 4 degrees, then this condition will always be satisfied. So why bother.

What if they're not. This doesn't seem to be a very robust approach. Or you didn't describe your plan very well.

10

Hi, thanks for replies!! Dave, yes, you are right, those measurements will always fall in that range, so I adjusted for that in my code! Thx. OK, have written a good base code to work with. Bearing and drive functions are already written using compass and US detector (XFPD, thx!: see my other XRAD azimuth post...did not want to repeat issue here). It is this second distance sensor on robot head that I was having issues with and the forum directed me in the right way! Using a very focused laser, I can now get good definition of obstacles in a relatively fast servo scan time. If laser MAX distance fall OUT of range, a turn angle is NOT created. When in range, a NEW turn angle is calculated, which feeds into the turn funtion controlling motors via heading/compass. It is a basic autonomous control system which evaluates obstacles further out than the US sensor, so that robot can plan turn direction in advance. nothing as fancy as SLAM...but a cool project to figure out! yes...two sensors,,,why?? because.....

serial data example:

Distance in mm: 452
Distance in mm: 451
Distance in mm: 455
Distance in mm: 451
Distance in mm: 429
Distance in mm: 390
Distance in mm: 369
The max value (%d) is: 455
The max value MINUS is: 451
The max value PLUS is: 451
MAX is at index %d: 26
% Minus: 99
% Plus: 99
Drive Angle: -22


#include <Servo.h>
#include <Wire.h>
#include <VL53L1X.h>


VL53L1X sensor;

Servo servo;

int angle = 0;
const int LED = 13;

int distanceLaserArray[30];  //30 readings for total sweep
int arrayIndexLaser = 0;
int distanceLaser = 0;

int driveAngle = 0;


void setup() {
  Serial.begin(9600);

  Wire.begin();
  Wire.setClock(400000);  // use 400 kHz I2C serial1
  sensor.setTimeout(500);
  sensor.init();

  //ToF laser stuff
  sensor.setDistanceMode(VL53L1X::Short);
  //sensor.setPresetMode(&dev, VL53L1X, PRESETMODE_AUTONOMOUS);
  //sensor.setMeasurementTimingBudget(500000);  //not needed w/dataReady()
  sensor.startContinuous(10);

  servo.attach(11);
  servo.write(angle);
  pinMode(LED, OUTPUT);
  arrayIndexLaser = 0;  //start at first array location
}

void loop() {

  // scan laser from 60 to 120 degrees
  for (angle = 60; angle <= 120; angle = angle + 2) {
    sensor.dataReady();
    distanceLaser = (sensor.read(1));
    Serial.print("Distance in mm: ");
    Serial.println(distanceLaser);
    distanceLaserArray[arrayIndexLaser++] = distanceLaser;
    servo.write(angle);
  }
  arrayIndexLaser = 0;
  findBestAngleLaser();
  /*//debugging stuff
  if (arrayIndexLaser = 40) {
    for (int i = 0; i < arrayIndexLaser; i++) {
      Serial.println(distanceLaserArray[i]);
      delay(50);
    }
    arrayIndexLaser = 0;
  }
*/
  // now scan back from 120 to 60 degrees
  for (angle = 120; angle >= 60; angle = angle - 2) {
    sensor.dataReady();
    distanceLaser = (sensor.read(1));
    Serial.print("Distance in mm: ");
    Serial.println(distanceLaser);
    distanceLaserArray[arrayIndexLaser++] = distanceLaser;
    servo.write(angle);
  }
  arrayIndexLaser = 0;
  findBestAngleLaser();
}

//============find best drive angle with LASER =========

void findBestAngleLaser() {
  //MAX distance values from array
  int max_v = 0;
  int max_v_minus = 0;
  int max_v_plus = 0;

  int max_i = 0;  //array element location

  //get PEAK values and array locations
  for (int i = 0; i < sizeof(distanceLaserArray) / sizeof(distanceLaserArray[0]); i++) {
    if (distanceLaserArray[i] > max_v) {
      //PEAK distance value
      max_v = distanceLaserArray[i];
      max_i = i;
      //Array location +/- 1 from MAX
      max_v_minus = distanceLaserArray[i - 1];
      max_v_plus = distanceLaserArray[i + 1];
    }
  }
  Serial.print("The max value (%d) is: ");
  Serial.println(max_v);
  Serial.print("The max value MINUS is: ");
  Serial.println(max_v_minus);
  Serial.print("The max value PLUS is: ");
  Serial.println(max_v_plus);
  Serial.print("MAX is at index %d: ");
  Serial.println(max_i);

  //calculate percent deviation from MAX distance and if OK....
  int percentMinus = max_v_minus * 100 / max_v;
  int percentPlus = max_v_plus * 100 / max_v;
  Serial.print("% Minus: ");
  Serial.println(percentMinus);
  Serial.print("% Plus: ");
  Serial.println(percentPlus);

  if (max_v <= 1000 && max_v >= 50) {//kinda ballparking range here, will test when assemled
    if ((percentMinus >= 90) && (percentPlus >= 90)) {  //need a few adjacent similar reads to move forward, no spurious reads
      //digitalWrite(LED, HIGH);//test me
      Serial.print("% Minus: ");
      Serial.println(percentMinus);
      Serial.print("% Plus: ");
      Serial.println(percentPlus);

      //create turn angle here!!!!!!
      int y = map(max_i, 1, 30, 30, -30);  //move 2 degrees for every array location
      driveAngle = y;                      //goes into turn funtion argument

      Serial.print("Drive Angle: ");
      Serial.println(driveAngle);
      //delay(5000);//test me
    }
    //digitalWrite(LED, LOW);//test me
  }
}
11

BTW...i did make an update to above code mapping of angle for reverse servo sweep..

  //create turn angle here!!!!!!
      if (mapAngleReverse == false) {
        newDriveAngle = map(max_i, 1, 30, 30, -30);  //move 2 degrees for every array location
      } else {
        newDriveAngle = map(max_i, 1, 30, -30, 30);
      }

??but i guess I shuold be mapping the index correctly ...as zero and 29 are the min and max array locations....
newDriveAngle = map(max_i, 0, 29, 30, -30);