#include #include "mpu6050.hpp" #include "imu.hpp" #include "servo-control.hpp" #include "movement.hpp" #include "noise.hpp" #include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise #define DENOISE_PREVIOUS_INERTIA_RATE 0.9999 #define DENOISE_CURRENT_INERTIA_RATE 0.0001 #define DENOISE_PREVIOUS_MPU_RATE 0.9999 #define DENOISE_CURRENT_MPU_RATE 0.0001 measurement current_mpu_measurement; inertia current_mpu_inertia; inertia previous_mpu_inertia; measurement_double_precission previous_mpu_measurement_dp; measurement_double_precission current_mpu_measurement_dp; unsigned long loop_counter = 0; int servo_id; int roll = 125; int pitch; int toggle = 50; //Switch const int switch_programm = A3; int state_switch_programm = 0; //header void move_protesis_with_sensor_data(); void denoise_inertia_with_complementary_filter(); void denoise_mpu_measurement_with_complementary_filter(); double denoise_complementary(const double previous, const double current); void print_inertia_gyroscope_values(); void print_inertia_acceleration_values(); void print_mpu_acceleration_data(); void print_mpu_gyroscope_data(); void print_mpu_offset(); void print_loop_time_micros(); void print_servo_torques(); void readMPU(); void setup() { Serial.begin(57600); Serial.println("booting"); Serial.println("setup registers"); setup_mpu_6050_registers(); Serial.println("calc mpu offsets"); calculate_mpu_offsets_as_average(); Serial.println("read mpu data"); read_mpu_6050_data(current_mpu_measurement); Serial.println("init inertia"); initialize_inertia(current_mpu_inertia, current_mpu_measurement); Serial.println("init servos"); servos_init_with_fixed_position(); //servos_init_with_current_position(); Serial.println("wait servos"); servos_in_position_wait(); pinMode(switch_programm, INPUT_PULLUP); matrix_weights_update(); //initialize weights matrix Serial.println("booted"); } void loop() { unsigned long loopmillis=millis(); static unsigned long last_loop_movement_millis; static unsigned long last_loop_readpos_millis; static unsigned long last_servo_send_millis; static unsigned long last_loop_sendmatrix_millis; static unsigned long last_loop_sendtorque_millis; if (loopmillis-last_loop_movement_millis>50) { last_loop_movement_millis=loopmillis; 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(); 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"); } if (loopmillis-last_loop_sendtorque_millis>1000 && loopmillis-last_servo_send_millis>=4) { set_servo_torque(); last_servo_send_millis=millis(); last_loop_sendtorque_millis=millis(); } 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. } if (loopmillis-last_loop_sendmatrix_millis>1000) { last_loop_sendmatrix_millis=loopmillis; transmit_matrix_weights(); } } 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.); 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.print(mood.wakefulness); Serial.print(", "); Serial.println(mood.loneliness); */ 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); 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_roll_to_servos_with_initial_position_and_move(current_mpu_inertia.roll_acceleration); map_pitch_to_servos_with_initial_position_and_move(current_mpu_inertia.pitch_acceleration); // PRINT DATA TO TERMINAL //print_mpu_acceleration_data(); //print_mpu_gyroscope_data(); //print_inertia_acceleration_values(); //print_inertia_gyroscope_values(); //print_mpu_offset(); //print_servo_torques(); } 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 denoise_inertia_with_complementary_filter() { current_mpu_inertia.roll_gyroscope = previous_mpu_inertia.roll_gyroscope * DENOISE_PREVIOUS_INERTIA_RATE + current_mpu_inertia.roll_gyroscope * DENOISE_CURRENT_INERTIA_RATE; current_mpu_inertia.pitch_gyroscope = previous_mpu_inertia.pitch_gyroscope * DENOISE_PREVIOUS_INERTIA_RATE + current_mpu_inertia.pitch_gyroscope * DENOISE_CURRENT_INERTIA_RATE; previous_mpu_inertia = current_mpu_inertia; } void denoise_mpu_measurement_with_complementary_filter() { current_mpu_measurement_dp.acceleration.x = denoise_complementary( previous_mpu_measurement_dp.acceleration.x, current_mpu_measurement_dp.acceleration.x); current_mpu_measurement_dp.acceleration.y = denoise_complementary( previous_mpu_measurement_dp.acceleration.y, current_mpu_measurement_dp.acceleration.y); current_mpu_measurement_dp.acceleration.z = denoise_complementary( previous_mpu_measurement_dp.acceleration.z, current_mpu_measurement_dp.acceleration.z); current_mpu_measurement_dp.gyroscope.x = denoise_complementary( previous_mpu_measurement_dp.gyroscope.x, current_mpu_measurement_dp.gyroscope.x); current_mpu_measurement_dp.gyroscope.y = denoise_complementary( previous_mpu_measurement_dp.gyroscope.y, current_mpu_measurement_dp.gyroscope.y); current_mpu_measurement_dp.gyroscope.z = denoise_complementary( previous_mpu_measurement_dp.gyroscope.y, current_mpu_measurement_dp.gyroscope.z); previous_mpu_measurement_dp = current_mpu_measurement_dp; } double denoise_complementary(const double previous, const double current) { return previous * DENOISE_PREVIOUS_MPU_RATE + current * DENOISE_CURRENT_MPU_RATE; } void print_inertia_acceleration_values() { Serial.print("acc_rp=["); Serial.print(current_mpu_inertia.roll_acceleration); Serial.print(";"); Serial.print(current_mpu_inertia.pitch_acceleration); Serial.println("]"); } void print_inertia_gyroscope_values() { Serial.print("gyr_rp=["); Serial.print(current_mpu_inertia.roll_gyroscope); Serial.print(";"); Serial.print(current_mpu_inertia.pitch_gyroscope); Serial.println("]"); } void print_mpu_acceleration_data() { Serial.print("acc=["); Serial.print( acceleration_with_precission( current_mpu_measurement.acceleration.x)); //Serial.print(current_mpu_measurement.acceleration.x); Serial.print(";"); Serial.print( acceleration_with_precission( current_mpu_measurement.acceleration.y)); //Serial.print(current_mpu_measurement.acceleration.y); Serial.print(";"); Serial.print( acceleration_with_precission( current_mpu_measurement.acceleration.z)); //Serial.print(current_mpu_measurement.acceleration.z); Serial.println("]"); } void print_mpu_gyroscope_data() { Serial.print("gyro=["); Serial.print((int) current_mpu_measurement_dp.gyroscope.x); //Serial.print( //gyroscope_with_precission(current_mpu_measurement.gyroscope.x)); //Serial.print(current_mpu_measurement.gyroscope.x); Serial.print(";"); Serial.print((int) current_mpu_measurement_dp.gyroscope.y); //Serial.print( //gyroscope_with_precission(current_mpu_measurement.gyroscope.y)); //Serial.print(current_mpu_measurement.gyroscope.y); Serial.print(";"); Serial.print((int)current_mpu_measurement_dp.gyroscope.z); //Serial.print( //gyroscope_with_precission(current_mpu_measurement.gyroscope.z)); //Serial.print(current_mpu_measurement.gyroscope.z); Serial.println("]"); } void print_mpu_offset() { Serial.print("offset=["); Serial.print(get_mpu_offset().acceleration.x); Serial.print(";"); Serial.print(get_mpu_offset().acceleration.y); Serial.print(";"); Serial.print(get_mpu_offset().acceleration.z); Serial.println("]"); } /*void print_loop_time_micros() { Serial.print(loop_counter++); Serial.print(" diff ms: "); Serial.println(micros()-current_timestamp_us); } */ void print_servo_torques() { servos_print_current_torque(); }