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()
{
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];
}
}
}