Fixed some bugs, the system works end to end now but might overshoot.