philipp.kramer/protesis_03
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

implement overload recovery

Browse source
This commit is contained in:
Philipp Kramer 2023-01-13 10:36:54 +01:00
parent dce686f500
commit 3ff1d8bf7d
2 changed files with 29 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

27
protesis_03_dxl/servo-control.cpp
View file

@ -24,6 +24,10 @@ float matrix_weights[SERVO_COUNT][6]; //mapping outputs, sensors/moods
#define W_SIN 4 #define W_SIN 4
#define W_COS 5 #define W_COS 5
unsigned long last_overloaderror_millis;
dxl_servo servos[SERVO_COUNT];
void matrix_weights_update(){ void matrix_weights_update(){
//0=pitch //0=pitch
@ -101,13 +105,8 @@ void matrix_weights_update(){
matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1); matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
} }
*/ */
} }
dxl_servo servos[SERVO_COUNT];
void servos_set_current_initial_position() { void servos_set_current_initial_position() {
for(int i = 0; i < SERVO_COUNT ; i++){ for(int i = 0; i < SERVO_COUNT ; i++){
@ -348,20 +347,32 @@ void read_servos_positions()
if (errorcode!=0) { if (errorcode!=0) {
Serial.print("Error present on "); Serial.print(i); Serial.print("errorcode="); Serial.println(errorcode); 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
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 if (CHECK_BIT(errorcode,5)) { //overload error
Serial.print("Init Servo "); Serial.println(i); 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 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); dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed); dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
dxlTorqueOn(servos[i].id); dxlTorqueOn(servos[i].id);
dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO); dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
dxlSetGoalPosition(servos[i].id, servos[i].initial_position); dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
last_overloaderror_millis=0; //reset flag
} }
} }
}
} }

2
protesis_03_dxl/servo-control.hpp
View file

@ -118,4 +118,6 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
void read_servos_positions(); void read_servos_positions();
#endif /* SERVO_CONTROL_HPP_ */ #endif /* SERVO_CONTROL_HPP_ */