[Demo] ramsete controller

Notes & Credits

ref: https://file.tavsys.net/control/controls-engineering-in-frc.pdf pp.123-127