From d5adcfeaec2dca7cf3f2a790c8adabd9e4fa3d9d Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Mon, 7 Nov 2022 17:06:48 +0100 Subject: [PATCH] roll pitch control and automatic self movement --- noise.cpp | 86 +++++++++++++++++++++++++++++++++++++++ noise.hpp | 16 ++++++++ protesis_03_dxl.ino | 48 +++++++++++++++++++--- servo-control.cpp | 99 ++++++++++++++++++++++++++++++++++++++------- servo-control.hpp | 12 +++++- 5 files changed, 240 insertions(+), 21 deletions(-) create mode 100644 noise.cpp create mode 100644 noise.hpp diff --git a/noise.cpp b/noise.cpp new file mode 100644 index 0000000..948772a --- /dev/null +++ b/noise.cpp @@ -0,0 +1,86 @@ + +#include "noise.hpp" + +float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min; +} + + +//using the algorithm from http://freespace.virgin.net/hugo.elias/models/m_perlin.html +// thanks to hugo elias +float Noise2(float x, float y) +{ + long noise; + noise = x + y * 57; + //noise = pow(noise << 13,noise); + noise = (noise << 13)*noise; + return ( 1.0 - ( long(noise * (noise * noise * 15731L + 789221L) + 1376312589L) & 0x7fffffff) / 1073741824.0); +} + +float SmoothNoise2(float x, float y) +{ + float corners, sides, center; + corners = ( Noise2(x-1, y-1)+Noise2(x+1, y-1)+Noise2(x-1, y+1)+Noise2(x+1, y+1) ) / 16; + sides = ( Noise2(x-1, y) +Noise2(x+1, y) +Noise2(x, y-1) +Noise2(x, y+1) ) / 8; + center = Noise2(x, y) / 4; + return (corners + sides + center); +} + +float InterpolatedNoise2(float x, float y) +{ + float v1,v2,v3,v4,i1,i2,fractionX,fractionY; + long longX,longY; + + longX = long(x); + fractionX = x - longX; + + longY = long(y); + fractionY = y - longY; + + v1 = SmoothNoise2(longX, longY); + v2 = SmoothNoise2(longX + 1, longY); + v3 = SmoothNoise2(longX, longY + 1); + v4 = SmoothNoise2(longX + 1, longY + 1); + + i1 = Interpolate(v1 , v2 , fractionX); + i2 = Interpolate(v3 , v4 , fractionX); + + + return(Interpolate(i1 , i2 , fractionY)); +} + +float Interpolate(float a, float b, float x) +{ + //cosine interpolations + return(CosineInterpolate(a, b, x)); +} + +float LinearInterpolate(float a, float b, float x) +{ + return(a*(1-x) + b*x); +} + +float CosineInterpolate(float a, float b, float x) +{ + float ft = x * 3.1415927; + float f = (1 - cos(ft)) * .5; + + return(a*(1-f) + b*f); +} + +float PerlinNoise2(float x, float y, float persistance, int octaves) +{ + float frequency, amplitude; + float total = 0.0; + + for (int i = 0; i <= octaves - 1; i++) + { + frequency = pow(2,i); + amplitude = pow(persistance,i); + + total = total + InterpolatedNoise2(x * frequency, y * frequency) * amplitude; + } + + return(total); +} \ No newline at end of file diff --git a/noise.hpp b/noise.hpp new file mode 100644 index 0000000..6e956e4 --- /dev/null +++ b/noise.hpp @@ -0,0 +1,16 @@ +#include + +#ifndef NOISE_HPP_ +#define NOISE_HPP_ + +float mapfloat(float x, float in_min, float in_max, float out_min, float out_max); + +float Noise2(float x, float y); +float SmoothNoise2(float x, float y); +float InterpolatedNoise2(float x, float y); +float Interpolate(float a, float b, float x); +float LinearInterpolate(float a, float b, float x); +float CosineInterpolate(float a, float b, float x); +float PerlinNoise2(float x, float y, float persistance, int octaves); + +#endif \ No newline at end of file diff --git a/protesis_03_dxl.ino b/protesis_03_dxl.ino index 2f3741f..cadd539 100644 --- a/protesis_03_dxl.ino +++ b/protesis_03_dxl.ino @@ -3,6 +3,7 @@ #include "imu.hpp" #include "servo-control.hpp" #include "movement.hpp" +#include "noise.hpp" #define DENOISE_PREVIOUS_INERTIA_RATE 0.9999 #define DENOISE_CURRENT_INERTIA_RATE 0.0001 @@ -42,12 +43,13 @@ void print_loop_time_micros(); void print_servo_torques(); + void setup() { Serial.begin(115200); Serial.println("booting"); - Serial.println("setupr registers"); + Serial.println("setup registers"); setup_mpu_6050_registers(); Serial.println("calc mpu offsets"); calculate_mpu_offsets_as_average(); @@ -61,25 +63,29 @@ void setup() { //servos_init_with_current_position(); Serial.println("wait servos"); servos_in_position_wait(); - pinMode(switch_programm, INPUT); + pinMode(switch_programm, INPUT_PULLUP); current_timestamp_us = micros(); + + + matrix_weights_update(); //initialize weights matrix Serial.println("booted"); } void loop() { - Serial.print("state_switch_programm="); - Serial.println(state_switch_programm); int state_switch_programm = digitalRead(switch_programm); + //Serial.println(digitalRead(switch_programm)); if(state_switch_programm == 1) { slither(); } else { - move_protesis_with_sensor_data(); + //move_protesis_with_sensor_data(); + update_mood(); + move_protesis_organic(); } @@ -89,6 +95,22 @@ 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.) ; + #define GYROSCOPE_FILTER_WAKEFULNESS 0.99 + mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.); + + Serial.print(gyro); + Serial.print(", "); + Serial.print(mood.shakiness); + Serial.print(", "); + Serial.println(mood.wakefulness); + + matrix_weights_update(); +} + void move_protesis_with_sensor_data() { read_mpu_6050_data_with_offset_substraction(current_mpu_measurement); calculate_inertia(current_mpu_inertia, current_mpu_measurement); @@ -111,6 +133,22 @@ void move_protesis_with_sensor_data() { //print_servo_torques(); } +void move_protesis_organic() { + 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(); + //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 diff --git a/servo-control.cpp b/servo-control.cpp index a851231..c4a2545 100644 --- a/servo-control.cpp +++ b/servo-control.cpp @@ -10,17 +10,66 @@ int angle_to_dxl_servo_position(const int deg); int rpm_to_dxl_servo_speed(const int rpm); + +Mood mood; + +float matrix_weights[SERVO_COUNT][3]; //mapping outputs, sensors/moods +#define W_PITCH 0 +#define W_ROLL 1 +#define W_NOISE 2 + + +void matrix_weights_update(){ + //0=pitch + //1=roll + + + float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise + + + + + //testing on servo + matrix_weights[0][W_NOISE]=90.0*map_mode; + matrix_weights[0][W_ROLL]=1.0* (1-map_mode); + + matrix_weights[1][W_NOISE]=90.0*map_mode; + matrix_weights[1][W_PITCH]=1.0* (1-map_mode); + + matrix_weights[2][W_NOISE]=90.0*map_mode; + matrix_weights[2][W_ROLL]=1.0* (1-map_mode); + + matrix_weights[3][W_NOISE]=90.0*map_mode; + matrix_weights[3][W_PITCH]=1.0* (1-map_mode); + + matrix_weights[4][W_NOISE]=90.0*map_mode; + matrix_weights[4][W_ROLL]=1.0* (1-map_mode); + + + + float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 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; + } + + + + +} + dxl_servo servos[SERVO_COUNT]; void servos_set_current_initial_position() { for(int i = 0; i < SERVO_COUNT ; i++){ - servos[i].intial_position = dxlGetPosition(servos[i].id); + servos[i].initial_position = dxlGetPosition(servos[i].id); } } void servos_setup() { servos[0].id = 1; - servos[0].intial_position = DXL_NOSE_MOTOR_INITIAL_POSITION; + servos[0].initial_position = DXL_NOSE_MOTOR_INITIAL_POSITION; servos[0].initial_speed = DXL_NOSE_MOTOR_INITIAL_SPEED; servos[0].max_position = DXL_NOSE_MOTOR_MAX_POSITION_CCW; servos[0].min_position = DXL_NOSE_MOTOR_MIN_POSITION_CW; @@ -28,7 +77,7 @@ void servos_setup() { servos[0].joint_orientation = - 1; servos[1].id = 2; - servos[1].intial_position = DXL_MID_MOTOR_INITIAL_POSITION; + servos[1].initial_position = DXL_MID_MOTOR_INITIAL_POSITION; servos[1].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED; servos[1].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW; servos[1].min_position = DXL_MID_MOTOR_MIN_POSITION_CW; @@ -36,7 +85,7 @@ void servos_setup() { servos[1].joint_orientation = - 1; servos[2].id = 3; - servos[2].intial_position = DXL_MID_MOTOR_INITIAL_POSITION; + servos[2].initial_position = DXL_MID_MOTOR_INITIAL_POSITION; servos[2].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED; servos[2].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW; servos[2].min_position = DXL_MID_MOTOR_MIN_POSITION_CW; @@ -44,7 +93,7 @@ void servos_setup() { servos[2].joint_orientation = + 1; servos[3].id = 4; - servos[3].intial_position = DXL_MID_MOTOR_INITIAL_POSITION; + servos[3].initial_position = DXL_MID_MOTOR_INITIAL_POSITION; servos[3].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED; servos[3].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW; servos[3].min_position = DXL_MID_MOTOR_MIN_POSITION_CW; @@ -52,7 +101,7 @@ void servos_setup() { servos[3].joint_orientation = + 1; servos[4].id = 5; - servos[4].intial_position = DXL_BASE_MOTOR_INITIAL_POSITION; + servos[4].initial_position = DXL_BASE_MOTOR_INITIAL_POSITION; servos[4].initial_speed = DXL_BASE_MOTOR_INITIAL_SPEED; servos[4].max_position = DXL_BASE_MOTOR_MAX_POSITION_CCW; servos[4].min_position = DXL_BASE_MOTOR_MIN_POSITION_CW; @@ -78,7 +127,7 @@ void servos_init() { 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].intial_position); + dxlSetGoalPosition(servos[i].id, servos[i].initial_position); } } @@ -86,7 +135,7 @@ void servo_init(const int servo_id) { int index = servo_id - 1; dxlSetJointMode(servos[index].id, servos[index].min_position, servos[index].max_position); dxlSetGoalSpeed(servos[index].id, servos[index].initial_speed); - dxlSetGoalPosition(servos[index].id, servos[index].intial_position); + dxlSetGoalPosition(servos[index].id, servos[index].initial_position); } void servo_to_initial_position(const int servo_id) { @@ -95,7 +144,7 @@ void servo_to_initial_position(const int servo_id) { void servos_to_initial_position() { for(int i = 0; i < SERVO_COUNT; i++) { - dxlSetGoalPosition(servos[i].id, servos[i].intial_position); + dxlSetGoalPosition(servos[i].id, servos[i].initial_position); } } @@ -135,7 +184,7 @@ void servo_move_to_angle(const int servo_id, const int deg) { } void map_angle_to_servo_with_initial_position(const int servo_id, const int deg) { - dxlSetGoalPosition(servo_id, servos[servo_id -1].intial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg)); + dxlSetGoalPosition(servo_id, servos[servo_id -1].initial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg)); } void servos_move_to_angle(const int deg) { @@ -166,18 +215,38 @@ void map_pitch_to_servos_and_move(const int pitch){ } void map_roll_to_servos_with_initial_position_and_move(const int roll){ - dxlSetGoalPosition(servos[0].id, servos[0].intial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll))); - dxlSetGoalPosition(servos[2].id, servos[2].intial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll))); - dxlSetGoalPosition(servos[4].id, servos[4].intial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll))); + dxlSetGoalPosition(servos[0].id, servos[0].initial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll))); + dxlSetGoalPosition(servos[2].id, servos[2].initial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll))); + dxlSetGoalPosition(servos[4].id, servos[4].initial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll))); delay(1); } void map_pitch_to_servos_with_initial_position_and_move(const int pitch){ - dxlSetGoalPosition(servos[1].id, servos[1].intial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch))); - dxlSetGoalPosition(servos[3].id, servos[3].intial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch))); + dxlSetGoalPosition(servos[1].id, servos[1].initial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch))); + dxlSetGoalPosition(servos[3].id, servos[3].initial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch))); delay(1); } +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); + //Serial.print(mood.shakiness); + //Serial.print(", "); + //Serial.println(millis_add); + + + for (uint8_t i=0;i<=SERVO_COUNT;i++) { + float pnoise=PerlinNoise2((millis()+millis_add)/1000.0,i*10,0.25,3); //x,y,persistance,octaves + int angle=0; + angle=pitch*matrix_weights[i][0]+roll*matrix_weights[i][1]+pnoise*matrix_weights[i][2]; + dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle))); + } + delay(1); +} + + int servo_calculate_torque_with_direction(const int servo_id) { int current_torque = dxlGetTorque(servo_id); int direction = current_torque & MOTOR_TORQUE_DIRECTION_MASK; diff --git a/servo-control.hpp b/servo-control.hpp index 5d21a70..eee902d 100644 --- a/servo-control.hpp +++ b/servo-control.hpp @@ -6,12 +6,19 @@ */ #include +#include "noise.hpp" #ifndef SERVO_CONTROL_HPP_ #define SERVO_CONTROL_HPP_ #define SERVO_COUNT 5 +struct Mood{ + float shakiness; + float wakefulness; +}; +extern Mood mood; + #define RPM_TO_SPEED_RATIO 0.111 #define ANGLE_PER_DIGIT_RATIO .2925 //#define ANGLE_PER_DIGIT_RATIO 0.29 // From datasheet @@ -60,7 +67,7 @@ struct dxl_servo { int id; int initial_speed; - int intial_position; + int initial_position; int min_position; int max_position; int torque_limit; @@ -68,6 +75,8 @@ struct dxl_servo { int joint_orientation; }; +void matrix_weights_update(); + void servos_setup(); void servos_set_current_initial_position(); @@ -101,5 +110,6 @@ void map_angle_to_servo_with_initial_position(const int servo_id, const int deg) void map_roll_to_servos_with_initial_position_and_move(const int roll); 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); #endif /* SERVO_CONTROL_HPP_ */