Rocket Artillery Python

# VARIABLES TO BE EDITED

# key bindings
TARGET_KEY = KeyCode.T
LAUNCH_KEY = KeyCode.L

EXPLOSIVE_CHARGE = 2

THRUST_TO_VALUE_RATIO = 60.0  # average rocket speed per unit of thrust slider value
TRAJECTORY_CURVATURE = 2.0  # more for more curved flight

# turret tuning
TURRET_ELEVATION_TUNING = {"p": 1, "i": 1, "d": 2}  # proportional-integral-derivative tuning ratio
TURRET_HEADING_TUNING = {"p": 1, "i": 0.1, "d": 2}
TURRET_ELEVATION_OUTPUT_LIMIT = 30  # maximum degrees per second
TURRET_HEADING_OUTPUT_LIMIT = 90

import math

# 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


class Turret:
    """Turret class definition"""

    def __init__(self):
        self.remaining_rockets = len(ROCKETS)
        self.next_rocket = ROCKETS[self.remaining_rockets - 1]
        self.elevation_controller = AngularController(
            TURRET_ELEVATION_TUNING["p"],
            TURRET_ELEVATION_TUNING["i"],
            TURRET_ELEVATION_TUNING["d"],
            TURRET_ELEVATION_OUTPUT_LIMIT)
        self.heading_controller = AngularController(
            TURRET_HEADING_TUNING["p"],
            TURRET_HEADING_TUNING["i"],
            TURRET_HEADING_TUNING["d"],
            TURRET_HEADING_OUTPUT_LIMIT)
        self.target_set = False

    def update(self):
        bottom_wheel_speed = 0
        upper_wheel_speed = 0

        # guide rockets
        for rocket in ROCKETS:
            if not rocket.destroyed:
                rocket.update()
                rocket.guide()

        if self.next_rocket is None:
            return

        if self.target_set and self.remaining_rockets > 0:
            # calculate launch angles
            self.next_rocket.prepare_launch()

            # debug
            Besiege.Watch("Launch elevation:", self.next_rocket.launch_elevation)
            Besiege.Watch("Rocket elevation:", self.next_rocket.elevation)
            Besiege.Watch("Launch heading:", self.next_rocket.launch_heading)
            Besiege.Watch("Rocket heading:", self.next_rocket.heading)

            # call PID controllers
            upper_wheel_speed = self.elevation_controller.control(self.next_rocket.elevation,
                                                                  self.next_rocket.launch_elevation)
            bottom_wheel_speed = self.heading_controller.control(self.next_rocket.heading,
                                                                 self.next_rocket.launch_heading)

            # check if ready to fire
            required_elevation_accuracy = clamp(1 / math.pow(self.next_rocket.launch_distance / 100.0, 2), 0.5, 1)
            required_heading_accuracy = clamp(1 / math.pow(self.next_rocket.launch_distance / 100.0, 2), 0.5, 1)
            elevation_error = abs(Mathf.DeltaAngle(self.next_rocket.elevation, self.next_rocket.launch_elevation))
            heading_error = abs(Mathf.DeltaAngle(self.next_rocket.heading, self.next_rocket.launch_heading))
            accurate = elevation_error < required_elevation_accuracy and heading_error < required_heading_accuracy

            if accurate and not self.next_rocket.armed:
                self.next_rocket.arm(True)
                mark.SetColor(Color(0, 1, 0))
            if not accurate and self.next_rocket.armed:
                self.next_rocket.arm(False)
                mark.SetColor(Color(1, 0, 0))

        BOTTOM_WHEEL.SetSliderValue("SPEED", bottom_wheel_speed)
        LEFT_WHEEL.SetSliderValue("SPEED", upper_wheel_speed)
        RIGHT_WHEEL.SetSliderValue("SPEED", upper_wheel_speed)

    def set_target(self, target):
        self.next_rocket.set_target(target)
        self.target = target
        self.target_set = True

    def launch_rocket(self):
        if self.remaining_rockets > 0 and self.next_rocket.armed:
            self.next_rocket.launch()
            self.remaining_rockets -= 1
            if self.remaining_rockets > 0:
                self.next_rocket = ROCKETS[self.remaining_rockets - 1]
                self.next_rocket.set_target(self.target)
            else:
                self.next_rocket = None
                self.target_set = False


class Rocket:
    """Rocket class definition"""

    def __init__(self, id, grabber_id):
        self.rocket = Besiege.GetBlock(id)
        self.grabber = Besiege.GetBlock(grabber_id)
        self.destroyed = False
        self.launched = False
        self.armed = False

    def set_target(self, target):
        """To be called when new target is set."""
        self.target = target
        self.target_set = True

    def update(self):
        """Only to be called from inside the class."""
        if self.destroyed:
            return

        self.position = self.rocket.Position
        self.velocity = self.rocket.Velocity
        self.rotation = self.rocket.EulerAngles
        self.angular = self.rocket.AngularVelocity

        up = self.rocket.Up
        h = Vector3(up.x, 0, up.z).normalized
        self.elevation = math.asin(up.y) * Mathf.Rad2Deg
        self.heading = math.atan2(h.z, h.x) * Mathf.Rad2Deg
        self.heading = (self.heading + 360) % 360

    def prepare_launch(self):
        """To be called on every frame when targeting."""
        if not self.target_set:
            return

        target_relative_position = self.target - self.position
        x = Vector3(target_relative_position.x, 0, target_relative_position.z).magnitude
        y = - target_relative_position.y
        g = 10 * TRAJECTORY_CURVATURE
        v = 45
        while v * v * v * v - g * (g * x * x - 2 * y * v * v) < 0:
            v = v + 15

        self.thrust = (v / THRUST_TO_VALUE_RATIO) * 3
        self.rocket.SetSliderValue("THRUST", self.thrust)

        self.launch_distance = (self.target - self.position).magnitude
        self.launch_elevation = math.atan(
            (v * v - math.sqrt(v * v * v * v - g * (g * x * x - 2 * y * v * v))) / (g * x)) * Mathf.Rad2Deg
        self.launch_heading = math.atan2(target_relative_position.z, target_relative_position.x) * Mathf.Rad2Deg
        self.launch_heading = (self.launch_heading + 360) % 360

    def arm(self, armed):
        """To be called once when rocket is ready to launch"""
        if not self.target_set:
            return
        self.armed = armed

    def launch(self):
        """To be called when launch key is pressed."""
        if not self.armed:
            return
        if self.launched:
            return
        self.rocket.Action("LAUNCH")
        self.grabber.Action("DETACH")
        self.launch_time = Besiege.GetTime()
        self.launch_position = self.position
        self.stage = 1
        self.launched = True
        self.grabber.SetToggleMode("AUTO-GRAB", False)

    def guide(self):
        """To be called on every frame the rocket needs to be guided."""
        if not self.rocket.Exists:
            self.destroyed = True
        if not self.launched or self.destroyed:
            return

        # rocket and target info
        target_relative_vector = self.target - self.position
        distance = target_relative_vector.magnitude
        velocity_angle = math.asin(self.velocity.normalized.y) * Mathf.Rad2Deg
        target_angle = math.asin(target_relative_vector.normalized.y) * Mathf.Rad2Deg

        x = Vector3(target_relative_vector.x, 0, target_relative_vector.z).magnitude
        y = - target_relative_vector.y
        g = 10 * TRAJECTORY_CURVATURE
        v = self.velocity.magnitude

        Besiege.Watch("Velocity:", v)
        Besiege.Watch("Velocity angle:", velocity_angle)
        Besiege.Watch("Target angle:", target_angle)

        # first stage (launch)
        if self.stage == 1:
            if v * v * v * v - g * (g * x * x - 2 * y * v * v) > 0:
                self.stage = 2

        # second stage (guide)
        if self.stage == 2:
            if v * v * v * v - g * (g * x * x - 2 * y * v * v) < 0:
                self.stage = 1
                return
            trajectory_angle = math.atan(
                (v * v - math.sqrt(v * v * v * v - g * (g * x * x - 2 * y * v * v))) / (g * x)) * Mathf.Rad2Deg
            deviation = (trajectory_angle - velocity_angle) / (trajectory_angle - target_angle)

            # thrust adjustment
            self.thrust = v / THRUST_TO_VALUE_RATIO
            self.thrust = clamp(self.thrust + self.thrust * deviation, 0, 5)

            # time to impact
            relative_velocity = Vector3.Project(self.velocity, target_relative_vector.normalized)
            tti = distance / relative_velocity.magnitude

            # target proximity check
            if distance < 5:
                self.destroy()
            else:
                self.flight_time = Besiege.GetTime() - self.launch_time + tti

            # check for miss
            travel = self.position - self.launch_position
            if travel.magnitude > self.launch_distance:
                self.destroy()

            # set new values
            self.rocket.SetSliderValue("THRUST", self.thrust)
            self.rocket.SetSliderValue("FLIGHT DURATION", self.flight_time)

            # debug
            Besiege.Watch("Trajectory angle:", trajectory_angle)
            Besiege.Watch("Deviation:", deviation)
            Besiege.Watch("Thrust:", self.thrust)
            Besiege.Watch("TTI:", tti)

    def destroy(self):
        self.flight_time = 0
        self.rocket.SetSliderValue("FLIGHT DURATION", self.flight_time)
        self.launched = False
        self.armed = False
        self.target_set = False
        self.destroyed = True


# INITIALIZE BLOCK HANDLERS

ROCKETS = [
    Rocket("ROCKET 1", "GRABBER 1"),
    Rocket("ROCKET 2", "GRABBER 2"),
    Rocket("ROCKET 3", "GRABBER 3"),
    Rocket("ROCKET 4", "GRABBER 4")]
BOTTOM_WHEEL = Besiege.GetBlock("918ea244-8279-4118-9f7e-ba252fd34f6f")
LEFT_WHEEL = Besiege.GetBlock("1902526e-208e-4aa6-ac57-f197f709a435")
RIGHT_WHEEL = Besiege.GetBlock("b3cf6f9c-b993-43c3-81bd-1f6f74924ee8")

# INITIAL MACHINE SETTINGS

BOTTOM_WHEEL.SetToggleMode("AUTOMATIC", True)
LEFT_WHEEL.SetToggleMode("AUTOMATIC", True)
RIGHT_WHEEL.SetToggleMode("AUTOMATIC", True)

BOTTOM_WHEEL.SetSliderValue("SPEED", 0)
LEFT_WHEEL.SetSliderValue("SPEED", 0)
RIGHT_WHEEL.SetSliderValue("SPEED", 0)

for r in ROCKETS:
    r.rocket.SetSliderValue("EXPLOSIVE CHARGE", EXPLOSIVE_CHARGE)
    r.rocket.SetSliderValue("FLIGHT DURATION", 10)
    r.rocket.ClearKeys("LAUNCH")
    r.grabber.ClearKeys("DETACH")

# GLOBAL VARIABLES

turret = Turret()
mark = None


def Update():
    global mark
    global turret

    turret.update()

    if Input.GetKeyDown(TARGET_KEY):
        try:
            hit = Besiege.GetRaycastHit()
            if mark is None:
                mark = Besiege.CreateMark(hit)
            else:
                mark.Move(hit)
            turret.set_target(hit)
        except:
            pass
    if Input.GetKeyDown(LAUNCH_KEY):
        turret.launch_rocket()


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