sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

make drone move away from object if it is too close

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-25 15:52:26 +02:00
parent b94007eabc
commit 28f8a79a9a
1 changed files with 9 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/drone_controls/src/PositionChanger.cpp
View File

@@ -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() void apply_collision_weights()
{ {
if (this->current_speed_x > 0) // if moving forward 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]; this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_FRONT];
} }
else
{
this->current_speed_x = 0;
}
} }
else // moving backward 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]; 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->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]; this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT];
}
else
{
this->current_speed_y = 0;
} }
} }
else // moving left 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]; this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT];
}
else
{
this->current_speed_y = 0;
} }
} }
} }