From dd7e136da725d31f73d1893d427fb5cc6875a04b Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Mon, 7 Nov 2022 11:07:34 +0100 Subject: [PATCH] inital project, add booting serial prints --- .cproject | 16 +++ .project | 20 ++++ imu.cpp | 172 +++++++++++++++++++++++++++++++ imu.hpp | 48 +++++++++ movement.cpp | 25 +++++ movement.hpp | 22 ++++ mpu6050.cpp | 101 +++++++++++++++++++ mpu6050.hpp | 57 +++++++++++ protesis-dxl.cpp.orig | 201 +++++++++++++++++++++++++++++++++++++ protesis_03_dxl.ino | 228 ++++++++++++++++++++++++++++++++++++++++++ servo-control.cpp | 206 ++++++++++++++++++++++++++++++++++++++ servo-control.hpp | 105 +++++++++++++++++++ 12 files changed, 1201 insertions(+) create mode 100644 .cproject create mode 100644 .project create mode 100644 imu.cpp create mode 100644 imu.hpp create mode 100644 movement.cpp create mode 100644 movement.hpp create mode 100644 mpu6050.cpp create mode 100644 mpu6050.hpp create mode 100644 protesis-dxl.cpp.orig create mode 100644 protesis_03_dxl.ino create mode 100644 servo-control.cpp create mode 100644 servo-control.hpp 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_ */