diff --git a/odrive_controller.py b/odrive_controller.py
index 8eca461..61a1baf 100644
--- a/odrive_controller.py
+++ b/odrive_controller.py
@@ -3,13 +3,15 @@ import signal
 import logging
 from odrive.enums import (AXIS_STATE_IDLE,
                           AXIS_STATE_FULL_CALIBRATION_SEQUENCE,
-                          AXIS_STATE_CLOSED_LOOP_CONTROL)
+                          AXIS_STATE_CLOSED_LOOP_CONTROL,
+                          INPUT_MODE_POS_FILTER)
 
 odrive_params = {
     'current_lim': 10,  # A
     'vel_lim': 60000,  # Rev/s
     'accel_lim': 1000000/1.2,
     'decel_lim': 1000000/1.2,
+    'pos_filter': 2,  # For smooth position control
     'encoder_cpr': 4000,  # Counts per revolution
     'desired_velocity': 10000,  # Rev/s
     'brake_resistance': 50  # Ohms
@@ -59,6 +61,7 @@ class Motors:
             axis.trap_traj.config.vel_limit = self.params.get('vel_lim', 1)
             axis.trap_traj.config.accel_limit = self.params.get('accel_lim', 1)
             axis.trap_traj.config.decel_limit = self.params.get('decel_lim', 1)
+            axis.controller.config.input_filter_bandwidth = self.params.get('pos_filter', 2)
 
     def run_calibration(self):
         self.drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
@@ -68,6 +71,13 @@ class Motors:
             pass
         self.drive.save_configuration()
 
+    def enable_watchdog(self):
+        self.drive.axis0.config.watchdog_timeout = 1  # second
+        self.drive.axis1.config.watchdog_timeout = 1  # second
+        # Don't forget to push the axis.watchdog_feed() at n second intervals
+        self.drive.axis0.config.enable_watchdog = True
+        self.drive.axis1.config.enable_watchdog = True
+
     def set_idle(self):
         self.drive.axis0.requested_state = AXIS_STATE_IDLE
         self.drive.axis1.requested_state = AXIS_STATE_IDLE
@@ -76,12 +86,16 @@ class Motors:
         self.drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
         self.drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
 
+    def set_filtered_pos_ctrl(self):
+        self.drive.axis0.controller.config.input_mode = INPUT_MODE_POS_FILTER
+        self.drive.axis1.controller.config.input_mode = INPUT_MODE_POS_FILTER
+
     def index_calibration(self):
         pass
-        # TODO: here it will look for the endstop and set encoder counts to zero
-        # Freeze one motor
-        # Move other motor slowly until endstop activated
-        # https://docs.odriverobotics.com/api/odrive.endstop.config
+        #  TODO: here it will look for the endstop and zero the encoder counts
+        #  Freeze one motor
+        #  Move other motor slowly until endstop activated
+        #  https://docs.odriverobotics.com/api/odrive.endstop.config
 
     def cartesian_to_angle(self, position: dict):
         '''
@@ -92,4 +106,12 @@ class Motors:
         '''
         target_x = position['x']
         target_y = position['y']
-        pass
+        # TODO: this
+        print(target_x, target_y)
+
+    def move_to_angle(self, axis: object, position: int):
+        '''
+        This is for position ctrl, not trajectory!!!
+        '''
+        axis.controller.move_to_pos(position)
+        # PID control: https://docs.odriverobotics.com/control.html