From f33b02311cf0ba7bb18dfa34ac6f9777f02ae07c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 5 Dec 2023 11:13:13 +0100 Subject: [PATCH] Updated Thing --- Perception.cpp | 20 +++++++------- Placement.h | 51 +++++++++++++++++++++++++++++------- Propulsion.cpp | 71 +++++--------------------------------------------- Thing.cpp | 23 ++++++++++++++++ Thing.h | 45 ++++++++++++++++++++------------ 5 files changed, 107 insertions(+), 103 deletions(-) create mode 100644 Thing.cpp diff --git a/Perception.cpp b/Perception.cpp index ee577c1..6d4a2d3 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -13,7 +13,7 @@ void Perception::AddSensors(Placement* things, unsigned int thingCount) { sensorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if ((thing->type & Thing::SensorType) != 0) + if (thing->IsSensor()) sensorCount++; } @@ -22,9 +22,8 @@ void Perception::AddSensors(Placement* things, unsigned int thingCount) { unsigned int sensorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if ((thing->type & Thing::SensorType) != 0) { + if (thing->IsSensor()) sensorPlacements[sensorIx++] = things[thingIx]; - } } } @@ -37,7 +36,7 @@ Sensor* Perception::GetSensor(unsigned int sensorId) { return nullptr; Thing* thing = this->sensorPlacements[sensorId].thing; - if ((thing->type & Thing::SensorType) != 0) + if (thing->IsSensor()) return (Sensor*)thing; return nullptr; @@ -48,7 +47,7 @@ float Perception::DistanceForward(float angle) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; - if ((sensor->type & Thing::SensorType) != 0) + if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; @@ -67,12 +66,11 @@ float Perception::DistanceLeft(float angle) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; - if ((sensor->type & Thing::SensorType) != 0) + if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; float sensorAngle = placement.horizontalDirection; - // Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle); if (sensorAngle < 0 && sensorAngle > -angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } @@ -102,7 +100,7 @@ float Perception::DistanceUp(float angle) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; - if ((sensor->type & Thing::SensorType) != 0) + if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; @@ -119,7 +117,7 @@ float Perception::DistanceDown(float angle) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; - if ((sensor->type & Thing::SensorType) != 0) + if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; @@ -143,11 +141,11 @@ bool Perception::SwitchOn(float fromAngle, float toAngle) { if (thing == nullptr) continue; - if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) { + if (thing->type == Thing::DistanceSensorType) { DistanceSensor* distanceSensor = (DistanceSensor*)thing; if (distanceSensor != nullptr && distanceSensor->IsOn()) return true; - } else if ((thing->type & (int)Thing::Type::Switch) != 0) { + } else if (thing->type == Thing::SwitchType) { Switch* switchSensor = (Switch*)thing; if (switchSensor != nullptr && switchSensor->IsOn()) return true; diff --git a/Placement.h b/Placement.h index b95ba50..6526653 100644 --- a/Placement.h +++ b/Placement.h @@ -11,26 +11,57 @@ namespace Passer { namespace RoboidControl { /// @brief A plament is used to specify where a Thing is placed on the Roboid. +/// +/// It is not always necessary to exactly specify the position and orientation +/// of a Thing. You can use a simple constructor in that case: +/// +/// \code +/// Thing* thing = new Thing(); +/// Placement p = Placement(thing); +/// \endcode +/// +/// The thing can be placed using carthesian coordinates in meters, while the +/// orientation is specified with the horizontal and vertical direction. Whenö +/// both horizontal and vertical direction are zero, the Thing is aligned with +/// the parent thing in the hierarchy. When there is no parent, the thing is +/// directed such that it is looking forward. +/// +/// \code +/// Thing* thing = new Thing(); +/// Placement p = Placement(thing, Vector3(-0.04F, 0.0F, 0.06F), 0.0F, 0.0F); +/// \endcode +/// In the example above, the thing is placed 4 cm to the left and 6 cm to the +/// front relative to the parent. The orientation is the same as the parent. The +/// second line can be simplified, as the default angles are zero and a Vector2 +/// position can be used which is a placement in the horizontal plane: +/// +/// \code +/// Placement p = Placement(thing, Vector2(-0.04F, 0.06F)); +/// \endcode class Placement { public: - /// @brief Default constructor with a zero placement - Placement(); - /// @brief Placement of a Thing on the Roboid + /// @brief Placement of a Thing on a 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 + /// @param position The position of the Thing in carthesian coordinates + /// @param horizontalDirection The horizontal direction angle of the Thing. + /// Negative angles are to the left. + /// @param verticalAngle The vertical direction angle of the Thing. Negative + /// angles are downward. 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 + /// @brief Placement of a Thing on a Roboid 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 + /// @param horizontalDirection The horizontal direction angle of the Thing. + /// Negative angles are to the left. + /// @param verticalDirection The vertical direction angle of the Thing. + /// Negative angles are downward. Placement(Thing* thing, float horizontalDirection, - float verticalAnlge = 0.0F); + float verticalDirection = 0.0F); + /// @brief Default constructor with a zero placement + Placement(); /// @brief The parent placement in the Roboid hierarchy /// @remark Reserved for future use diff --git a/Propulsion.cpp b/Propulsion.cpp index 40fd01f..625b375 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -14,9 +14,9 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { this->motorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if ((thing->type & Thing::MotorType) != 0) + if (thing->IsMotor()) motorCount++; - if (thing->type == (int)Thing::Type::ControlledMotor) + if (thing->type == Thing::ControlledMotorType) hasOdometer = true; } this->placement = new Placement[motorCount]; @@ -24,15 +24,11 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { unsigned int motorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; - if ((thing->type & Thing::MotorType) != 0) + if (thing->IsMotor()) this->placement[motorIx++] = things[thingIx]; } } -// void Propulsion::AddQuadcopter(Quadcopter* quadcopter) { -// this->quadcopter = quadcopter; -// } - unsigned int Propulsion::GetMotorCount() { return this->motorCount; } @@ -42,7 +38,7 @@ Motor* Propulsion::GetMotor(unsigned int motorId) { return nullptr; Thing* thing = this->placement[motorId].thing; - if ((thing->type & Thing::MotorType) != 0) + if (thing->IsMotor()) return (Motor*)thing; return nullptr; @@ -53,7 +49,7 @@ Placement* Propulsion::GetMotorPlacement(unsigned int motorId) { return nullptr; Placement* placement = &this->placement[motorId]; - if ((placement->thing->type & Thing::MotorType) != 0) + if (placement->thing->IsMotor()) return placement; return nullptr; @@ -74,45 +70,6 @@ void Propulsion::SetMaxSpeed(float maxSpeed) { this->maxSpeed = abs(maxSpeed); } -// void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { -// for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { -// Thing *thing = placement[motorIx].thing; -// if (thing->type == Thing::UncontrolledMotorType) { -// Motor *motor = (Motor *)thing; -// if (motor == nullptr) -// continue; - -// float xPosition = placement[motorIx].position.x; -// if (xPosition < 0) -// motor->SetSpeed(leftSpeed); -// else if (xPosition > 0) -// motor->SetSpeed(rightSpeed); - -// } else if (thing->type == Thing::ControlledMotorType) { -// ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing; -// if (motor == nullptr) -// continue; - -// float xPosition = placement[motorIx].position.x; -// if (xPosition < 0) -// motor->SetTargetSpeed(leftSpeed); -// else if (xPosition > 0) -// motor->SetTargetSpeed(rightSpeed); -// } -// }; -// } - -// void Propulsion::SetDiffDriveVelocities(float leftVelocity, -// float rightVelocity) { -// for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { -// // Placement placement = placement[motorIx]; -// // if (placement.position.x < 0) -// // placement.controlledMotor->SetTargetVelocity(leftVelocity); -// // else if (placement.position.x > 0) -// // placement.controlledMotor->SetTargetVelocity(rightVelocity); -// }; -// } - void Propulsion::SetTwistSpeed(float forward, float yaw) {} void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {} @@ -120,23 +77,7 @@ void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {} void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch, - float roll) { - // if (quadcopter != nullptr) - // quadcopter->SetTwistSpeed(linear, yaw); - // else - // SetTwistSpeed(linear.z, yaw); -} - -// void Propulsion::SetLinearSpeed(Vector3 velocity, -// float yawSpeed, -// float rollSpeed) { -// if (quadcopter != nullptr) -// quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); -// } - -// Quadcopter* Propulsion::GetQuadcopter() { -// return quadcopter; -// } + float roll) {} /// @brief Odometer returns the total distance traveled since start /// @return The total distance diff --git a/Thing.cpp b/Thing.cpp new file mode 100644 index 0000000..67f3e4b --- /dev/null +++ b/Thing.cpp @@ -0,0 +1,23 @@ +#include "Thing.h" + +using namespace Passer::RoboidControl; + +Thing::Thing() { + this->type = (unsigned int)Type::Undetermined; +} + +const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch; +const unsigned int Thing::DistanceSensorType = + SensorType | (unsigned int)Type::DistanceSensor; +const unsigned int Thing::ControlledMotorType = + MotorType | (unsigned int)Type::ControlledMotor; +const unsigned int Thing::UncontrolledMotorType = + MotorType | (unsigned int)Type::UncontrolledMotor; + +bool Thing::IsMotor() { + return (type & Thing::MotorType) != 0; +} + +bool Thing::IsSensor() { + return (type & Thing::SensorType) != 0; +} \ No newline at end of file diff --git a/Thing.h b/Thing.h index db6dd6b..e35cc9b 100644 --- a/Thing.h +++ b/Thing.h @@ -5,32 +5,43 @@ namespace RoboidControl { /// @brief A thing is a functional component on a robot class Thing { -public: - Thing() { type = (int)Type::Undetermined; } + public: + /// @brief Default constructor for a Thing + Thing(); + /// @brief The type of Thing + unsigned int type; + + static const unsigned int SwitchType; + static const unsigned int DistanceSensorType; + static const unsigned int ControlledMotorType; + static const unsigned int UncontrolledMotorType; + + /// @brief Check if the Thing is a Motor + /// @returns True when the Thing is a Motor and False otherwise + bool IsMotor(); + /// @brief Check if the Thing is a Sensor + /// @returns True when the Thing is a Sensor and False otherwise + bool IsSensor(); + + protected: + /// @brief Bitmask for Motor type + static const unsigned int MotorType = 0x8000; + /// @brief Bitmap for Sensor type + static const unsigned int SensorType = 0x4000; + + /// @brief Basic Thing types enum class Type { Undetermined, - /// Sensor, + // Sensor, Switch, DistanceSensor, // Motor, ControlledMotor, UncontrolledMotor, }; - static const unsigned int MotorType = 0x8000; - static const unsigned int SensorType = 0x4000; - static const unsigned int SwitchType = - SensorType | (unsigned int)Type::Switch; - static const unsigned int DistanceSensorType = - SensorType | (unsigned int)Type::DistanceSensor; - static const unsigned int ControlledMotorType = - MotorType | (unsigned int)Type::ControlledMotor; - static const unsigned int UncontrolledMotorType = - MotorType | (unsigned int)Type::UncontrolledMotor; - - unsigned int type = (int)Type::Undetermined; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file