add more behaviour

prosthesis_controller/behaviour.cpp

@@ -6,17 +6,128 @@ SimplexNoise sn;
 
 Mood mood;
 
+float map_mode_rollpitch=0;
+float map_mode_walking=0;
+float map_mode_sleeping=0;
+float map_mode_noise=0;
+
 float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods
 float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
 float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
 
+void initBehaviour() {
+  mood.arousal=0;
+  mood.valence=0;
+}
 
-void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR,float height,float temp_ambient,float temp_object,float pumpspeed, float servoerror) {
+
+void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,float maxAccX,float maxAccY,float maxAccZ,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR,float height,float temp_ambient,float temp_object,float pumpspeed, float servoerror) {
   static unsigned long last_mood_update=0;
   if (millis-last_mood_update>MOOD_UPDATE_INTERVAL) {
     last_mood_update=millis;
-    mood.arousal=0;
+    
-    mood.valence=0;
+
+    float step=MOOD_UPDATE_INTERVAL/1000.0;
+
+    if (body_present && pumpspeed<0.8) { //body present and vacuum present (pump below speed 1.0)
+      if (mood.valence<0.0) { //when attached, and valence low
+        mood.valence=0.2; //jump valence up quickly
+      }
+      if (mood.valence<0.5) { //increase valence up to a certain point
+        mood.valence+=step*0.05;
+      }
+    }
+    if (!body_present) {
+      if (mood.valence>-0.5) { //decrease valence down to a certain point
+        mood.valence-=step*0.01;
+      }
+    }
+
+    static unsigned long last_contact_time=0;
+    static bool last_contact_main=false;
+    static bool last_contact_peripheralR=false;
+    static bool last_contact_peripheralL=false;
+    
+
+    if (contact_main || contact_peripheralL || contact_peripheralR) {
+      if ((contact_main && !last_contact_main) ||  (contact_peripheralL && !last_contact_peripheralL) || (contact_peripheralR && !last_contact_peripheralR)) { //on first contact
+
+        long _contacttime_min=1000;
+        long _contacttime_max=20000;
+        float _contact_increase_arousal=0.7;
+        float _contact_increase_valence=0.1;
+        if (mood.valence<-0.2) {
+          long _contacttime_min=500;
+          long _contacttime_max=2000;
+          float _contact_increase_arousal=0.7;
+          float _contact_increase_valence=-0.2;
+        }
+        float olda=mood.arousal;
+        float oldv=mood.valence;
+        mood.arousal+=constrain(mapfloat(millis-last_contact_time,_contacttime_min,_contacttime_max,0,_contact_increase_arousal),0,_contact_increase_arousal);
+        mood.valence+=constrain(mapfloat(millis-last_contact_time,_contacttime_min,_contacttime_max,0,_contact_increase_valence),0,_contact_increase_valence);
+        Serial.print("Contact. increase arousal:"); Serial.print(mood.arousal-olda); Serial.print(" valence:"); Serial.print(mood.valence-oldv); Serial.println();
+      }
+      last_contact_time=millis;
+    }
+
+    last_contact_main=contact_main;
+    last_contact_peripheralR=contact_peripheralR;
+    last_contact_peripheralL=contact_peripheralL;
+
+
+    static float last_height=0;
+    static float heightdiffsmooth=0; //goes up to 0.2 when lifted or lowered quickly 1m and slowly deacreases to 0
+    heightdiffsmooth+=height-last_height;
+    if (heightdiffsmooth>0) {
+      heightdiffsmooth-=behaviour_confortable_lift*step;
+    }else if (heightdiffsmooth<0) {
+      heightdiffsmooth+=behaviour_confortable_lift*step; //0.1*step means 0.1m of heightchange per second does not affect heightdiffsmooth
+    }
+    
+    //Serial.print("heightdiffsmooth:"); Serial.println(heightdiffsmooth,3);
+
+    
+    float maxAcc=sqrt(pow(maxAccX,2)+pow(maxAccY,2)+pow(maxAccZ,2));
+    float preferredMaxAcc=0.5;
+    if (constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)>0.0) {
+      digitalWrite(PIN_LED,LOW); //On
+    }else{
+      digitalWrite(PIN_LED,HIGH); //Off
+    }
+    if (mood.arousal<-0.5) {
+      mood.arousal+=constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)*step*0.2;
+    }else{
+      mood.valence+=constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)*step*0.2;
+      if (mood.arousal>0.7) {
+        mood.arousal-=step*0.04;
+      }else if(mood.arousal<0.3) {
+        mood.arousal+=step*0.03;
+      }
+    }
+
+    if (abs(heightdiffsmooth)>0.05) {
+      mood.valence-=abs(heightdiffsmooth)*step*1.0;
+      if (mood.arousal<0.9){
+        mood.arousal+=abs(heightdiffsmooth)*step*3.0;
+      }
+    }
+
+    last_height=height;
+
+
+    mood.arousal-=step*0.02; //slowly deacrease arousal
+
+    if (mood.valence<0) {
+      mood.valence+=step*0.03;
+    }else if (mood.valence>0) {
+      mood.valence-=step*0.01;
+    }
+    
+
+    mood.arousal=constrain(mood.arousal,-1.0,1.0);
+    mood.valence=constrain(mood.valence,-1.0,1.0);
+
   }
 }
 
@@ -30,10 +141,7 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
 
     
     //### Update Mode ###
-    static float map_mode_rollpitch=0;
+    
-    static float map_mode_walking=0;
-    static float map_mode_sleeping=0;
-    static float map_mode_noise=0;
 
     float aim_map_mode_rollpitch=0;
     float aim_map_mode_walking=0;
@@ -43,13 +151,18 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
     
 
     if (body_present) { //attached    
-      aim_map_mode_rollpitch=1.0;
+      
+      aim_map_mode_rollpitch=constrain(mapfloat(mood.valence,-0.3,0.3,0,1.0),0,1.0);
+      
+      aim_map_mode_noise=constrain(mapfloat(mood.valence,-0.2,-1.0,0,1.0),0,1.0);
+      
 
     }else{ //detached
       if (bellyup) { //belly up
         aim_map_mode_walking=1.0;
       }else{ //sitting on a level survace
-        aim_map_mode_noise=1.0;
+        aim_map_mode_noise=constrain(mapfloat(mood.arousal,-0.5,0.5, 0.0,1.0),0,1.0);
+        map_mode_sleeping=constrain(mapfloat(mood.arousal,-1.0,-0.6, 1.0,0.0),0,1.0);
       }
     }
     
@@ -59,13 +172,9 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
     map_mode_noise=interpolateTo(map_mode_noise,aim_map_mode_noise,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
 
 
-    /*
+    
-    Serial.print("Walking: \t"); Serial.println(map_mode_walking);
+    
-    Serial.print("RollPitch: \t"); Serial.println(map_mode_rollpitch);
+    
-    Serial.print("Sleeping: \t"); Serial.println(map_mode_sleeping);
-    Serial.print("Noise: \t"); Serial.println(map_mode_noise);
-    Serial.println();
-    */
 
     float map_sum=map_mode_walking+map_mode_rollpitch+map_mode_sleeping+map_mode_noise;
     map_mode_walking/=map_sum;
@@ -73,8 +182,7 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
     map_mode_sleeping/=map_sum;
     map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
 
-
+    analogWrite(PIN_VIBRATION,contact_main?64:0);
-    analogWrite(PIN_VIBRATION,contact_main*map_mode_noise*64); //TODO, remove
     
 
     //reset all weights
@@ -124,6 +232,13 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
   }
 }
 
+void printModes() {
+  Serial.print("Walking: \t"); Serial.println(map_mode_walking);
+  Serial.print("RollPitch: \t"); Serial.println(map_mode_rollpitch);
+  Serial.print("Sleeping: \t"); Serial.println(map_mode_sleeping);
+  Serial.print("Noise: \t"); Serial.println(map_mode_noise);
+  Serial.println();
+}
 
 void updateServosByWeights(unsigned long millis,float pitch,float roll){
   static unsigned long last_servoweights_update=0;
@@ -216,3 +331,7 @@ float mapfloat(float x, float in_min, float in_max, float out_min, float out_max
   return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
 }
 
+void printMood(){
+  Serial.print("A:"); Serial.print(mood.arousal); Serial.print(" V:"); Serial.print(mood.valence);
+  Serial.println();
+}

prosthesis_controller/behaviour.h

@@ -38,12 +38,16 @@ struct Mood{
 };
 extern Mood mood;
 
+const float behaviour_confortable_lift=0.1; // Meters per seond. heightchange below does not affect mood
 
 
-void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR,float height,float temp_ambient,float temp_object,float pumpspeed, float servoerror);
+void initBehaviour();
+void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,float maxAccX,float maxAccY,float maxAccZ,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR,float height,float temp_ambient,float temp_object,float pumpspeed, float servoerror);
 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 interpolateTo(float value,float aimvalue,float timestep,float speed);
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
+void printMood();
+void printModes();
 
 #endif

prosthesis_controller/imu.cpp

@@ -4,6 +4,10 @@
 
 MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);
 
+float maxAccX;
+float maxAccY;
+float maxAccZ;
+
 
 bool imu_bellyup=false;
 
@@ -34,6 +38,23 @@ void loopIMU(unsigned long millis) {
     Serial.print("\tangleZ : ");
     Serial.println(mpu6050.getAngleZ());*/
 
+    
+    maxAccX-=UPDATE_IMU_INTERVAL/1000.0*1.0; //slowly reduce values
+    maxAccY-=UPDATE_IMU_INTERVAL/1000.0*1.0;
+    maxAccZ-=UPDATE_IMU_INTERVAL/1000.0*1.0;
+    static float last_accX;
+    static float last_accY;
+    static float last_accZ;
+    float _accX=getAccX();
+    float _accY=getAccY();
+    float _accZ=getAccZ();
+    maxAccX=max(abs(last_accX-_accX), maxAccX);
+    maxAccY=max(abs(last_accY-_accY), maxAccY);
+    maxAccZ=max(abs(last_accZ-_accZ), maxAccZ);
+    last_accX=_accX;
+    last_accY=_accY;
+    last_accZ=_accZ;
+
     if(mpu6050.getAccZ()<-0.5) { //is upside down
       imu_bellyup=true;
     }else{
@@ -71,6 +92,22 @@ float getAccZ() {
   return mpu6050.getAccZ();
 }
 
+
+float getMaxAccX() {
+  return maxAccX;
+}
+float getMaxAccY() {
+  return maxAccY;
+}
+float getMaxAccZ() {
+  return maxAccZ;  
+}
+void resetMaxAcc(){
+  maxAccX=0;
+  maxAccY=0;
+  maxAccZ=0;
+}
+
 float getGyroAngleX() {
   return mpu6050.getGyroAngleX();
 }

prosthesis_controller/imu.h

@@ -24,6 +24,11 @@ float getAccX();
 float getAccY();
 float getAccZ();
 
+float getMaxAccX();
+float getMaxAccY();
+float getMaxAccZ();
+void resetMaxAcc();
+
 float getGyroAngleX();
 float getGyroAngleY();
 float getGyroAngleZ();

prosthesis_controller/prosthesis_controller.ino

@@ -16,6 +16,8 @@ void setup() {
   pinMode(PIN_LED,OUTPUT);
   digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
 
+  initBehaviour();
+
   initVacuum();
   initBodytemp();
 
@@ -24,9 +26,9 @@ void setup() {
   initIMU(7.54,-15.26,-1.41);
 
 
-  pinMode(PIN_INPUTARM1,INPUT);
+  pinMode(PIN_INPUTARM1,INPUT_PULLDOWN);
-  pinMode(PIN_INPUTARM2,INPUT);
+  pinMode(PIN_INPUTARM2,INPUT_PULLDOWN);
-  pinMode(PIN_INPUTARM3,INPUT);
+  pinMode(PIN_INPUTARM3,INPUT_PULLDOWN);
 
   pinMode(PIN_INPUTAUX,INPUT);
 
@@ -41,7 +43,7 @@ void loop() {
  
   
   bool body_present=checkBodypresence(loopmillis);
-  digitalWrite(PIN_LED,!body_present);
+  //digitalWrite(PIN_LED,!body_present);
 
   loopVacuum(loopmillis);
 
@@ -66,9 +68,11 @@ void loop() {
   float pitch=-getAngleY();
   float roll=-getAngleX();
 
+  float maxAccX=getMaxAccX();
+  float maxAccY=getMaxAccY();
+  float maxAccZ=getMaxAccZ();
 
-
+  updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),maxAccX,maxAccY,maxAccZ,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror());
-  updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror());
   updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
   updateServosByWeights(loopmillis,pitch,roll);
 
@@ -80,7 +84,7 @@ void loop() {
 
   //Print Debug Information
   static unsigned long last_print=0;
-  if (loopmillis - last_print >100) {
+  if (loopmillis - last_print >500) {
     last_print=loopmillis;
     //printVacuumValues();
     //Serial.print(" Vac="); Serial.print(vac);
@@ -92,10 +96,16 @@ void loop() {
     
     Serial.print("Servo Error:");Serial.print(getAverageServoPositionerror()); Serial.println();
     Serial.print("Avg Pump:");Serial.print(getAveragePumpspeed()); Serial.println();
-    Serial.print("Height:");Serial.print(getBarometerHeight()); Serial.println();
+    Serial.print("Height:");Serial.print(getBarometerHeight(),3); Serial.println();
+    Serial.print("Contact:");Serial.print(contact_main); Serial.print(", "); Serial.print(contact_peripheralL); Serial.print(", "); Serial.print(contact_peripheralR);Serial.println();
     
-    Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
     
+    //Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
+    float maxAcc=sqrt(pow(maxAccX,2)+pow(maxAccY,2)+pow(maxAccZ,2));
+    Serial.print("Accel: ");Serial.print(maxAccX); Serial.print(", ");Serial.print(maxAccY); Serial.print(", ");Serial.print(maxAccZ); Serial.print(", max: ");Serial.print(maxAcc); Serial.println();
+    printMood();
+    
+    printModes();
     
     
     Serial.println();

prosthesis_controller/servo.cpp

@@ -15,6 +15,7 @@ void initServos()
 {
   //Main Arm first Servo. Model 1020
   servos[0].id=11;
+  servos[0].velocity=50;
   servos[0].mode=OP_CURRENT_BASED_POSITION;
   servos[0].current=10.0;
   servos[0].offset_angle=180;
@@ -25,6 +26,7 @@ void initServos()
 
   //Main Arm second Servo. Model 1080
   servos[1].id=12;
+  servos[1].velocity=75;
   servos[1].mode=OP_POSITION;
   servos[1].offset_angle=180;
   servos[1].min_angle=180-90;
@@ -34,6 +36,7 @@ void initServos()
 
   //Main Arm third Servo. Model 1080
   servos[2].id=13;
+  servos[2].velocity=100;
   servos[2].mode=OP_POSITION;
   servos[2].offset_angle=180;
   servos[2].min_angle=180-90;
@@ -83,7 +86,7 @@ void initServos()
         dxl.setGoalCurrent(servos[i].id, servos[i].current, UNIT_PERCENT);
       }
 
-      dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, 30); //was 30
+      dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, servos[i].velocity); //was 30
       dxl.writeControlTableItem(ControlTableItem::PROFILE_ACCELERATION, servos[i].id, 10);
       
       dxl.torqueOn(servos[i].id);
@@ -139,9 +142,9 @@ float getAverageServoPositionerror()
   float angleerror=0;
   for (uint8_t i=0;i<SERVO_COUNT;i++) {
     float current_error=servos[i].angle+servos[i].offset_angle - servos[i].actual_angle;
-    if (current_error<0){
+    
-      current_error*=-1;
+    current_error=abs(current_error);
-    }
+    
     angleerror+=current_error;
   }
   angleerror/=SERVO_COUNT;

prosthesis_controller/servo.h

@@ -26,8 +26,9 @@
 
 
 struct dxl_servo {
-	int id;
+	uint8_t id;
 	//int initial_speed;
+  int32_t velocity;
 	float offset_angle; //constant offset. set this to match angle=0 to a straight arm
   float angle; //commanded angle
   float actual_angle; //angle read from servo