API tools faq
paste
Advertisement
lencH

Besiege SAM script

lencH
Jul 3rd, 2016
388
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 13.93 KB | None | 0 0
raw download clone embed print report
  1. # PID TUNING
  2. TURRET_HEADING_PID = {"p": 0.5, "i": 0, "d": 1, "max": 90}
  3. TURRET_ELEVATION_PID = {"p": 1.3, "i": 0, "d": 0.5, "max": 25}
  4. MISSILE_VELOCITY_PID = {"p": 1, "i": 1, "d": 1, "max": 300}
  5. MISSILE_PITCH_PID = {"p": 1, "i": 0.5, "d": 1, "max": 45}
  6. MISSILE_HEADING_PID = {"p": 1, "i": 0, "d": 1, "max": 45}
  7. PITCH_GAIN = 1.4
  8. HEADING_GAIN = 1.0
  9.  
  10. import math
  11.  
  12. # ANGULAR PID CONTROLLER CLASS DEFINITION
  13. class AngularController:
  14.     def __init__(self, p, i, d, max):
  15.         sum = float(p + i + d)
  16.         self.p = p / sum
  17.         self.i = i / sum
  18.         self.d = d / sum
  19.         self.err = float(0)
  20.         self.int = float(0)
  21.         self.der = float(0)
  22.         self.max = float(max)
  23.         self.first_call = True
  24.         self.on_target = False
  25.         self.last_time = None
  26.         self.last_current = None
  27.  
  28.     def control(self, current, target):
  29.         time = Besiege.GetTime()
  30.         output = 0
  31.         if self.first_call:
  32.             self.first_call = False
  33.         else:
  34.             self.delta_time = (time - self.last_time)
  35.             self.err = clamp(Mathf.DeltaAngle(current, target), self.p * self.max * -1, self.p * self.max)
  36.             self.int = clamp(self.int + self.err * self.delta_time, self.i * self.max * -1, self.i * self.max)
  37.             self.der = Mathf.DeltaAngle(current, self.last_current) * self.delta_time
  38.             output = (self.err * self.p) + (self.int * self.i) + (self.der * self.d)
  39.             self.on_target = abs(self.err / self.max) < 0.05 and abs(self.der / self.max) < 0.05
  40.         self.last_current = current
  41.         self.last_time = time
  42.         output = clamp(output, self.max * -1, self.max) / self.max
  43.         return output
  44.  
  45.  
  46. # LINEAR PID CONTROLLER CLASS DEFINITION
  47. class LinearController:
  48.     def __init__(self, p, i, d, max):
  49.         sum = float(p + i + d)
  50.         self.p = p / sum
  51.         self.i = i / sum
  52.         self.d = d / sum
  53.         self.err = float(0)
  54.         self.int = float(0)
  55.         self.der = float(0)
  56.         self.max = float(max)
  57.         self.first_call = True
  58.         self.on_target = False
  59.         self.last_time = None
  60.         self.last_current = None
  61.  
  62.     def control(self, current, target):
  63.         time = Besiege.GetTime()
  64.         output = 0
  65.         if self.first_call:
  66.             self.first_call = False
  67.         else:
  68.             self.delta_time = (time - self.last_time)
  69.             self.err = clamp(current - target, self.p * self.max * -1, self.p * self.max)
  70.             self.int = clamp(self.int + self.err * self.delta_time, self.i * self.max * -1, self.i * self.max)
  71.             self.der = (current - self.last_current) * self.delta_time
  72.             output = (self.err * self.p) + (self.int * self.i) + (self.der * self.d)
  73.             self.on_target = abs(self.err / self.max) < 0.05 and abs(self.der / self.max) < 0.05
  74.         self.last_current = current
  75.         self.last_time = time
  76.         output = clamp(output, self.max * -1, self.max) / self.max
  77.         return output
  78.  
  79.  
  80. # MISSILE CLASS DEFINITION
  81. class Missile:
  82.     def __init__(self, left_motor, right_motor, top_motor, bottom_motor, decoupler_front, decoupler_rear, bomb, arm):
  83.         self.left_motor = left_motor
  84.         self.right_motor = right_motor
  85.         self.top_motor = top_motor
  86.         self.bottom_motor = bottom_motor
  87.         self.decoupler_front = decoupler_front
  88.         self.decoupler_rear = decoupler_rear
  89.         self.bomb = bomb
  90.         self.arm = arm
  91.         self.destroyed = False
  92.         self.launched = False
  93.         self.velocity_controller = LinearController(MISSILE_VELOCITY_PID["p"], MISSILE_VELOCITY_PID["i"],
  94.                                                     MISSILE_VELOCITY_PID["d"], MISSILE_VELOCITY_PID["max"])
  95.         self.pitch_controller = AngularController(MISSILE_PITCH_PID["p"], MISSILE_PITCH_PID["i"],
  96.                                                   MISSILE_PITCH_PID["d"], MISSILE_PITCH_PID["max"])
  97.         self.heading_controller = AngularController(MISSILE_HEADING_PID["p"], MISSILE_HEADING_PID["i"],
  98.                                                     MISSILE_HEADING_PID["d"], MISSILE_HEADING_PID["max"])
  99.  
  100.     def launch(self, target):
  101.         if self.destroyed:
  102.             return
  103.         self.target = target
  104.         self.last_target_pos = target.Position
  105.         self.launched = True
  106.         self.launch_time = Besiege.GetTime()
  107.         self.left_motor.SetSliderValue("FLIGHT DURATION", 10)
  108.         self.right_motor.SetSliderValue("FLIGHT DURATION", 10)
  109.         self.top_motor.SetSliderValue("FLIGHT DURATION", 10)
  110.         self.bottom_motor.SetSliderValue("FLIGHT DURATION", 10)
  111.         self.left_motor.SetSliderValue("THRUST", LAUNCH_THRUST)
  112.         self.right_motor.SetSliderValue("THRUST", LAUNCH_THRUST)
  113.         self.top_motor.SetSliderValue("THRUST", LAUNCH_THRUST)
  114.         self.bottom_motor.SetSliderValue("THRUST", LAUNCH_THRUST * 1.5)
  115.         self.arm.SetSliderValue("ROTATION SPEED", 3)
  116.         self.arm.SetAngle(80)
  117.         self.decoupler_front.Action("EXPLODE")
  118.         self.decoupler_rear.Action("EXPLODE")
  119.         self.left_motor.Action("LAUNCH")
  120.         self.right_motor.Action("LAUNCH")
  121.         self.top_motor.Action("LAUNCH")
  122.         self.bottom_motor.Action("LAUNCH")
  123.  
  124.     def guide(self):
  125.         if not self.bomb.Exists:
  126.             self.destroyed = True
  127.  
  128.         if self.destroyed or not self.launched or Time.deltaTime == 0:
  129.             return
  130.  
  131.         position = self.left_motor.Position
  132.         rotation = self.left_motor.EulerAngles
  133.         velocity = self.left_motor.Velocity
  134.         horizontal_velocity = Vector3(velocity.x, 0, velocity.z)
  135.  
  136.         target_pos = self.target.Position
  137.         target_velocity = (target_pos - self.last_target_pos) / Time.deltaTime
  138.         target_relative = target_pos - position
  139.         tt_target = target_relative.magnitude / Vector3.Project(velocity, target_relative.normalized).magnitude
  140.         self.last_target_pos = target_pos
  141.  
  142.         predict_pos = target_pos + target_velocity * tt_target
  143.         predict_relative = predict_pos - position
  144.         tt_predict = predict_relative.magnitude / Vector3.Project(velocity, predict_relative.normalized).magnitude
  145.  
  146.         intercept_pos = predict_pos
  147.         intercept_relative = intercept_pos - position
  148.  
  149.         velocity_pitch = math.asin(velocity.normalized.y) * Mathf.Rad2Deg
  150.         velocity_heading = math.atan2(horizontal_velocity.normalized.z,
  151.                                       horizontal_velocity.normalized.x) * Mathf.Rad2Deg
  152.  
  153.         x = Vector3(intercept_relative.x, 0, intercept_relative.z).magnitude
  154.         y = - intercept_relative.y
  155.         v = velocity.magnitude
  156.         g = 8
  157.  
  158.         if v < 120 or v * v * v * v - g * (g * x * x - 2 * y * v * v) < 0:
  159.             return
  160.  
  161.         target_pitch = math.atan(
  162.             (v * v - math.sqrt(v * v * v * v - g * (g * x * x - 2 * y * v * v))) / (g * x)) * Mathf.Rad2Deg
  163.         target_heading = math.atan2(intercept_relative.z, intercept_relative.x) * Mathf.Rad2Deg
  164.  
  165.         missile_pitch = velocity_pitch
  166.         missile_heading = velocity_heading
  167.  
  168.         velocity_adjustment = self.velocity_controller.control(velocity.magnitude, MISSILE_VELOCITY)
  169.         heading_adjustment = self.heading_controller.control(missile_heading, target_heading)
  170.         pitch_adjustment = self.pitch_controller.control(missile_pitch, target_pitch)
  171.         adjustment_magnitude = math.sqrt(heading_adjustment * heading_adjustment + pitch_adjustment * pitch_adjustment)
  172.         if adjustment_magnitude > math.sqrt(2):
  173.             k = math.sqrt(2) / adjustment_magnitude
  174.             heading_adjustment *= k
  175.             pitch_adjustment *= k
  176.  
  177.         heading_adjustment = heading_adjustment * HEADING_GAIN * math.sqrt(velocity.magnitude)
  178.         pitch_adjustment = pitch_adjustment * PITCH_GAIN * math.sqrt(velocity.magnitude)
  179.  
  180.         base_thrust = MISSILE_THRUST
  181.         thrust = base_thrust - base_thrust * velocity_adjustment
  182.  
  183.         missile_roll_sin = math.sin(rotation.x * Mathf.Deg2Rad)
  184.         missile_roll_cos = math.cos(rotation.x * Mathf.Deg2Rad)
  185.         left_thrust = base_thrust * (- heading_adjustment * missile_roll_cos + pitch_adjustment * missile_roll_sin)
  186.         right_thrust = base_thrust * (heading_adjustment * missile_roll_cos - pitch_adjustment * missile_roll_sin)
  187.         top_thrust = thrust + base_thrust * (
  188.         heading_adjustment * missile_roll_sin - pitch_adjustment * missile_roll_cos)
  189.         bottom_thrust = thrust + base_thrust * (
  190.         - heading_adjustment * missile_roll_sin + pitch_adjustment * missile_roll_cos)
  191.  
  192.         self.left_motor.SetSliderValue("THRUST", left_thrust)
  193.         self.right_motor.SetSliderValue("THRUST", right_thrust)
  194.         self.top_motor.SetSliderValue("THRUST", top_thrust)
  195.         self.bottom_motor.SetSliderValue("THRUST", bottom_thrust)
  196.  
  197.         if intercept_relative.magnitude < 10:
  198.             self.destroy()
  199.  
  200.     def destroy(self):
  201.         self.left_motor.SetSliderValue("FLIGHT DURATION", 0)
  202.         self.right_motor.SetSliderValue("FLIGHT DURATION", 0)
  203.         self.top_motor.SetSliderValue("FLIGHT DURATION", 0)
  204.         self.bottom_motor.SetSliderValue("FLIGHT DURATION", 0)
  205.         self.destroyed = True
  206.  
  207.  
  208. # CONSTANTS
  209. LAUNCH_THRUST = 20
  210. MISSILE_THRUST = 3
  211. MISSILE_VELOCITY = 200
  212.  
  213. # BLOCK HANDLERS
  214. turret_heading_spinning = Besiege.GetBlock("87471ace-3f6d-4e9b-95a6-3cf717a62ee4")
  215. turret_elevation_left_steering = Besiege.GetBlock("eecc1be4-ab43-4f5f-b96e-8826ef78cdbf")
  216. turret_elevation_right_steering = Besiege.GetBlock("ee2cb232-70f2-457d-aa8d-fda7a18b5711")
  217.  
  218. missiles = [
  219.     Missile(
  220.         Besiege.GetBlock("ea8cb52b-eb83-49ec-9791-a81f286af316"),  # left motor
  221.         Besiege.GetBlock("1cf03895-77ae-4584-bb55-086a5e39424f"),  # right motor
  222.         Besiege.GetBlock("af9885b5-94e7-4850-89cd-88f53d40980d"),  # top motor
  223.         Besiege.GetBlock("0fab5b20-ede2-45f5-9690-7bd2abd8cdad"),  # bottom motor
  224.         Besiege.GetBlock("2a17b5ce-ca20-4d4d-9b19-7b1a48496e5c"),  # front decoupler
  225.         Besiege.GetBlock("e00b947f-6fad-4874-aa5c-b19eab8597f6"),  # rear decoupler
  226.         Besiege.GetBlock("893a8bc8-1a0f-4817-97f5-b3d06bc496ab"),  # bomb
  227.         Besiege.GetBlock("3234278a-6ba7-4818-bc10-171cd7300071")),  # arm
  228.     Missile(
  229.         Besiege.GetBlock("11a96476-880a-4887-afdf-1452663a18cf"),
  230.         Besiege.GetBlock("e4864062-f4af-46f4-8b81-1efaab781cc0"),
  231.         Besiege.GetBlock("324ccdb6-8270-4966-845b-4a346dc21e64"),
  232.         Besiege.GetBlock("d8667611-96e2-432c-9655-c9cd575b51f0"),
  233.         Besiege.GetBlock("351e66b5-3b7c-4e7d-b87b-9f76727bdcdd"),
  234.         Besiege.GetBlock("d854aa4b-de55-4898-b7d5-ac7f6c0ce7c0"),
  235.         Besiege.GetBlock("dbed74ee-f178-48b9-8b47-3d23e469f354"),
  236.         Besiege.GetBlock("10985473-fd0e-41f0-85d9-91534120d29e")),
  237.     Missile(
  238.         Besiege.GetBlock("d5a4a8c8-1017-4f4d-a4c3-082f7541aadc"),
  239.         Besiege.GetBlock("8bfbdf2c-95a6-47ac-b731-1148ee5522f4"),
  240.         Besiege.GetBlock("2486c33c-8e83-403f-a398-d7f7feb18fd7"),
  241.         Besiege.GetBlock("ba29d0e9-f635-4bb2-a3ab-a5b460b2aabb"),
  242.         Besiege.GetBlock("e40ed68c-5a90-4250-a297-ce415801cfef"),
  243.         Besiege.GetBlock("24aab8ca-1d61-4c2e-b735-50ca1cff6d03"),
  244.         Besiege.GetBlock("ac8b8775-1232-449a-b482-426b6cfebe1c"),
  245.         Besiege.GetBlock("18ea0012-499f-4c4c-abae-a246f3ea01f8")),
  246.     Missile(
  247.         Besiege.GetBlock("15f1ef0a-4f05-4e6a-8524-26075b2ce851"),
  248.         Besiege.GetBlock("1b263b73-a84f-4e03-80d9-c2c8e63f051d"),
  249.         Besiege.GetBlock("080e9a89-08be-462e-bca8-d90f3523f1f0"),
  250.         Besiege.GetBlock("fba21339-047c-49af-8bfe-8057c990e0bd"),
  251.         Besiege.GetBlock("58cbc2b1-bb4e-40ab-bca6-68b42fd9a2c9"),
  252.         Besiege.GetBlock("b4ad1d9c-0723-4efc-8699-4d9e403528a5"),
  253.         Besiege.GetBlock("60aa4136-94c9-4821-af1c-02a386591c4b"),
  254.         Besiege.GetBlock("0898bc2e-760b-4001-9a02-3d40bc8c8e4a"))
  255. ]
  256.  
  257. # CONTROLLER INITIALISATION
  258. heading_controller = AngularController(
  259.     TURRET_HEADING_PID["p"],
  260.     TURRET_HEADING_PID["i"],
  261.     TURRET_HEADING_PID["d"],
  262.     TURRET_HEADING_PID["max"])
  263. elevation_controller = AngularController(
  264.     TURRET_ELEVATION_PID["p"],
  265.     TURRET_ELEVATION_PID["i"],
  266.     TURRET_ELEVATION_PID["d"],
  267.     TURRET_ELEVATION_PID["max"])
  268.  
  269. # GLOBAL VARIABLES
  270. target_tracking = None
  271. target_set = False
  272. remaining_missiles = 4
  273.  
  274. turret_elevation_left_steering.ClearKeys("LEFT")
  275. turret_elevation_left_steering.ClearKeys("RIGHT")
  276. turret_elevation_right_steering.ClearKeys("LEFT")
  277. turret_elevation_right_steering.ClearKeys("RIGHT")
  278.  
  279.  
  280. def Update():
  281.     global target_set
  282.     global target_tracking
  283.     global remaining_missiles
  284.     global missiles
  285.  
  286.     for m in missiles:
  287.         m.guide()
  288.  
  289.     if target_set and remaining_missiles > 0:
  290.         next_missile = missiles[4 - remaining_missiles]
  291.         missile_position = next_missile.bomb.Position
  292.         missile_pitch = 360 - next_missile.bomb.EulerAngles.x
  293.         missile_heading = - next_missile.bomb.EulerAngles.y + 90
  294.  
  295.         target = target_tracking.Position
  296.         target_relative_vector = target - missile_position
  297.         target_pitch = math.asin(target_relative_vector.normalized.y) * Mathf.Rad2Deg
  298.         target_heading = math.atan2(target_relative_vector.z, target_relative_vector.x) * Mathf.Rad2Deg
  299.  
  300.         heading_adjustment = heading_controller.control(missile_heading, target_heading)
  301.         elevation_adjustment = elevation_controller.control(missile_pitch, target_pitch)
  302.  
  303.         turret_heading_spinning.SetSliderValue("SPEED", heading_adjustment)
  304.         turret_elevation_left_steering.SetInput(elevation_adjustment)
  305.         turret_elevation_right_steering.SetInput(elevation_adjustment)
  306.  
  307.     if Input.GetKeyDown(KeyCode.O):
  308.         try:
  309.             target_tracking = Besiege.GetRaycastCollider()
  310.             target_set = True
  311.         except:
  312.             pass
  313.     if Input.GetKeyDown(KeyCode.L):
  314.         missiles[4 - remaining_missiles].launch(target_tracking)
  315.         remaining_missiles -= 1
  316.  
  317.  
  318. def clamp(value, minvalue, maxvalue):
  319.     return max(min(value, maxvalue), minvalue)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!