diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index c7f9612..7d64d9b 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -5,27 +5,30 @@ DifferentialDrive::DifferentialDrive(){}; -DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement, - Placement rightMotorPlacement) { +DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor, + float separation) { this->motorCount = 2; - this->placement = new Placement[2]; - this->placement[0] = leftMotorPlacement; - this->placement[1] = rightMotorPlacement; - Motor *leftMotor = (Motor *)leftMotorPlacement.thing; + this->motors = new Motor *[2]; + this->motors[0] = leftMotor; + this->motors[1] = rightMotor; + + float distance = separation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; - Motor *rightMotor = (Motor *)rightMotorPlacement.thing; + leftMotor->position.angle = -90; + leftMotor->position.distance = distance; rightMotor->direction = Motor::Direction::Clockwise; + rightMotor->position.angle = 90; + rightMotor->position.distance = distance; } void DifferentialDrive::SetTargetSpeeds(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; + Motor *motor = motors[motorIx]; + if (motor == nullptr) + continue; - float xPosition = placement[motorIx].position.x; + if (motor->type == Thing::UncontrolledMotorType) { + float xPosition = motors[motorIx]->position.angle; if (xPosition < 0) motor->SetSpeed(leftSpeed); else if (xPosition > 0) @@ -50,8 +53,8 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch, } Polar DifferentialDrive::GetVelocity() { - Motor *leftMotor = (Motor *)placement[0].thing; - Motor *rightMotor = (Motor *)placement[1].thing; + Motor *leftMotor = motors[0]; + Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetSpeed(); float rightSpeed = rightMotor->GetSpeed(); float speed = (leftSpeed + rightSpeed) / 2; @@ -62,8 +65,8 @@ Polar DifferentialDrive::GetVelocity() { } float DifferentialDrive::GetAngularVelocity() { - Motor *leftMotor = (Motor *)placement[0].thing; - Motor *rightMotor = (Motor *)placement[1].thing; + Motor *leftMotor = motors[0]; + Motor *rightMotor = motors[1]; float leftSpeed = leftMotor->GetSpeed(); float rightSpeed = rightMotor->GetSpeed(); float angularVelocity = leftSpeed - rightSpeed; diff --git a/DifferentialDrive.h b/DifferentialDrive.h index 2b51025..8bcbadc 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -25,8 +25,10 @@ public: /// driving forward, while the right motor will turn Clockwise. /// @note When not using controlled motors, the placement of the motors is /// irrelevant. - DifferentialDrive(Placement leftMotorPlacement, - Placement rightMotorPlacement); + // DifferentialDrive(Placement leftMotorPlacement, + // Placement rightMotorPlacement); + + DifferentialDrive(Motor *leftMotor, Motor *rightMotor, float separation = 1); /// @brief Set the target speeds of the motors directly /// @param leftSpeed The target speed of the left Motor diff --git a/Perception.cpp b/Perception.cpp index ded98e0..1ae47aa 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -7,14 +7,17 @@ #include Perception::Perception() { + this->trackedObjects = new TrackedObject *[maxObjectCount]; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) this->trackedObjects[objIx] = nullptr; } -Perception::Perception(Placement *sensors, unsigned int sensorCount) +Perception::Perception(Sensor **sensors, unsigned int sensorCount) : Perception() { this->sensorCount = sensorCount; - this->sensorPlacements = (Placement *)sensors; + this->sensors = new Sensor *[this->sensorCount]; + for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) + this->sensors[sensorIx] = sensors[sensorIx]; } unsigned int Perception::GetSensorCount() { return this->sensorCount; } @@ -23,16 +26,13 @@ Sensor *Perception::GetSensor(unsigned int sensorId) { if (sensorId >= this->sensorCount) return nullptr; - Thing *thing = this->sensorPlacements[sensorId].thing; - if (thing->IsSensor()) - return (Sensor *)thing; - - return nullptr; + Sensor *sensor = this->sensors[sensorId]; + return sensor; } Sensor *Perception::FindSensorOfType(unsigned int sensorType) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Sensor *sensor = (Sensor *)this->sensorPlacements[sensorIx].thing; + Sensor *sensor = this->sensors[sensorIx]; if (sensor->type == sensorType) return sensor; } @@ -108,6 +108,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { else { if (obj->IsTheSameAs(this->trackedObjects[objIx])) { this->trackedObjects[objIx]->Refresh(obj->position); + delete obj; return; } // Is this the fartest object we see? @@ -128,9 +129,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { // If this object is closer than the farthest object, then replace it else if (obj->position.distance < this->trackedObjects[farthestObjIx]->position.distance) { + delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; - // we may want to destroy the fartest object, but if it is created - // externally, other links may still exist... + } else { + // No available slot, delete trackedobject + delete obj; } } @@ -154,23 +157,22 @@ void Perception::Update(float currentTimeMs) { // Update sensing for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement thingPlacement = sensorPlacements[sensorIx]; - Thing *thing = thingPlacement.thing; - if (thing == nullptr) + Sensor *sensor = sensors[sensorIx]; + if (sensor == nullptr) continue; - if (thing->type == Thing::DistanceSensorType) { - DistanceSensor *distanceSensor = (DistanceSensor *)thing; + if (sensor->type == Thing::DistanceSensorType) { + DistanceSensor *distanceSensor = (DistanceSensor *)sensor; float distance = distanceSensor->GetDistance(); - float angle = thingPlacement.horizontalDirection; + float angle = sensor->position.angle; Polar position = Polar(angle, distance); AddTrackedObject(distanceSensor, position); - } else if (thing->type == Thing::SwitchType) { - Switch *switchSensor = (Switch *)thing; - if (switchSensor != nullptr && switchSensor->IsOn()) { - Polar position = - Polar(thingPlacement.horizontalDirection, nearbyDistance); + + } else if (sensor->type == Thing::SwitchType) { + Switch *switchSensor = (Switch *)sensor; + if (switchSensor->IsOn()) { + Polar position = Polar(sensor->position.angle, nearbyDistance); AddTrackedObject(switchSensor, position); } } @@ -186,10 +188,9 @@ void Perception::Update(float currentTimeMs) { if (roboid != nullptr && roboid->networkSync != nullptr) roboid->networkSync->DestroyObject(obj); this->trackedObjects[objIx] = nullptr; + delete obj; } } - if (this->trackedObjects[0] != nullptr) { - } } void Perception::UpdatePose(Polar translation) { diff --git a/Perception.h b/Perception.h index 339431f..1c60872 100644 --- a/Perception.h +++ b/Perception.h @@ -1,11 +1,12 @@ #pragma once -#include "Placement.h" #include "Sensor.h" #include "TrackedObject.h" #include "VectorAlgebra/Polar.h" #include "VectorAlgebra/Quaternion.h" +// #include + namespace Passer { namespace RoboidControl { @@ -20,7 +21,7 @@ public: /// @brief Create a perception setup with the given Sensors /// @param sensors The Placement of Sensors on the Roboid /// @param sensorCount The number of sensors in the placement array - Perception(Placement *sensors, unsigned int sensorCount); + Perception(Sensor **sensors, unsigned int sensorCount); /// @brief The roboid of this perception system Roboid *roboid = nullptr; @@ -122,17 +123,17 @@ public: float nearbyDistance = 0.3F; public: - /// @brief The Placement of the Sensors used for Perception - Placement *sensorPlacements = nullptr; + /// @brief The Sensors used for Perception + Sensor **sensors = nullptr; /// @brief The number of Sensors used for Perception unsigned int sensorCount = 0; float lastUpdateTimeMs = 0; - static const unsigned char maxObjectCount = 7; - TrackedObject *trackedObjects[maxObjectCount]; // 7 is typically the maximum - // number of object which can - // be tracked by a human + unsigned char maxObjectCount = 7; // 7 is typically the maximum + // number of object which can + // be tracked by a human + TrackedObject **trackedObjects; }; } // namespace RoboidControl diff --git a/Placement.cpp b/Placement.cpp deleted file mode 100644 index 9c3594b..0000000 --- a/Placement.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "Placement.h" - -Placement::Placement() { - this->position = Vector3::zero; - this->thing = nullptr; -} - -Placement::Placement(Thing *thing, Vector3 position, float horizontalDirection, - float verticalDirection) - : horizontalDirection(horizontalDirection), - verticalDirection(verticalDirection) { - this->thing = thing; - this->position = position; -} - -Placement::Placement(Thing *thing, float horizontalDirection, - float verticalDirection) - : Placement(thing, Vector3::zero, horizontalDirection, verticalDirection) {} diff --git a/Placement.h b/Placement.h deleted file mode 100644 index ab94dfa..0000000 --- a/Placement.h +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -#include "Thing.h" -#include "VectorAlgebra/Vector2.h" -#include "VectorAlgebra/Vector3.h" - -namespace Passer { -namespace RoboidControl { - -/// @brief A plament is used to specify where a Thing is placed on the Roboid. -/// @details -/// 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 Placement of a Thing on a Roboid - /// @param thing The Thing which is placed - /// @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 a Roboid without position - /// @param thing The Thing which is place - /// @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 verticalDirection = 0.0F); - /// @brief Default constructor with a zero placement - Placement(); - - /// @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; - - /// @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 -} // namespace Passer -using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Propulsion.cpp b/Propulsion.cpp index 7555c45..deab295 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -4,7 +4,7 @@ #include "VectorAlgebra/FloatSingle.h" Propulsion::Propulsion() { - this->placement = nullptr; + this->motors == nullptr; this->motorCount = 0; } @@ -14,22 +14,8 @@ Motor *Propulsion::GetMotor(unsigned int motorId) { if (motorId >= this->motorCount) return nullptr; - Thing *thing = this->placement[motorId].thing; - if (thing->IsMotor()) - return (Motor *)thing; - - return nullptr; -} - -Placement *Propulsion::GetMotorPlacement(unsigned int motorId) { - if (motorId >= this->motorCount) - return nullptr; - - Placement *placement = &this->placement[motorId]; - if (placement->thing->IsMotor()) - return placement; - - return nullptr; + Motor *motor = this->motors[motorId]; + return motor; } void Propulsion::Update(float currentTimeMs) {} diff --git a/Propulsion.h b/Propulsion.h index 954d9c7..62c5c85 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -1,7 +1,6 @@ #pragma once #include "Motor.h" -#include "Placement.h" #include "VectorAlgebra/Polar.h" #include "VectorAlgebra/Quaternion.h" #include "VectorAlgebra/Vector2.h" @@ -37,7 +36,7 @@ public: /// @param motorIx The index of the Motor /// @return Returns the Placement or a nullptr when no Placement with the give /// index could be found - Placement *GetMotorPlacement(unsigned int motorIx); + // Placement *GetMotorPlacement(unsigned int motorIx); /// @brief Sets the forward and rotation speed of a (grounded) Roboid /// @param forward The target forward speed @@ -76,7 +75,8 @@ protected: /// @brief The number of motors used for Propulsion unsigned int motorCount = 0; /// @brief The Placement of the motors used for Propulsion - Placement *placement = nullptr; + // Placement *placement = nullptr; + Motor **motors = nullptr; }; } // namespace RoboidControl diff --git a/Switch.cpp b/Switch.cpp index c773cf6..d49b0d3 100644 --- a/Switch.cpp +++ b/Switch.cpp @@ -1,5 +1,5 @@ #include "Switch.h" -Switch::Switch() {} +Switch::Switch() { this->type = Thing::SwitchType; } bool Switch::IsOn() { return false; } diff --git a/Thing.cpp b/Thing.cpp index 1fc973c..0817bba 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -2,7 +2,9 @@ using namespace Passer::RoboidControl; -Thing::Thing() { this->type = (unsigned int)Type::Undetermined; } +Thing::Thing() : position(Polar::zero) { + this->type = (unsigned int)Type::Undetermined; +} const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch; const unsigned int Thing::DistanceSensorType = diff --git a/Thing.h b/Thing.h index 4bccc58..3f2b6be 100644 --- a/Thing.h +++ b/Thing.h @@ -1,5 +1,7 @@ #pragma once +#include "VectorAlgebra/Polar.h" + namespace Passer { namespace RoboidControl { @@ -30,6 +32,8 @@ public: /// @returns True when the Thing is a Sensor and False otherwise bool IsSensor(); + Polar position; + protected: /// @brief Bitmask for Motor type static const unsigned int MotorType = 0x8000; diff --git a/TrackedObject.h b/TrackedObject.h index 06c2d1b..fe36a44 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -62,7 +62,7 @@ public: protected: static constexpr unsigned char maxConfidence = 255; - static constexpr unsigned char confidenceDropSpeed = 2; + static constexpr unsigned char confidenceDropSpeed = 1; // 2; unsigned char confidence; };