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

inital project, add booting serial prints

Browse source
This commit is contained in:
Philipp Kramer 2022-11-07 11:07:34 +01:00
commit dd7e136da7
12 changed files with 1201 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
.cproject Normal file
View file

@ -0,0 +1,16 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="org.eclipse.cdt.core.default.config.129737279">
<storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.129737279" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
<externalSettings/>
<extensions/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.pathentry">
<pathentry kind="src" path=""/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
</cproject>

20
.project Normal file
View file

@ -0,0 +1,20 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>protesis-dxl</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.core.cBuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>org.eclipse.cdt.arduino.core.arduinoNature</nature>
</natures>
</projectDescription>

172
imu.cpp Normal file
View file

@ -0,0 +1,172 @@
/*
* imu.cpp
*
* Created on: 09.01.2021
* Author: frank
*/
#include <Arduino.h>
#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;
}

48
imu.hpp Normal file
View file

@ -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_ */

25
movement.cpp Normal file
View file

@ -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++;
}

22
movement.hpp Normal file
View file

@ -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_ */

101
mpu6050.cpp Normal file
View file

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

57
mpu6050.hpp Normal file
View file

@ -0,0 +1,57 @@
/*
* mpu6050.h
*
* Created on: 08.01.2021
* Author: frank
*/
#ifndef MPU6050_HPP_
#define MPU6050_HPP_
#include <Arduino.h>
#include <Wire.h>
//#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_ */

201
protesis-dxl.cpp.orig Normal file
View file

@ -0,0 +1,201 @@
#include <Arduino.h>
#include <ax12.h>
#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();
}

228
protesis_03_dxl.ino Normal file
View file

@ -0,0 +1,228 @@
#include <ax12.h>
#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();
}

206
servo-control.cpp Normal file
View file

@ -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("]");
}

105
servo-control.hpp Normal file
View file

@ -0,0 +1,105 @@
/*
* servo-control.hpp
*
* Created on: 12.01.2021
* Author: frank
*/
#include <ax12.h>
#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_ */