RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases 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" #include "DistanceSensor.h"
DistanceSensor::DistanceSensor() { DistanceSensor::DistanceSensor() { this->type = Thing::DistanceSensorType; }
this->type = Thing::DistanceSensorType;
}
DistanceSensor::DistanceSensor(float triggerDistance) { DistanceSensor::DistanceSensor(float triggerDistance) {
this->triggerDistance = triggerDistance; this->triggerDistance = triggerDistance;
} }
float DistanceSensor::GetDistance() { float DistanceSensor::GetDistance() { return distance; };
return distance;
};
bool DistanceSensor::ObjectNearby() { bool DistanceSensor::ObjectNearby() {
bool isOn = GetDistance() <= triggerDistance; bool isOn = GetDistance() <= triggerDistance;

62
Perception.cpp
View File

@ -84,25 +84,25 @@ bool Perception::ObjectNearby(float direction, float range) {
if (range < 0) if (range < 0)
range = -range; range = -range;
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { // for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; // Placement placement = sensorPlacements[sensorIx];
if (placement.horizontalDirection > direction - range && // if (placement.horizontalDirection > direction - range &&
placement.horizontalDirection < direction + range) { // placement.horizontalDirection < direction + range) {
Thing *thing = placement.thing; // Thing *thing = placement.thing;
if (thing == nullptr) // if (thing == nullptr)
continue; // continue;
if (thing->type == Thing::DistanceSensorType) { // if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing; // DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) // if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
return true; // return true;
} else if (thing->type == Thing::SwitchType) { // } else if (thing->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)thing; // Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn()) // if (switchSensor != nullptr && switchSensor->IsOn())
return true; // return true;
} // }
} // }
} // }
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx]; PerceivedObject *obj = perceivedObjects[objIx];
@ -243,6 +243,32 @@ void Perception::Update(float currentTimeMs) {
lastUpdateTimeMs = 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++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
PerceivedObject *obj = perceivedObjects[objIx]; PerceivedObject *obj = perceivedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)