Last active
February 5, 2021 09:49
-
-
Save xdedss/881313620d02055e0764ae8436f924ee to your computer and use it in GitHub Desktop.
krpc tutorial 03
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import krpc | |
| from math import sin, cos, pi | |
| import time | |
| conn = krpc.connect(name='controller') | |
| space_center = conn.space_center # SpaceCenter对象 | |
| vessel = space_center.active_vessel # 当前载具 | |
| body = vessel.orbit.body # 当前载具所处的天体 | |
| lat = -0.0972 | |
| lon = -74.5577 | |
| body_frame = body.reference_frame # 地固系 | |
| # 绕y轴旋转-lon度 | |
| temp1 = space_center.ReferenceFrame.create_relative(body_frame, | |
| rotation=(0., sin(-lon / 2. * pi / 180), 0., cos(-lon / 2. * pi / 180))) | |
| # 绕z轴旋转lat度 | |
| temp2 = space_center.ReferenceFrame.create_relative(temp1, | |
| rotation=(0., 0., sin(lat / 2. * pi / 180), cos(lat / 2. * pi / 180))) | |
| # 沿x轴平移 | |
| height = body.surface_height(lat, lon) + body.equatorial_radius | |
| target_frame = space_center.ReferenceFrame.create_relative(temp2, | |
| position=(height, 0., 0.)) | |
| # 用于限制一个数的范围 | |
| def clamp(num, limit1, limit2): | |
| return max(min(num, max(limit1, limit2)), min(limit1, limit2)) | |
| # PID控制器 | |
| class PID: | |
| def __init__(self, kp, ki, kd, integral_output_limit = 1): | |
| self.kp = kp | |
| self.ki = ki | |
| self.kd = kd | |
| self.integral_output_limit = integral_output_limit | |
| self.integral = 0 | |
| self.error_prev = 0 | |
| def update(self, error, dt): | |
| # P | |
| p = error * self.kp | |
| # I | |
| self.integral += error * dt * self.ki | |
| self.integral = clamp(self.integral, self.integral_output_limit, -self.integral_output_limit) | |
| i = self.integral | |
| # D | |
| d = (error - self.error_prev) / dt * self.kd | |
| self.error_prev = error | |
| return p + i + d | |
| # 初始化一个PID控制器 | |
| height_pid = PID(kp=0.1, ki=0.0, kd=0.2) | |
| game_prev_time = space_center.ut # 记录上一帧时间 | |
| while (True): | |
| time.sleep(0.001) | |
| ut = space_center.ut # 获取游戏内时间 | |
| game_delta_time = ut - game_prev_time # 计算上一帧到这一帧消耗的时间 | |
| if game_delta_time < 0.019: # 如果游戏中还没有经过一个物理帧，不进行计算 | |
| continue | |
| # 在这里写控制代码 | |
| height = vessel.position(target_frame)[0] | |
| balanced_throttle = vessel.mass * 9.81 / vessel.available_thrust | |
| error = 100 - height | |
| vessel.control.throttle = balanced_throttle + height_pid.update(error, game_delta_time) | |
| # 打印出dt和error | |
| print('dt=%.3f, error=%.2f ' % (game_delta_time, error), end='\r') | |
| game_prev_time = ut # 更新上一帧时间记录 | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment