diff --git a/prosthesis_controller/behaviour.cpp b/prosthesis_controller/behaviour.cpp index bbf3340..e067f5b 100644 --- a/prosthesis_controller/behaviour.cpp +++ b/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; @@ -42,14 +150,19 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b - if (body_present) { //attached - aim_map_mode_rollpitch=1.0; + if (body_present) { //attached + + 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*map_mode_noise*64); //TODO, remove + analogWrite(PIN_VIBRATION,contact_main?64:0); //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(); +} diff --git a/prosthesis_controller/behaviour.h b/prosthesis_controller/behaviour.h index 0072299..adb7b35 100644 --- a/prosthesis_controller/behaviour.h +++ b/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 \ No newline at end of file diff --git a/prosthesis_controller/imu.cpp b/prosthesis_controller/imu.cpp index e29d502..6859ca0 100644 --- a/prosthesis_controller/imu.cpp +++ b/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(); } diff --git a/prosthesis_controller/imu.h b/prosthesis_controller/imu.h index 388ef1f..94aae01 100644 --- a/prosthesis_controller/imu.h +++ b/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(); diff --git a/prosthesis_controller/prosthesis_controller.ino b/prosthesis_controller/prosthesis_controller.ino index ba6c3e0..1301674 100644 --- a/prosthesis_controller/prosthesis_controller.ino +++ b/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_INPUTARM2,INPUT); - pinMode(PIN_INPUTARM3,INPUT); + pinMode(PIN_INPUTARM1,INPUT_PULLDOWN); + pinMode(PIN_INPUTARM2,INPUT_PULLDOWN); + 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(),bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror()); + updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),maxAccX,maxAccY,maxAccZ,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(); diff --git a/prosthesis_controller/servo.cpp b/prosthesis_controller/servo.cpp index 6dac099..3f4ba26 100644 --- a/prosthesis_controller/servo.cpp +++ b/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