From 41e9824ea1a6974d08e4dff65208a1dac69a728e Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Fri, 13 Jan 2023 13:21:53 +0100 Subject: [PATCH] add slow fault recovery by increasing torque --- positionCapture/positionCapture.pde | 10 ++++++++++ protesis_03_dxl/protesis_03_dxl.ino | 8 ++++++++ protesis_03_dxl/servo-control.cpp | 31 +++++++++++++++++++++++------ protesis_03_dxl/servo-control.hpp | 2 ++ 4 files changed, 45 insertions(+), 6 deletions(-) diff --git a/positionCapture/positionCapture.pde b/positionCapture/positionCapture.pde index 297a765..f235526 100644 --- a/positionCapture/positionCapture.pde +++ b/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; diff --git a/protesis_03_dxl/protesis_03_dxl.ino b/protesis_03_dxl/protesis_03_dxl.ino index 6a908ff..86da3c7 100644 --- a/protesis_03_dxl/protesis_03_dxl.ino +++ b/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. diff --git a/protesis_03_dxl/servo-control.cpp b/protesis_03_dxl/servo-control.cpp index e938779..b1f61ee 100644 --- a/protesis_03_dxl/servo-control.cpp +++ b/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; } } } diff --git a/protesis_03_dxl/servo-control.hpp b/protesis_03_dxl/servo-control.hpp index 5c17b94..5c3e571 100644 --- a/protesis_03_dxl/servo-control.hpp +++ b/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_ */