philipp.kramer/protesis_03
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

add noise slow

Browse source
This commit is contained in:
Philipp Kramer 2023-01-10 12:48:39 +01:00
parent d5adcfeaec
commit d7b6bb8c86
1 changed files with 10 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
servo-control.cpp
View file

@ -13,10 +13,11 @@ int rpm_to_dxl_servo_speed(const int rpm);
Mood mood;
float matrix_weights[SERVO_COUNT][3]; //mapping outputs, sensors/moods
float matrix_weights[SERVO_COUNT][4]; //mapping outputs, sensors/moods
#define W_PITCH 0
#define W_ROLL 1
#define W_NOISE 2
#define W_NOISESLOW 3
void matrix_weights_update(){
@ -31,18 +32,23 @@ void matrix_weights_update(){
//testing on servo
matrix_weights[0][W_NOISE]=90.0*map_mode;
matrix_weights[0][W_NOISESLOW]=90.0*map_mode;
matrix_weights[0][W_ROLL]=1.0* (1-map_mode);
matrix_weights[1][W_NOISE]=90.0*map_mode;
matrix_weights[1][W_NOISESLOW]=90.0*map_mode;
matrix_weights[1][W_PITCH]=1.0* (1-map_mode);
matrix_weights[2][W_NOISE]=90.0*map_mode;
matrix_weights[2][W_NOISESLOW]=90.0*map_mode;
matrix_weights[2][W_ROLL]=1.0* (1-map_mode);
matrix_weights[3][W_NOISE]=90.0*map_mode;
matrix_weights[3][W_NOISESLOW]=90.0*map_mode;
matrix_weights[3][W_PITCH]=1.0* (1-map_mode);
matrix_weights[4][W_NOISE]=90.0*map_mode;
matrix_weights[4][W_NOISESLOW]=90.0*map_mode;
matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
@ -238,9 +244,10 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
for (uint8_t i=0;i<=SERVO_COUNT;i++) {
float pnoise=PerlinNoise2((millis()+millis_add)/1000.0,i*10,0.25,3); //x,y,persistance,octaves
float pnoise=PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
float pnoiseslow=PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
int angle=0;
angle=pitch*matrix_weights[i][0]+roll*matrix_weights[i][1]+pnoise*matrix_weights[i][2];
angle=pitch*matrix_weights[i][W_PITCH]+roll*matrix_weights[i][W_ROLL]+pnoise*matrix_weights[i][W_NOISE]+pnoiseslow*matrix_weights[i][W_NOISESLOW];
dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));
}
delay(1);