From b66234823647bf76a3df618078adf5cce6a8ec87 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Wed, 25 Sep 2024 11:28:31 +0200
Subject: [PATCH] Improve servo angle limits

---
 LinearAlgebra  | 2 +-
 ServoMotor.cpp | 3 +--
 ServoMotor.h   | 4 ++--
 3 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/LinearAlgebra b/LinearAlgebra
index 9eca318..95a6fb3 160000
--- a/LinearAlgebra
+++ b/LinearAlgebra
@@ -1 +1 @@
-Subproject commit 9eca3189918ecfbaae462427619c263f636b6d9d
+Subproject commit 95a6fb3a4b5eca70dee33381611e5df9d28fdcee
diff --git a/ServoMotor.cpp b/ServoMotor.cpp
index 4867d4c..4f99da7 100644
--- a/ServoMotor.cpp
+++ b/ServoMotor.cpp
@@ -11,8 +11,7 @@ ServoMotor::ServoMotor()
 }
 
 void ServoMotor::SetTargetAngle(Angle16 angle) {
-  angle = Angle16::Degrees(Float::Clamp(angle.InDegrees(), minAngle.InDegrees(),
-                                        maxAngle.InDegrees()));
+  angle = Angle16::Clamp(angle, minAngle, maxAngle);
 
   if (maxSpeed == 0.0F) {
     SetAngle(angle);
diff --git a/ServoMotor.h b/ServoMotor.h
index 569338f..204c8aa 100644
--- a/ServoMotor.h
+++ b/ServoMotor.h
@@ -11,8 +11,8 @@ class ServoMotor : public Thing {
   ServoMotor();
 
   Vector3 rotationAxis = Vector3::up;
-  Angle minAngle = Angle::Degrees(-90);
-  Angle maxAngle = Angle::Degrees(90);
+  Angle16 minAngle = Angle16::Degrees(-90);
+  Angle16 maxAngle = Angle16::Degrees(90);
 
   enum ControlMode { Position, Velocity };
   ControlMode controlMode = ControlMode::Position;