PID now working (forwards)