commit dd7e136da725d31f73d1893d427fb5cc6875a04b Author: Philipp Kramer Date: Mon Nov 7 11:07:34 2022 +0100 inital project, add booting serial prints diff --git a/.cproject b/.cproject new file mode 100644 index 0000000..4b6243b --- /dev/null +++ b/.cproject @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.project b/.project new file mode 100644 index 0000000..b81ff4d --- /dev/null +++ b/.project @@ -0,0 +1,20 @@ + + + protesis-dxl + + + + + + org.eclipse.cdt.core.cBuilder + clean,full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.arduino.core.arduinoNature + + diff --git a/imu.cpp b/imu.cpp new file mode 100644 index 0000000..41ce72d --- /dev/null +++ b/imu.cpp @@ -0,0 +1,172 @@ +/* + * imu.cpp + * + * Created on: 09.01.2021 + * Author: frank + */ + +#include +#include "imu.hpp" + +inertia inertia_offset; + +void initialize_inertia(inertia &inertia, const measurement mpu_measurement) { + + inertia_offset.roll_vector_length = roll_vector_length(get_mpu_offset()); + inertia_offset.pitch_vector_length = pitch_vector_length(get_mpu_offset()); + inertia_offset.yaw_vector_length = yaw_vector_length(get_mpu_offset()); + + calculate_inertia(inertia, mpu_measurement); + + //inertia_offset.roll_vector_length = inertia.roll_vector_length; + //inertia_offset.pitch_vector_length = inertia.pitch_vector_length; + //inertia_offset.yaw_vector_length = inertia.pitch_vector_length; + + inertia_offset.roll_acceleration = inertia.roll_acceleration; + inertia_offset.pitch_acceleration = inertia.pitch_acceleration; + + inertia_offset.roll_gyroscope = inertia.roll_gyroscope; + inertia_offset.pitch_gyroscope = inertia.pitch_gyroscope; + + inertia_offset.vector_length = inertia.vector_length; + + inertia.roll_gyroscope = inertia.roll_acceleration; + inertia.pitch_gyroscope = inertia.pitch_acceleration; +} + +void calculate_inertia_with_offset_substraction(inertia &inertia, + const measurement mpu_measurement) { + calculate_inertia(inertia, mpu_measurement); + substract_acceleration_offset(inertia); +} + +void calculate_inertia(inertia &inertia, const measurement mpu_measurement) { + + inertia.vector_length = vector_length_3d(mpu_measurement); + + /*inertia.roll_acceleration = asin( + (float) mpu_measurement.acceleration.y / inertia.vector_length) + * -RADIAN_CONST + ; + inertia.pitch_acceleration = asin( + (float) mpu_measurement.acceleration.x / inertia.vector_length) + * RADIAN_CONST + ;*/ + + inertia.roll_acceleration = atan2(mpu_measurement.acceleration.y, + mpu_measurement.acceleration.z) * RAD_TO_DEG; + + inertia.pitch_acceleration = atan2(-mpu_measurement.acceleration.x, + vector_length_2d(mpu_measurement.acceleration.y, + mpu_measurement.acceleration.z)) * RAD_TO_DEG; + + //inertia.roll_acceleration = 180 * (asin(mpu_measurement.acceleration.y / vector_length_3d(mpu_measurement)))/M_PI; + //inertia.pitch_acceleration = 180 * (asin(mpu_measurement.acceleration.x / vector_length_3d(mpu_measurement)))/M_PI; + + /*inertia.roll_vector_length = roll_vector_length(mpu_measurement); + inertia.pitch_vector_length = pitch_vector_length(mpu_measurement); + inertia.yaw_vector_length = yaw_vector_length(mpu_measurement); + + inertia.roll_acceleration = roll_by_acceleration(inertia, mpu_measurement); + inertia.pitch_acceleration = pitch_by_acceleration(inertia, + mpu_measurement);*/ + + //TODO Evaluate correctness of dimensions (actually x -> roll, y -> pitch) + inertia.roll_gyroscope += gyroscope_with_precission( + mpu_measurement.gyroscope.x); + inertia.pitch_gyroscope += gyroscope_with_precission( + mpu_measurement.gyroscope.y); + + inertia.roll_gyroscope -= inertia.pitch_gyroscope + * sin(gyroscope_with_precission(mpu_measurement.gyroscope.z)) * RAD_TO_DEG; //If the IMU has yawed transfer the pitch angle to the roll angel + inertia.pitch_gyroscope += inertia.roll_gyroscope + * sin(gyroscope_with_precission(mpu_measurement.gyroscope.z)) * RAD_TO_DEG; //If the IMU has yawed transfer the roll angle to the pitch angel +} + +void substract_acceleration_offset(inertia &inertia) { + inertia.roll_acceleration -= inertia_offset.roll_acceleration; + inertia.pitch_acceleration -= inertia_offset.pitch_acceleration; +} + +void correct_inertia_drift(inertia &inertia) { + inertia.roll_gyroscope = inertia.roll_gyroscope * DRIFT_CORRECTION_RATE_GYRO + + inertia.roll_acceleration * DRIFT_CORRECTION_RATE_ACCEL; + + inertia.pitch_gyroscope = inertia.pitch_gyroscope + * DRIFT_CORRECTION_RATE_GYRO + + inertia.pitch_acceleration * DRIFT_CORRECTION_RATE_ACCEL; +} + +long square(const long value) { + return value * value; +} + +double roll_vector_length(const measurement mpu_measurement) { + return vector_length_2d(mpu_measurement.acceleration.y, + mpu_measurement.acceleration.z); +} + +double pitch_vector_length(const measurement mpu_measurement) { + return vector_length_2d(mpu_measurement.acceleration.x, + mpu_measurement.acceleration.z); +} + +double yaw_vector_length(const measurement mpu_measurement) { + return vector_length_2d(mpu_measurement.acceleration.x, + mpu_measurement.acceleration.y); +} + +double roll_by_acceleration(const inertia inertia, + const measurement mpu_measurement) { + + long scalar_product = mpu_measurement.acceleration.x + * get_mpu_offset().acceleration.x + + mpu_measurement.acceleration.z * get_mpu_offset().acceleration.z; + + double area = inertia.roll_vector_length + * inertia_offset.roll_vector_length; + /*Serial.print("sp="); + Serial.print(scalar_product); + Serial.print(",a="); + Serial.print(area); + Serial.print(",v="); + Serial.print(inertia_offset.roll_vector_length); + Serial.println();*/ + return acos(scalar_product / area) * RADIAN_CONST; +} + +double pitch_by_acceleration(const inertia inertia, + const measurement mpu_measurement) { + long scalar_product = mpu_measurement.acceleration.y + * get_mpu_offset().acceleration.y + + mpu_measurement.acceleration.z * get_mpu_offset().acceleration.z; + + double area = inertia.pitch_vector_length + * inertia_offset.pitch_vector_length; + + return acos(scalar_product / area) * RADIAN_CONST; +} + +double vector_length_2d(const long x, const long y) { + return sqrt((double) square(x) + square(y)); +} + +double vector_length_3d(measurement const measurement) { + return sqrt( + (double) square(measurement.acceleration.x) + + square(measurement.acceleration.y) + + square(measurement.acceleration.z)); +} + +double acceleration_with_precission(const long value) { + return value * ACCELEROMETER_CONST; +} + +double gyroscope_with_precission(const long value) { + return value * GYRO_CONST; +} + +inertia get_inertia_offset() { + return inertia_offset; +} + diff --git a/imu.hpp b/imu.hpp new file mode 100644 index 0000000..a762c83 --- /dev/null +++ b/imu.hpp @@ -0,0 +1,48 @@ +/* + * imu.hpp + * + * Created on: 09.01.2021 + * Author: frank + */ + +#ifndef IMU_HPP_ +#define IMU_HPP_ + +#include "mpu6050.hpp" + +#define RADIAN_CONST 57.296; // = 1 / (PI / 180) +#define CONST_TO_RADIANS (PI / 180) + +#define DRIFT_CORRECTION_RATE_GYRO 0.9996; +#define DRIFT_CORRECTION_RATE_ACCEL 0.0004; + +struct inertia { + double roll_acceleration; + double pitch_acceleration; + double roll_gyroscope; + double pitch_gyroscope; + double vector_length; + double roll_vector_length; + double pitch_vector_length; + double yaw_vector_length; +}; + +void calculate_inertia(inertia &inertia, const measurement measurement); +void calculate_inertia_with_offset_substraction(inertia &inertia, + const measurement measurement); +void correct_inertia_drift(inertia &inertia); +void initialize_inertia(inertia &inertia, const measurement measurement); +double pitch_by_acceleration(const inertia inertia, const measurement measurement); +double roll_by_acceleration(const inertia inertia, const measurement measurement); +void substract_acceleration_offset(inertia &inertia); +double yaw_by_acceleration(const inertia inertia, const measurement measurement); +double vector_length_2d(const long x, const long y); +double vector_length_3d(measurement const measurement); +double roll_vector_length(const measurement mpu_measurement); +double pitch_vector_length(const measurement mpu_measurement); +double yaw_vector_length(const measurement mpu_measurement); +double acceleration_with_precission(const long value); +double gyroscope_with_precission(const long value); +inertia get_inertia_offset(); + +#endif /* IMU_HPP_ */ diff --git a/movement.cpp b/movement.cpp new file mode 100644 index 0000000..b108fb4 --- /dev/null +++ b/movement.cpp @@ -0,0 +1,25 @@ +/* + * movement.cpp + * + * Created on: 17.01.2021 + * Author: frank + */ + +#include "Arduino.h" +#include "movement.hpp" + +int angle = 0; +int motor_position; + +void slither(){ + float radiant = (angle % 360) * DEG_TO_RAD; + for(int servo_id = SERVO_COUNT; servo_id > 0; servo_id--) { + motor_position = AMPLITUDE * sin(radiant + servo_id * PHASE_SHIFT * WAVE_LENGTH); + map_angle_to_servo_with_initial_position(servo_id, motor_position); + delay(INTER_SERVO_MOVEMENT_DELAY_MS); + } + angle++; +} + + + diff --git a/movement.hpp b/movement.hpp new file mode 100644 index 0000000..0266e71 --- /dev/null +++ b/movement.hpp @@ -0,0 +1,22 @@ +/* + * movement.hpp + * + * Created on: 17.01.2021 + * Author: frank + */ + +#ifndef MOVEMENT_HPP_ +#define MOVEMENT_HPP_ + +#include "ax12.h" +#include "servo-control.hpp" + +#define AMPLITUDE 35 +#define WAVE_LENGTH 3 +#define PHASE_SHIFT M_PI / SERVO_COUNT +#define INTER_SERVO_MOVEMENT_DELAY_MS 4 + +void slither(); + + +#endif /* MOVEMENT_HPP_ */ diff --git a/mpu6050.cpp b/mpu6050.cpp new file mode 100644 index 0000000..eb2d544 --- /dev/null +++ b/mpu6050.cpp @@ -0,0 +1,101 @@ +#include "mpu6050.hpp" + +measurement mpu_offset; + +void setup_mpu_6050_registers() { + Wire.begin(); + //Activate the MPU-6050 + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x6B); //Send the requested starting register + Wire.write(0x00); //Set the requested starting register + Wire.endTransmission(); + //Configure the accelerometer (+/-8g) + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x1C); //Send the requested starting register + Wire.write(0x10); //Set the requested starting register + Wire.endTransmission(); + //Configure the gyro (500dps full scale) + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x1B); //Send the requested starting register + //Wire.write(0x08); //Set the requested starting register 500° + Wire.write(0x10); //Set the requested starting register 250° + Wire.endTransmission(); + //TODO Configure digital low pass filter + /*Wire.beginTransmission(0x68); + Wire.write(0x1A); + Wire.write(0x04); + Wire.endTransmission();*/ +} + +void read_mpu_6050_data_with_offset_substraction(measurement &measurement) { + read_mpu_6050_data(measurement); + //TODO: check if necessary + substract_gyroscope_offset(measurement); + //substract_accelerometer_offset(measurement); +} + +void read_mpu_6050_data(measurement &measurement) { //Subroutine for reading the raw gyro and accelerometer data + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x3B); //Send the requested starting register + Wire.endTransmission(); //End the transmission + Wire.requestFrom(0x68, 14); //Request 14 bytes from the MPU-6050 + while (Wire.available() < 14); //Wait until all the bytes are received + measurement.acceleration.x = Wire.read() << 8 | Wire.read(); + measurement.acceleration.y = Wire.read() << 8 | Wire.read(); + measurement.acceleration.z = Wire.read() << 8 | Wire.read(); + measurement.temperature = Wire.read() << 8 | Wire.read(); + measurement.gyroscope.x = Wire.read() << 8 | Wire.read(); + measurement.gyroscope.y = Wire.read() << 8 | Wire.read(); + measurement.gyroscope.z = Wire.read() << 8 | Wire.read(); +} + +void substract_gyroscope_offset(measurement &measurement) { + measurement.gyroscope.x -= mpu_offset.gyroscope.x; + measurement.gyroscope.y -= mpu_offset.gyroscope.y; + measurement.gyroscope.z -= mpu_offset.gyroscope.z; +} + +void substract_accelerometer_offset(measurement &measurement) { + measurement.acceleration.x -= mpu_offset.acceleration.x; + measurement.acceleration.y -= mpu_offset.acceleration.y; + measurement.acceleration.z -= mpu_offset.acceleration.z; +} + +void calculate_mpu_offsets_as_average() { + mpu_offset.acceleration.x = 0; + mpu_offset.acceleration.y = 0; + mpu_offset.acceleration.z = 0; + measurement measurement; + for (int cal_int = 0; cal_int < 1000; cal_int++) { //Read the raw acc and gyro data from the MPU-6050 for 1000 times + read_mpu_6050_data(measurement); + mpu_offset.gyroscope.x += measurement.gyroscope.x; //Add the gyro x offset to the gyro_x_cal variable + mpu_offset.gyroscope.y += measurement.gyroscope.y; //Add the gyro y offset to the gyro_y_cal variable + mpu_offset.gyroscope.z += measurement.gyroscope.z; //Add the gyro z offset to the gyro_z_cal variable + mpu_offset.acceleration.x += measurement.acceleration.x; + mpu_offset.acceleration.y += measurement.acceleration.y; + mpu_offset.acceleration.z += measurement.acceleration.z; + delay(3); //Delay 3us to have 250Hz for-loop + } + + // get average offset + mpu_offset.gyroscope.x /= 1000; + mpu_offset.gyroscope.y /= 1000; + mpu_offset.gyroscope.z /= 1000; + mpu_offset.acceleration.x /= 1000; + mpu_offset.acceleration.y /= 1000; + mpu_offset.acceleration.z /= 1000; +} + +measurement get_mpu_offset() { + return mpu_offset; +} + +void mpu_measurement_with_precission(measurement_double_precission &measurement_dp, measurement const measurement) { + measurement_dp.acceleration.x = measurement.acceleration.x * ACCELEROMETER_CONST; + measurement_dp.acceleration.y = measurement.acceleration.y * ACCELEROMETER_CONST; + measurement_dp.acceleration.z = measurement.acceleration.z * ACCELEROMETER_CONST; + + measurement_dp.gyroscope.x = measurement.gyroscope.x * GYRO_CONST; + measurement_dp.gyroscope.y = measurement.gyroscope.y * GYRO_CONST; + measurement_dp.gyroscope.z = measurement.gyroscope.z * GYRO_CONST; +} diff --git a/mpu6050.hpp b/mpu6050.hpp new file mode 100644 index 0000000..07d72c8 --- /dev/null +++ b/mpu6050.hpp @@ -0,0 +1,57 @@ +/* + * mpu6050.h + * + * Created on: 08.01.2021 + * Author: frank + */ + +#ifndef MPU6050_HPP_ +#define MPU6050_HPP_ + +#include +#include + +//#define GYRO_CONST_250HZ_131 0.000030534 // = 1 / (250Hz x 131) +//#define GYRO_CONST_500HZ_65_5 0.000030534 +#define GYRO_CONST_250_DPS 0.0076336 +#define GYRO_CONST_500_DPS 0.015267 +#define GYRO_CONST GYRO_CONST_250_DPS +#define ACCELEROMETER_CONST_2G 0.000061035 +#define ACCELEROMETER_CONST_4G 0.00012207 +#define ACCELEROMETER_CONST_8G 0.00024414 +#define ACCELEROMETER_CONST ACCELEROMETER_CONST_8G + +struct vector{ + long x; + long y; + long z; +}; + +struct vector_double_precision { + double x; + double y; + double z; +}; + +struct measurement { + int temperature; + vector acceleration; + vector gyroscope; +}; + +struct measurement_double_precission { + double temperature; + vector_double_precision acceleration; + vector_double_precision gyroscope; +}; + +void calculate_mpu_offsets_as_average(); +measurement get_mpu_offset(); +void read_mpu_6050_data(measurement &measurement); +void read_mpu_6050_data_with_offset_substraction(measurement &measurement); +void setup_mpu_6050_registers(); +void substract_gyroscope_offset(measurement &measurement); +void substract_accelerometer_offset(measurement &measurement); +void mpu_measurement_with_precission(measurement_double_precission &measurement_dp, const measurement measurement); + +#endif /* MPU6050_HPP_ */ diff --git a/protesis-dxl.cpp.orig b/protesis-dxl.cpp.orig new file mode 100644 index 0000000..80aec5f --- /dev/null +++ b/protesis-dxl.cpp.orig @@ -0,0 +1,201 @@ +#include +#include +#include "mpu6050.hpp" +#include "imu.hpp" +#include "servo-control.hpp" + +#define DENOISE_PREVIOUS_INERTIA_RATE 0.999 +#define DENOISE_CURRENT_INERTIA_RATE 0.001 + +#define DENOISE_PREVIOUS_MPU_RATE 0.999 +#define DENOISE_CURRENT_MPU_RATE 0.001 + +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 current_timestamp_us; +unsigned long loop_counter = 0; +int servo_id; +int roll = 125; +int pitch; +int toggle = 50; + +//header +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 setup() { + setup_mpu_6050_registers(); + calculate_mpu_offsets_as_average(); + read_mpu_6050_data(current_mpu_measurement); + initialize_inertia(current_mpu_inertia, current_mpu_measurement); + + //servos_init_with_fixed_position(); + servos_init_with_current_position(); + servos_in_position_wait(); + Serial.begin(115200); + current_timestamp_us = micros(); +} + +void loop() { +<<<<<<< HEAD + read_mpu_6050_data(current_mpu_measurement); +======= + read_mpu_6050_data_with_offset_substraction(current_mpu_measurement); +>>>>>>> vector-based-inertia + 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(); + + + // PRINT DATA TO TERMINAL + //print_mpu_acceleration_data(); + //print_mpu_gyroscope_data(); + //print_inertia_acceleration_values(); + //print_inertia_gyroscope_values(); + //print_mpu_offset(); + + //servos_collect_current_torque(); + //print_servo_torques(); + + 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); + + while (micros() - current_timestamp_us < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop + //print_loop_time_micros(); + current_timestamp_us = micros(); //Reset the loop timer + +} + +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(); +} + diff --git a/protesis_03_dxl.ino b/protesis_03_dxl.ino new file mode 100644 index 0000000..2f3741f --- /dev/null +++ b/protesis_03_dxl.ino @@ -0,0 +1,228 @@ +#include +#include "mpu6050.hpp" +#include "imu.hpp" +#include "servo-control.hpp" +#include "movement.hpp" + +#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 current_timestamp_us; +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 setup() { + Serial.begin(115200); + + Serial.println("booting"); + + Serial.println("setupr 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); + + current_timestamp_us = micros(); + + 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(); + + } + + while (micros() - current_timestamp_us < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop + //print_loop_time_micros(); + current_timestamp_us = micros(); //Reset the loop timer + +} + +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 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(); +} diff --git a/servo-control.cpp b/servo-control.cpp new file mode 100644 index 0000000..a851231 --- /dev/null +++ b/servo-control.cpp @@ -0,0 +1,206 @@ +/* + * servo-control.cpp + * + * Created on: 12.01.2021 + * Author: frank + */ + +#include "servo-control.hpp" + +int angle_to_dxl_servo_position(const int deg); +int rpm_to_dxl_servo_speed(const int rpm); + +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); + } +} + +void servos_setup() { + servos[0].id = 1; + servos[0].intial_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; + servos[0].torque_limit = DXL_NOSE_MOTOR_TORQUE_LIMIT; + servos[0].joint_orientation = - 1; + + servos[1].id = 2; + servos[1].intial_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; + servos[1].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT; + servos[1].joint_orientation = - 1; + + servos[2].id = 3; + servos[2].intial_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; + servos[2].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT; + servos[2].joint_orientation = + 1; + + servos[3].id = 4; + servos[3].intial_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; + servos[3].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT; + servos[3].joint_orientation = + 1; + + servos[4].id = 5; + servos[4].intial_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; + servos[4].torque_limit = DXL_BASE_MOTOR_TORQUE_LIMIT; + servos[4].joint_orientation = - 1; +} + +void servos_init_with_fixed_position() { + servos_setup(); + servos_init(); +} + +void servos_init_with_current_position() { + servos_setup(); + servos_set_current_initial_position(); + servos_init(); +} + +void servos_init() { + for(int i = 0; i< SERVO_COUNT; i++) { + dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board + dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position); + 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); + } +} + +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); +} + +void servo_to_initial_position(const int servo_id) { + dxlSetGoalPosition(servo_id, DXL_MID_MOTOR_INITIAL_POSITION); +} + +void servos_to_initial_position() { + for(int i = 0; i < SERVO_COUNT; i++) { + dxlSetGoalPosition(servos[i].id, servos[i].intial_position); + } +} + +int servo_moving(const int servo_id) { + return dxlGetMoving(servo_id); +} + +int servos_moving() { + int counter = 0; + for(int i = 0; i < SERVO_COUNT; i++) { + counter += dxlGetMoving(servos[i].id); + } + + if(counter == SERVO_COUNT) { + return 1; + } else { + return 0; + } +} + +void servos_in_position_wait() { + while(servos_moving() == 1); +} + +void servo_in_position_wait(const int servo_id) { + while(dxlGetMoving(servo_id)); +} + + +int servo_set_speed_rpm(const int servo_id, const int rpm) { + dxlSetGoalSpeed(servo_id, rpm); + return rpm; +} + +void servo_move_to_angle(const int servo_id, const int deg) { + dxlSetGoalPosition(servo_id, angle_to_dxl_servo_position(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)); +} + +void servos_move_to_angle(const int deg) { + for(int i = 0; i < SERVO_COUNT; i++) { + dxlSetGoalPosition(servos[i].id, angle_to_dxl_servo_position(deg)); + } +} + +int angle_to_dxl_servo_position(const int deg) { + return deg / ANGLE_PER_DIGIT_RATIO; +} + +int rpm_to_dxl_servo_speed(const int rpm) { + return rpm / RPM_TO_SPEED_RATIO; +} + +void map_roll_to_servos_and_move(const int roll){ + dxlSetGoalPosition(servos[0].id, servos[0].joint_orientation * angle_to_dxl_servo_position(roll)); + dxlSetGoalPosition(servos[2].id, servos[2].joint_orientation * angle_to_dxl_servo_position(roll)); + dxlSetGoalPosition(servos[4].id, servos[4].joint_orientation * angle_to_dxl_servo_position(roll)); + delay(1); +} + +void map_pitch_to_servos_and_move(const int pitch){ + dxlSetGoalPosition(servos[1].id, servos[1].joint_orientation * angle_to_dxl_servo_position(pitch)); + dxlSetGoalPosition(servos[3].id, servos[3].joint_orientation * angle_to_dxl_servo_position(pitch)); + delay(1); +} + +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))); + 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))); + 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; + current_torque &= MOTOR_TORQUE_VALUE_MASK; + current_torque *= MOTOR_TORQUE_RATIO; + if(direction == 0) { + current_torque *= -1; + } + return current_torque; +} + +void servos_collect_current_torque() { + for(int i = 0; i < SERVO_COUNT; i++) { + servos[i].current_torque = servo_calculate_torque_with_direction(servos[i].id); + } +} + +void servos_print_current_torque() { + Serial.print("s_t=["); + Serial.print(servos[SERVO_COUNT - 1].current_torque); + for(int i=SERVO_COUNT - 2; i >= 0; i--) { + Serial.print(","); + Serial.print(servos[i].current_torque); + } + Serial.println("]"); +} diff --git a/servo-control.hpp b/servo-control.hpp new file mode 100644 index 0000000..5d21a70 --- /dev/null +++ b/servo-control.hpp @@ -0,0 +1,105 @@ +/* + * servo-control.hpp + * + * Created on: 12.01.2021 + * Author: frank + */ + +#include + +#ifndef SERVO_CONTROL_HPP_ +#define SERVO_CONTROL_HPP_ + +#define SERVO_COUNT 5 + +#define RPM_TO_SPEED_RATIO 0.111 +#define ANGLE_PER_DIGIT_RATIO .2925 +//#define ANGLE_PER_DIGIT_RATIO 0.29 // From datasheet + +#define MOTOR_TORQUE_LIMIT 75 +#define MOTOR_TORQUE_RATIO 0.1 +#define MOTOR_TORQUE_DIRECTION_MASK 0x00000200 +#define MOTOR_TORQUE_VALUE_MASK 0x000001ff + +#define BASE_MOTOR_INITIAL_POSITION_DEG 150 +#define BASE_MOTOR_INITIAL_SPEED_RPM 5 +#define BASE_MOTOR_MAX_ANGLE_DEG 48 +#define BASE_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT + +#define MID_MOTOR_INITIAL_POSITION_DEG 150 +#define MID_MOTOR_INITIAL_SPEED_RPM 7 +#define MID_MOTOR_MAX_ANGLE_DEG 105 +#define MID_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT + +#define NOSE_MOTOR_INITIAL_POSITION_DEG 150 +#define NOSE_MOTOR_INITIAL_SPEED_RPM 10 +#define NOSE_MOTOR_MAX_ANGLE_DEG 105 +#define NOSE_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT + +#define DXL_BASE_MOTOR_INITIAL_POSITION BASE_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO +#define DXL_BASE_MOTOR_INITIAL_SPEED BASE_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO +#define DXL_BASE_MOTOR_MAX_ANGLE BASE_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO +#define DXL_BASE_MOTOR_MIN_POSITION_CW DXL_BASE_MOTOR_INITIAL_POSITION - DXL_BASE_MOTOR_MAX_ANGLE +#define DXL_BASE_MOTOR_MAX_POSITION_CCW DXL_BASE_MOTOR_INITIAL_POSITION + DXL_BASE_MOTOR_MAX_ANGLE +#define DXL_BASE_MOTOR_TORQUE_LIMIT BASE_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO + +#define DXL_MID_MOTOR_INITIAL_POSITION MID_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO +#define DXL_MID_MOTOR_INITIAL_SPEED MID_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO +#define DXL_MID_MOTOR_MAX_ANGLE MID_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO +#define DXL_MID_MOTOR_MIN_POSITION_CW DXL_MID_MOTOR_INITIAL_POSITION - DXL_MID_MOTOR_MAX_ANGLE +#define DXL_MID_MOTOR_MAX_POSITION_CCW DXL_MID_MOTOR_INITIAL_POSITION + DXL_MID_MOTOR_MAX_ANGLE +#define DXL_MID_MOTOR_TORQUE_LIMIT MID_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO + +#define DXL_NOSE_MOTOR_INITIAL_POSITION NOSE_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO +#define DXL_NOSE_MOTOR_INITIAL_SPEED NOSE_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO +#define DXL_NOSE_MOTOR_MAX_ANGLE NOSE_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO +#define DXL_NOSE_MOTOR_MIN_POSITION_CW DXL_NOSE_MOTOR_INITIAL_POSITION - DXL_NOSE_MOTOR_MAX_ANGLE +#define DXL_NOSE_MOTOR_MAX_POSITION_CCW DXL_NOSE_MOTOR_INITIAL_POSITION + DXL_NOSE_MOTOR_MAX_ANGLE +#define DXL_NOSE_MOTOR_TORQUE_LIMIT NOSE_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO + +struct dxl_servo { + int id; + int initial_speed; + int intial_position; + int min_position; + int max_position; + int torque_limit; + int current_torque; + int joint_orientation; +}; + +void servos_setup(); +void servos_set_current_initial_position(); + +void servos_init_with_fixed_position(); +void servos_init_with_current_position(); + +void servo_init(const int servo_id); +void servos_init(); + +void servo_to_initial_position(const int servo_id); +void servos_to_initial_position(); + +int servo_moving(const int servo_id); +int servos_moving(); + +void servos_in_position_wait(); +void servo_in_position_wait(int servo_id); + +int servo_set_speed_rpm(const int servo_id, const int rpm); + +void servo_move_to_angle(const int servo_id, const int deg); +void servos_move_to_angle(const int deg); + +void servos_collect_current_torque(); +void servos_print_current_torque(); + +void map_roll_to_servos_and_move(const int roll); +void map_pitch_to_servos_and_move(const int pitch); + +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); + + +#endif /* SERVO_CONTROL_HPP_ */