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();