diff --git a/mep3_behavior/include/mep3_behavior/move_action.hpp b/mep3_behavior/include/mep3_behavior/move_action.hpp
index c22cf081..8d9734db 100644
--- a/mep3_behavior/include/mep3_behavior/move_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/move_action.hpp
@@ -165,6 +165,7 @@ namespace mep3_behavior
           BT::InputPort<std::string>("frame_id"),
           BT::InputPort<bool>("ignore_obstacles"),
           BT::InputPort<int>("mode"),
+          BT::InputPort<int>("reversing"),
           BT::OutputPort<int>("error")};
     }
 
@@ -172,6 +173,7 @@ namespace mep3_behavior
     {
       double yaw_deg;
       int mode;
+      int reversing;
       std::string position;
       std::string token;
 
@@ -183,6 +185,7 @@ namespace mep3_behavior
       getInput<double>("angular_velocity", goal.angular_properties.max_velocity);
       getInput<double>("angular_velocity", goal.angular_properties.max_acceleration);
       getInput<int>("mode", mode);
+      getInput<int>("reversing", reversing);
 
       std::istringstream iss(position);
       std::getline(iss, token, ';');
@@ -194,6 +197,7 @@ namespace mep3_behavior
       std::getline(iss, token, ';');
       goal.target.theta = std::stod(token) * M_PI / 180.0;
       goal.mode = mode;
+      goal.reversing = reversing;
 
       std::cout << "RotateAction: setGoal" << std::endl;
       std::cout << "  x: " << goal.target.x << std::endl;
@@ -206,6 +210,7 @@ namespace mep3_behavior
       std::cout << "  anguar_velocity: " << goal.angular_properties.max_velocity << std::endl;
       std::cout << "  anguar_acceleration: " << goal.angular_properties.max_acceleration << std::endl;
       std::cout << "  ignore_obstacles: " << goal.ignore_obstacles << std::endl;
+      std::cout << "  reversing: " << goal.reversing << std::endl;
 
       return true;
     }
diff --git a/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp
index 984f15dc..d04ddb94 100644
--- a/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp
@@ -18,6 +18,7 @@
 #include <string>
 #include <cmath>
 #include <iostream>
+#include <vector>
 
 #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
@@ -25,11 +26,13 @@
 #include "mep3_behavior/pose_2d.hpp"
 #include "mep3_behavior/blackboard.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "nav2_msgs/action/follow_path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace mep3_behavior
 {
   class NavigateToAction
-    : public BT::RosActionNode<nav2_msgs::action::NavigateToPose>
+      : public BT::RosActionNode<nav2_msgs::action::NavigateToPose>
   {
   public:
     NavigateToAction(const std::string &name,
@@ -40,36 +43,35 @@ namespace mep3_behavior
     {
       if (!getInput("goal", target_pose_))
         throw BT::RuntimeError(
-          "Navigate action requires 'goal' argument"
-        );
+            "Navigate action requires 'goal' argument");
       getInput("behavior_tree", behavior_tree_);
 
       std::string table = this->config().blackboard->get<std::string>("table");
       BT::Pose2D goal_offset;
-      if (table.length() > 0 && getInput("goal_" + table, goal_offset)) {
+      if (table.length() > 0 && getInput("goal_" + table, goal_offset))
+      {
         target_pose_ += goal_offset;
       }
 
       BT::TeamColor color = this->config().blackboard->get<BT::TeamColor>("color");
-      if (color == BT::TeamColor::GREEN) {
+      if (color == BT::TeamColor::GREEN)
+      {
         target_pose_ = BT::mirrorPose(target_pose_);
       }
-
     }
 
     static BT::PortsList providedPorts()
     {
       // Static parameters
       BT::PortsList port_list = {
-        BT::InputPort<BT::Pose2D>("goal"),
-        BT::InputPort<std::string>("behavior_tree")
-      };
+          BT::InputPort<BT::Pose2D>("goal"),
+          BT::InputPort<std::string>("behavior_tree")};
 
       // Dynamic parameters
-      for (std::string table : BT::SharedBlackboard::access()->get<std::vector<std::string>>("predefined_tables")) {
+      for (std::string table : BT::SharedBlackboard::access()->get<std::vector<std::string>>("predefined_tables"))
+      {
         port_list.insert(
-          BT::InputPort<BT::Pose2D>("goal_" + table)
-        );
+            BT::InputPort<BT::Pose2D>("goal_" + table));
       }
 
       return port_list;
@@ -77,9 +79,9 @@ namespace mep3_behavior
 
     bool setGoal(Goal &goal) override
     {
-      std::cout << "Navigating to x=" << target_pose_.x \
-              << " y=" << target_pose_.y \
-              << " θ=" << target_pose_.theta << "°" << std::endl;
+      std::cout << "Navigating to x=" << target_pose_.x
+                << " y=" << target_pose_.y
+                << " θ=" << target_pose_.theta << "°" << std::endl;
 
       goal.pose.header.frame_id = "map";
       goal.pose.header.stamp = node_->get_clock()->now();
@@ -98,7 +100,7 @@ namespace mep3_behavior
       return true;
     }
 
-    BT::NodeStatus onResultReceived(const WrappedResult &/*wr*/) override
+    BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override
     {
       return BT::NodeStatus::SUCCESS;
     }
@@ -108,6 +110,87 @@ namespace mep3_behavior
     std::string behavior_tree_;
   };
 
+  class FollowPathAction
+      : public BT::RosActionNode<nav2_msgs::action::FollowPath>
+  {
+  public:
+    FollowPathAction(const std::string &name,
+                     const BT::NodeConfiguration &conf,
+                     const BT::ActionNodeParams &params,
+                     typename std::shared_ptr<ActionClient> action_client)
+        : BT::RosActionNode<nav2_msgs::action::FollowPath>(name, conf, params, action_client)
+    {
+      if (!getInput("path", target_pose_))
+        throw BT::RuntimeError(
+            "Follow path action requires 'path' argument");
+    }
+
+    static BT::PortsList providedPorts()
+    {
+      // Static parameters
+      BT::PortsList port_list = {
+          BT::InputPort<std::string>("path")};
+
+      return port_list;
+    }
+
+    bool setGoal(Goal &goal) override
+    {
+      // std::cout << "Foll to x=" << target_pose_.x
+      //           << " y=" << target_pose_.y
+      //           << " θ=" << target_pose_.theta << "°" << std::endl;
+
+      // // Position
+
+      std::istringstream iss(target_pose_);
+      std::string entry;
+      std::vector<geometry_msgs::msg::PoseStamped> poses;
+      goal.path.header.frame_id = "map";
+      goal.path.header.stamp = node_->get_clock()->now();
+
+      while (std::getline(iss, entry, '|'))
+      {
+        std::istringstream entryStream(entry);
+        std::string x, y, angle;
+
+        std::getline(entryStream, x, ';');
+        std::getline(entryStream, y, ';');
+        std::getline(entryStream, angle, ';');
+        double theta = std::stod(angle) * M_PI / 180.0;
+        auto pose_stamped = geometry_msgs::msg::PoseStamped();
+
+        pose_stamped.header.frame_id = "map";
+        pose_stamped.header.stamp = node_->get_clock()->now();
+
+        pose_stamped.pose.position.x = std::stod(x);
+        pose_stamped.pose.position.y = std::stod(y);
+        pose_stamped.pose.position.z = 0.0;
+
+        pose_stamped.pose.orientation.x = 0.0;
+        pose_stamped.pose.orientation.y = 0.0;
+
+        // https://math.stackexchange.com/a/1972382
+        pose_stamped.pose.orientation.z = std::sin(theta / 2.0);
+        pose_stamped.pose.orientation.w = std::cos(theta / 2.0);
+
+        poses.push_back(pose_stamped);
+
+        std::cout
+            << "x: " << x << " y: " << y << " theta: " << theta << std::endl;
+      }
+      goal.path.poses = poses;
+      return true;
+    }
+
+    BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override
+    {
+      return BT::NodeStatus::SUCCESS;
+    }
+
+  private:
+    std::string target_pose_;
+  };
+
 } // namespace mep3_behavior
 
 #endif // MEP3_BEHAVIOR_TREE__NAVIGATE_TO_ACTION_HPP_
diff --git a/mep3_behavior/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp
index 8db74432..103c9acf 100644
--- a/mep3_behavior/src/mep3_behavior_tree.cpp
+++ b/mep3_behavior/src/mep3_behavior_tree.cpp
@@ -135,6 +135,7 @@ int main(int argc, char **argv)
 
   BT::RegisterRosAction<mep3_behavior::JointPositionCommandAction>(factory, "JointPosition", {node, "joint_position_command", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::NavigateToAction>(factory, "Navigate", {node, "navigate_to_pose", std::chrono::seconds(30)});
+  BT::RegisterRosAction<mep3_behavior::FollowPathAction>(factory, "FollowPath", {node, "follow_path", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::PumpAction>(factory, "Pump", {node, "pump", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::TranslateAction>(factory, "Translate", {node, "move/move", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::RotateAction>(factory, "Rotate", {node, "move/move", std::chrono::seconds(30)});
diff --git a/mep3_behavior/strategies/project.btproj b/mep3_behavior/strategies/project.btproj
index 840158fc..d82a16f3 100644
--- a/mep3_behavior/strategies/project.btproj
+++ b/mep3_behavior/strategies/project.btproj
@@ -29,6 +29,9 @@
             <input_port name="can_id">For example, 0x12345</input_port>
             <input_port name="message">For example, 0x32;0x34</input_port>
         </Action>
+        <Action ID="FollowPath" editable="true">
+            <input_port name="path"/>
+        </Action>
         <Action ID="JointPosition" editable="true">
             <input_port name="instance"/>
             <input_port name="max_effort" default="0"/>
@@ -48,6 +51,7 @@
             <input_port name="mode" default="7"/>
             <input_port name="linear_acceleration" default="0.8"/>
             <input_port name="angular_acceleration" default="1.8"/>
+            <input_port name="reversing" default="0"/>
         </Action>
         <Action ID="Navigate" editable="true">
             <input_port name="behavior_tree"/>
diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml
index c6ab7975..ea111a0a 100644
--- a/mep3_behavior/strategies/try_translate.xml
+++ b/mep3_behavior/strategies/try_translate.xml
@@ -1,21 +1,60 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
   <BehaviorTree ID="test">
-    <TaskSequence>
-      <WaitMatchStart state="2"/>
-      <Sleep msec="10000"/>
-      <ScoreboardTask points="33"
-                      task="store_sample_to_work_shed"/>
-      <Sleep msec="1000000"/>
-      <Navigate behavior_tree=""
-                goal="0;0;0"/>
-      <Navigate behavior_tree=""
-                goal="-0.4;0.5;180"/>
-      <Navigate behavior_tree=""
-                goal="-0.4;-0.5;0"/>
-      <Navigate behavior_tree=""
-                goal="-0.4;0.5;180"/>
-    </TaskSequence>
+    <ForceSuccess>
+      <TaskSequence>
+        <WaitMatchStart state="2"/>
+        <Parallel failure_count="1"
+                  success_count="-1">
+          <Move goal="0.65;-0.2;90"
+                ignore_obstacles="true"
+                linear_velocity="1.8"
+                frame_id="map"
+                angular_velocity="1.8"
+                mode="7"
+                linear_acceleration="1.5"
+                angular_acceleration="1.8"
+                reversing="0"/>
+          <JointPosition instance="m11"
+                         max_effort="0"
+                         max_velocity="50"
+                         position="112"
+                         tolerance="10"
+                         feedback_effort="{feedback_effort}"
+                         feedback_position="{feedback_position}"
+                         result="{result}"/>
+        </Parallel>
+        <FollowPath path="0.65;-0.2;-90|0.50;-0.42;-90|0.3;-0.5;-90|0.1;-0.42;-60"/>
+        <Translate frame_id="base_link"
+                   ignore_obstacles="false"
+                   x="-0.10"
+                   error="{error}"
+                   linear_velocity="0.5"
+                   linear_acceleration="0.8"
+                   enable_sensors="false"/>
+        <JointPosition instance="m11"
+                       max_effort="0"
+                       max_velocity="100"
+                       position="170"
+                       tolerance="10"
+                       feedback_effort="{feedback_effort}"
+                       feedback_position="{feedback_position}"
+                       result="{result}"/>
+        <ForceSuccess>
+          <Sequence>
+            <AlwaysFailure/>
+            <Navigate behavior_tree=""
+                      goal="0;0;0"/>
+            <Navigate behavior_tree=""
+                      goal="-0.4;0.5;180"/>
+            <Navigate behavior_tree=""
+                      goal="-0.4;-0.5;0"/>
+            <Navigate behavior_tree=""
+                      goal="-0.4;0.5;180"/>
+          </Sequence>
+        </ForceSuccess>
+      </TaskSequence>
+    </ForceSuccess>
   </BehaviorTree>
 
   <BehaviorTree ID="test_lifts">
@@ -45,7 +84,8 @@
               angular_velocity="1.0"
               mode="7"
               linear_acceleration="0.8"
-              angular_acceleration="1.8"/>
+              angular_acceleration="1.8"
+              reversing="0"/>
       </Parallel>
       <Translate frame_id="base_link"
                  ignore_obstacles="true"
@@ -128,17 +168,27 @@
   </BehaviorTree>
 
   <BehaviorTree ID="test_mechanism">
-    <Sequence>
-      <Sleep msec="5000"/>
-      <Move goal="-0.6;0.6;180"
-            ignore_obstacles="false"
-            linear_velocity="1.0"
+    <TaskSequence>
+      <WaitMatchStart state="2"/>
+      <Move goal="0.80;-0.3;-90"
+            ignore_obstacles="true"
+            linear_velocity="0.5"
             frame_id="map"
             angular_velocity="1.0"
             mode="7"
             linear_acceleration="0.8"
-            angular_acceleration="1.8"/>
-    </Sequence>
+            angular_acceleration="1.8"
+            reversing="2"/>
+      <FollowPath path="0.72;-0.15;-90|0.49;-0.05;-90|0.30;-0.18;-90|0.3;-0.3;90"/>
+      <JointPosition instance="m12"
+                     max_effort="0"
+                     max_velocity="150"
+                     position="100"
+                     tolerance="10"
+                     feedback_effort="{feedback_effort}"
+                     feedback_position="{feedback_position}"
+                     result="{result}"/>
+    </TaskSequence>
   </BehaviorTree>
 
   <BehaviorTree ID="test_sensors">
@@ -201,26 +251,15 @@
 
   <BehaviorTree ID="try_translate">
     <Sequence>
-      <WaitMatchStart state="2"/>
       <Sleep msec="3000"/>
-      <ScoreboardTask points="11"
-                      task="store_sample_to_work_shed"/>
-      <Repeat num_cycles="7">
-        <Sequence>
-          <SubTree ID="gentle_attack"
-                   goal="0.4;-0.5;-90"
-                   type="7"/>
-          <SubTree ID="gentle_attack"
-                   goal="-0.4;0.5;180"
-                   type="7"/>
-          <SubTree ID="gentle_attack"
-                   goal="-0.4;-0.5;0"
-                   type="7"/>
-          <SubTree ID="gentle_attack"
-                   goal="0.4;0.5;0"
-                   type="7"/>
-        </Sequence>
-      </Repeat>
+      <JointPosition instance="m14"
+                     max_effort="0"
+                     max_velocity="180"
+                     position="0"
+                     tolerance="10"
+                     feedback_effort="{feedback_effort}"
+                     feedback_position="{feedback_position}"
+                     result="{result}"/>
     </Sequence>
   </BehaviorTree>
 
@@ -236,7 +275,8 @@
             angular_velocity="1.0"
             mode="7"
             linear_acceleration="0.8"
-            angular_acceleration="1.8"/>
+            angular_acceleration="1.8"
+            reversing="0"/>
       <Sleep msec="50000"/>
       <Move goal="-0.43;-0.5;-90"
             ignore_obstacles="true"
@@ -245,7 +285,8 @@
             angular_velocity="1.0"
             mode="7"
             linear_acceleration="0.8"
-            angular_acceleration="1.8"/>
+            angular_acceleration="1.8"
+            reversing="0"/>
       <Move goal="-0.43;-0.1;-90"
             ignore_obstacles="true"
             linear_velocity="0.5"
@@ -253,7 +294,8 @@
             angular_velocity="1.0"
             mode="2"
             linear_acceleration="0.8"
-            angular_acceleration="1.8"/>
+            angular_acceleration="1.8"
+            reversing="0"/>
       <JointPosition instance="m12"
                      max_effort="0"
                      max_velocity="180"
@@ -282,6 +324,10 @@
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Action ID="FollowPath"
+            editable="true">
+      <input_port name="path"/>
+    </Action>
     <Action ID="JointPosition"
             editable="true">
       <input_port name="instance"/>
@@ -316,19 +362,14 @@
                   default="0.8"/>
       <input_port name="angular_acceleration"
                   default="1.8"/>
+      <input_port name="reversing"
+                  default="0"/>
     </Action>
     <Action ID="Navigate"
             editable="true">
       <input_port name="behavior_tree"/>
       <input_port name="goal"/>
     </Action>
-    <Action ID="ScoreboardTask"
-            editable="true">
-      <input_port name="points"
-                  default="0">points scored, can be negative</input_port>
-      <input_port name="task"
-                  default="store_sample_to_work_shed">unique task name</input_port>
-    </Action>
     <Control ID="TaskSequence"
              editable="true"/>
     <Action ID="Translate"
diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py
index 9c46d43f..47c4fa26 100644
--- a/mep3_bringup/launch/robot_launch.py
+++ b/mep3_bringup/launch/robot_launch.py
@@ -28,6 +28,9 @@
     ('big', 'blue_a', [0.045, 1.31, -pi/2]), # centralno polje
     ('big', 'yellow_a', [0.045, -1.31, pi/2]), # centralno polje
 
+    ('big', 'blue_b', [0.81, -1.34, -pi/2]), # unazad kod panela
+    ('big', 'yellow_b', [0.045, -1.31, pi/2]), # unazad kod panela
+
     ('small', 'blue', [0.72, -1.16, pi/2])
 ]
 PREDEFINED_TABLE_NAMES = [
@@ -87,7 +90,7 @@ def launch_setup(context, *args, **kwargs):
     color = LaunchConfiguration('color')
     table = LaunchConfiguration('table', default='')
     should_live_reload = ('live' in strategy.perform(context))
-    if color.perform(context) not in ['blue', 'yellow', 'blue_a', 'yellow_a']:
+    if color.perform(context) not in ['blue', 'yellow', 'blue_a', 'yellow_a', 'blue_b', 'yellow_b']:
         print('ERROR: The `color` parameter must be either `blue` or `yellow`.')
         sys.exit(1)
     if namespace.perform(context) not in ['big', 'small']:
diff --git a/mep3_bringup/launch/simulation_launch.py b/mep3_bringup/launch/simulation_launch.py
index 7571ae25..d95c62ef 100644
--- a/mep3_bringup/launch/simulation_launch.py
+++ b/mep3_bringup/launch/simulation_launch.py
@@ -14,7 +14,7 @@
 
 def generate_launch_description():
     use_behavior_tree = LaunchConfiguration('bt', default=True)
-    big_strategy = LaunchConfiguration('big_strategy', default='try_translate')
+    big_strategy = LaunchConfiguration('big_strategy', default='test')
     small_strategy = LaunchConfiguration('small_strategy', default='wait_everytime')
     color = LaunchConfiguration('color', default='blue')
     namespace = LaunchConfiguration('namespace', default='big')
diff --git a/mep3_hardware/resource/big_controllers.yaml b/mep3_hardware/resource/big_controllers.yaml
index f0de39c6..38d38b57 100644
--- a/mep3_hardware/resource/big_controllers.yaml
+++ b/mep3_hardware/resource/big_controllers.yaml
@@ -30,8 +30,8 @@ big:
       odom_frame_id: odom
       base_frame_id: base_link
 
-      wheel_separation: 0.28670
-      wheel_radius: 0.04053
+      wheel_separation: 0.28870
+      wheel_radius: 0.04044
       right_wheel_radius_multiplier: 1.0
 
       linear.x.has_velocity_limits: true
diff --git a/mep3_navigation/launch/navigation_launch.py b/mep3_navigation/launch/navigation_launch.py
index fa08dda4..7f34a90f 100644
--- a/mep3_navigation/launch/navigation_launch.py
+++ b/mep3_navigation/launch/navigation_launch.py
@@ -152,7 +152,7 @@ def generate_launch_description():
                 'update_rate': 100,
                 'angular.stuck_coeff': 2000.0,
                 'linear.stuck_coeff': 2000.0,
-                'linear.kp': 2.5,
+                'linear.kp': 3.0,
                 # 'angular.kp': 3.0,
             }
         ],
diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml
index ea601cbc..635d36f4 100644
--- a/mep3_navigation/params/nav2_params_big.yaml
+++ b/mep3_navigation/params/nav2_params_big.yaml
@@ -42,8 +42,8 @@
       yaw_goal_tolerance: 0.05
     FollowPath:
       plugin: "mep3_navigation::RegulatedPurePursuitController"
-      desired_linear_vel: 0.5
-      max_linear_accel: 0.8
+      desired_linear_vel: 0.35
+      max_linear_accel: 0.6
       max_linear_decel: 0.8
       max_linear_jerk: 99999.0
       max_angular_jerk: 99999.0
diff --git a/mep3_simulation/webots_data/controllers/judge/judge.py b/mep3_simulation/webots_data/controllers/judge/judge.py
index b3c03350..9b05ba59 100644
--- a/mep3_simulation/webots_data/controllers/judge/judge.py
+++ b/mep3_simulation/webots_data/controllers/judge/judge.py
@@ -6,8 +6,8 @@
 
 
 INITIAL_POSE_MATRIX = [
-    ('big', 'blue', [0.03, -1.15, pi/2]), 
-    ('small', 'blue', [0.72, -1.16, pi/2]),
+    ('big', 'blue', [0.815, -1.33, pi/2]), 
+    ('small', 'blue', [-0.815, -1.33, pi/2]),
     ('big', 'green', [-0.72, 1.16, -pi/2]),
     ('small', 'green', [0.72, 1.16, -pi/2]),
 ]
diff --git a/mep3_simulation/webots_data/protos/StaticObjects.proto b/mep3_simulation/webots_data/protos/StaticObjects.proto
index eb2dac3a..4a44ad1a 100644
--- a/mep3_simulation/webots_data/protos/StaticObjects.proto
+++ b/mep3_simulation/webots_data/protos/StaticObjects.proto
@@ -17,6 +17,7 @@ Group {
       contactMaterial "floor"
       size 2 3
       tileSize 2 3
+      rotation 0 0 1 3.1404
       appearance PBRAppearance {
         baseColorMap ImageTexture {
           url [
@@ -32,10 +33,6 @@ Group {
     Walls {}
     FixedBeaconSupports {}
     CentralTrackingDevice {}
-    BasketSupport {}
-    BasketSupport {
-      teamColor "green"
-    }
   ]
 }
 }
diff --git a/mep3_simulation/webots_data/worlds/.eurobot.wbproj b/mep3_simulation/webots_data/worlds/.eurobot.wbproj
index f82f604c..b44e8fcc 100644
--- a/mep3_simulation/webots_data/worlds/.eurobot.wbproj
+++ b/mep3_simulation/webots_data/worlds/.eurobot.wbproj
@@ -1,10 +1,11 @@
 Webots Project File version R2023b
-perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff000000030000078000000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000043300000001000000020000000100000008fc00000000
+perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff0000000300000780000000b6fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000003b600000001000000020000000100000008fc00000000
 simulationViewPerspectives: 000000ff000000010000000200000101000002c40100000002010000000100
 sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000200
 maximizedDockId: -1
 centralWidgetVisible: 1
 orthographicViewHeight: 1
 textFiles: -1
+globalOptionalRendering: CoordinateSystem
 consoles: Console:All:All
 renderingDevicePerspectives: camera_central:RasPi0;0;0.113021;0;0
diff --git a/mep3_simulation/webots_data/worlds/eurobot.wbt b/mep3_simulation/webots_data/worlds/eurobot.wbt
index eed80cc8..0c45d306 100644
--- a/mep3_simulation/webots_data/worlds/eurobot.wbt
+++ b/mep3_simulation/webots_data/worlds/eurobot.wbt
@@ -26,8 +26,8 @@ WorldInfo {
   ]
 }
 Viewpoint {
-  orientation 0.006249707430588943 0.9999401686365654 -0.008977767211780799 1.21630611235842
-  position -1.8930923762430245 -0.048865452007669896 5.216846605544976
+  orientation 0.4819430059392584 -0.005549339884766831 -0.8761849940812015 3.1538728487414
+  position 2.988041384032802 0.28050982519477397 4.535328706366026
 }
 TexturedBackground {
 }
@@ -65,13 +65,13 @@ Robot {
 }
 DEF ROBOT_SMALL GenericRobot {
   name "robot_small"
-  translation 0.72 -1.16 0
+  translation -0.75 -1.25 0
   rotation 0 0 1 1.5708
   arucoNumber 6
 }
 DEF ROBOT_BIG GenericRobot {
   name "robot_big"
-  translation 0.09 -1.16 0
+  translation 0.815 -1.33 0
   rotation 0 0 1 1.5708
   arucoNumber 7
   baseSlot [
diff --git a/sensor.py b/sensor.py
new file mode 100644
index 00000000..9c5ed6e4
--- /dev/null
+++ b/sensor.py
@@ -0,0 +1,55 @@
+import can
+
+# Inicijalizirajte CAN bus s filtrom za adrese 0x6D10, 0x6D11, 0x6D12 i 0x6D13
+bus = can.interface.Bus(channel='can0', bustype='socketcan')
+
+
+# try:
+#     while True:
+#         message = bus.recv()
+#         if 0x6d10 <= message.arbitration_id < 0x6d14:
+#             print(message)        # Ispisivanje primljene poruke
+# except KeyboardInterrupt:
+#     bus.shutdown()           # Zatvaranje veze s CAN busom pri prekidu
+
+
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32
+import can
+from can import Message
+
+filters = [
+{"can_id": 0x6d10, "can_mask": 0x1FFFFFFF, "extended": True},
+{"can_id": 0x6d11, "can_mask": 0x1FFFFFFF, "extended": True},
+{"can_id": 0x6d12, "can_mask": 0x1FFFFFFF, "extended": True},
+{"can_id": 0x6d13, "can_mask": 0x1FFFFFFF, "extended": True},
+]
+class IRSensorPublisher(Node):
+    def __init__(self):
+        super().__init__('ir_sensor_publisher')
+        self._publisher = self.create_publisher(Int32, 'ir_sensor_state', 10)
+        self._timer = self.create_timer(0.005, self.publish_ir_sensor_state)
+        # self._can_bus = can.interface.Bus(channel='can0', bustype='socketcan')
+        self._can_bus = can.interface.Bus(channel="can0", interface="socketcan", can_filters=filters)
+
+    def publish_ir_sensor_state(self):
+        message = self._can_bus.recv()
+        
+        # if 0x6d10 <= message.arbitration_id <= 0x6d13:
+        print(message)
+        msg = Int32()
+        msg.data = message.data[0]
+        print(message.data[0], message.arbitration_id)
+        self._publisher.publish(msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    ir_sensor_publisher = IRSensorPublisher()
+    rclpy.spin(ir_sensor_publisher)
+    ir_sensor_publisher.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()