(PositionControlViz)=
# Positional Control with Visualization

This example drives the actuator through a sinusoidal position, using a simple PD controller, while streaming to a PlotJuggler client. This example shows how to control a motor with a dynamic target using tuned gains, and how to send data to be visualized. The port used in this example is 5556.

## Demo Video
::::{youtube} TPDbrZND5xw
::::

:::{note} the above video shows an example for getting PlotJuggler set up, but doesn't use the below script exactly.
:::

```python
#/usr/bin/env python3
"""
This script will run the actuator output shaft position through 
a sinusoidal pattern using a PD controller.
"""
import sys
import time
import math
from epicallypowerful.actuation import ActuatorGroup
from epicallypowerful.toolbox import TimedLoop
from epicallypowerful.toolbox.visualization import PlotJugglerUDPClient


##################################################################
# SET CLOCK SPECIFICATIONS
##################################################################

# Set control loop frequency
OPERATING_FREQ = 200 # [Hz]
clocking_loop = TimedLoop(rate=OPERATING_FREQ)

##################################################################
# INITIALIZE ACTUATOR
##################################################################

# Determine number of connected actuators. This assumes a uniform actuator type (i.e. all are AK80-9)
actuator_type = input(
    "\nSpecify actuator type (see actuation.motor_data for possible types): "
)

# Get actuator IDs
actuator_id = input("Specify actuator id: ")
actuator_id = int(actuator_id)
initialization_dict = {actuator_id:actuator_type}

# Initialize actuator object from dictionary
acts = ActuatorGroup.from_dict(initialization_dict)

##################################################################
# INITIALIZE VISUALIZER
##################################################################

PORT = 5556
udp_server_ip_address = input(
    "\nSpecify IP address of the computer on your network running PlotJuggler (e.g. 192.168.0.227): "
)

print(f"\nPublishing viz. messages on UDP server with IP address {udp_server_ip_address} on port {PORT}")

# Initialize visualizer instance
pj_client = PlotJugglerUDPClient(addr=udp_server_ip_address, port=PORT)
viz_data = {
    'data': {
        'actuator_id': actuator_id,
        'position_desired': 0,
        'position_actual': 0,
        'error': 0,
        'error_dot': 0,
        'torque_desired': 0,
        'actuator_position': 0,
        'actuator_velocity': 0,
        'actuator_torque': 0,
    },
    'timestamp': time.time(),
}
pj_client.send(viz_data)

##################################################################
# SET CONTROLLER PARAMETERS
##################################################################

GAIN_KP = 5 # proportional gain
GAIN_KD = 0.25 # derivative gain
rad_range = 3.14159 # Angular peak-to-peak sine wave range (rad) that controller will sweep
error_current = 0 # initialize, will change in loop
prev_error = 0 # initialize, will change in loop
t0 = time.time()

##################################################################
# MAIN OPERATING LOOP
##################################################################

# Zero actuator encoder
acts.zero_encoder(actuator_id)

# Run control loop at set frequency
while clocking_loop():
    print('\033[A\033[A\033[A')
    print(f'| Actuator | Position [rad] | Velocity [rad/s] | Torque [Nm] |')

    # Get data from actuator
    act_data = acts.get_data(actuator_id)

    # Update desired position
    time_since_start = time.time() - t0
    position_desired = math.sin(time_since_start) * (rad_range / 2)
    
    # Update position error
    position_current = acts.get_position(can_id=actuator_id, degrees=False)
    prev_error = error_current
    error_current = position_desired - position_current
    errordot_current = (error_current - prev_error) / (1 / OPERATING_FREQ)

    # Update torques
    torque_desired = GAIN_KP*error_current + GAIN_KD*errordot_current
    acts.set_torque(can_id=actuator_id, torque=torque_desired)

    print(f'| {int(actuator_id):^5} | {act_data.current_position:^14.2f} | {act_data.current_velocity:^16.2f} | {act_data.current_torque:^11.2f} |')

    # Send outputs for visualization
    viz_data = {
        'data': {
            'actuator_id': actuator_id,
            'position_desired': position_desired,
            'position_actual': position_current,
            'error': error_current,
            'error_dot': errordot_current,
            'torque_desired': torque_desired,
            'actuator_position': act_data.current_position,
            'actuator_velocity': act_data.current_velocity,
            'actuator_torque': act_data.current_torque,
        },
        'timestamp': time.time(),
    }
    pj_client.send(viz_data)

```
::::{button-link} https://github.com/gatech-epic-power/epically-powerful/blob/main/examples/sample_position_controller_with_visualizer.py
    :expand:
    :color: primary
    :shadow:
    **Code is available here**
::::