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

Question

我正在使用 Webots 软件来模拟无人机。 我想让我的无人机起飞，在达到所需高度后，它会在 position 停留大约 5 秒，然后自动开始其他一些动作。 我试图通过使用此代码来创建延迟：`

// 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.
 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

我不知道你的time_step变量的值，但假设它是 10，你为什么不能简单地调用wb_robot_step(500 * time_step); ？ 这样就简单多了。 现在，关于无人机的行为，之前发送的命令将在等待期间继续应用，这可能是不稳定的原因。 也许您应该考虑在进入等待期之前更改这些速度命令（将它们重置为 0？）。