class CarPID(Car):
def __init__(self, iterations: int, headway: float):
super().__init__(iterations, headway)
self.pid = PID()
self.pid.out_min = -1
self.pid.out_max = 1
self.motor = Motor(4, 8)
def tick(self, frame: int, time_passed: float):
self.drive(frame, time_passed)
super().tick(frame, time_passed)
# For override reasons an extra method
def drive(self, frame: int, time_passed: float):
self.pid.compute(self.v[frame - 1])
self.a[frame] = self.motor.get_acceleration(self.v[frame - 1],
self.pid.output)
def create_lateral_control():
Kp = 0.2
Kd = 3.0 * Kp
lateral_acc = 0.5 * surface_gravity
control = CascadeControl(
PID(
Kp=Kp,
Kd=Kd,
sample_time=tick,
),
PID(
Kp=1.0,
Kd=0.0,
output_limits=(-lateral_acc, lateral_acc),
sample_time=tick,
))
control.controls[0] = lambda *x: 0
return control
def __init__(self, iterations: int, connection: Connection,
headway: float):
super().__init__(iterations, headway)
self.connection = connection
self.todo = np.full(iterations, None, dtype=PacketTodo)
self.last_packet: PacketTodo = None
self.pid.set_tuning(0.25, 0.0, 0.0, 1 * MILLISECOND)
self.pid.out_max = 0.75 # like leader
self.motor.max_acceleration = 4
self.motor.max_break = 8
self.last_todo = 0
# Second PID for the data transmission
self.pid2 = PID()
self.pid2.set_tuning(1.0, 0.0, 0.0, 1 * MILLISECOND)
self.pid2.out_min = -1
self.pid2.out_max = 1
def __init__(self, iterations: int, headway: float):
super().__init__(iterations, headway)
self.pid = PID()
self.pid.out_min = -1
self.pid.out_max = 1
self.motor = Motor(4, 8)
def execute(self):
ksc = self.conn.space_center
vessel = self.vessel
vessel.control.rcs = True
rf = vessel.surface_reference_frame # x: up, y: north, z: east
ap = vessel.auto_pilot
ap.engage()
ap.reference_frame = rf
body = vessel.orbit.body
surface_gravity = body.surface_gravity
tick = 0.01
r = body.equatorial_radius
degrees_per_meter = 180 / r / math.pi
lat0 = self.latitude() + 50 * degrees_per_meter
lat1 = lat0 + 10 * degrees_per_meter
lon0 = self.longitude()
lon1 = lon0 + 20 * degrees_per_meter
target = lambda: None
if not self.TWR():
vessel.control.activate_next_stage()
def create_lateral_control():
Kp = 0.2
Kd = 3.0 * Kp
lateral_acc = 0.5 * surface_gravity
control = CascadeControl(
PID(
Kp=Kp,
Kd=Kd,
sample_time=tick,
),
PID(
Kp=1.0,
Kd=0.0,
output_limits=(-lateral_acc, lateral_acc),
sample_time=tick,
))
control.controls[0] = lambda *x: 0
return control
controllers = [
PID(1.0, 0.0, 1.5), # up/down
create_lateral_control(), # N/S
create_lateral_control(), # E/W
]
error = [[], [], []]
def update_throttle():
delta_h = target.altitude - self.surface_altitude()
twr = self.TWR()
err = delta_h
acc = surface_gravity + controllers[0](-err)
throttle = acc / twr
error[0].append(err)
return throttle
def force_vector():
p = -np.array(target.pointer)
v = np.array(target.relative_velocity)
scale = min(1.0, max(target.distance, 2.0 * target.speed))
scale = 1.0
vec = np.array([
surface_gravity, scale * controllers[1](p[1], v[1]),
scale * controllers[2](p[2], v[2])
])
error[1].append(ap.error)
error[2].append(v[1])
return vec
target.altitude = 50.0
target.latitude = self.latitude()
target.longitude = self.longitude()
while vessel.met < 8.0:
vessel.control.throttle = update_throttle()
ap.target_direction = (1, 0, 0)
wait(tick)
while not self.abort():
if self.brakes():
target.altitude = 100.0
target.latitude = lat1
target.longitude = lon1
else:
target.altitude = 50
target.latitude = lat0
target.longitude = lon0
target.position = body.position_at_altitude(
target.latitude, target.longitude, self.mean_altitude(),
body.reference_frame)
target.reference_frame = body.reference_frame
target.pointer = ksc.transform_position(target.position,
target.reference_frame, rf)
target.distance = np.linalg.norm(target.pointer)
target.relative_velocity = -np.array(
ksc.transform_velocity(target.position,
(0, 0, 0), target.reference_frame, rf))
target.speed = np.linalg.norm(target.relative_velocity)
notify(target.distance, target.speed)
F = force_vector()
ap.target_direction = F
scaled_force = np.linalg.norm(F) / surface_gravity
vessel.control.throttle = update_throttle() * scaled_force
wait(tick)
vessel.control.throttle = 0
err1 = np.array(error[1])
err2 = np.array(error[2])
t = np.arange(len(err1))
plt.plot(t, np.zeros_like(t), label='zero')
plt.plot(t, np.log(np.abs(err1) + 1.0), label='log err1')
plt.plot(t, err1, label='raw err1')
plt.plot(t, np.log(np.abs(err2) + 1.0), label='log err2')
plt.plot(t, err2, label='raw err2')
plt.xlabel('time')
plt.ylabel('error')
plt.legend()
plt.show()
wait()
class FollowerControlData(CarPID):
def __init__(self, iterations: int, connection: Connection,
headway: float):
super().__init__(iterations, headway)
self.connection = connection
self.todo = np.full(iterations, None, dtype=PacketTodo)
self.last_packet: PacketTodo = None
self.pid.set_tuning(0.25, 0.0, 0.0, 1 * MILLISECOND)
self.pid.out_max = 0.75 # like leader
self.motor.max_acceleration = 4
self.motor.max_break = 8
self.last_todo = 0
# Second PID for the data transmission
self.pid2 = PID()
self.pid2.set_tuning(1.0, 0.0, 0.0, 1 * MILLISECOND)
self.pid2.out_min = -1
self.pid2.out_max = 1
def receive(self, packets):
found: Packet = self.last_packet
for packet in packets:
if isinstance(packet, PacketAll):
if found is None or found.timestamp < packet.timestamp:
found = packet
self.last_packet = found
def tick(self, frame: int, time_passed: float):
packets = self.connection.read()
for packet in packets:
if isinstance(packet, PacketTodo):
if packet.timestamp < frame:
self.pid.setpoint = packet.v
else:
self.todo[packet.timestamp] = packet
self.receive(packets)
if self.todo[frame]:
self.pid.setpoint = self.todo[frame].v
self.last_todo = frame
if self.last_packet is not None:
p = self.last_packet
self.pid2.setpoint = calc_ref_v(
self.headway, self.x[frame - 1], p.a, p.v, p.x,
(frame - p.timestamp - 1) * MILLISECOND)
super().tick(frame, time_passed)
def drive(self, frame: int, time_passed: float):
# mix both PIDs
self.pid.compute(self.v[frame - 1])
self.pid2.compute(self.v[frame - 1])
sum = 0
sum += self.motor.get_acceleration(self.v[frame - 1], self.pid.output)
sum += self.motor.get_acceleration(self.v[frame - 1], self.pid2.output)
if sum > 4:
sum = 4
elif sum < -8:
sum = -8
self.a[frame] = sum
def get_name(self):
return "Future + Data"
def controlled_descent(self, target=None):
LANDED = self.conn.space_center.VesselSituation.landed
SPLASHED = self.conn.space_center.VesselSituation.splashed
vessel = self.vessel
control = vessel.control
control.trottle = 0
control.brakes = True
control.gear = True
control.lights = True
control.solar_panels = False
control.sas = False
lat, lon = target
# rf = vessel.surface_reference_frame
rf = vessel.orbit.body.reference_frame
flight = vessel.flight(rf)
_pos = self.conn.add_stream(getattr, flight, 'center_of_mass')
pos = lambda: np.array(_pos())
target_pos = lambda: np.array(
vessel.orbit.body.surface_position(lat, lon, rf))
_surface_velocity = self.conn.add_stream(getattr, flight, 'velocity')
surface_velocity = lambda: -np.array(_surface_velocity())
ap = vessel.auto_pilot
ap.engage()
ap.reference_frame = rf
ap.target_direction = surface_velocity()
landing_speed = 0.5
v_target = lambda: 0.2 * self.surface_altitude() + landing_speed
base_throttle = lambda: self.surface_gravity / self.TWR()
effective_twr = self.TWR() - self.surface_gravity
notify('Waiting for landing burn...')
while self.surface_altitude(
) > self.stopping_distance() + 2.0 * wait.interval * self.speed():
ap.target_direction = surface_velocity()
wait()
control.throttle = 1.0
notify('Landing burn!')
notify(f'altitude: {self.surface_altitude()}')
while self.speed() > 5 * effective_twr:
d = surface_velocity()
ap.target_direction = d / np.linalg.norm(d)
wait()
notify(f'effective_twr: {effective_twr}')
pid = PID(0.2,
0.0,
0.0,
sample_time=wait.interval,
output_limits=(-1.0, 1.0))
notify('Entering controlled descent...')
while self.situation() not in [LANDED, SPLASHED]:
altitude = self.surface_altitude()
base = base_throttle()
target = v_target()
actual = -self.vertical_speed()
pid_input = target - actual
pid_output = pid(pid_input)
# notify({
# 'altitude': altitude,
# 'target': target,
# 'actual': actual,
# 'input': pid_input,
# 'output': pid_output,
# 'pos': pos(),
# 'target_pos', target_pos,
# })
notify(
f'altitude:{altitude:.2f}, target:{target:.2f}, actual:{actual:.2f}, input:{pid_input:.2f}, output:{pid_output:.2f}, pos: {pos()}, target: {target_pos()}'
)
control.throttle = base + pid_output
if self.surface_altitude() < 25:
ap.target_direction = pos()
else:
d = surface_velocity()
ap.target_direction = d
wait()
control.throttle = 0
ap.disengage()
notify('done')