diff --git a/protesis_03_dxl.ino b/protesis_03_dxl.ino index cadd539..9db2081 100644 --- a/protesis_03_dxl.ino +++ b/protesis_03_dxl.ino @@ -84,13 +84,13 @@ void loop() { slither(); } else { //move_protesis_with_sensor_data(); - update_mood(); + //update_mood(); move_protesis_organic(); } while (micros() - current_timestamp_us < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop - //print_loop_time_micros(); + print_loop_time_micros(); current_timestamp_us = micros(); //Reset the loop timer } diff --git a/servo-control.cpp b/servo-control.cpp index f686625..6e30c4e 100644 --- a/servo-control.cpp +++ b/servo-control.cpp @@ -33,9 +33,11 @@ void matrix_weights_update(){ + matrix_weights[0][W_COS]=40.0; //testing on servo + /* matrix_weights[0][W_COS]=30 *map_mode; matrix_weights[0][W_NOISE]=90.0 *map_mode; matrix_weights[1][W_SIN]=30 *map_mode; @@ -53,6 +55,7 @@ void matrix_weights_update(){ matrix_weights[2][W_ROLL]=1.0* (1-map_mode); matrix_weights[3][W_PITCH]=1.0* (1-map_mode); matrix_weights[4][W_ROLL]=1.0* (1-map_mode); + */ /* matrix_weights[0][W_NOISE]=90.0*map_mode; @@ -260,10 +263,12 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const //Serial.println(millis_add); float vsin=sin((millis()+millis_add)/1000.0); float vcos=cos((millis()+millis_add)/1000.0); - + for (uint8_t i=0;i<=SERVO_COUNT;i++) { - 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 + //unsigned long blastart=micros(); + float pnoise=0;//PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves + float pnoiseslow=0;//PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves + //Serial.print("perlin micros="); Serial.println(micros()-blastart); int angle=0; angle= @@ -273,9 +278,14 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const +pnoiseslow*matrix_weights[i][W_NOISESLOW] +vsin*matrix_weights[i][W_SIN] +vcos*matrix_weights[i][W_COS]; + + dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle))); + + } - delay(1); + //delay(1); + }