Testing PID controller

This notebook shows how to test and tune PID controller.

The PID controller has to be implemented in pid.py first;
Make sure ipywidgets is enabled with command:
```
jupyter nbextension enable --py widgetsnbextension
```
read more in ipywidgets wiki



In [ ]:

    
%matplotlib notebook
from matplotlib import pylab as plt
from ipywidgets import interact
from IPython import display
from collections import deque

start simspark

Start SimSpark simulator as described in last lecture

run PIDAgent

Run all code below, there is plot show current joint angle and desired joint angle.

You can set target value and tune the PID parameters to get better results.



In [ ]:

    
from pid import PIDAgent
agent = PIDAgent()



In [ ]:

    
@interact(kp=(0, 100, 1), ki=(-1, 1, 0.1), kd=(-1, 1, 0.1), delay=(0, 5))
def set_pid_parameters(kp, ki, kd, delay=0):
    global agent
    agent.joint_controller.Kp = kp
    agent.joint_controller.Ki = ki
    agent.joint_controller.Kd = kd
    agent.joint_controller.set_delay(delay)

joint_name = 'HeadYaw'
sensor = deque(maxlen=100)
target = deque(maxlen=100)

@interact(name=agent.joint_names)
def set_joint_name(name):
    global joint_name
    if joint_name != name:
        joint_name = name
        sensor.clear()
        target.clear()

@interact(value=(-1, 1, 0.1))
def set_joint_target(value):
    agent.target_joints[joint_name] = value



In [ ]:

    
# inject plotting input agent's loop
cycle = 0
orig_sense_think_act = agent.sense_think_act
def sense_think_act():
    global cycle
    cycle += 1
    orig_sense_think_act()
    sensor.append(agent.perception.joint[joint_name])
    target.append(agent.target_joints[joint_name])
    if cycle % 10 == 0:  # plot every 10 cycles
        plt.cla()
        plt.plot(target, '-r')
        plt.plot(sensor, '-b')
        plt.show()
        #display.clear_output(wait=True)
        #display.display(gcf())
agent.sense_think_act = sense_think_act
agent.start()



In [ ]: