diff --git a/Perception.cpp b/Perception.cpp index 67aaf54..5f1a162 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -156,23 +156,35 @@ void PerceivedObject::Refresh(Polar position, float radius) { this->confidence = maxConfidence; } +void Perception::AddPerceivedObject(Polar position) { + // int objCount = PerceivedObjectCount(); + // printf("perc obj count %d\n"); + + PerceivedObject *obj = new PerceivedObject(position); + // objCount = PerceivedObjectCount(); + // printf("perc obj count %d\n"); + AddPerceivedObject(obj); +} + void Perception::AddPerceivedObject(PerceivedObject *obj) { unsigned char farthestObjIx = 0; unsigned char availableSlotIx = 0; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + // printf("[%d] %d\n", objIx, this->perceivedObjects[objIx]); // Is this slot available? - if (perceivedObjects[objIx] == nullptr) { + if (this->perceivedObjects[objIx] == nullptr) { availableSlotIx = objIx; } // Do we see the same object? - else if (obj->IsTheSameAs(perceivedObjects[objIx])) { - perceivedObjects[objIx]->Refresh(obj->position, obj->radius); + else if (obj->IsTheSameAs(this->perceivedObjects[objIx])) { + printf("[%d] Updating...\n", objIx); + this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius); return; } // Is this the fartest object we see? - else if (perceivedObjects[farthestObjIx] == nullptr || - (perceivedObjects[objIx]->position.distance > - perceivedObjects[farthestObjIx]->position.distance)) { + else if (this->perceivedObjects[farthestObjIx] == nullptr || + (this->perceivedObjects[objIx]->position.distance > + this->perceivedObjects[farthestObjIx]->position.distance)) { farthestObjIx = objIx; } } @@ -181,12 +193,13 @@ void Perception::AddPerceivedObject(PerceivedObject *obj) { // max number of objects) if (availableSlotIx < maxObjectCount) { // a slot is available - perceivedObjects[availableSlotIx] = obj; + printf("[%d] new object \n", availableSlotIx); + this->perceivedObjects[availableSlotIx] = obj; } // If this object is closer than the farthest object, then replace it else if (obj->position.distance < - perceivedObjects[farthestObjIx]->position.distance) { - perceivedObjects[farthestObjIx] = obj; + this->perceivedObjects[farthestObjIx]->position.distance) { + this->perceivedObjects[farthestObjIx] = obj; // we may want to destroy the fartest object, but if it is created // externally, other links may still exist... } @@ -195,13 +208,15 @@ void Perception::AddPerceivedObject(PerceivedObject *obj) { unsigned char Perception::PerceivedObjectCount() { unsigned char objectCount = 0; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - if (perceivedObjects[objIx] != nullptr) + if (this->perceivedObjects[objIx] != nullptr) objectCount++; } return objectCount; } -PerceivedObject **Perception::GetPerceivedObjects() { return perceivedObjects; } +PerceivedObject **Perception::GetPerceivedObjects() { + return this->perceivedObjects; +} void Perception::Update(float currentTimeMs) { float deltaTime = currentTimeMs - lastUpdateTimeMs; @@ -217,12 +232,13 @@ void Perception::Update(float currentTimeMs) { if (obj->DegradeConfidence(deltaTime) == false) { // delete obj - perceivedObjects[objIx] = nullptr; + printf("[%d] delete object\n", objIx); + this->perceivedObjects[objIx] = nullptr; } else { - // Serial.printf("[%d] confidence: %d\n", objIx, - // perceivedObjects[objIx]->confidence); + Serial.printf("[%d] confidence: %d\n", objIx, + this->perceivedObjects[objIx]->confidence); } } - if (perceivedObjects[0] != nullptr) { + if (this->perceivedObjects[0] != nullptr) { } } \ No newline at end of file diff --git a/Perception.h b/Perception.h index d66aa4c..5524d75 100644 --- a/Perception.h +++ b/Perception.h @@ -10,7 +10,7 @@ namespace RoboidControl { class PerceivedObject { public: PerceivedObject(); - PerceivedObject(Polar position, float radius); + PerceivedObject(Polar position, float radius = 0.1F); static constexpr float equalityDistance = 0.3F; static constexpr float equalityAngle = 5.0F; @@ -94,6 +94,7 @@ public: // Object Perception + void AddPerceivedObject(Polar position); void AddPerceivedObject(PerceivedObject *obj); unsigned char PerceivedObjectCount(); PerceivedObject **GetPerceivedObjects(); diff --git a/Propulsion.cpp b/Propulsion.cpp index 54b8300..a62fb8f 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -7,40 +7,37 @@ Propulsion::Propulsion() { this->motorCount = 0; } -unsigned int Propulsion::GetMotorCount() { - return this->motorCount; -} +unsigned int Propulsion::GetMotorCount() { return this->motorCount; } -Motor* Propulsion::GetMotor(unsigned int motorId) { +Motor *Propulsion::GetMotor(unsigned int motorId) { if (motorId >= this->motorCount) return nullptr; - Thing* thing = this->placement[motorId].thing; + Thing *thing = this->placement[motorId].thing; if (thing->IsMotor()) - return (Motor*)thing; + return (Motor *)thing; return nullptr; } -Placement* Propulsion::GetMotorPlacement(unsigned int motorId) { +Placement *Propulsion::GetMotorPlacement(unsigned int motorId) { if (motorId >= this->motorCount) return nullptr; - Placement* placement = &this->placement[motorId]; + Placement *placement = &this->placement[motorId]; if (placement->thing->IsMotor()) return placement; return nullptr; } - void Propulsion::Update(float currentTimeMs) {} void Propulsion::SetTwistSpeed(float forward, float yaw) {} void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {} -void Propulsion::SetTwistSpeed(Vector3 linear, - float yaw, - float pitch, +void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch, float roll) {} + +Polar Propulsion::GetVelocity() { return Polar(0, 0); } \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index 20d0dc1..a4086a2 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -2,6 +2,7 @@ #include "Motor.h" #include "Placement.h" +#include "Polar.h" #include "Vector2.h" namespace Passer { @@ -14,7 +15,7 @@ namespace RoboidControl { /// robot. This base class does not implement the functions to move the Roboid /// around. class Propulsion { - public: +public: /// @brief Default Constructor for Propulsion Propulsion(); @@ -29,12 +30,12 @@ class Propulsion { /// @param motorIx The index of the motor /// @return Returns the motor or a nullptr when no motor with the given index /// could be found - Motor* GetMotor(unsigned int motorIx); + Motor *GetMotor(unsigned int motorIx); /// @brief Get the Placement of a specific Motor /// @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); // Velocity control @@ -56,18 +57,18 @@ class Propulsion { /// @param yaw The target rotation speed around the vertical axis /// @param pitch The target rotation speed around the sideward axis /// @param roll The target rotation speed around hte forward axis - virtual void SetTwistSpeed(Vector3 linear, - float yaw = 0.0F, - float pitch = 0.0F, - float roll = 0.0F); + virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F, + float pitch = 0.0F, float roll = 0.0F); - protected: + virtual Polar GetVelocity(); + +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; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file