diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py
index 0ede088..6aefd4e 100644
--- a/simulated_pcb/odo.py
+++ b/simulated_pcb/odo.py
@@ -54,10 +54,10 @@ class SimulatedPCB:
# --- motion ---
if wp['type'] == 1: # precise waypoint
- speed = min(120.0, distance * 1)
- self.vtheta = max(-6.0, min(6.0, theta_error * 2))
+ speed = min(250.0, distance * 2.0)
+ self.vtheta = max(-10.0, min(10.0, theta_error * 4.0))
else: # transit waypoint
- speed = min(180.0, distance * 2)
+ speed = min(400.0, distance * 3.0)
self.vtheta = 0.0
self.vx = speed * math.cos(target_angle)
diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml
index c8cf8c9..2160a02 100644
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -36,8 +36,8 @@
-
-
+
+
diff --git a/src/modelec_strat/data/pami_zone.xml b/src/modelec_strat/data/pami_zone.xml
index 56e35eb..ec4eb86 100644
--- a/src/modelec_strat/data/pami_zone.xml
+++ b/src/modelec_strat/data/pami_zone.xml
@@ -1,8 +1,8 @@
-
+
\ No newline at end of file
diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp
index 9a2bdec..10aa9b2 100644
--- a/src/modelec_strat/src/missions/free_mission.cpp
+++ b/src/modelec_strat/src/missions/free_mission.cpp
@@ -74,11 +74,17 @@ namespace Modelec {
{
auto currPos = nav_->GetCurrentPos();
+ target_deposite_zone_ = nav_->GetClosestDepositeZone(currPos, {}, true);
+
+ if (target_deposite_zone_ == nullptr)
+ {
+ status_ = MissionStatus::FAILED;
+ return;
+ }
+
auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta),
nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0);
- target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true);
-
auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
auto pos = depoPoint.GetTakePosition(dist);
diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp
index d972358..3647008 100644
--- a/src/modelec_strat/src/missions/take_mission.cpp
+++ b/src/modelec_strat/src/missions/take_mission.cpp
@@ -74,6 +74,12 @@ namespace Modelec {
closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos());
+ if (closestBox == nullptr)
+ {
+ status_ = MissionStatus::FAILED;
+ break;
+ }
+
action_executor_->box_obstacles_[front_] = closestBox;
auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();