RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Object perception with distance sensors

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-29 15:35:56 +01:00
parent e51f5a38e0
commit 4f7e911be1
2 changed files with 46 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
DistanceSensor.cpp
View File

@ -1,16 +1,12 @@
#include "DistanceSensor.h"
DistanceSensor::DistanceSensor() {
this->type = Thing::DistanceSensorType;
}
DistanceSensor::DistanceSensor() { this->type = Thing::DistanceSensorType; }
DistanceSensor::DistanceSensor(float triggerDistance) {
this->triggerDistance = triggerDistance;
}
float DistanceSensor::GetDistance() {
return distance;
};
float DistanceSensor::GetDistance() { return distance; };
bool DistanceSensor::ObjectNearby() {
bool isOn = GetDistance() <= triggerDistance;

62
Perception.cpp
View File

@ -84,25 +84,25 @@ bool Perception::ObjectNearby(float direction, float range) {
if (range < 0)
range = -range;
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
if (placement.horizontalDirection > direction - range &&
placement.horizontalDirection < direction + range) {
Thing *thing = placement.thing;
if (thing == nullptr)
continue;
// for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
// Placement placement = sensorPlacements[sensorIx];
// if (placement.horizontalDirection > direction - range &&
// placement.horizontalDirection < direction + range) {
// Thing *thing = placement.thing;
// if (thing == nullptr)
// continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
return true;
} else if (thing->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn())
return true;
}
}
}
// if (thing->type == Thing::DistanceSensorType) {
// DistanceSensor *distanceSensor = (DistanceSensor *)thing;
// if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
// return true;
// } else if (thing->type == Thing::SwitchType) {
// Switch *switchSensor = (Switch *)thing;
// if (switchSensor != nullptr && switchSensor->IsOn())
// return true;
// }
// }
// }
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx];
@ -243,6 +243,32 @@ void Perception::Update(float currentTimeMs) {
lastUpdateTimeMs = currentTimeMs;
// Update sensing
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement thingPlacement = sensorPlacements[sensorIx];
Thing *thing = thingPlacement.thing;
if (thing == nullptr)
continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
printf("S%d: %d %f\n", sensorIx, (int)distanceSensor,
distanceSensor->GetDistance());
if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) {
Polar position = Polar(thingPlacement.horizontalDirection,
distanceSensor->GetDistance());
AddPerceivedObject(position);
}
} else if (thing->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn()) {
Polar position =
Polar(thingPlacement.horizontalDirection, nearbyDistance);
AddPerceivedObject(position);
}
}
}
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx];
if (obj == nullptr)