Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement