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

implement more mood control

Browse source
This commit is contained in:
Philipp Kramer 2023-01-12 13:32:12 +01:00
parent c1a5d8e2e4
commit 8d5c40deb3
3 changed files with 78 additions and 47 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

27
protesis_03_dxl/protesis_03_dxl.ino
View file

@ -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,13 +111,25 @@ 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();

93
protesis_03_dxl/servo-control.cpp
View file

@ -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);

1
protesis_03_dxl/servo-control.hpp
View file

@ -19,6 +19,7 @@
struct Mood{
float shakiness;
float wakefulness;
float loneliness;
};
extern Mood mood;