Code for elevator PID. Hope it works! (issue #3)