diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index b4f3933..600aebc 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -14,10 +14,10 @@ DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) { float distance = this->wheelSeparation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; - leftMotor->position.horizontalAngle = -90; + leftMotor->position.horizontal = -90; leftMotor->position.distance = distance; rightMotor->direction = Motor::Direction::Clockwise; - rightMotor->position.horizontalAngle = 90; + rightMotor->position.horizontal = 90; rightMotor->position.distance = distance; } @@ -39,7 +39,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, if (motor == nullptr) continue; - float xPosition = motors[motorIx]->position.horizontalAngle.ToFloat(); + float xPosition = motors[motorIx]->position.horizontal.ToFloat(); if (xPosition < 0) motor->SetTargetSpeed(leftSpeed); else if (xPosition > 0) diff --git a/LinearAlgebra b/LinearAlgebra index 353cb1b..9c3503f 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 353cb1bc7f3c12f703c0f963d89d08d567fc446a +Subproject commit 9c3503f3cb678622fcdc7935b791a28c6c48d052 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index ac7b851..6548264 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -70,7 +70,7 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { buffer[ix++] = parentThing->id; else buffer[ix++] = 0x00; - SendSpherical(buffer, &ix, thing->position); + SendSpherical16(buffer, &ix, thing->position); SendBuffer(ix); PublishModel(thing); @@ -162,7 +162,7 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) { buffer[ix++] = PoseMsg; buffer[ix++] = thing->id; buffer[ix++] = Pose_Position | Pose_Orientation; - SendSpherical(buffer, &ix, thing->position); + SendSpherical16(buffer, &ix, thing->position); SendQuat32(buffer, &ix, thing->orientation); SendBuffer(ix); @@ -361,7 +361,14 @@ void NetworkSync::SendSpherical(unsigned char* data, Spherical s) { SendAngle8(data, (*startIndex)++, s.horizontalAngle.ToFloat()); SendAngle8(data, (*startIndex)++, s.verticalAngle.ToFloat()); - // SendAngle8(data, startIndex++, s.distance); + SendFloat16(data, startIndex, s.distance); +} + +void NetworkSync::SendSpherical16(unsigned char* data, + unsigned char* startIndex, + Spherical16 s) { + SendAngle8(data, (*startIndex)++, s.horizontal.ToFloat()); + SendAngle8(data, (*startIndex)++, s.vertical.ToFloat()); SendFloat16(data, startIndex, s.distance); } diff --git a/NetworkSync.h b/NetworkSync.h index 3b9866f..d539a9b 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -10,20 +10,20 @@ namespace RoboidControl { /// @brief Interface for synchronizaing state between clients across a network class NetworkSync { -public: - NetworkSync(Roboid *roboid); + public: + NetworkSync(Roboid* roboid); unsigned char networkId; /// @brief Retreive and send the roboid state /// @param roboid The roboid for which the state is updated - virtual void NetworkUpdate(Roboid *roboid) = 0; + virtual void NetworkUpdate(Roboid* roboid) = 0; /// @brief Inform that the given object is no longer being tracked /// @param obj - virtual void DestroyObject(InterestingThing *obj); - virtual void NewObject(InterestingThing *obj); - virtual void PublishModel(Roboid *obj); - void PublishModel(Thing *thing); + virtual void DestroyObject(InterestingThing* obj); + virtual void NewObject(InterestingThing* obj); + virtual void PublishModel(Roboid* obj); + void PublishModel(Thing* thing); /// @brief The id of a Pose message static const unsigned char PoseMsg = 0x10; @@ -52,58 +52,66 @@ public: static const unsigned char ClientMsg = 0xA0; static const unsigned char NetworkIdMsg = 0xA1; - typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize); + typedef void (*Buffer)(UInt8* buffer, UInt16 bufferSize); - void ReceiveMessage(Roboid *roboid, unsigned char bytecount); + void ReceiveMessage(Roboid* roboid, unsigned char bytecount); void ReceiveNetworkId(); - void SendInvestigateThing(InterestingThing *thing); + void SendInvestigateThing(InterestingThing* thing); - void SendPoseMsg(Buffer sendBuffer, Roboid *roboid); - void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj); + void SendPoseMsg(Buffer sendBuffer, Roboid* roboid); + void SendDestroyObject(Buffer sendBuffer, InterestingThing* obj); void PublishNewObject(); - void PublishRelativeThing(Thing *thing, bool recurse = false); + void PublishRelativeThing(Thing* thing, bool recurse = false); - void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects); + void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects); virtual void SendPosition(Vector3 worldPosition) {}; virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; - void SendPose(Roboid *roboid, bool recurse = true); - void SendPose(Thing *thing, bool recurse = true); + void SendPose(Roboid* roboid, bool recurse = true); + void SendPose(Thing* thing, bool recurse = true); - virtual void SendText(const char *s); + virtual void SendText(const char* s); void SendInt(const int x); -protected: - Roboid *roboid; - NetworkPerception *networkPerception; + protected: + Roboid* roboid; + NetworkPerception* networkPerception; - void PublishTrackedObject(Roboid *roboid, InterestingThing *object); - void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, - InterestingThing *object); + void PublishTrackedObject(Roboid* roboid, InterestingThing* object); + void PublishRelativeObject(Buffer sendBuffer, + UInt8 parentId, + InterestingThing* object); - void SendSingle100(unsigned char *data, unsigned int startIndex, float value); - void SendFloat16(unsigned char *data, unsigned char *startIndex, float value); + void SendSingle100(unsigned char* data, unsigned int startIndex, float value); + void SendFloat16(unsigned char* data, unsigned char* startIndex, float value); - void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value); - void SendAngle8(unsigned char *data, unsigned int startIndex, + void SendInt32(unsigned char* data, unsigned int startIndex, Int32 value); + void SendAngle8(unsigned char* data, + unsigned int startIndex, const float value); // void SendAngle16(unsigned char *data, unsigned int startIndex, // const float value); // void SendAngle32(unsigned char *data, unsigned int startIndex, // const float value); - void SendVector3(unsigned char *data, unsigned char *startIndex, + void SendVector3(unsigned char* data, + unsigned char* startIndex, const Vector3 v); - void SendQuaternion(unsigned char *data, const int startIndex, + void SendQuaternion(unsigned char* data, + const int startIndex, const Quaternion q); - void SendPolar(unsigned char *data, unsigned char *startIndex, Polar p); - void SendSpherical(unsigned char *data, unsigned char *startIndex, + void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p); + void SendSpherical(unsigned char* data, + unsigned char* startIndex, Spherical s); - // void SendSpherical16(unsigned char *data, int startIndex, Spherical s); + void SendSpherical16(unsigned char* data, + unsigned char* startIndex, + Spherical16 s); // void SendSpherical32(unsigned char *data, int startIndex, Spherical s); - void SendQuat32(unsigned char *data, unsigned char *startIndex, + void SendQuat32(unsigned char* data, + unsigned char* startIndex, const Quaternion q); unsigned char buffer[256]; @@ -112,5 +120,5 @@ protected: void PublishClient(); }; -} // namespace RoboidControl -} // namespace Passer \ No newline at end of file +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Perception.cpp b/Perception.cpp index 8a7fea8..d7a8424 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -483,9 +483,9 @@ void Perception::Update(unsigned long currentTimeMs) { float distance = distanceSensor->GetDistance(); if (distance >= 0) { - Angle angle = sensor->position.horizontalAngle; + Angle16 angle = sensor->position.horizontal; // Polar position = Polar(angle, distance); - Polar position = Polar(distance, angle); + Polar position = Polar(distance, angle.ToFloat()); AddTrackedObject(distanceSensor, position); } @@ -494,7 +494,7 @@ void Perception::Update(unsigned long currentTimeMs) { if (switchSensor->IsOn()) { // Polar position = Polar(sensor->position.angle, nearbyDistance); Polar position = - Polar(nearbyDistance, sensor->position.horizontalAngle); + Polar(nearbyDistance, sensor->position.horizontal.ToFloat()); // AddTrackedObject(switchSensor, position); } } else { diff --git a/Thing.cpp b/Thing.cpp index 49c2891..c154374 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -1,10 +1,9 @@ #include "Thing.h" -// #include "Roboid.h" - using namespace Passer::RoboidControl; -Thing::Thing(unsigned char id) : position(Polar::zero), id(id) { +Thing::Thing(unsigned char id) : id(id) { + // this->position = SphericalOf::zero; this->type = (unsigned int)Type::Undetermined; this->childCount = 0; this->parent = nullptr; @@ -20,25 +19,35 @@ const unsigned int Thing::UncontrolledMotorType = MotorType | (unsigned int)Type::UncontrolledMotor; const unsigned int Thing::ServoType = (unsigned int)Type::Servo; -bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; } +bool Thing::IsMotor() { + return (type & Thing::MotorType) != 0; +} -bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; } +bool Thing::IsSensor() { + return (type & Thing::SensorType) != 0; +} -bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; } +bool Thing::IsRoboid() { + return (type & Thing::RoboidType) != 0; +} -void Thing::SetModel(const char *url) { this->modelUrl = url; } +void Thing::SetModel(const char* url) { + this->modelUrl = url; +} -void Thing::SetParent(Thing *parent) { +void Thing::SetParent(Thing* parent) { if (parent == nullptr) return; parent->AddChild(this); } -Thing *Thing::GetParent() { return this->parent; } +Thing* Thing::GetParent() { + return this->parent; +} -void Thing::AddChild(Thing *child) { - Thing **newChildren = new Thing *[this->childCount + 1]; +void Thing::AddChild(Thing* child) { + Thing** newChildren = new Thing*[this->childCount + 1]; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { newChildren[childIx] = this->children[childIx]; if (this->children[childIx] == child) { @@ -58,7 +67,7 @@ void Thing::AddChild(Thing *child) { this->childCount++; } -Thing *Thing::GetChild(unsigned char childIx) { +Thing* Thing::GetChild(unsigned char childIx) { if (childIx >= 0 && childIx < this->childCount) { return this->children[childIx]; } else diff --git a/Thing.h b/Thing.h index 5631852..2e5e467 100644 --- a/Thing.h +++ b/Thing.h @@ -1,14 +1,15 @@ #pragma once -#include "LinearAlgebra/Polar.h" +// #include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Quaternion.h" +#include "LinearAlgebra/Spherical.h" namespace Passer { namespace RoboidControl { /// @brief A thing is a functional component on a robot class Thing { -public: + public: /// @brief Default constructor for a Thing Thing(unsigned char id); @@ -41,7 +42,7 @@ public: /// @brief The position of this Thing /// @remark When this Thing has a parent, the position is relative to the /// parent's position and orientation - Spherical position; + Spherical16 position; /// @brief The orientation of this Thing /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation @@ -50,27 +51,27 @@ public: /// @brief Sets the parent Thing /// @param parent The Thing which should become the parnet /// @remark This is equivalent to calling parent->AddChild(this); - virtual void SetParent(Thing *parent); + virtual void SetParent(Thing* parent); /// @brief Gets the parent Thing /// @return The parent Thing - Thing *GetParent(); + Thing* GetParent(); /// @brief Add a child Thing to this Thing /// @param child The Thing which should become a child /// @remark When the Thing is already a child, it will not be added again - virtual void AddChild(Thing *child); + virtual void AddChild(Thing* child); /// @brief Get the child at the given index /// @param childIx The index of the child /// @return The child at the given index or nullptr when the index is invalid /// or the child could not be found - Thing *GetChild(unsigned char childIx); + Thing* GetChild(unsigned char childIx); /// @brief Sets the location from where the 3D model of this Thing can be /// loaded from /// @param url The url of the model /// @remark Although the roboid implementation is not dependent on the model, /// the only official supported model format is .obj - void SetModel(const char *url); + void SetModel(const char* url); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds @@ -78,9 +79,9 @@ public: unsigned char childCount = 0; - const char *modelUrl = nullptr; + const char* modelUrl = nullptr; -protected: + protected: /// @brief Bitmask for Motor type static const unsigned int MotorType = 0x8000; /// @brief Bitmap for Sensor type @@ -102,10 +103,10 @@ protected: ExternalSensor, }; - Thing *parent = nullptr; - Thing **children = nullptr; + Thing* parent = nullptr; + Thing** children = nullptr; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file