From 8d5c40deb393b1bb8a7c5af863f0c70f78ae5f32 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Thu, 12 Jan 2023 13:32:12 +0100 Subject: [PATCH] implement more mood control --- protesis_03_dxl/protesis_03_dxl.ino | 27 ++++++-- protesis_03_dxl/servo-control.cpp | 97 ++++++++++++++++------------- protesis_03_dxl/servo-control.hpp | 1 + 3 files changed, 78 insertions(+), 47 deletions(-) diff --git a/protesis_03_dxl/protesis_03_dxl.ino b/protesis_03_dxl/protesis_03_dxl.ino index 4bb8aba..adcb5c6 100644 --- a/protesis_03_dxl/protesis_03_dxl.ino +++ b/protesis_03_dxl/protesis_03_dxl.ino @@ -41,7 +41,7 @@ void print_mpu_gyroscope_data(); void print_mpu_offset(); void print_loop_time_micros(); void print_servo_torques(); - +void readMPU(); void setup() { @@ -88,6 +88,7 @@ void loop() { //move_protesis_with_sensor_data(); //old movement function //slither(); + readMPU(); update_mood(); move_protesis_organic(); last_servo_send_millis=millis(); @@ -97,7 +98,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. } @@ -110,12 +111,24 @@ void update_mood() { #define GYROSCOPE_FILTER_WAKEFULNESS 0.99 mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.); - /*Serial.print(gyro); + double roll=current_mpu_inertia.roll_acceleration; + double pitch=current_mpu_inertia.pitch_acceleration; + double rollmapped=constrain( mapfloat(abs(roll),5,10,0,1),0,1); + double pitchmapped=constrain( mapfloat(abs(pitch),5,10,0,1),0,1); + + #define LONELINESS_FILTER 0.99 + mood.loneliness= constrain( mood.loneliness*LONELINESS_FILTER + (1-(rollmapped+pitchmapped)/2)*(1-LONELINESS_FILTER) ,0.0,1.0); + + /* + Serial.print(gyro); Serial.print(", "); Serial.print(mood.shakiness); Serial.print(", "); - Serial.println(mood.wakefulness); + Serial.print(mood.wakefulness); + Serial.print(", "); + Serial.println(mood.loneliness); */ + matrix_weights_update(); } @@ -142,11 +155,15 @@ void move_protesis_with_sensor_data() { //print_servo_torques(); } -void move_protesis_organic() { +void readMPU() { read_mpu_6050_data_with_offset_substraction(current_mpu_measurement); calculate_inertia(current_mpu_inertia, current_mpu_measurement); correct_inertia_drift(current_mpu_inertia); 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(); diff --git a/protesis_03_dxl/servo-control.cpp b/protesis_03_dxl/servo-control.cpp index 2660d3f..9281f19 100644 --- a/protesis_03_dxl/servo-control.cpp +++ b/protesis_03_dxl/servo-control.cpp @@ -30,62 +30,76 @@ void matrix_weights_update(){ //1=roll - float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise + //float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise //float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise - + float map_mode_noise=constrain(mapfloat(mood.wakefulness,0,0.2, 0,1),0,1); + float map_mode_rollpitch=constrain(mapfloat(mood.shakiness,0,0.5, 0,1),0,1); + float map_mode_slither=0; + float map_mode_sleeping=mood.loneliness; + - //testing on servo - - matrix_weights[0][W_COS]=30 *map_mode; - matrix_weights[0][W_NOISE]=90.0 *map_mode; - matrix_weights[0][W_NOISESLOW]=0.0 *map_mode; - matrix_weights[1][W_SIN]=30 *map_mode; - matrix_weights[1][W_NOISE]=70.0 *map_mode; - matrix_weights[1][W_NOISESLOW]=20.0 *map_mode; - matrix_weights[2][W_COS]=-30 *map_mode; - matrix_weights[2][W_NOISE]=50.0 *map_mode; - matrix_weights[2][W_NOISESLOW]=40.0 *map_mode; - matrix_weights[3][W_SIN]=-30 *map_mode; - matrix_weights[3][W_NOISE]=20.0 *map_mode; - matrix_weights[3][W_NOISESLOW]=70.0 *map_mode; - matrix_weights[4][W_COS]=30 *map_mode; - matrix_weights[4][W_NOISE]=0.0 *map_mode; - matrix_weights[4][W_NOISESLOW]=60.0 *map_mode; + float map_sum=map_mode_noise+map_mode_rollpitch+map_mode_slither+map_mode_sleeping; + map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0 + map_mode_rollpitch/=map_sum; + map_mode_slither/=map_sum; + map_mode_sleeping/=map_sum; - matrix_weights[0][W_ROLL]=1.0* (1-map_mode); - matrix_weights[1][W_PITCH]=1.0* (1-map_mode); - matrix_weights[2][W_ROLL]=1.0* (1-map_mode); - matrix_weights[3][W_PITCH]=1.0* (1-map_mode); - matrix_weights[4][W_ROLL]=1.0* (1-map_mode); + 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; + matrix_weights[0][W_NOISESLOW]=0.0 *map_mode_noise; + + matrix_weights[1][W_NOISE]=70.0 *map_mode_noise; + matrix_weights[1][W_NOISESLOW]=20.0 *map_mode_noise; + + matrix_weights[2][W_NOISE]=50.0 *map_mode_noise; + matrix_weights[2][W_NOISESLOW]=40.0 *map_mode_noise; + + matrix_weights[3][W_NOISE]=20.0 *map_mode_noise; + matrix_weights[3][W_NOISESLOW]=70.0 *map_mode_noise; + + matrix_weights[4][W_NOISE]=0.0 *map_mode_noise; + matrix_weights[4][W_NOISESLOW]=60.0 *map_mode_noise; + + //Mode Roll Pitch + matrix_weights[0][W_ROLL]=1.0* map_mode_rollpitch; + matrix_weights[1][W_PITCH]=1.0* map_mode_rollpitch; + matrix_weights[2][W_ROLL]=1.0* map_mode_rollpitch; + matrix_weights[3][W_PITCH]=1.0* map_mode_rollpitch; + matrix_weights[4][W_ROLL]=1.0* map_mode_rollpitch; + + //Mode Slither + matrix_weights[0][W_COS]=30 *map_mode_slither; + matrix_weights[1][W_SIN]=30 *map_mode_slither; + matrix_weights[2][W_COS]=-30 *map_mode_slither; + matrix_weights[3][W_SIN]=-30 *map_mode_slither; + 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[3][W_NOISESLOW]=15 *map_mode_sleeping; + matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping; /* - matrix_weights[0][W_NOISE]=90.0*map_mode; - matrix_weights[0][W_NOISESLOW]=90.0*map_mode; - matrix_weights[1][W_NOISE]=90.0*map_mode; - matrix_weights[1][W_NOISESLOW]=90.0*map_mode; - matrix_weights[2][W_NOISE]=90.0*map_mode; - matrix_weights[2][W_NOISESLOW]=90.0*map_mode; - matrix_weights[3][W_NOISE]=90.0*map_mode; - matrix_weights[3][W_NOISESLOW]=90.0*map_mode; - matrix_weights[4][W_NOISE]=90.0*map_mode; - matrix_weights[4][W_NOISESLOW]=90.0*map_mode; - - - */ - - - float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 0,1),0,1); + float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 0,1),0,1); for(int i = 0; i < SERVO_COUNT ; i++){ matrix_weights[i][W_PITCH]*=sleeping; matrix_weights[i][W_ROLL]*=sleeping; matrix_weights[i][W_NOISE]*=sleeping; + matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1); } - + */ @@ -261,7 +275,6 @@ void map_pitch_to_servos_with_initial_position_and_move(const int pitch){ } void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch){ - static unsigned long millis_add; #define MILLISADD_MAX 100 millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX); diff --git a/protesis_03_dxl/servo-control.hpp b/protesis_03_dxl/servo-control.hpp index b308731..dbc66cb 100644 --- a/protesis_03_dxl/servo-control.hpp +++ b/protesis_03_dxl/servo-control.hpp @@ -19,6 +19,7 @@ struct Mood{ float shakiness; float wakefulness; + float loneliness; }; extern Mood mood;