Some helper code for the PID controller.

PID_Fragment.py

import math
def returnPIDInput(self):
    angle = self.gyro.getAngle()
    set_point = self.pid_controller.getSetPoint()

    angle_greater = angle + 2*math.pi
    angle_lesser = angle - 2*maht.pi

    use_angle = math.fabs(angle-set_point) < math.fabs(angle_greater - set_point)
    use_angle = use_angle and math.fabs(angle-set_point) < math.fabs(angle_lesser - set_point)
    if use_angle:
        return angle
    elif math.fabs(angle_greater-set_point) < math.fabs(angle_lesser - set_point):
        return angle_greater
    else:
        return angle_lesser


def usePIDOutput(self, output):
    self.pid_output = output