Add directional sensor

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);
+}

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;

DistanceSensor.cpp

@@ -1,6 +1,7 @@
 #include "DistanceSensor.h"
 
-#include "NetworkPerception.h"
+// #include "NetworkPerception.h"
+#include "Messages.h"
 #include <math.h>
 
 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);
 }

LinearAlgebra

@@ -1 +1 @@
-Subproject commit 18756ba1a5fea48d0678737f6f878570e73fec61
+Subproject commit 536d6cef19a0b50b9ef6d5e2ba119c0d3f24a26b

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;
+}

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;

Thing.h

@@ -124,6 +124,7 @@ protected:
     // Sensor,
     Switch,
     DistanceSensor,
+    DirectionalSensor,
     TemperatureSensor,
     // Motor,
     ControlledMotor,