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

Cleanup Placement

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-05 10:21:40 +01:00
parent 8e432e4ef9
commit 7c5a531e0c
3 changed files with 53 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Perception.cpp
View File

@ -52,8 +52,10 @@ float Perception::DistanceForward(float angle) {
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
float sensorAngle = placement.direction.z;
if (sensorAngle > 0 && sensorAngle < angle) {
if (placement.verticalDirection > -angle &&
placement.verticalDirection < angle &&
placement.horizontalDirection > -angle &&
placement.horizontalDirection < angle) {
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
}
@ -69,7 +71,7 @@ float Perception::DistanceLeft(float angle) {
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
float sensorAngle = placement.direction.y;
float sensorAngle = placement.horizontalDirection;
// Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle);
if (sensorAngle < 0 && sensorAngle > -angle) {
minDistance = fmin(minDistance, distanceSensor->GetDistance());
@ -87,7 +89,7 @@ float Perception::DistanceRight(float angle) {
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
float sensorAngle = placement.direction.y;
float sensorAngle = placement.horizontalDirection;
if (sensorAngle > 0 && sensorAngle < angle) {
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
@ -104,7 +106,7 @@ float Perception::DistanceUp(float angle) {
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
float sensorAngle = placement.direction.y; // not correct!
float sensorAngle = placement.verticalDirection;
if (sensorAngle > 0 && sensorAngle < angle) {
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
@ -121,7 +123,7 @@ float Perception::DistanceDown(float angle) {
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
float sensorAngle = placement.direction.y; // not correct!
float sensorAngle = placement.verticalDirection;
if (sensorAngle < 0 && sensorAngle > -angle) {
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
@ -135,7 +137,7 @@ bool Perception::SwitchOn(float fromAngle, float toAngle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
float angle = placement.direction.y;
float angle = placement.horizontalDirection;
if (angle > fromAngle && angle < toAngle) {
Thing* thing = placement.thing;
if (thing == nullptr)
@ -171,7 +173,7 @@ float Perception::GetDistance(float angle) {
} else {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
float placementAngle = placement.direction.x;
float placementAngle = placement.horizontalDirection;
if (placementAngle == angle) {
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
return distanceSensor->GetDistance();
@ -208,7 +210,7 @@ DistanceSensor* Perception::GetSensor(float angle) {
for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
Placement placement = this->sensorPlacements[ix];
if (abs(placement.direction.y - angle) < 0.01F)
if (abs(placement.horizontalDirection - angle) < 0.01F)
return (DistanceSensor*)placement.thing;
}

25
Placement.cpp
View File

@ -5,20 +5,17 @@ Placement::Placement() {
this->thing = nullptr;
}
Placement::Placement(Thing* thing, Vector3 position) {
this->position = position;
this->direction = Vector3::zero;
Placement::Placement(Thing* thing,
Vector3 position,
float horizontalDirection,
float verticalDirection) {
this->thing = thing;
this->position = position;
this->horizontalDirection = horizontalDirection;
this->verticalDirection = verticalDirection;
}
Placement::Placement(Thing* thing, Vector3 position, float horizontalDirection) {
this->position = position;
this->direction = Vector3(0, horizontalDirection, 0);
this->thing = thing;
}
Placement::Placement(Thing* thing, float horizontalDirection) {
this->position = Vector3::zero;
this->direction = Vector3(0, horizontalDirection, 0);
this->thing = thing;
}
Placement::Placement(Thing* thing,
float horizontalDirection,
float verticalDirection)
: Placement(thing, Vector3::zero, horizontalDirection, verticalDirection) {}

36
Placement.h
View File

@ -10,20 +10,46 @@
namespace Passer {
namespace RoboidControl {
/// @brief A plament is used to specify where a Thing is placed on the Roboid.
class Placement {
public:
/// @brief Default constructor with a zero placement
Placement();
Placement(Thing* thing, Vector3 position);
Placement(Thing* thing, Vector3 position, float horizontalAngle);
Placement(Thing* thing, float horizontalAngle);
/// @brief Placement of a Thing on the Roboid
/// @param thing The Thing which is placed
/// @param position The position of the Thing in carthesian coordinated
/// @param horizontalDirection The horizontal direction angle of the Thing
/// @param verticalAngle The vertical direction angle of the Thing
Placement(Thing* thing,
Vector3 position = Vector3::zero,
float horizontalDirection = 0.0F,
float verticalAngle = 0.0F);
/// @brief Placement of a Thing on the roboit without position
/// @param thing The Thing which is place
/// @param horizontalDirection The horizontal direction angle of the Thing
/// @param verticalAngle The vertical direction angle of the Thing
Placement(Thing* thing,
float horizontalDirection,
float verticalAnlge = 0.0F);
/// @brief The parent placement in the Roboid hierarchy
/// @remark Reserved for future use
Placement* parent = nullptr;
/// @brief An array of children of this placement in the Roboid hierarchy
/// @remark Reserved for future use
Placement** children = nullptr;
/// @brief The number of children of this placemet in the Roboid hierarchy
/// @remark Reserved for future use
unsigned int childCount = 0;
Vector3 position;
Vector3 direction;
/// @brief The Thing which is placed
Thing* thing;
/// @brief The position of the Thing in carthesian coordinates
Vector3 position;
/// @brief The angle or direction of the Thing in the horizontal plane
float horizontalDirection;
/// @brief The angle or direction of the Thing in the vertical plane
float verticalDirection;
};
} // namespace RoboidControl