From 733a09ce91c29746b1ff49eb444f419750b157db Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <21057728+passervr@users.noreply.github.com>
Date: Wed, 6 Dec 2023 09:59:35 +0100
Subject: [PATCH] Crazyflie compatibility

---
 Perception.cpp |  5 ++++-
 Propulsion.cpp |  6 +++---
 Roboid.cpp     |  1 -
 Switch.cpp     |  7 +++++++
 Switch.h       | 18 ++++++++++++++++++
 VectorAlgebra  |  2 +-
 6 files changed, 33 insertions(+), 6 deletions(-)
 create mode 100644 Switch.cpp
 create mode 100644 Switch.h

diff --git a/Perception.cpp b/Perception.cpp
index 788e950..18ea8e3 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -3,6 +3,8 @@
 #include "DistanceSensor.h"
 #include "Switch.h"
 
+#include <math.h>
+
 Perception::Perception() {}
 
 Perception::Perception(Placement* sensors, unsigned int sensorCount) {
@@ -209,7 +211,8 @@ Sensor* Perception::GetSensor(float angle) {
 
   for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
     Placement placement = this->sensorPlacements[ix];
-    if (abs(placement.horizontalDirection - angle) < 0.01F)
+    float delta = placement.horizontalDirection - angle;
+    if (delta > -0.01F && delta < 0.01F)
       return (Sensor*)placement.thing;
   }
 
diff --git a/Propulsion.cpp b/Propulsion.cpp
index dada9fc..a715712 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -2,8 +2,6 @@
 
 #include "FloatSingle.h"
 
-#include <Arduino.h>
-
 Propulsion::Propulsion() {
   this->placement = nullptr;
   this->motorCount = 0;
@@ -57,7 +55,9 @@ void Propulsion::Update(float currentTimeMs) {
 }
 
 void Propulsion::SetMaxSpeed(float maxSpeed) {
-  this->maxSpeed = abs(maxSpeed);
+  if (maxSpeed < 0)
+    maxSpeed = 0;
+  this->maxSpeed = maxSpeed;
 }
 
 void Propulsion::SetTwistSpeed(float forward, float yaw) {}
diff --git a/Roboid.cpp b/Roboid.cpp
index 3b41bf1..79d1a6a 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -1,5 +1,4 @@
 #include "Roboid.h"
-#include <Arduino.h>
 
 Roboid::Roboid() {}
 
diff --git a/Switch.cpp b/Switch.cpp
new file mode 100644
index 0000000..42d717d
--- /dev/null
+++ b/Switch.cpp
@@ -0,0 +1,7 @@
+#include <Switch.h>
+
+Switch::Switch() {}
+
+bool Switch::IsOn() {
+  return false;
+}
diff --git a/Switch.h b/Switch.h
new file mode 100644
index 0000000..6fddb69
--- /dev/null
+++ b/Switch.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A digital switch input
+class Switch : public Sensor {
+ public:
+  /// @brief Default constructor
+  Switch();
+
+  virtual bool IsOn();
+};
+
+}  // namespace RoboidControl
+}  // namespace Passer
\ No newline at end of file
diff --git a/VectorAlgebra b/VectorAlgebra
index 006ea04..493a3f7 160000
--- a/VectorAlgebra
+++ b/VectorAlgebra
@@ -1 +1 @@
-Subproject commit 006ea046e4bb57b033823ba5a3fad0dfc7f5dfba
+Subproject commit 493a3f748907b4fb7e64177f94b7cb98a951af4c