Add probably invalid implementation of pid and swing to test it