New PIDPOD6 and edited PIDPOD5 sketches

PIDPOD6 for cleaner code
PIDPOD5 has new setpoint algorithm