Current: Linear + P /// In progress: Linear + PID implementation