From 3ff1d8bf7d77d5902d1f1ebc3a277317e57643c1 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Fri, 13 Jan 2023 10:36:54 +0100
Subject: [PATCH] implement overload recovery

---
 protesis_03_dxl/servo-control.cpp | 43 +++++++++++++++++++------------
 protesis_03_dxl/servo-control.hpp |  2 ++
 2 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/protesis_03_dxl/servo-control.cpp b/protesis_03_dxl/servo-control.cpp
index e4f43df..dc029ce 100644
--- a/protesis_03_dxl/servo-control.cpp
+++ b/protesis_03_dxl/servo-control.cpp
@@ -24,6 +24,10 @@ float matrix_weights[SERVO_COUNT][6]; //mapping outputs, sensors/moods
 #define W_SIN 4
 #define W_COS 5
 
+unsigned long last_overloaderror_millis;
+
+
+dxl_servo servos[SERVO_COUNT];
 
 void matrix_weights_update(){
   //0=pitch
@@ -101,13 +105,8 @@ void matrix_weights_update(){
     matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
   }
   */
-  
-
-	
-  
 }
 
-dxl_servo servos[SERVO_COUNT];
 
 void servos_set_current_initial_position() {
 	for(int i = 0; i < SERVO_COUNT ; i++){
@@ -349,19 +348,31 @@ void read_servos_positions()
       Serial.print("Error present on "); Serial.print(i); Serial.print("errorcode="); Serial.println(errorcode);
     }
     if (CHECK_BIT(errorcode,5)) { //overload error
-      Serial.print("Init Servo "); Serial.println(i);
-      delay(1000);
-      dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
-      dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
-      dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
-      dxlTorqueOn(servos[i].id);
-      dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
-      dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
-
-      
-      
+      if (last_overloaderror_millis==0) { //error just appeared
+        last_overloaderror_millis=millis();
+      }
     }
   }
+
+  #define DELAY_OVERLOADERROR_RESET 2000
+  if (millis()-last_overloaderror_millis>DELAY_OVERLOADERROR_RESET) { //wait before resetting
+    for(int i = 0; i< SERVO_COUNT; i++) {
+      int errorcode=errors[i];
+      if (CHECK_BIT(errorcode,5)) { //overload error
+        Serial.print("Init Servo "); Serial.println(i);
+        
+        dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
+        dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
+        dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
+        dxlTorqueOn(servos[i].id);
+        dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
+        dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
+        last_overloaderror_millis=0; //reset flag
+      
+      }
+    }
+  }
+
 }
 
 
diff --git a/protesis_03_dxl/servo-control.hpp b/protesis_03_dxl/servo-control.hpp
index dbc66cb..5da8c07 100644
--- a/protesis_03_dxl/servo-control.hpp
+++ b/protesis_03_dxl/servo-control.hpp
@@ -118,4 +118,6 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
 
 void read_servos_positions();
 
+
+
 #endif /* SERVO_CONTROL_HPP_ */