Besiege Python Drone

"""BLOCK GUIDS"""
starting_block = Besiege.GetBlock("c62db254-76f6-4374-ae36-607808eace52")
front_left_rotor = Besiege.GetBlock("1b920418-8c49-491b-aeca-3e5c16d35964")
front_right_rotor = Besiege.GetBlock("7d1b703b-d46f-4a3b-8013-05441cbb736f")
rear_left_rotor = Besiege.GetBlock("84e3bbfa-345a-4494-9144-ffeb85ff6a9b")
rear_right_rotor = Besiege.GetBlock("adb019f0-3b1f-4825-95cb-9a18d8a1caf1")

"""PID TUNING"""
ALTITUDE_PID = {"p" : 1.5, "i" : 1, "d" : 0.5, "max" : 5}
PITCH_PID = {"p" : 1, "i" : 0.1, "d" : 0.5, "max" : 90}
ROLL_PID = {"p" : 1, "i" : 0.1, "d" : 0.5, "max" : 90}

"""CONSTANTS DEFINITIONS"""
HOVER_SPEED = 2
HOLD_ALTITUDE = True

"""ANGULAR PID CONTROLLER CLASS DEFINITION"""
class AngularController:
    def __init__(self, p, i, d, max):
        sum = float(p + i + d)
        self.p = p / sum
        self.i = i / sum
        self.d = d / sum
        self.err = float(0)
        self.int = float(0)
        self.der = float(0)
        self.max = float(max)
        self.first_call = True
        self.on_target = False
        self.last_time = None
        self.last_current = None

    def control(self, current, target):
        time = Besiege.GetTime()
        output = 0
        if self.first_call:
            self.first_call = False
        else:
            self.delta_time = (time - self.last_time)
            self.err = clamp(Mathf.DeltaAngle(current, target), self.p * self.max * -1, self.p * self.max)
            self.int = clamp(self.int + self.err * self.delta_time, self.i * self.max * -1, self.i * self.max)
            self.der = Mathf.DeltaAngle(current, self.last_current) * self.delta_time
            output = (self.err * self.p) + (self.int * self.i) + (self.der * self.d)
            self.on_target = abs(self.err / self.max) < 0.05 and abs(self.der / self.max) < 0.05
        self.last_current = current
        self.last_time = time
        output = clamp(output, self.max * -1, self.max) / self.max
        return output

"""LINEAR PID CONTROLLER CLASS DEFINITION"""
class LinearController:
    def __init__(self, p, i, d, max):
        sum = float(p + i + d)
        self.p = p / sum
        self.i = i / sum
        self.d = d / sum
        self.err = float(0)
        self.int = float(0)
        self.der = float(0)
        self.max = float(max)
        self.first_call = True
        self.on_target = False
        self.last_time = None
        self.last_current = None

    def control(self, current, target):
        time = Besiege.GetTime()
        output = 0
        if self.first_call:
            self.first_call = False
        else:
            self.delta_time = (time - self.last_time)
            self.err = clamp(current - target, self.p * self.max * -1, self.p * self.max)
            self.int = clamp(self.int + self.err * self.delta_time, self.i * self.max * -1, self.i * self.max)
            self.der = (current - self.last_current) * self.delta_time
            output = (self.err * self.p) + (self.int * self.i) + (self.der * self.d)
            self.on_target = abs(self.err / self.max) < 0.05 and abs(self.der / self.max) < 0.05
        self.last_current = current
        self.last_time = time
        output = clamp(output, self.max * -1, self.max) / self.max
        return output


"""GLOBAL VARIABLES"""
target_altitude = starting_block.Position.y

"""PID CONTROLLERS"""
drone_altitude_controller = LinearController(ALTITUDE_PID["p"], ALTITUDE_PID["i"], ALTITUDE_PID["d"], ALTITUDE_PID["max"])
drone_pitch_controller = AngularController(PITCH_PID["p"], PITCH_PID["i"], PITCH_PID["d"], PITCH_PID["max"])
drone_roll_controller = AngularController(ROLL_PID["p"], ROLL_PID["i"], ROLL_PID["d"], ROLL_PID["max"])

"""AUTOMATIC TOGGLES"""
front_left_rotor.SetToggleMode("AUTOMATIC", True)
front_right_rotor.SetToggleMode("AUTOMATIC", True)
rear_left_rotor.SetToggleMode("AUTOMATIC", True)
rear_right_rotor.SetToggleMode("AUTOMATIC", True)

def Update():
    global target_altitude

    target_pitch = Input.GetAxis("Vertical") * 15
    target_roll = Input.GetAxis("Horizontal") * -15

    if Input.GetKey(KeyCode.I):
        target_altitude += 5 * Time.deltaTime
    if Input.GetKey(KeyCode.K):
        target_altitude -= 5 * Time.deltaTime

    position = starting_block.Position
    rotation = starting_block.EulerAngles

    altitude_adjustment = - drone_altitude_controller.control(position.y, target_altitude)
    pitch_adjustment = drone_pitch_controller.control(rotation.x, target_pitch)
    roll_adjustment = drone_roll_controller.control(rotation.z, target_roll)

    front_left_speed = HOVER_SPEED + HOVER_SPEED * (altitude_adjustment - pitch_adjustment - roll_adjustment)
    front_right_speed = HOVER_SPEED + HOVER_SPEED * (altitude_adjustment - pitch_adjustment + roll_adjustment)
    rear_left_speed = HOVER_SPEED + HOVER_SPEED * (altitude_adjustment + pitch_adjustment - roll_adjustment)
    rear_right_speed = HOVER_SPEED + HOVER_SPEED * (altitude_adjustment + pitch_adjustment + roll_adjustment)

    front_left_rotor.SetSliderValue("SPEED", front_left_speed)
    front_right_rotor.SetSliderValue("SPEED", front_right_speed)
    rear_left_rotor.SetSliderValue("SPEED", rear_left_speed)
    rear_right_rotor.SetSliderValue("SPEED", rear_right_speed)

    Besiege.Watch("target altitude", target_altitude)
    Besiege.Watch("current altitude", position.y)
    Besiege.Watch("altitude_adjustment", altitude_adjustment)
    Besiege.Watch("target pitch", target_pitch)
    Besiege.Watch("current pitch", rotation.x)
    Besiege.Watch("pitch_adjustment", pitch_adjustment)
    Besiege.Watch("target roll", target_roll)
    Besiege.Watch("current roll", rotation.z)
    Besiege.Watch("roll_adjustment", roll_adjustment)

def clamp(value, minvalue, maxvalue):
    return max(min(value, maxvalue), minvalue)