add slow fault recovery by increasing torque

positionCapture/positionCapture.pde

@@ -20,6 +20,8 @@ String mwheader[] = new String[32];
 float matrixweights[][] = new float[32][32]; //[servo][weight]
 int matrixrows=0; //when matrix received this will be the number of rows
 
+float torquemultiplier=0;
+
 void setup() {
   size(800,600);
   background(0);
@@ -108,6 +110,9 @@ void draw() {
       }
       matrixrows=s+1;
       print("Matrixrows="); println(matrixrows);
+    }else if (list[0].equalsIgnoreCase("torque")) { //is torque multiplier value
+      torquemultiplier=parseFloat(list[1].replaceAll("[^0-9.]", "")); //remove non digit characters
+      
     }
   }
   
@@ -133,6 +138,11 @@ void draw() {
     drawBargraph(servoPositionsActual[i],valRange,10,30+50*i);
   }
   
+  //Torque Multiplier
+  textSize(16); color(255);
+  textAlign(LEFT);
+  text("Torque="+torquemultiplier, width-200,20);
+  
   //Matrix Weights
   int matrixPosX=10;
   int matrixPosY=300;

protesis_03_dxl/protesis_03_dxl.ino

@@ -79,6 +79,7 @@ void loop() {
   static unsigned long last_loop_readpos_millis;
   static unsigned long last_servo_send_millis;
   static unsigned long last_loop_sendmatrix_millis;
+  static unsigned long last_loop_sendtorque_millis;
 
   
 
@@ -99,6 +100,13 @@ void loop() {
     //Serial.println("moved servos");
   }
 
+  if (loopmillis-last_loop_sendtorque_millis>1000 && loopmillis-last_servo_send_millis>=4) {
+    set_servo_torque();
+    last_servo_send_millis=millis();
+    last_loop_sendtorque_millis=millis();
+  }
+  
+
   if (loopmillis-last_loop_readpos_millis>100 && loopmillis-last_servo_send_millis>=4) { //limit frequency and minimum delay after send positions
     last_loop_readpos_millis=loopmillis;
     read_servos_positions(); //takes around 8000us for all servos. read and send over serial.

protesis_03_dxl/servo-control.cpp

@@ -27,10 +27,12 @@ float matrix_weights[SERVO_COUNT][WEIGHT_COUNT]; //mapping outputs, sensors/mood
 String weightNames[] = {"Pitch","Roll","Noise","NoiseSlow","Sin","Cos"};
 
 unsigned long last_overloaderror_millis;
+bool overloaderror_flag=false;
 
 
 dxl_servo servos[SERVO_COUNT];
 
+
 void matrix_weights_update(){
   //0=pitch
   //1=roll
@@ -200,7 +202,7 @@ void servos_init() {
 		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);
+		dxlSetRunningTorqueLimit(servos[i].id,servos[i].torque_limit);
 		dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
 	}
 }
@@ -341,6 +343,23 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
   
 }
 
+#define DELAY_OVERLOADERROR_RESET 1000
+#define TORQUERECOVERTIME 5000
+
+void set_servo_torque() {
+  float torque_multiplier=1; //value between 0 and 1
+  static float last_torque_multiplier=0;
+  torque_multiplier=constrain( mapfloat(millis()-last_overloaderror_millis, DELAY_OVERLOADERROR_RESET,TORQUERECOVERTIME, 0.0,1.0), 0,1);
+  if (last_torque_multiplier!=torque_multiplier) { //only if changed
+    Serial.print("torque,");
+    Serial.println(torque_multiplier);
+    for(int i = 0; i < SERVO_COUNT; i++) {
+      dxlSetRunningTorqueLimit(servos[i].id,servos[i].torque_limit* torque_multiplier);
+    }
+    last_torque_multiplier=torque_multiplier;
+  }
+}
+
 #define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
 void read_servos_positions()
 {
@@ -374,13 +393,14 @@ 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
-      if (last_overloaderror_millis==0) { //error just appeared
+      if (!overloaderror_flag) { //error just appeared
         last_overloaderror_millis=millis();
+        overloaderror_flag=true;
       }
     }
   }
 
-  #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];
@@ -391,10 +411,9 @@ void read_servos_positions()
         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);
+        dxlSetRunningTorqueLimit(servos[i].id,0); //enable with no torque
         dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
-        last_overloaderror_millis=0; //reset flag
+        overloaderror_flag=false;
-      
       }
     }
   }

protesis_03_dxl/servo-control.hpp

@@ -120,5 +120,7 @@ void read_servos_positions();
 
 void transmit_matrix_weights();
 
+void set_servo_torque();
+
 
 #endif /* SERVO_CONTROL_HPP_ */