diff --git a/servo-control.cpp b/servo-control.cpp index c4a2545..738c719 100644 --- a/servo-control.cpp +++ b/servo-control.cpp @@ -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);