From 705914f91cf8bbca02d9b04b284b6476bddffe96 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Thu, 12 Sep 2024 09:38:28 +0200
Subject: [PATCH] add servo position constraints

---
 prosthesis_controller/servo.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/prosthesis_controller/servo.cpp b/prosthesis_controller/servo.cpp
index 0011da7..3425884 100644
--- a/prosthesis_controller/servo.cpp
+++ b/prosthesis_controller/servo.cpp
@@ -18,6 +18,8 @@ void initServos()
   servos[0].mode=OP_CURRENT_BASED_POSITION;
   servos[0].current=10.0;
   servos[0].offset_angle=180;
+  servos[0].min_angle=180-90;
+  servos[0].max_angle=180+90;
   servos[0].orientation=1;
   servos[0].modelnumber=1020;
 
@@ -25,6 +27,8 @@ void initServos()
   servos[1].id=12;
   servos[1].mode=OP_POSITION;
   servos[1].offset_angle=180;
+  servos[1].min_angle=180-90;
+  servos[1].max_angle=180+90;
   servos[1].orientation=1;
   servos[1].modelnumber=1080;
 
@@ -32,6 +36,8 @@ void initServos()
   servos[2].id=13;
   servos[2].mode=OP_POSITION;
   servos[2].offset_angle=180;
+  servos[2].min_angle=180-90;
+  servos[2].max_angle=180+90;
   servos[2].orientation=1;
   servos[2].modelnumber=1080;
 
@@ -108,7 +114,7 @@ void loopServos(unsigned long millis) {
   if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){
     last_servo_update=millis;
     
-    dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE); //asdf
+    dxl.setGoalPosition(servos[servoUpdateI].id, constrain((servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation) , servos[servoUpdateI].min_angle,servos[servoUpdateI].max_angle), UNIT_DEGREE); //asdf
     servoUpdateI++;
     servoUpdateI%=SERVO_COUNT;
   }