Finished adding in the pickup PID code

PID code still not tested and MAX and MIN and THRESHOLD need to be set.