Update for fiddling with stuff.
Made a new class for PID controller. Don't know how I want to implement it yet, but getting there.