mirror of
https://gitlab.ewi.tudelft.nl/ee2l1/2025-2026/A.K.03.git
synced 2025-12-12 16:00:56 +01:00
116 lines
4.5 KiB
Python
116 lines
4.5 KiB
Python
import time
|
|
import numpy as np
|
|
|
|
# try:
|
|
# from KITT_Simulator.shared_state import SharedState
|
|
# except ImportError:
|
|
# from shared_state import SharedState
|
|
# else:
|
|
# raise ImportError("Could not import the required modules.")
|
|
|
|
class Dynamics:
|
|
def __init__(self, start_state):
|
|
"""Initializes the Dynamics class with the start state and parameters."""
|
|
self.start_time = time.time()
|
|
self.state = start_state
|
|
|
|
self.mass = 5.6 # Mass of the car in kg
|
|
self.L = 1 # Length of the car in cm
|
|
self.T = 1 # Track width in cm
|
|
|
|
self.F = {}
|
|
self.b = 5
|
|
self.R = {}
|
|
|
|
try:
|
|
with open("simulator_data/motor_parameters.txt", "r") as f:
|
|
for line in f:
|
|
line = line.split(sep=",")
|
|
self.F[int(line[0])] = float(line[1])
|
|
with open("simulator_data/servo_parameters.txt", "r") as f:
|
|
for line in f:
|
|
line = line.split(sep=",")
|
|
self.R[int(line[0])] = float(line[1])
|
|
except(FileNotFoundError):
|
|
with open("KITT_Simulator/simulator_data/motor_parameters.txt", "r") as f:
|
|
for line in f:
|
|
line = line.split(sep=",")
|
|
self.F[int(line[0])] = float(line[1])
|
|
with open("KITT_Simulator/simulator_data/servo_parameters.txt", "r") as f:
|
|
for line in f:
|
|
line = line.split(sep=",")
|
|
self.R[int(line[0])] = float(line[1])
|
|
else:
|
|
raise FileNotFoundError("Could not find the motor and servo parameters.")
|
|
|
|
self.motor_command = 150
|
|
self.servo_command = 150
|
|
|
|
self.update_state()
|
|
|
|
def update_state(self):
|
|
"""Updates the state of the car based on the motor and servo commands."""
|
|
# Get the parameters for the motor and servo commands
|
|
F, b, R, direction = self.map_command(self.motor_command, self.servo_command) # F = motor force, b = drag coefficient, R = turn radius, direction = left or right turn
|
|
|
|
# Calculate time since last update
|
|
update_time = time.time()
|
|
delta_t = update_time - self.state.last_update
|
|
self.state.last_update = update_time
|
|
|
|
# Calculate how far the car moves during delta_t
|
|
arc_length = self.calculate_arc_length(F, b, delta_t)
|
|
self.arc_length = arc_length
|
|
self.update_position(arc_length, R, direction)
|
|
|
|
self.motor_command = self.state.motor_command
|
|
self.servo_command = self.state.servo_command
|
|
|
|
def map_command(self, motor_command, servo_command):
|
|
"""Converts a command into motor force and turn radius."""
|
|
try:
|
|
motor_force = self.F.get(motor_command)
|
|
turn_radius = self.R.get(servo_command)
|
|
except:
|
|
motor_force = 0
|
|
turn_radius = 0
|
|
direction = "Left" if float(servo_command) > 150 else "Right"
|
|
return motor_force, self.b, turn_radius, direction
|
|
|
|
def calculate_arc_length(self, F, b, delta_t):
|
|
"""Calculates the arc length the car moves during delta_t."""
|
|
arc_length = (F)/(b)*delta_t + (F*self.mass-self.mass*b*self.state.v)/(b**2)*np.exp(-(b)/(self.mass)*delta_t) - (F*self.mass-self.mass*b*self.state.v)/(b**2)
|
|
# Calculate the new velocity of the car
|
|
self.state.v = (F)/(b) + (self.state.v - (F)/(b))*(np.exp(-(b)/(self.mass)*delta_t))
|
|
return arc_length
|
|
|
|
def update_position(self, arc_length, R, direction):
|
|
"""Updates the position of the car based on the arc length and turn radius."""
|
|
x0 = self.state.x
|
|
y0 = self.state.y
|
|
theta0 = self.state.theta
|
|
|
|
if R != 0:
|
|
arc_angle = arc_length / (R) # angle between start and end of path along the circle
|
|
delta_x = R - R*np.cos(arc_angle)
|
|
delta_y = R*np.sin(arc_angle)
|
|
if direction == 'Left':
|
|
delta_x = -delta_x
|
|
arc_angle = -arc_angle
|
|
x = x0 + delta_x*np.cos(theta0 - np.pi/2) + delta_y*np.cos(theta0)
|
|
y = y0 + delta_x*np.sin(theta0 - np.pi/2) + delta_y*np.sin(theta0)
|
|
theta = theta0 - arc_angle
|
|
else:
|
|
x = x0 + arc_length*np.cos(theta0)
|
|
y = y0 + arc_length*np.sin(theta0)
|
|
theta = theta0
|
|
if theta < -np.pi:
|
|
theta += 2*np.pi
|
|
if theta > np.pi:
|
|
theta -= 2*np.pi
|
|
|
|
# Update the state
|
|
self.state.x = x
|
|
self.state.y = y
|
|
self.state.theta = theta
|