283 lines
9 KiB
C++
283 lines
9 KiB
C++
#include <ax12.h>
|
|
#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;
|
|
|
|
|
|
|
|
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_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.
|
|
}
|
|
|
|
|
|
}
|
|
|
|
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();
|
|
}
|