.. _position-mode:

Position Mode
=============

Position mode is a mode of operation in ORCA series motors in which position targets are streamed directly to the motor, and the motor attempts to follow the position target to the best of its ability. This mode is most appropriate when the motor needs to perform custom motion profiles, or when fast responses to new information are required. Because control is managed via updating the position target directly, high speed communication is often desirable when operating the motor via this mode to minimize jerky motions.

In this tutorial we will use position mode to generate a sine wave motion. 

Prerequisites
-------------

 - :ref:`motor-modes`
 - :ref:`motor-error-handling`
 - :ref:`command-stream`
 - :ref:`high-speed-communication`
  
----

We'll start with the following code as our baseline:

.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. literalinclude:: shared/boilerplate-streaming-position.cpp
			:language: cpp
			
	.. tab-item:: Python
		:sync: python

		.. literalinclude:: shared/boilerplate-streaming-position.py
			:language: python

Additional Setup
----------------

Before moving onto the bulk of the tutorial, we'll make some slight modifications to the starting code.

First, because position mode benefits particularly from high speed communication, we'll use a high baud rate for our serial port. Make sure to also update your motor's baud in IrisControls as described in :ref:`high-speed-communication`!

Second, position mode is one of the modes of operation in which the ORCA will raise a communication timeout (2048) error if communication stops while in the mode. Because of this, we'll set the motor to ``SleepMode`` at the beginning of the program to clear the error which may have been raised following previous iterations.

.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. code-block:: cpp
			
			...
			motor.open_serial_port(serial_port, 1000000, 80);

			motor.set_mode(MotorMode::SleepMode);
			...
			
	.. tab-item:: Python
		:sync: python

		.. code-block:: python

			from pyorcasdk import MotorMode
			...
			motor.open_serial_port(serial_port, 1000000, 80)

			motor.set_mode(MotorMode.SleepMode)
			...

Generate a Sine Wave
--------------------

To generate a sine wave, first we'll need to record time to pass as input. We'll do this using built-in libraries.

.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. code-block:: cpp
			
			#include <chrono>
			...
			auto start_time = std::chrono::steady_clock::now();
			
			while (true) {
				auto now = std::chrono::steady_clock::now();
				std::chrono::duration<double> current_time_seconds = now - start_time;

				motor.run();

				std::cout << "Current Position: " << motor.stream_cache.position << "             \r";
			}
			...
			
	.. tab-item:: Python
		:sync: python

		.. code-block:: python

			from time import time
			...
			start_time = time()
			while True:
				current_time_seconds = time() - start_time

				motor.run()

				print("Current Position: " + str(motor.get_stream_data().position), end="        \r")

Now we'll need to create a function that outputs our desired position using our time input.

.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. code-block:: cpp
			
			#include <cmath>
			...
			int get_sine_position(double time_seconds)
			{
				constexpr int offset_um = 40000;
				constexpr int amplitude = 25000;

				return (std::sin(time_seconds) * amplitude) + offset_um;
			}

			int main() {
			...
			
	.. tab-item:: Python
		:sync: python

		.. code-block:: python

			from math import sin
			...
			def get_sine_position(time_seconds):
				offset_um = 40000
				amplitude = 25000
				return (sin(time_seconds) * amplitude) + offset_um
			...

Move the Motor
--------------

First, in order to actually control the motor via position mode, we must first put the motor into position mode.


.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. code-block:: cpp
			
			...
			motor.set_mode(MotorMode::PositionMode);
			...
			
	.. tab-item:: Python
		:sync: python

		.. code-block:: python

			...
			motor.set_mode(MotorMode.PositionMode)
			...

Then finally we must simply pass in our position target to the motor. While streaming we can do this using your ``Actuator`` object's ``set_streamed_position_um()`` method.

.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. code-block:: cpp
			
			...
			while (true) {
				auto now = std::chrono::steady_clock::now();
				std::chrono::duration<double> current_time_seconds = now - start_time;

				motor.set_streamed_position_um(get_sine_position(current_time_seconds.count()));

				motor.run();

				std::cout << "Current Position: " << motor.stream_cache.position << "             \r";
			}
			...
			
	.. tab-item:: Python
		:sync: python

		.. code-block:: python

			...
			while True:
				current_time_seconds = time() - start

				motor.set_streamed_position_um(int(get_sine_position(current_time_seconds)))

				motor.run()

				print("Current Position: " + str(motor.get_stream_data().position), end="        \r")

----

Complete Example
----------------

.. tab-set::
	:sync-group: language

	.. tab-item:: C++
		:sync: cpp

		.. literalinclude:: Position-Mode/main.cpp
			:language: cpp
			
	.. tab-item:: Python
		:sync: python

		.. literalinclude:: Position-Mode/main.py
			:language: python