philipp.kramer
/
Protesis_03
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add simple overload fault recovery

This commit is contained in:
Philipp Kramer 2023-01-12 15:54:50 +01:00
parent 8d5c40deb3
commit dce686f500
2 changed files with 47 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
protesis_03_dxl/protesis_03_dxl.ino
View File

@ -80,17 +80,20 @@ void loop() {
static unsigned long last_servo_send_millis;
if (loopmillis-last_loop_movement_millis>50) {
last_loop_movement_millis=loopmillis;
//int state_switch_programm = digitalRead(switch_programm); //currently not used
int state_switch_programm = digitalRead(switch_programm); //currently not used
//Serial.println(digitalRead(switch_programm));
//move_protesis_with_sensor_data(); //old movement function
//slither();
readMPU();
update_mood();
move_protesis_organic();
map_servos_by_weights_with_initial_position_and_move(current_mpu_inertia.roll_acceleration,current_mpu_inertia.pitch_acceleration);
last_servo_send_millis=millis();
//Serial.println("moved servos");
@ -98,7 +101,7 @@ void loop() {
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.
read_servos_positions(); //takes around 8000us for all servos. read and send over serial.
}
@ -107,7 +110,7 @@ void loop() {
void update_mood() {
#define GYROSCOPE_FILTER_SHAKINESS 0.8
float gyro=constrain(sqrt(pow(current_mpu_measurement.gyroscope.x,2)+pow(current_mpu_measurement.gyroscope.y,2)) / 10000.0 ,0.0, 1.0);
mood.shakiness = constrain(mood.shakiness*GYROSCOPE_FILTER_SHAKINESS + (1-GYROSCOPE_FILTER_SHAKINESS)*gyro, 0,1.) ;
mood.shakiness = constrain(mood.shakiness*GYROSCOPE_FILTER_SHAKINESS + (1-GYROSCOPE_FILTER_SHAKINESS)*gyro, 0,1.);
#define GYROSCOPE_FILTER_WAKEFULNESS 0.99
mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
@ -162,19 +165,6 @@ void readMPU() {
denoise_inertia_with_complementary_filter();
}
void move_protesis_organic() {
//substract_gyroscope_offset(current_mpu_measurement);
//mpu_measurement_with_precission(current_mpu_measurement_dp, current_mpu_measurement);
//denoise_mpu_measurement_with_complementary_filter();
//servos_collect_current_torque();
map_servos_by_weights_with_initial_position_and_move(current_mpu_inertia.roll_acceleration,current_mpu_inertia.pitch_acceleration);
}
void denoise_inertia_with_complementary_filter() {
current_mpu_inertia.roll_gyroscope = previous_mpu_inertia.roll_gyroscope
* DENOISE_PREVIOUS_INERTIA_RATE

45
protesis_03_dxl/servo-control.cpp
View File

@ -48,11 +48,12 @@ void matrix_weights_update(){
map_mode_slither/=map_sum;
map_mode_sleeping/=map_sum;
/*
Serial.print(map_mode_noise); Serial.print(",");
Serial.print(map_mode_rollpitch); Serial.print(",");
Serial.print(map_mode_slither); Serial.print(",");
Serial.print(map_mode_sleeping); Serial.println();
*/
//Mode Noise
matrix_weights[0][W_NOISE]=90.0 *map_mode_noise;
@ -85,9 +86,9 @@ void matrix_weights_update(){
matrix_weights[4][W_COS]=30 *map_mode_slither;
//Mode Sleeping
matrix_weights[0][W_NOISESLOW]=5 *map_mode_sleeping;
matrix_weights[1][W_NOISESLOW]=10 *map_mode_sleeping;
matrix_weights[2][W_NOISESLOW]=20 *map_mode_sleeping;
matrix_weights[0][W_NOISESLOW]=20 *map_mode_sleeping;
matrix_weights[1][W_NOISESLOW]=20 *map_mode_sleeping;
matrix_weights[2][W_NOISESLOW]=30 *map_mode_sleeping;
matrix_weights[3][W_NOISESLOW]=15 *map_mode_sleeping;
matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping;
@ -167,6 +168,7 @@ void servos_init_with_current_position() {
servos_init();
}
void servos_init() {
for(int i = 0; i< SERVO_COUNT; 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
@ -314,9 +316,10 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
}
#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
void read_servos_positions()
{
int errors[SERVO_COUNT];
Serial.print("dxlgp");
for (uint8_t i=0;i<SERVO_COUNT;i++) {
//Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
@ -324,9 +327,41 @@ void read_servos_positions()
Serial.print(",");
Serial.print(dxlGetPosition(servos[i].id));
errors[i]=ax12GetLastError();
}
Serial.println();
Serial.print("error");
for (uint8_t i=0;i<SERVO_COUNT;i++) {
Serial.print(",");
Serial.print(errors[i]);
}
Serial.println();
for(int i = 0; i< SERVO_COUNT; i++) {
int errorcode=errors[i];
if (errorcode!=0) {
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);
}
}
}