FIX: PID calculate