Got rid of the double negatives in the PID sum and added serial output for analysis.