Rocket Artillery Lua

b = besiege

------ VARIABLES TO BE EDITED ------

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

EXPLOSIVE_CHARGE = 2

THRUST_TO_VALUE_RATIO = 60 -- average rocket speed per unit of thrust slider value
TRAJECTORY_CURVATURE = 2 -- more for more curved flight

-- turret tuning
TURRET_ELEVATION_TUNING = { p=1, i=3, d=2 } -- proportional-integral-derivative tuning ratio
TURRET_HEADING_TUNING = { p=2, i=0, d=4 }
TURRET_ELEVATION_OUTPUT_LIMIT = 30 -- maximum degrees per second
TURRET_HEADING_OUTPUT_LIMIT = 90

------ PID CONTROLLER CLASS DEFINITION ------
Controller = {}

function Controller:new(p, i, d, max)
    local o = {}
    o.sum = p + i + d
    o.p = p / o.sum
    o.i = i / o.sum
    o.d = d / o.sum
    o.err = 0
    o.int = 0
    o.der = 0
    o.max = max
    o.first_call = true
    o.last_time = b:getScaledTime()
    o.last_current = nil
    setmetatable(o, Controller)
    self.__index = self
    return o
end

function Controller:control(current, target)
    local time = b:getScaledTime()
    local output = 0
    self.delta_time = (time - self.last_time) / 1000
    self.last_time = time
    if self.first_call then
        self.first_call = false
    else
        self.err = Mathf.Clamp(Mathf.DeltaAngle(target, current), self.p * self.max * -1, self.p * self.max)
        self.int = Mathf.Clamp(self.int + self.err * self.delta_time, self.i * self.max * -1, self.i * self.max)
        self.der = Mathf.DeltaAngle(self.last_current, current) * self.delta_time
        output = (self.err * self.p) + (self.int * self.i) + (self.der * self.d)
    end
    self.last_current = current
    output = Mathf.Clamp(output, self.max * -1, self.max) / self.max
    return output
end


------ TURRET CLASS DEFINITION ------
Turret = {}

function Turret:new(o)
    local o = o or {}
    o.remaining_rockets = table.getn(ROCKETS)
    o.next_rocket = ROCKETS[o.remaining_rockets]
    o.elevation_controller = Controller:new(
        TURRET_ELEVATION_TUNING.p,
        TURRET_ELEVATION_TUNING.i,
        TURRET_ELEVATION_TUNING.d,
        TURRET_ELEVATION_OUTPUT_LIMIT)
    o.heading_controller = Controller:new(
        TURRET_HEADING_TUNING.p,
        TURRET_HEADING_TUNING.i,
        TURRET_HEADING_TUNING.d,
        TURRET_HEADING_OUTPUT_LIMIT)
    setmetatable(o, Turret)
    self.__index = self
    return o
end

function Turret:update()
    local bottom_wheel_speed = 0
    local upper_wheel_speed = 0

    -- guide rockets
    for i, rocket in ipairs(ROCKETS) do
        if not rocket.destroyed then
            rocket:update()
            rocket:guide()
        end
    end

    if self.next_rocket == nil then
        return
    end

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

        -- debug
        b:watch("Launch elevation:", self.next_rocket.launch_elevation)
        b:watch("Rocket elevation:", self.next_rocket.elevation)
        b:watch("Launch heading:", self.next_rocket.launch_heading)
        b: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
        local required_elevation_accuracy = Mathf.Clamp(1 / (Mathf.Pow(self.next_rocket.launch_distance / 100, 2)), 0.5, 1)
        local required_heading_accuracy = Mathf.Clamp(1 / (Mathf.Pow(self.next_rocket.launch_distance / 100, 2)), 0.5, 1)
        local elevation_error = Mathf.Abs(Mathf.DeltaAngle(self.next_rocket.elevation, self.next_rocket.launch_elevation))
        local heading_error = Mathf.Abs(Mathf.DeltaAngle(self.next_rocket.heading, self.next_rocket.launch_heading))
        local accurate = elevation_error < required_elevation_accuracy and heading_error < required_heading_accuracy
        if accurate and not self.next_rocket.armed then
            self.next_rocket:arm(true)
            mark:setColor(Color(0, 1, 0))
        end
        if not accurate and self.next_rocket.armed then
            self.next_rocket:arm(false)
            mark:setColor(Color(1, 0, 0))
        end
    end
    BOTTOM_WHEEL:setSliderValue("SPEED", bottom_wheel_speed)
    LEFT_WHEEL:setSliderValue("SPEED", upper_wheel_speed)
    RIGHT_WHEEL:setSliderValue("SPEED", upper_wheel_speed)
end

function Turret:set_target(target)
    self.next_rocket:set_target(target)
    self.target = target
    self.target_set = true
end

function Turret:launch_rocket()
    if self.remaining_rockets > 0 and self.next_rocket.armed then
        self.next_rocket:launch()
        self.remaining_rockets = self.remaining_rockets - 1
        if self.remaining_rockets > 0 then
            self.next_rocket = ROCKETS[self.remaining_rockets]
            self.next_rocket:set_target(self.target)
        else
            self.next_rocket = nil
            self.target_set = false;
        end
    end
end


------ ROCKET CLASS DEFINITION ------
Rocket = {}

function Rocket:new(id, grabber_id)
    local o = {}
    o.rocket = b:getBlock(id)
    o.grabber = b:getBlock(grabber_id)
    o.destroyed = false
    setmetatable(o, Rocket)
    self.__index = self
    return o
end

-- to be called when new target is set
function Rocket:set_target(target)
    self.target = target
    self.target_set = true
end

-- not to be called from outside of class
function Rocket:update()
    if self.destroyed then
        return
    end
    self.position = self.rocket:getPosition()
    self.velocity = self.rocket:getVelocity()
    self.rotation = self.rocket:getEulerAngles()
    self.angular = self.rocket:getAngularVelocity()

    local up = self.rocket:getUp()
    local h = Vector3(up.x, 0, up.z).normalized
    self.elevation = Mathf.Asin(up.y) * Mathf.Rad2Deg
    self.heading = Mathf.Atan2(h.z, h.x) * Mathf.Rad2Deg
    self.heading = mod(self.heading + 360, 360);
end

-- to be called every frame when targeting
function Rocket:prepare_launch()
    if not self.target_set then
        return
    end

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

    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 = Mathf.Atan((v*v - Mathf.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 = mod(self.launch_heading + 360, 360)
end

-- to be called once when rocket is ready to launch
function Rocket:arm(armed)
    if not self.target_set then
        return
    end
    self.armed = armed
end

-- to be called once when launch key is pressed
function Rocket:launch()
    if not self.armed then
        return
    end
    if self.launched then
        return
    end
    self.rocket:action("LAUNCH")
    self.grabber:action("DETACH")
    self.launch_time = b:getScaledTime()
    self.launch_position = self.position
    self.stage = 1
    self.launched = true
    self.grabber:setToggleMode("AUTO-GRAB", false)
end

-- to be called on every frame the rocket needs to be guided
function Rocket:guide()
    if not self.rocket:exists() then
        self.destroyed = true
    end
    if not self.launched or self.destroyed then
        return
    end

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

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

    b:watch("Velocity:", v)
    b:watch("Velocity angle:", velocity_angle)
    b:watch("Target angle:", target_angle)

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

    -- second stage (guide)
    if self.stage == 2 then
        local trajectory_angle = Mathf.Atan((v*v - Mathf.Sqrt(v*v*v*v - g*(g*x*x - 2*y*v*v))) / (g*x)) * Mathf.Rad2Deg
        local deviation = (trajectory_angle - velocity_angle) / (trajectory_angle - target_angle)

        -- thrust adjustment
        self.thrust = v / THRUST_TO_VALUE_RATIO
        self.thrust = Mathf.Clamp(self.thrust + self.thrust * deviation, 0, 5)

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

        -- target proximity check
        if distance < 5 then
            self:destroy()
        else
            self.flight_time = b:getScaledTime() - self.launch_time + tti
        end

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

        -- set new values
        self.rocket:setSliderValue("THRUST", self.thrust)
        self.rocket:setSliderValue("FLIGHT DURATION", self.flight_time)

        -- debug
        b:watch("Trajectory angle:", trajectory_angle)
        b:watch("Deviation:", deviation)
        b:watch("Thrust:", self.thrust)
        b:watch("TTI:", tti)
    end
end

function Rocket:destroy()
    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
end

function mod(a, n)
    return a - Mathf.Floor(a/n)*n
end

------ INITIALIZE BLOCK HANDLERS ------

ROCKETS = {
    Rocket:new("ROCKET 1", "GRABBER 1"),
    Rocket:new("ROCKET 2", "GRABBER 2"),
    Rocket:new("ROCKET 3", "GRABBER 3"),
    Rocket:new("ROCKET 4", "GRABBER 4")}
BOTTOM_WHEEL = b:getBlock("918ea244-8279-4118-9f7e-ba252fd34f6f")
LEFT_WHEEL = b:getBlock("1902526e-208e-4aa6-ac57-f197f709a435")
RIGHT_WHEEL = b: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 i, r in ipairs(ROCKETS) do
    r.rocket:setSliderValue("EXPLOSIVE CHARGE", EXPLOSIVE_CHARGE)
    r.rocket:setSliderValue("FLIGHT DURATION", 10)
    r.rocket:clearKeys("LAUNCH")
    r.grabber:clearKeys("DETACH")
end

------ GLOBAL VARIABLES ------

turret = Turret:new()
mark = nil

function onUpdate()
    turret:update()
end

function onKeyHeld(key)

end

function onKeyDown(key)
    if key == TARGET_KEY then
        hit = b:getRaycastHit()
        if mark == nil then
            mark = b:createMark(hit)
            turret:set_target(hit)
        else
            mark:move(hit)
            turret:set_target(hit)
        end
    end
    if key == LAUNCH_KEY then
        turret:launch_rocket()
    end
end