Fixed simulation bugs, added controller design