Creating delay between drone movements in 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);}

Yes, I have tried sleep function as well but whatever I use to create a delay inside the control(feedback) loop, it all affects the drone's performance. — Namfield, Nov 11 '20 at 11:12
Then remove the while loop and put the code you need to happen inside the if() — Shlomi Agiv, Nov 11 '20 at 13:37

score 0 · Answer 1 · answered Nov 11 '20 at 11:10

0

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.

answered Nov 11 '20 at 11:10

Olivier Michel

755
6
10

Thanks for letting me know what could be causing my problem. However, with your suggested solution, if I set the velocity to be 0, the drone will stop immediately without making any movement. – Namfield Nov 11 '20 at 11:51

Creating delay between drone movements in Webots

1 Answers1