From 83c49947350c9f6eec68c61d1165b4120454f851 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Wed, 11 Sep 2024 14:19:10 +0200 Subject: [PATCH] add imu and orientation detection --- prosthesis_controller/behaviour.cpp | 8 +-- prosthesis_controller/behaviour.h | 5 +- prosthesis_controller/imu.cpp | 72 +++++++++++++++++++ prosthesis_controller/imu.h | 29 ++++++++ .../prosthesis_controller.ino | 32 +++++++-- 5 files changed, 134 insertions(+), 12 deletions(-) create mode 100644 prosthesis_controller/imu.cpp create mode 100644 prosthesis_controller/imu.h diff --git a/prosthesis_controller/behaviour.cpp b/prosthesis_controller/behaviour.cpp index f8c55bc..4730949 100644 --- a/prosthesis_controller/behaviour.cpp +++ b/prosthesis_controller/behaviour.cpp @@ -10,7 +10,7 @@ float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, se float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; -void updateMatrixWeights(unsigned long millis, bool body_present,bool contact_main){ +void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll){ mood.loneliness=1.0; static unsigned long last_weights_update=0; @@ -63,12 +63,6 @@ void updateMatrixWeights(unsigned long millis, bool body_present,bool contact_ma map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0 map_mode_scared/=map_sum; - /* - Serial.print(map_mode_noise); Serial.print(","); - Serial.print(map_mode_rollpitch); Serial.print(","); - Serial.print(map_mode_slither); Serial.print(","); - Serial.print(map_mode_sleeping); Serial.println(); - */ //Mode Noise matrix_weights_main[0][W_NOISE]=30.0 *map_mode_noise; diff --git a/prosthesis_controller/behaviour.h b/prosthesis_controller/behaviour.h index 0003abb..6d41831 100644 --- a/prosthesis_controller/behaviour.h +++ b/prosthesis_controller/behaviour.h @@ -36,7 +36,10 @@ struct Mood{ }; extern Mood mood; -void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main); + + + +void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll); void updateServosByWeights(unsigned long millis); float mapfloat(float x, float in_min, float in_max, float out_min, float out_max); diff --git a/prosthesis_controller/imu.cpp b/prosthesis_controller/imu.cpp new file mode 100644 index 0000000..9af8038 --- /dev/null +++ b/prosthesis_controller/imu.cpp @@ -0,0 +1,72 @@ +#include "Arduino.h" +#include "imu.h" + + +MPU6050 mpu6050(Wire,IMU_AC,IMU_GC); + + +bool imu_bellyup=false; + +void initIMU() { + initIMU(0,0,0); +} + +void initIMU(float x,float y,float z) { + Wire.begin(25,24); + mpu6050.begin(); + + if (x==0 && y==0 && z==0) { + mpu6050.calcGyroOffsets(true); + }else{ + mpu6050.setGyroOffsets(x, y, z); + } +} + +void loopIMU(unsigned long millis) { + static unsigned long last_update_imu=0; + if (millis-last_update_imu>UPDATE_IMU_INTERVAL) { + last_update_imu=millis; + mpu6050.update(); + /*Serial.print("angleX : "); + Serial.print(mpu6050.getAngleX()); + Serial.print("\tangleY : "); + Serial.print(mpu6050.getAngleY()); + Serial.print("\tangleZ : "); + Serial.println(mpu6050.getAngleZ());*/ + + if(mpu6050.getAccZ()<-0.5) { //is upside down + imu_bellyup=true; + }else{ + if (mpu6050.getAccZ()>0.7) { //is on flat surface + imu_bellyup=false; + } + } + + } +} + +bool isBellyUp() { + return imu_bellyup; +} + +float getAngleX() { + return mpu6050.getAngleX(); +} + +float getAngleY() { + return mpu6050.getAngleY(); +} + +float getAngleZ() { + return mpu6050.getAngleZ(); +} + +float getAccX() { + return mpu6050.getAccX(); +} +float getAccY() { + return mpu6050.getAccY(); +} +float getAccZ() { + return mpu6050.getAccZ(); +} diff --git a/prosthesis_controller/imu.h b/prosthesis_controller/imu.h new file mode 100644 index 0000000..278961f --- /dev/null +++ b/prosthesis_controller/imu.h @@ -0,0 +1,29 @@ + +#ifndef IMU_h +#define IMU_h + +#include + +#define UPDATE_IMU_INTERVAL 1000/50 + +#define IMU_AC 0.1f //default 0.02f +#define IMU_GC 0.90f //default 0.98f + + + +void initIMU(); +void initIMU(float x,float y,float z); +void loopIMU(unsigned long millis); +bool isBellyUp(); + +float getAngleX(); +float getAngleY(); +float getAngleZ(); + +float getAccX(); +float getAccY(); +float getAccZ(); + + + +#endif \ No newline at end of file diff --git a/prosthesis_controller/prosthesis_controller.ino b/prosthesis_controller/prosthesis_controller.ino index ad663b3..3b48927 100644 --- a/prosthesis_controller/prosthesis_controller.ino +++ b/prosthesis_controller/prosthesis_controller.ino @@ -3,6 +3,7 @@ #include "servo.h" #include "bodytemp.h" #include "behaviour.h" +#include "imu.h" void setup() { Serial.begin(115200); @@ -17,6 +18,8 @@ void setup() { initVacuum(); initBodytemp(); + initIMU(7.54,-15.26,-1.41); + pinMode(PIN_INPUTARM1,INPUT); pinMode(PIN_INPUTARM2,INPUT); @@ -39,6 +42,12 @@ void loop() { loopVacuum(loopmillis); + + + loopIMU(loopmillis); + + bool bellyup=isBellyUp(); + bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed if (buttonstate) { //Button pressed @@ -48,11 +57,16 @@ void loop() { } bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm + bool contact_peripheralL=digitalRead(PIN_INPUTARM2); //Sensor on tip of arm + bool contact_peripheralR=digitalRead(PIN_INPUTARM3); //Sensor on tip of arm + + float imu_pitch=0; + float imu_roll=0; - updateMatrixWeights(loopmillis,body_present,contact_main); + updateMatrixWeights(loopmillis,body_present,contact_main,contact_peripheralL,contact_peripheralR,imu_pitch,imu_roll); updateServosByWeights(loopmillis); - loopServos(loopmillis); + //loopServos(loopmillis); @@ -65,9 +79,19 @@ void loop() { //printVacuumValues(); //Serial.print(" Vac="); Serial.print(vac); - printServoDebug(); - Serial.println(); + //printServoDebug(); + //Serial.println(); //printBodytempDebug(); + + + Serial.print("angleX : "); + Serial.print(getAccX()); + Serial.print("\tangleY : "); + Serial.print(getAccY()); + Serial.print("\tangleZ : "); + Serial.print(getAccZ()); + + Serial.print("\t "); Serial.print(bellyup?"Belly Up":"");