PID values of (1, 0.01, 0.01) (working fairly well, not perfect)