philipp.kramer/haustorial
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

change modes, add vibration on contact

Browse source
This commit is contained in:
Philipp Kramer 2024-09-11 17:15:34 +02:00
parent 83c4994735
commit 04465fc867
5 changed files with 87 additions and 71 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

127
prosthesis_controller/behaviour.cpp
View file

@ -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_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
float matrix_weights_peripheralR[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){ void updateMatrixWeights(unsigned long millis,bool bellyup, 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; static unsigned long last_weights_update=0;
if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) { if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
last_weights_update=millis; 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.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 //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_slither=0;
float map_mode_sleeping=mood.loneliness;*/ 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; if (body_present) {
float map_mode_slither=0; map_mode_rollpitch+=0.3 *WEIGHT_UPDATE_INTERVAL/1000.0;
float map_mode_sleeping=0; map_mode_walking-=0.5 *WEIGHT_UPDATE_INTERVAL/1000.0;
map_mode_noise-=0.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
}else{
float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0); map_mode_rollpitch-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
float map_mode_noise=1.0-contact_main_smooth; //idle movement if (bellyup) {
float map_mode_scared=0.0+contact_main_smooth; //contracted position 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_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 //Mode Noise
matrix_weights_main[0][W_NOISE]=30.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[0][W_NOISESLOW]+=0.0 *map_mode_noise;
matrix_weights_main[1][W_NOISE]=30.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_NOISESLOW]+=20.0 *map_mode_noise;
matrix_weights_main[2][W_NOISE]=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_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 //Mode Roll Pitch
matrix_weights_main[0][W_ROLL]=1.0* map_mode_rollpitch; matrix_weights_main[0][W_PITCH]+=1.0* map_mode_rollpitch;
matrix_weights_main[1][W_PITCH]=1.0* map_mode_rollpitch; matrix_weights_main[1][W_ROLL]+=1.0* map_mode_rollpitch;
matrix_weights_main[2][W_ROLL]=1.0* map_mode_rollpitch; matrix_weights_main[2][W_PITCH]+=1.0* map_mode_rollpitch;
//Mode Slither //Mode Walking
matrix_weights_main[0][W_COS]=30 *map_mode_slither; matrix_weights_main[0][W_NOISESLOW]+=5 *map_mode_walking;
matrix_weights_main[1][W_SIN]=30 *map_mode_slither; matrix_weights_main[0][W_SIN]+=10 *map_mode_walking;
matrix_weights_main[2][W_COS]=-30 *map_mode_slither; 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 //Mode Sleeping
matrix_weights_main[0][W_NOISESLOW]=20 *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[1][W_NOISESLOW]+=20 *map_mode_sleeping;
matrix_weights_main[2][W_NOISESLOW]=30 *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); 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; static unsigned long last_servoweights_update=0;
if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL) if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL)
{ {
@ -116,8 +137,6 @@ void updateServosByWeights(unsigned long millis){
#define MILLISADD_MAX 100 #define MILLISADD_MAX 100
millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX); 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 vsin=sin((millis()+millis_add)/1000.0);
float vcos=cos((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 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 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; float angle=0;
@ -155,7 +177,8 @@ void updateServosByWeights(unsigned long millis){
+pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW] +pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW]
+vsin*matrix_weights_main[servoPosInMatrix][W_SIN] +vsin*matrix_weights_main[servoPosInMatrix][W_SIN]
+vcos*matrix_weights_main[servoPosInMatrix][W_COS] +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 }else if(i<SERVO_COUNT_MAIN+SERVO_COUNT_PERIPHERAL) { //Peripheral Left
servoPosInMatrix=i-3; servoPosInMatrix=i-3;

7
prosthesis_controller/behaviour.h
View file

@ -13,7 +13,7 @@ extern dxl_servo servos[SERVO_COUNT];
#define SERVO_COUNT_MAIN 3 #define SERVO_COUNT_MAIN 3
#define SERVO_COUNT_PERIPHERAL 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 #define WEIGHT_UPDATE_INTERVAL 100
@ -27,6 +27,7 @@ extern dxl_servo servos[SERVO_COUNT];
#define W_SIN 4 #define W_SIN 4
#define W_COS 5 #define W_COS 5
#define W_LOOKDIRECTION 6 #define W_LOOKDIRECTION 6
#define W_OFFSET 7
struct Mood{ 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 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); 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); float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
#endif #endif

2
prosthesis_controller/definitions.h
View file

@ -30,7 +30,7 @@
#define PIN_VALVE1 PIN_OUT1 #define PIN_VALVE1 PIN_OUT1
#define PIN_VALVE2 PIN_OUT3 #define PIN_VALVE2 PIN_OUT3
#define PIN_VIBRATION PIN_OUT2 #define PIN_VIBRATION PIN_OUT6
#define PIN_SDA 25 #define PIN_SDA 25
#define PIN_SCL 24 #define PIN_SCL 24

2
prosthesis_controller/imu.h
View file

@ -7,7 +7,7 @@
#define UPDATE_IMU_INTERVAL 1000/50 #define UPDATE_IMU_INTERVAL 1000/50
#define IMU_AC 0.1f //default 0.02f #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

20
prosthesis_controller/prosthesis_controller.ino
View file

@ -60,13 +60,13 @@ void loop() {
bool contact_peripheralL=digitalRead(PIN_INPUTARM2); //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 bool contact_peripheralR=digitalRead(PIN_INPUTARM3); //Sensor on tip of arm
float imu_pitch=0; float pitch=-getAngleY();
float imu_roll=0; float roll=-getAngleX();
updateMatrixWeights(loopmillis,body_present,contact_main,contact_peripheralL,contact_peripheralR,imu_pitch,imu_roll); updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
updateServosByWeights(loopmillis); updateServosByWeights(loopmillis,pitch,roll);
//loopServos(loopmillis); loopServos(loopmillis);
@ -83,19 +83,11 @@ void loop() {
//Serial.println(); //Serial.println();
//printBodytempDebug(); //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();
} }
} }