From 04465fc8671176c80925f65e8aeeb0ff49d81749 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Wed, 11 Sep 2024 17:15:34 +0200
Subject: [PATCH] change modes, add vibration on contact

---
 prosthesis_controller/behaviour.cpp           | 127 +++++++++++-------
 prosthesis_controller/behaviour.h             |   7 +-
 prosthesis_controller/definitions.h           |   2 +-
 prosthesis_controller/imu.h                   |   2 +-
 .../prosthesis_controller.ino                 |  20 +--
 5 files changed, 87 insertions(+), 71 deletions(-)

diff --git a/prosthesis_controller/behaviour.cpp b/prosthesis_controller/behaviour.cpp
index 4730949..2b89823 100644
--- a/prosthesis_controller/behaviour.cpp
+++ b/prosthesis_controller/behaviour.cpp
@@ -10,28 +10,13 @@ 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, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll){
-  mood.loneliness=1.0;
+void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll){
+  
   
   static unsigned long last_weights_update=0;
   if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
     last_weights_update=millis;
 
-    static float contact_main_persist=0;
-    if (contact_main) {
-      contact_main_persist=1.0;
-    }else{
-      if (contact_main_persist>0) {
-        contact_main_persist-=0.2*WEIGHT_UPDATE_INTERVAL/1000.0;
-      }else{
-        contact_main_persist=0;
-      }
-    }
-    
-    //0=pitch
-    //1=roll
-    
-
     //float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
     //float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
 
@@ -40,59 +25,95 @@ void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_ma
     float map_mode_slither=0;
     float map_mode_sleeping=mood.loneliness;*/
 
+    
+    static float map_mode_rollpitch=0;
+    static float map_mode_walking=0;
+    static float map_mode_sleeping=0;
+    static float map_mode_noise=0;
 
-    bool state_bellyup=false;
-    bool state_map_mode_detached=false;
-    bool state_map_mode_attached=false;
+    
 
-    float map_mode_rollpitch=0;
-    float map_mode_slither=0;
-    float map_mode_sleeping=0;
+    if (body_present) {      
+      map_mode_rollpitch+=0.3 *WEIGHT_UPDATE_INTERVAL/1000.0;
+      map_mode_walking-=0.5 *WEIGHT_UPDATE_INTERVAL/1000.0;
+      map_mode_noise-=0.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
 
-
-    float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0);
-    float map_mode_noise=1.0-contact_main_smooth; //idle movement
-    float map_mode_scared=0.0+contact_main_smooth; //contracted position
+    }else{        
+      map_mode_rollpitch-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
+      if (bellyup) {
+        map_mode_walking+=0.3 *WEIGHT_UPDATE_INTERVAL/1000.0;
+        map_mode_noise-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
+      }else{ //sitting on a level survace
+        map_mode_walking-=0.1 *WEIGHT_UPDATE_INTERVAL/1000.0;
+        map_mode_noise+=0.3*WEIGHT_UPDATE_INTERVAL/1000.0;
+      }
+    }
 
 
     
+    map_mode_walking=constrain(map_mode_walking,0.0,1.0);
+    map_mode_rollpitch=constrain(map_mode_rollpitch,0.0,1.0);
+    map_mode_noise=constrain(map_mode_noise,0.0,1.0);
+
     
+    Serial.print("Walking: \t"); Serial.println(map_mode_walking);
+    Serial.print("RollPitch: \t"); Serial.println(map_mode_rollpitch);
+    Serial.print("Noise: \t"); Serial.println(map_mode_noise);
+    Serial.println();
 
 
-    float map_sum=map_mode_noise+map_mode_scared;
+    float map_sum=map_mode_walking+map_mode_rollpitch+map_mode_noise;
+    map_mode_walking/=map_sum;
+    map_mode_rollpitch/=map_sum;
     map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
-    map_mode_scared/=map_sum;
 
+
+    analogWrite(PIN_VIBRATION,contact_main*map_mode_noise*64);
+    Serial.print("Vibrt:"); Serial.println(contact_main*map_mode_noise*160);
+
+    //reset all weights
+    for (uint8_t i=0;i<SERVO_COUNT_MAIN;i++) {
+      for (uint8_t w=0;w<WEIGHT_COUNT;w++) {
+        matrix_weights_main[i][w]=0;
+      }
+    }
+    for (uint8_t i=0;i<SERVO_COUNT_PERIPHERAL;i++) {
+      for (uint8_t w=0;w<WEIGHT_COUNT;w++) {
+        matrix_weights_peripheralL[i][w]=0;
+        matrix_weights_peripheralR[i][w]=0;
+      }
+    }
     
     //Mode Noise
-    matrix_weights_main[0][W_NOISE]=30.0 *map_mode_noise;
-    matrix_weights_main[0][W_NOISESLOW]=0.0 *map_mode_noise;
+    matrix_weights_main[0][W_NOISE]+=30.0 *map_mode_noise;
+    matrix_weights_main[0][W_NOISESLOW]+=0.0 *map_mode_noise;
     
-    matrix_weights_main[1][W_NOISE]=30.0 *map_mode_noise;
-    matrix_weights_main[1][W_NOISESLOW]=20.0 *map_mode_noise;
+    matrix_weights_main[1][W_NOISE]+=30.0 *map_mode_noise;
+    matrix_weights_main[1][W_NOISESLOW]+=20.0 *map_mode_noise;
     
-    matrix_weights_main[2][W_NOISE]=20.0 *map_mode_noise;
-    matrix_weights_main[2][W_NOISESLOW]=30.0 *map_mode_noise;
+    matrix_weights_main[2][W_NOISE]+=20.0 *map_mode_noise;
+    matrix_weights_main[2][W_NOISESLOW]+=30.0 *map_mode_noise;
     
-    matrix_weights_main[0][W_LOOKDIRECTION]=30* map_mode_scared; 
-    matrix_weights_main[1][W_LOOKDIRECTION]=30.0* map_mode_scared;  
-    matrix_weights_main[2][W_LOOKDIRECTION]=30.0* map_mode_scared; 
 
     
     //Mode Roll Pitch
-    matrix_weights_main[0][W_ROLL]=1.0* map_mode_rollpitch; 
-    matrix_weights_main[1][W_PITCH]=1.0* map_mode_rollpitch;  
-    matrix_weights_main[2][W_ROLL]=1.0* map_mode_rollpitch; 
+    matrix_weights_main[0][W_PITCH]+=1.0* map_mode_rollpitch; 
+    matrix_weights_main[1][W_ROLL]+=1.0* map_mode_rollpitch;  
+    matrix_weights_main[2][W_PITCH]+=1.0* map_mode_rollpitch; 
 
-    //Mode Slither
-    matrix_weights_main[0][W_COS]=30 *map_mode_slither;
-    matrix_weights_main[1][W_SIN]=30 *map_mode_slither;
-    matrix_weights_main[2][W_COS]=-30 *map_mode_slither;
+    //Mode Walking
+    matrix_weights_main[0][W_NOISESLOW]+=5 *map_mode_walking;
+    matrix_weights_main[0][W_SIN]+=10 *map_mode_walking;
+    matrix_weights_main[0][W_OFFSET]+=-60 *map_mode_walking;
+    matrix_weights_main[1][W_NOISESLOW]+=30 *map_mode_walking;
+    matrix_weights_main[2][W_COS]+=-20 *map_mode_walking;
+    matrix_weights_main[2][W_OFFSET]+=70 *map_mode_walking;
+    
     
     //Mode Sleeping
-    matrix_weights_main[0][W_NOISESLOW]=20 *map_mode_sleeping;
-    matrix_weights_main[1][W_NOISESLOW]=20 *map_mode_sleeping;
-    matrix_weights_main[2][W_NOISESLOW]=30 *map_mode_sleeping;
+    matrix_weights_main[0][W_NOISESLOW]+=20 *map_mode_sleeping;
+    matrix_weights_main[1][W_NOISESLOW]+=20 *map_mode_sleeping;
+    matrix_weights_main[2][W_NOISESLOW]+=30 *map_mode_sleeping;
 
     /*
     float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 0,1),0,1);
@@ -107,7 +128,7 @@ void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_ma
 }
 
 
-void updateServosByWeights(unsigned long millis){
+void updateServosByWeights(unsigned long millis,float pitch,float roll){
   static unsigned long last_servoweights_update=0;
   if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL) 
   {
@@ -116,8 +137,6 @@ void updateServosByWeights(unsigned long millis){
     #define MILLISADD_MAX 100
     millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
 
-    float pitch=0;
-    float roll=0;
 
     float vsin=sin((millis()+millis_add)/1000.0);
     float vcos=cos((millis()+millis_add)/1000.0);
@@ -128,6 +147,9 @@ void updateServosByWeights(unsigned long millis){
       
       float pnoise=sn.noise((millis()+millis_add)/5000.0,i*10); //simplexnoise -1 to 1
       float pnoiseslow=sn.noise((millis()+millis_add)/50000.0,i*10); //simplexnoise -1 to 1
+
+      pitch=constrain(pitch,-80,80);
+      roll=constrain(roll,-80,80);
       
 
       float angle=0;
@@ -155,7 +177,8 @@ void updateServosByWeights(unsigned long millis){
           +pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW]
           +vsin*matrix_weights_main[servoPosInMatrix][W_SIN]
           +vcos*matrix_weights_main[servoPosInMatrix][W_COS]
-          +lookdirection*matrix_weights_main[servoPosInMatrix][W_LOOKDIRECTION];
+          +lookdirection*matrix_weights_main[servoPosInMatrix][W_LOOKDIRECTION]
+          +1.0*matrix_weights_main[servoPosInMatrix][W_OFFSET];
         
       }else if(i<SERVO_COUNT_MAIN+SERVO_COUNT_PERIPHERAL) { //Peripheral Left
         servoPosInMatrix=i-3;
diff --git a/prosthesis_controller/behaviour.h b/prosthesis_controller/behaviour.h
index 6d41831..4506073 100644
--- a/prosthesis_controller/behaviour.h
+++ b/prosthesis_controller/behaviour.h
@@ -13,7 +13,7 @@ extern dxl_servo servos[SERVO_COUNT];
 #define SERVO_COUNT_MAIN 3
 #define SERVO_COUNT_PERIPHERAL 3
 
-#define WEIGHT_COUNT 7 //enter number of weights here for array size
+#define WEIGHT_COUNT 8 //enter number of weights here for array size
 
 
 #define WEIGHT_UPDATE_INTERVAL 100
@@ -27,6 +27,7 @@ extern dxl_servo servos[SERVO_COUNT];
 #define W_SIN 4
 #define W_COS 5
 #define W_LOOKDIRECTION 6
+#define W_OFFSET 7
 
 
 struct Mood{
@@ -39,8 +40,8 @@ extern Mood mood;
 
 
 
-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);
+void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll);
+void updateServosByWeights(unsigned long millis,float pitch,float roll);
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
 
 #endif
\ No newline at end of file
diff --git a/prosthesis_controller/definitions.h b/prosthesis_controller/definitions.h
index 3e44085..09e1e96 100644
--- a/prosthesis_controller/definitions.h
+++ b/prosthesis_controller/definitions.h
@@ -30,7 +30,7 @@
 #define PIN_VALVE1 PIN_OUT1
 #define PIN_VALVE2 PIN_OUT3
 
-#define PIN_VIBRATION PIN_OUT2
+#define PIN_VIBRATION PIN_OUT6
 
 #define PIN_SDA 25
 #define PIN_SCL 24
diff --git a/prosthesis_controller/imu.h b/prosthesis_controller/imu.h
index 278961f..5ca4f3e 100644
--- a/prosthesis_controller/imu.h
+++ b/prosthesis_controller/imu.h
@@ -7,7 +7,7 @@
 #define UPDATE_IMU_INTERVAL 1000/50
 
 #define IMU_AC 0.1f //default 0.02f
-#define IMU_GC 0.90f //default 0.98f
+#define IMU_GC 0.90f //default 0.98f. //AC + GC must equal 1.0
 
 
 
diff --git a/prosthesis_controller/prosthesis_controller.ino b/prosthesis_controller/prosthesis_controller.ino
index 3b48927..7263068 100644
--- a/prosthesis_controller/prosthesis_controller.ino
+++ b/prosthesis_controller/prosthesis_controller.ino
@@ -60,13 +60,13 @@ void loop() {
   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;
+  float pitch=-getAngleY();
+  float roll=-getAngleX();
   
-  updateMatrixWeights(loopmillis,body_present,contact_main,contact_peripheralL,contact_peripheralR,imu_pitch,imu_roll);
-  updateServosByWeights(loopmillis);
+  updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
+  updateServosByWeights(loopmillis,pitch,roll);
 
-  //loopServos(loopmillis);
+  loopServos(loopmillis);
 
 
 
@@ -83,19 +83,11 @@ void loop() {
     //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":"");
     
     
     
-    Serial.println();
+    //Serial.println();
   }
 
 }