97 lines
2.5 KiB
Python
97 lines
2.5 KiB
Python
import math
|
|
import numpy as np
|
|
|
|
try:
|
|
from lib import RTLFWriter
|
|
except ImportError:
|
|
class RTLFWriter:
|
|
def __init__(self, *args, **kwargs): pass
|
|
def __enter__(self): return self
|
|
def __exit__(self, *args): pass
|
|
def write_record(self, *args): pass
|
|
|
|
|
|
def simulate_closed_circuit(filename, car_id=7):
|
|
track_name = "Closed Circuit GP"
|
|
|
|
# Real-world reference point (Silverstone-ish)
|
|
base_lat = 52.0733
|
|
base_lon = -1.0147
|
|
|
|
dt = 0.05
|
|
lap_time = 95.0
|
|
samples = int(lap_time / dt)
|
|
|
|
# Track size in METERS (realistic)
|
|
a = 1200.0 # main straight length scale
|
|
b = 800.0
|
|
|
|
theta = np.linspace(0, 2 * math.pi, samples)
|
|
|
|
# Track shape (meters)
|
|
x_m = (
|
|
a * np.cos(theta)
|
|
+ 220 * np.cos(3 * theta)
|
|
+ 90 * np.sin(5 * theta)
|
|
)
|
|
y_m = (
|
|
b * np.sin(theta)
|
|
+ 170 * np.sin(2 * theta)
|
|
- 70 * np.cos(4 * theta)
|
|
)
|
|
|
|
# Convert meters → degrees
|
|
meters_per_deg_lat = 111_320.0
|
|
meters_per_deg_lon = meters_per_deg_lat * math.cos(math.radians(base_lat))
|
|
|
|
lat = base_lat + (y_m / meters_per_deg_lat)
|
|
lon = base_lon + (x_m / meters_per_deg_lon)
|
|
|
|
# Derivatives (still done in meters for physics)
|
|
dx = np.gradient(x_m, dt)
|
|
dy = np.gradient(y_m, dt)
|
|
ddx = np.gradient(dx, dt)
|
|
ddy = np.gradient(dy, dt)
|
|
|
|
curvature = np.abs(dx * ddy - dy * ddx) / np.power(dx*dx + dy*dy, 1.5)
|
|
curvature = np.nan_to_num(curvature)
|
|
|
|
# Speed profile
|
|
max_speed = 82.0 # ~295 km/h
|
|
min_speed = 30.0
|
|
speed = max_speed - 850 * curvature
|
|
speed = np.clip(speed, min_speed, max_speed)
|
|
|
|
# Acceleration
|
|
heading_norm = np.hypot(dx, dy)
|
|
ax = np.gradient(speed * dx / heading_norm, dt)
|
|
ay = np.gradient(speed * dy / heading_norm, dt)
|
|
az = np.zeros_like(ax)
|
|
|
|
# Forces
|
|
mass = 1250.0
|
|
fx = mass * ax
|
|
fy = mass * ay
|
|
fz = np.ones_like(fx) * mass * 9.81
|
|
|
|
with RTLFWriter(filename, car_id, track_name) as writer:
|
|
t = 0.0
|
|
for i in range(samples):
|
|
writer.write_record(
|
|
t,
|
|
float(lon[i]),
|
|
float(lat[i]),
|
|
float(fx[i]),
|
|
float(fy[i]),
|
|
float(fz[i]),
|
|
float(ax[i]),
|
|
float(ay[i]),
|
|
float(az[i]),
|
|
float(speed[i])
|
|
)
|
|
t += dt
|
|
|
|
|
|
if __name__ == "__main__":
|
|
simulate_closed_circuit("closed_loop.rtl")
|
|
print("Closed circuit simulation complete.") |