Add PID-PI loop functionality (#561)

- Adds PID-PI position/velocity loops.
- EMA filter is moved to the outer position loop in cascaded control.