diff --git a/ControlCore/CoreThing.cpp b/ControlCore/CoreThing.cpp index c33602e..7ead074 100644 --- a/ControlCore/CoreThing.cpp +++ b/ControlCore/CoreThing.cpp @@ -1,14 +1,18 @@ #include "CoreThing.h" -CoreThing::CoreThing( - // Participant *client, - unsigned char networkId, unsigned char thingId, unsigned char thingType) { - // this->client = client; - this->id = thingId; +#include + +CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) { this->type = thingType; this->networkId = networkId; this->Init(); - CoreThing::Add(this); + + int thingId = CoreThing::Add(this); + if (thingId < 0) { + std::cout << "ERROR: Thing store is full\n"; + this->id = 0; // what to do when we cannot store any more things? + } else + this->id = thingId; } void CoreThing::Init() {} @@ -26,13 +30,14 @@ CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) { return nullptr; } -bool CoreThing::Add(CoreThing *newThing) { +int CoreThing::Add(CoreThing *newThing) { for (unsigned char ix = 0; ix < 256; ix++) { CoreThing *thing = allThings[ix]; if (thing == nullptr) { allThings[ix] = newThing; - return true; + + return ix; } } - return false; + return -1; } \ No newline at end of file diff --git a/ControlCore/CoreThing.h b/ControlCore/CoreThing.h index 7e0d32d..bcbf463 100644 --- a/ControlCore/CoreThing.h +++ b/ControlCore/CoreThing.h @@ -6,6 +6,7 @@ class CoreThing { public: // Participant *client; unsigned char networkId; + /// @char The id of the thing unsigned char id; // CoreThing *parent; /// @brief The type of Thing @@ -14,16 +15,32 @@ public: const char *modelUrl = nullptr; // protected Sensor sensor; + /// @brief Basic Thing types + enum class Type { + Undetermined, + // Sensor, + Switch, + DistanceSensor, + DirectionalSensor, + TemperatureSensor, + // Motor, + ControlledMotor, + UncontrolledMotor, + Servo, + // Other + Humanoid, + ExternalSensor, + }; + +private: static CoreThing *allThings[]; public: - CoreThing( - // Participant *client, - unsigned char networkId, unsigned char thingId, - unsigned char thingType = 0); + CoreThing(unsigned char networkId = 0, + unsigned char thingType = (unsigned char)Type::Undetermined); static CoreThing *Get(unsigned char networkId, unsigned char thingId); - static bool Add(CoreThing *thing); + static int Add(CoreThing *thing); protected: virtual void Init(); diff --git a/Motor.cpp b/Motor.cpp index 35ee9cd..373641f 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -2,9 +2,7 @@ #include -Motor::Motor() : Thing(0) { // for now, id should be set properly later - this->type = (int)Thing::UncontrolledMotorType; -} +Motor::Motor() : Thing() { this->type = (int)Thing::UncontrolledMotorType; } float Motor::GetActualSpeed() { return this->currentTargetSpeed; } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index b7c5af7..36355a6 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,6 +1,6 @@ #include "NetworkSync.h" -#define RC_DEBUG 1 +// #define RC_DEBUG 1 #ifdef RC_DEBUG #include diff --git a/Roboid.cpp b/Roboid.cpp index 8b4ac5f..0a3ee67 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -11,7 +11,7 @@ #include #endif -Roboid::Roboid() : Thing(0) { +Roboid::Roboid() : Thing() { #ifdef RC_DEBUG Serial.begin(115200); #endif @@ -66,40 +66,6 @@ void Roboid::Update(unsigned long currentTimeMs) { lastUpdateTimeMs = currentTimeMs; } -/* -void Roboid::SetPosition(Spherical16 newWorldPosition) { - SwingTwist16 roboidOrientation = this->GetOrientation(); - Spherical16 translation = newWorldPosition - this->roboidPosition; - float distance = translation.distance; - Angle16 angle = Spherical16::SignedAngleBetween( - roboidOrientation * Spherical16::forward, translation, Spherical16::up); - Polar16 polarTranslation = Polar16( - distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); - if (perception != nullptr) { - printf("roboid translation %f, %f\n", polarTranslation.distance, - polarTranslation.angle.InDegrees()); - perception->UpdatePose(polarTranslation); - } - - this->position = newWorldPosition; // roboid is the root? - // World position should be set in the update recursion - // this->worldPosition = newWorldPosition; - - // if (networkSync != nullptr) - // // networkSync->SendPosition(this->worldPosition); - // networkSync->SendPose(this->worldPosition, roboidOrientation); -} - -#include -void Roboid::SetOrientation(SwingTwist16 newOrientation) { - SwingTwist16 delta = - SwingTwist16::Inverse(this->orientation) * newOrientation; - if (perception != nullptr) - perception->UpdatePose(delta); - - this->orientation = newOrientation; -} -*/ void Roboid::AddChild(Thing *child) { Thing::AddChild(child); if (child->IsSensor()) { @@ -112,6 +78,7 @@ void Passer::RoboidControl::Roboid::Release(Thing *child) { if (RemoveChild(child) != nullptr) { child->position = this->position; child->orientation = this->orientation; + // this creates an new thing, I wish I could avoid this. this->perception->AddTrackedObject(nullptr, child); } } diff --git a/ServoMotor.cpp b/ServoMotor.cpp index 4f99da7..6b0a9a5 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -2,8 +2,7 @@ #include "LinearAlgebra/FloatSingle.h" -ServoMotor::ServoMotor() - : Thing(0) { // for now, id should be set properly later +ServoMotor::ServoMotor() : Thing() { this->type = Thing::ServoType; this->controlMode = ControlMode::Position; this->targetAngle = Angle16(); @@ -23,9 +22,7 @@ void ServoMotor::SetTargetAngle(Angle16 angle) { this->hasTargetAngle = true; } -Angle16 ServoMotor::GetTargetAngle() { - return this->targetAngle; -} +Angle16 ServoMotor::GetTargetAngle() { return this->targetAngle; } void ServoMotor::SetMaximumVelocity(float maxVelocity) { this->maxSpeed = maxVelocity; @@ -36,18 +33,16 @@ void ServoMotor::SetTargetVelocity(float targetVelocity) { this->controlMode = ControlMode::Velocity; this->targetVelocity = targetVelocity; - this->hasTargetAngle = false; // can't we use the controlMode for this? + this->hasTargetAngle = false; // can't we use the controlMode for this? } -float ServoMotor::GetTargetVelocity() { - return this->targetVelocity; -} +float ServoMotor::GetTargetVelocity() { return this->targetVelocity; } void ServoMotor::Update(unsigned long currentTimeMs) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->GetChild(childIx); + Thing *child = this->GetChild(childIx); if (child != nullptr && child->type == Thing::ServoType) { - ServoMotor* servo = (ServoMotor*)child; + ServoMotor *servo = (ServoMotor *)child; servo->Update(currentTimeMs); } } @@ -100,6 +95,4 @@ void ServoMotor::Update(unsigned long currentTimeMs) { } } -void ServoMotor::SetAngle(Angle16 angle) { - this->actualAngle = angle; -}; +void ServoMotor::SetAngle(Angle16 angle) { this->actualAngle = angle; }; diff --git a/Thing.cpp b/Thing.cpp index 515a5cc..93fe1ed 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -6,25 +6,14 @@ using namespace Passer::RoboidControl; int Thing::lastThingId = 1; -Thing::Thing() - : CoreThing(0, lastThingId++, (unsigned char)Type::Undetermined) { - this->id = lastThingId++; - this->type = (unsigned int)Type::Undetermined; +Thing::Thing() : CoreThing() { + // this->id = lastThingId++; + // this->type = (unsigned int)Type::Undetermined; this->childCount = 0; this->parent = nullptr; this->children = nullptr; } -Thing::Thing(unsigned char id) - : CoreThing(0, id, (unsigned char)Type::Undetermined) { - this->id = id; - this->type = (unsigned int)Type::Undetermined; - this->childCount = 0; - this->parent = nullptr; - this->children = nullptr; - // this->position = SphericalOf::zero; -} - void Thing::SetName(const char *name) { this->name = name; } const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch; diff --git a/Thing.h b/Thing.h index ff09d83..1bed1d5 100644 --- a/Thing.h +++ b/Thing.h @@ -12,12 +12,10 @@ namespace RoboidControl { /// @brief A thing is a functional component on a robot class Thing : public CoreThing { public: - Thing(); /// @brief Default constructor for a Thing - Thing(unsigned char id); + Thing(); - /// @char The id of the thing - unsigned char id; + // unsigned char id; void SetName(const char *name); @@ -120,21 +118,21 @@ protected: static const unsigned char CustomType = 0x80; /// @brief Basic Thing types - enum class Type { - Undetermined, - // Sensor, - Switch, - DistanceSensor, - DirectionalSensor, - TemperatureSensor, - // Motor, - ControlledMotor, - UncontrolledMotor, - Servo, - // Other - Humanoid, - ExternalSensor, - }; + // enum class Type { + // Undetermined, + // // Sensor, + // Switch, + // DistanceSensor, + // DirectionalSensor, + // TemperatureSensor, + // // Motor, + // ControlledMotor, + // UncontrolledMotor, + // Servo, + // // Other + // Humanoid, + // ExternalSensor, + // }; Thing *parent = nullptr; Thing **children = nullptr; diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 3a7d3dc..b2003a1 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -2,7 +2,7 @@ #include -InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing(0) { +InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing() { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; @@ -14,7 +14,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing(0) { InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position, SwingTwist16 orientation) - : Thing(0) { + : Thing() { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor;