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_ */