diff --git a/DirectionalSensor.cpp b/DirectionalSensor.cpp new file mode 100644 index 0000000..fa1a974 --- /dev/null +++ b/DirectionalSensor.cpp @@ -0,0 +1,15 @@ +#include "DirectionalSensor.h" + +#include "Messages.h" + +DirectionalSensor::DirectionalSensor() { + this->type = DirectionalSensor::Type; + this->vector = Spherical16::zero; +} + +Spherical16 DirectionalSensor::GetVector() { return Spherical16::zero; } + +void DirectionalSensor::ProcessBytes(unsigned char *bytes) { + unsigned char ix = 0; + this->vector = Messages::ReceiveSpherical16(bytes, &ix); +} diff --git a/DirectionalSensor.h b/DirectionalSensor.h new file mode 100644 index 0000000..90a866f --- /dev/null +++ b/DirectionalSensor.h @@ -0,0 +1,29 @@ +#pragma once + +#include "Sensor.h" + +namespace Passer { +namespace RoboidControl { + +/// @brief A Sensor which can measure the distance to the nearest object +class DirectionalSensor : public Sensor { +public: + /// @brief Default constructor + DirectionalSensor(); + + const unsigned int Type = SensorType | (unsigned int)Type::DirectionalSensor; + + /// @brief Determine the distance to the nearest object + /// @return the measured distance in meters to the nearest object + virtual Spherical16 GetVector(); + + virtual void ProcessBytes(unsigned char *bytes) override; + +protected: + /// @brief Distance to the closest object + Spherical16 vector = Spherical16::zero; +}; + +} // namespace RoboidControl +} // namespace Passer +using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp index 5006e82..2a7aa60 100644 --- a/DistanceSensor.cpp +++ b/DistanceSensor.cpp @@ -1,6 +1,7 @@ #include "DistanceSensor.h" -#include "NetworkPerception.h" +// #include "NetworkPerception.h" +#include "Messages.h" #include DistanceSensor::DistanceSensor() { @@ -30,5 +31,5 @@ bool DistanceSensor::ObjectNearby() { void DistanceSensor::ProcessBytes(unsigned char *bytes) { unsigned char ix = 0; - this->distance = NetworkPerception::ReceiveFloat16(bytes, &ix); + this->distance = Messages::ReceiveFloat16(bytes, &ix); } diff --git a/LinearAlgebra b/LinearAlgebra index 18756ba..536d6ce 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 18756ba1a5fea48d0678737f6f878570e73fec61 +Subproject commit 536d6cef19a0b50b9ef6d5e2ba119c0d3f24a26b diff --git a/Messages.cpp b/Messages.cpp new file mode 100644 index 0000000..7afd6ba --- /dev/null +++ b/Messages.cpp @@ -0,0 +1,35 @@ +#include "Messages.h" + +#include "float16/float16.h" + +Angle8 Messages::ReceiveAngle8(unsigned char *data, unsigned char *startIndex) { + unsigned char binary = data[(*startIndex)++]; + + Angle8 angle = Angle8::Binary(binary); + + return angle; +} + +float Messages::ReceiveFloat16(unsigned char *data, unsigned char *startIndex) { + unsigned char ix = *startIndex; + unsigned short value = data[ix++] << 8 | data[ix]; + float16 f = float16(); + f.setBinary(value); + + *startIndex = ix; + return f.toDouble(); +} + +Spherical16 Messages::ReceiveSpherical16(unsigned char *data, + unsigned char *startIndex) { + float distance = ReceiveFloat16(data, startIndex); + + Angle8 horizontal8 = ReceiveAngle8(data, startIndex); + Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255); + + Angle8 vertical8 = ReceiveAngle8(data, startIndex); + Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255); + + Spherical16 s = Spherical16(distance, horizontal, vertical); + return s; +} \ No newline at end of file diff --git a/Messages.h b/Messages.h new file mode 100644 index 0000000..c0a3555 --- /dev/null +++ b/Messages.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Roboid.h" + +namespace Passer { +namespace RoboidControl { + +class Messages { +public: + static Angle8 ReceiveAngle8(unsigned char *data, unsigned char *startIndex); + static float ReceiveFloat16(unsigned char *data, unsigned char *startIndex); + static Spherical16 ReceiveSpherical16(unsigned char *data, + unsigned char *startIndex); +}; + +} // namespace RoboidControl +} // namespace Passer +using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Thing.h b/Thing.h index 967ab72..dea7cd9 100644 --- a/Thing.h +++ b/Thing.h @@ -124,6 +124,7 @@ protected: // Sensor, Switch, DistanceSensor, + DirectionalSensor, TemperatureSensor, // Motor, ControlledMotor,