def calculateSpeed(self, targetPosition, currentPosition):
    error = targetPosition - currentPosition
    proportionalTerm = PIDController.Kp * error
    self.totalError = self.totalError + error     
    integralTerm = PIDController.Ki * max(min(PIDController.GUARD_GAIN, self.totalError), -PIDController.GUARD_GAIN)
    derivativeTerm = PIDController.Kd * (error - self.lastError)                            
    # self.lastError = error
    return max(min(PIDController.K * (proportionalTerm + integralTerm + derivativeTerm), 50), -50)
dataset/ETHPy150Open ericboehlke/BB-8/PID.py/PIDController.calculateSpeed/original