From 28f8a79a9a7f0aad6f9c094639a4fd20ce9c0e8b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 15:52:26 +0200 Subject: [PATCH] make drone move away from object if it is too close --- src/drone_controls/src/PositionChanger.cpp | 32 ++++++---------------- 1 file changed, 9 insertions(+), 23 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index ff8207ff..a540b2a7 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -102,53 +102,39 @@ public: } /** - * @brief applies the collision prevention weights to the current x and y speed + * @brief applies the collision prevention weights to the current x and y speed if the drone is too close to an object. + * It moves the drone away from the object until it is far enough away * */ void apply_collision_weights() { if (this->current_speed_x > 0) // if moving forward { - if (this->move_direction_allowed[MOVE_DIRECTION_FRONT]) + if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) { this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_FRONT]; } - else - { - this->current_speed_x = 0; - } } else // moving backward { - if (this->move_direction_allowed[MOVE_DIRECTION_BACK]) + if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) { this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_BACK]; } - else - { - this->current_speed_x = 0; - } } if (this->current_speed_y > 0) // moving right { - if (this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) + if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) { - this->current_speed_y += collision_prevention_weights[MOVE_DIRECTION_RIGHT]; - } - else - { - this->current_speed_y = 0; + this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT]; } + } else // moving left { - if (this->move_direction_allowed[MOVE_DIRECTION_LEFT]) + if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) { - this->current_speed_y += collision_prevention_weights[MOVE_DIRECTION_LEFT]; - } - else - { - this->current_speed_y = 0; + this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT]; } } }