import numpy as np
from ...constants import *
from ..models import VehicleModel
[docs]
class Motor:
def __init__(
self,
vehicle_model: VehicleModel,
):
self.vehicle_model = vehicle_model
self.params = vehicle_model.pwrtn.motor
self.efficiency_map = None # will contain polyval coefficients
self.rpm_norm_params = None # needed for numerical stability in polyfit
self.torque_norm_params = None # as above
if self.params.use_efficiency_lut:
# Fill out self.efficiency_map
self.fit_efficiency_data(self.params.efficiency_data, 4, 4)
# Note: if the shape is especially weird, may need to increase
# fit degree. Keep even degrees. Keep degree as low as possible
# to limit evaluation complexity
[docs]
def get_torque_at_rpm(self, motor_rpm: float) -> float:
"""
Get motor torque at a given RPM using piecewise function.
The torque curve has three regions:
1. Flat region (0 to fw_rpm): constant max torque
2. Linear decay (fw_rpm to max_rpm): torque decreases linearly
3. Zero region (above max_rpm): no torque
Parameters
----------
motor_rpm : float
Motor RPM
Returns
-------
float
Motor torque at the given RPM (Nm)
"""
if motor_rpm < self.params.fw_rpm:
# Pre-field weakening: constant max torque
return self.params.max_torque
elif motor_rpm <= self.params.max_rpm:
# Field weakening: linear decrease from max_torque to fw_torque
# Handle edge case where fw_rpm == max_rpm (no field weakening region)
if self.params.max_rpm == self.params.fw_rpm:
return self.params.max_torque
slope = (self.params.max_torque - self.params.fw_torque) / (
self.params.max_rpm - self.params.fw_rpm
)
return self.params.max_torque - slope * (motor_rpm - self.params.fw_rpm)
else:
# Above max RPM: no torque
return 0.0
[docs]
def calculate_max_ground_force_and_motor_power(self, v, ratio):
"""
Calculates force and power output by the motor.
Parameters
----------
v : float
Velocity of the vehicle (m/s)
ratio : float
Gear ratio
Returns
-------
ground_force : float
Force at the ground (N)
motor_power : float
Motor power output (W)
"""
motor_rpm = self.v_to_rpm(v, ratio)
# Get motor torque from piecewise function
motor_torque = self.get_torque_at_rpm(motor_rpm)
# Clamp motor torque with power limit
motor_torque = min(
motor_torque,
self.params.pow_lim / max(rpm_to_rad_s(motor_rpm), 1e-6),
)
# Convert to wheel torque
wheel_torque = (
motor_torque
* ratio
* self.powertrain_efficiency(rpm=motor_rpm, torque=motor_torque)
) # torque_out = torque_in * ratio * efficiency
ground_force = wheel_torque / in_to_m(self.vehicle_model.tires.tire_radius)
# Calculate force and power
return ground_force, motor_torque * rpm_to_rad_s(motor_rpm)
[docs]
def v_to_rpm(self, v, ratio):
tire_cir = circumference(
in_to_m(self.vehicle_model.tires.tire_radius)
) # meters
# Calculate index of torque curve given current velocity
rot_per_sec = v / tire_cir
wheel_rpm = rot_per_sec * 60
return wheel_rpm * ratio # rpm_out = rpm_in / ratio
[docs]
def powertrain_efficiency(self, rpm=-1, torque=-1):
"""
Calculates the system efficiency of the powertrain.
Uses system efficiency if it is set, otherwise uses the product of the individual efficiencies.
If velocity (v, m/s) is provided and a motor efficiency map exists, will pull that.
Returns
-------
float
System efficiency of the powertrain
"""
# TODO: DIFF EFFICIENCY is set to 100% for now
if self.params.efficiency_method == "sys":
return self.params.system_efficiency
elif self.params.efficiency_method == "indiv":
partial_eff = (
self.params.moc_efficiency
* self.params.chain_efficiency
* self.params.diff_efficiency
)
if self.params.use_efficiency_lut and rpm >= 0 and torque >= 0:
return partial_eff * self.eval_efficiency_map(rpm, torque)
# NOTE: technically this isn't completely accurate, but it's
# better than nothing. irl should be iterated ~3x if at pow lim
return self.params.motor_efficiency * partial_eff
else:
raise ValueError(
"Invalid efficiency method. Must be 'sys' or 'indiv'. Got: ",
self.params.efficiency_method,
)
[docs]
def fit_efficiency_data(self, points, degv, degt):
"""
Fit a 2D polynomial z = f(x, y) to data points [[x, y, z], ...]
Returns coefficient matrix C[i,j] for x^i y^j
"""
pts = np.asarray(points, float)
rpm, torque, z = pts.T
# Store normalization parameters
self.rpm_norm_params = (rpm.min(), rpm.max())
self.torque_norm_params = (torque.min(), torque.max())
# Normalize to [0, 1] for numerical stability
x = (rpm - self.rpm_norm_params[0]) / (
self.rpm_norm_params[1] - self.rpm_norm_params[0]
)
y = (torque - self.torque_norm_params[0]) / (
self.torque_norm_params[1] - self.torque_norm_params[0]
)
# Build tensor-product basis
terms = [(x**i) * (y**j) for i in range(degv + 1) for j in range(degt + 1)]
A = np.column_stack(terms)
# Solve least squares
coeffs, *_ = np.linalg.lstsq(A, z, rcond=None)
self.efficiency_map = coeffs.reshape(degv + 1, degt + 1)
return self.efficiency_map
[docs]
def eval_efficiency_map(self, rpm, torque):
"""
Evaluate efficiency map with edge clamping and distance penalty for OOB.
"""
if self.efficiency_map is None:
print("WARNING: EFFICIENCY MAP IS EVALUATING BUT DOES NOT EXIST")
return self.params.motor_efficiency
C = self.efficiency_map
rpm_min, rpm_max = self.rpm_norm_params
torque_min, torque_max = self.torque_norm_params
rpm_range = rpm_max - rpm_min
torque_range = torque_max - torque_min
# Distance outside bounds (normalized)
rpm_dist = max(rpm_min - rpm, rpm - rpm_max, 0) / rpm_range
torque_dist = max(torque_min - torque, torque - torque_max, 0) / torque_range
total_dist = (rpm_dist**2 + torque_dist**2) ** 0.5
# Clamp inputs to valid range
rpm_clamped = max(rpm_min, min(rpm_max, rpm))
torque_clamped = max(torque_min, min(torque_max, torque))
# Normalize clamped inputs
x = (rpm_clamped - rpm_min) / rpm_range
y = (torque_clamped - torque_min) / torque_range
# Horner's method evaluation
degx = C.shape[0] - 1
degy = C.shape[1] - 1
tmp = [0.0] * (degx + 1)
for i in range(degx + 1):
eff = C[i, degy]
for j in range(degy - 1, -1, -1):
eff = eff * y + C[i, j]
tmp[i] = eff
eff = tmp[degx]
for i in range(degx - 1, -1, -1):
eff = eff * x + tmp[i]
# Apply distance penalty (exponential decay)
if total_dist > 0:
penalty = 2.71828 ** (-total_dist * 0.3) # ~0.74 at dist=1.0
eff = eff * penalty
# Floor at 66%, cap at 99%
return max(0.66, min(0.99, eff))
[docs]
class Powertrain:
def __init__(
self,
vehicle_model: VehicleModel,
):
self.vehicle_model = vehicle_model
self.params = vehicle_model.pwrtn
# Construct Motor object
self.motor = Motor(vehicle_model=vehicle_model)