PD Control Loop in Python