Skip to content

Small Sonar Improvements #152

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ShadedSelf wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Small Sonar Improvements #152

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a00278b
Update sonar.cpp
ShadedSelf Feb 25, 2024
9abd462
Update config_example.h
ShadedSelf Feb 25, 2024
d2014bb
Fixes
ShadedSelf Apr 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions sunray/config_example.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -253,6 +253,7 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
#define SONAR_INSTALLED 1 // uncomment if ultrasonic sensors are installed
//#define SONAR_ENABLE true // should ultrasonic sensor be used?
#define SONAR_ENABLE false
#define SONAR_OBSTACLE_SLOW_CM 30 // slow down mower below this distance from trigger (cm)
#define SONAR_TRIGGER_OBSTACLES true // should sonar be used to trigger obstacles? if not, mower will only slow down
#define SONAR_LEFT_OBSTACLE_CM 10 // stop mowing operation below this distance (cm)
#define SONAR_CENTER_OBSTACLE_CM 10 // stop mowing operation below this distance (cm)
Expand Down
70 changes: 43 additions & 27 deletions sunray/sonar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@



RunningMedian<unsigned int, 9> sonarLeftMeasurements;
RunningMedian<unsigned int, 9> sonarRightMeasurements;
RunningMedian<unsigned int, 9> sonarCenterMeasurements;
RunningMedian<unsigned int, 3> sonarLeftMeasurements;
RunningMedian<unsigned int, 3> sonarRightMeasurements;
RunningMedian<unsigned int, 3> sonarCenterMeasurements;

volatile unsigned long startTime = 0;
volatile unsigned long echoTime = 0;
Expand Down Expand Up @@ -91,20 +91,53 @@ void Sonar::run() {
distanceRight = distanceLeft = distanceCenter = 0;
return;
}

//sonar hit
if (echoDuration != 0) {
added = true;
unsigned long raw = echoDuration;
if (raw > MAX_DURATION) raw = MAX_DURATION;
if (sonarIdx == 0) sonarLeftMeasurements.add(raw);
else if (sonarIdx == 1) sonarCenterMeasurements.add(raw);
else sonarRightMeasurements.add(raw);

//left
if (sonarIdx == 0) {
sonarLeftMeasurements.add(raw);
sonarLeftMeasurements.getMedian(distanceLeft);
distanceLeft = convertCm(distanceLeft);
}
//center
else if (sonarIdx == 1) {
sonarCenterMeasurements.add(raw);
sonarCenterMeasurements.getMedian(distanceCenter);
distanceCenter = convertCm(distanceCenter);
}
//right
else {
sonarRightMeasurements.add(raw);
sonarRightMeasurements.getMedian(distanceRight);
distanceRight = convertCm(distanceRight);
}

echoDuration = 0;
}

if (millis() > timeoutTime) {
if (!added) {
if (sonarIdx == 0) sonarLeftMeasurements.add(MAX_DURATION);
else if (sonarIdx == 1) sonarCenterMeasurements.add(MAX_DURATION);
else sonarRightMeasurements.add(MAX_DURATION);
unsigned int td;
if (sonarIdx == 0) {
sonarLeftMeasurements.add(MAX_DURATION);
sonarLeftMeasurements.getMedian(td);
distanceLeft = convertCm(td);
}
else if (sonarIdx == 1) {
sonarCenterMeasurements.add(MAX_DURATION);
sonarCenterMeasurements.getMedian(td);
distanceCenter = convertCm(td);
}
else if (sonarIdx == 2) {
sonarRightMeasurements.add(MAX_DURATION);
sonarRightMeasurements.getMedian(td);
distanceRight = convertCm(td);
}
}
//if (millis() > nextSonarTime){
sonarIdx = (sonarIdx + 1) % 3;
Expand All @@ -117,23 +150,6 @@ void Sonar::run() {
timeoutTime = millis() + 50; // 10
added = false;
}
if (millis() > nextEvalTime) {
nextEvalTime = millis() + 200;
float value;
//sonarLeftMeasurements.getLowest(distanceLeft);
sonarLeftMeasurements.getMedian(distanceLeft);
//sonar1Measurements.getAverage(avg);
distanceLeft = convertCm(distanceLeft);

//sonarRightMeasurements.getLowest(distanceRight);
sonarRightMeasurements.getMedian(distanceRight);
distanceRight = convertCm(distanceRight);

//sonarCenterMeasurements.getLowest(distanceCenter);
sonarCenterMeasurements.getMedian(distanceCenter);
distanceCenter = convertCm(distanceCenter);

}
#endif
}

Expand Down Expand Up @@ -180,7 +196,7 @@ bool Sonar::nearObstacle()
{
#ifdef SONAR_INSTALLED
if (!enabled) return false;
int nearZone = 30; // cm
int nearZone = SONAR_OBSTACLE_SLOW_CM; // cm
if ((nearObstacleTimeout != 0) && (millis() < nearObstacleTimeout)) return true;
nearObstacleTimeout = 0;
bool res = ((distanceLeft < triggerLeftBelow + nearZone) || (distanceCenter < triggerCenterBelow + nearZone) || (distanceRight < triggerRightBelow + nearZone));
Expand Down