在 Webots 中的无人机移动之间创建延迟

Question

I'm using Webots software to simulate a drone. I want my drone to takeoff and after reaching the desired altitude, it will remain at that position for about 5 seconds before initiating some other moves automatically. I have tried to create that delay by using this code:`

// Wait 5 seconds.
    while (wb_robot_step(timestep) != -1) {
      if (wb_robot_get_time() > 5.0)
        break;
    }`

but it only works when left outside the control loop, when I use it inside the control loop`, it affects the drone's performance, the drone gets unbalanced and flies wildly. So can anyone tell me where I went wrong and if I want to create a delay between drone movements, what should I do? This is the code for movements that I have tried so far:

// Actuate the motors taking into consideration all the computed inputs.
 double front_left_motor_input = k_vertical_thrust + vertical_input - roll_input - pitch_input + 
yaw_input;
 double front_right_motor_input = k_vertical_thrust + vertical_input + roll_input - pitch_input - 
yaw_input;
 double rear_left_motor_input = k_vertical_thrust + vertical_input - roll_input + pitch_input - 
yaw_input;
  double rear_right_motor_input = k_vertical_thrust + vertical_input + roll_input + pitch_input + 
yaw_input;
wb_motor_set_velocity(front_left_motor, front_left_motor_input);
wb_motor_set_velocity(front_right_motor, -front_right_motor_input);
wb_motor_set_velocity(rear_left_motor, -rear_left_motor_input);
wb_motor_set_velocity(rear_right_motor, rear_right_motor_input);

 // Wait 5 seconds.
while (wb_robot_step(timestep) != -1) {
  if (wb_robot_get_time() > 5.0)
    break;
}

  // Actuate the motors taking into consideration all the computed inputs. 2
 for (double i=-1.5;i<1;i+=0.5){
 if(i<0)
 roll_input = k_roll_p * CLAMP(roll, -1.0, 1.0) + roll_acceleration +i ;
 else
 pitch_input = k_pitch_p * CLAMP(pitch, -1.0, 1.0) - pitch_acceleration +i ;
front_left_motor_input = k_vertical_thrust + vertical_input - roll_input -pitch_input  + yaw_input;
front_right_motor_input = k_vertical_thrust + vertical_input + roll_input-pitch_input  - yaw_input;
rear_left_motor_input = k_vertical_thrust + vertical_input - roll_input + pitch_input - yaw_input;
rear_right_motor_input = k_vertical_thrust + vertical_input + roll_input + pitch_input + yaw_input;
wb_motor_set_velocity(front_left_motor, front_left_motor_input);
wb_motor_set_velocity(front_right_motor, -front_right_motor_input);
wb_motor_set_velocity(rear_left_motor, -rear_left_motor_input);
wb_motor_set_velocity(rear_right_motor, rear_right_motor_input);}

Answer 1

I don't know the value of your time_step variable, but assuming it's 10, why can't you simply call wb_robot_step(500 * time_step); ? That's way simpler. Now, regarding the behavior of the drone, the commands send previously will continue to be applied during the waiting period, this may be the cause of the instabilities. Maybe you should consider changing these velocity commands (reset them to 0?) before entering the waiting period.