add imu and orientation detection

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;

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);
 

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();
+}

prosthesis_controller/imu.h

@@ -0,0 +1,29 @@
+
+#ifndef IMU_h
+#define IMU_h
+
+#include <MPU6050_tockn.h>
+
+#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

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":"");