diff --git a/Data/BatemoDataAnalysis.py b/Data/BatemoDataAnalysis.py new file mode 100644 index 0000000..ee4565b --- /dev/null +++ b/Data/BatemoDataAnalysis.py @@ -0,0 +1,34 @@ +import scipy +import scipy.integrate + +batemoData = scipy.io.loadmat("C:/Users/Goob/Downloads/OneDrive_2026-01-07/Batemo Sponsorship/Batemo Cell Data Molicel INR18650P30B/Batemo Cell Data Package/Molicel_INR18650P30B_measurement.mat", + simplify_cells=True) + + +header = batemoData["__header__"] +version = batemoData["__version__"] +globals = batemoData["__globals__"] ## Nothing +print(f"header = {header}") +print(f"version = {version}") +measurement = batemoData["measurement"] +firstLayerMeta = measurement['meta'] +Fu = measurement['fu'] + +## DCC (Discharge) +## CHC (Charging) +## DCP (Discharge Pulse) +## CHP (Charge Pulse) +## PRO (Profile Measurement) + + +# Each Has + # name + # T_amb (Ambient Temperature) + # t (Time Seconds) + # I (Current) + # V (Voltage) + # T_surf (Surface temperature, nominally 1xN but may be 2xN?) + + +for i in Fu['DCC']: + print(i['name']) \ No newline at end of file diff --git a/Data/emeterDecode.py b/Data/emeterDecode.py index 9f93c99..3743894 100644 --- a/Data/emeterDecode.py +++ b/Data/emeterDecode.py @@ -1,5 +1,6 @@ from nptdms import TdmsFile import polars as pl +import numpy as np import matplotlib.pyplot as plt from Data.FSLib.IntegralsAndDerivatives import * from Data.FSLib.AnalysisFunctions import * @@ -9,10 +10,10 @@ autoxDaniel12File = "FS-3/compEmeterData/autoxDaniel12.tdms" accel1 = "FS-3/compEmeterData/216_univ-of-calif---santa-cruz-_250620-203307_ ACCEL-EV.tdms" accel2 = "FS-3/compEmeterData/216_univ-of-calif---santa-cruz-_250620-205609_ ACCEL-EV.tdms" -endur1 = "FS-3/compEmeterData/216_univ-of-calif---santa-cruz-_250621-154731_ ENDUR-EV.tdms" -endur2 = "FS-3/compEmeterData/216_univ-of-calif---santa-cruz-_250621-160530_ ENDUR-EV.tdms" +endur1 = "../fs-data/FS-3/compEmeterData/216_univ-of-calif---santa-cruz-_250621-154731_ ENDUR-EV.tdms" +endur2 = "../fs-data/FS-3/compEmeterData/216_univ-of-calif---santa-cruz-_250621-160530_ ENDUR-EV.tdms" -dfLaptimes = pl.read_csv("FS-3/compLapTimes.csv") +dfLaptimes = pl.read_csv("../fs-data/FS-3/compLapTimes.csv") firstHalf = dfLaptimes.filter(pl.col("Lap") < 12)["Time"].sum() secondHalf = dfLaptimes.filter(pl.col("Lap") > 11)["Time"].sum() @@ -93,7 +94,37 @@ def fileTodf(path): pl.Series(arr).cast(pl.Int64).alias("Lap") ) +l = [] +for i in np.unique(dfendur1["Lap"]): + # plt.plot(dfendur1.filter(pl.col("Lap") == i)[I]) + l.append(dfendur1.filter(pl.col("Lap") == i)[I]) +plt.show() + +shortest = min([len(x) for x in l]) +l2 = [x[:shortest].alias(f"Current_Lap_{i}") for i, x in enumerate(l)] +df2 = pl.DataFrame(l2) +plt.plot(df2.mean_horizontal()) +plt.show() + +from scipy.fft import fft, ifft + +f = fft(df2.mean_horizontal().to_numpy()) +# freq = np.fft.fftfreq(len(df2.mean_horizontal()), d=0.01) + +plt.plot(np.append(np.log(f[-len(f)//2:]), np.log(f[:len(f)//2]))) +plt.show() + +fFiltered = np.where(np.log(f) > 10, f, 0) +invF = ifft(fFiltered) +plt.plot(invF) +plt.plot(df2.mean_horizontal()) +plt.show() + +dfTableCurrOut = pl.DataFrame({"Current": df2.mean_horizontal().gather_every(100), "Time": np.arange(0, df2.height/100)}) +dfTableCurrOut.write_csv("endur1Curr.csv") + +df2.mean_horizontal() dfendur2 = fileTodf(endur2).filter(pl.col(t) > endur2_StartTime).filter(pl.col(t) < endur2_EndTime) diff --git a/Data/temp.py b/Data/temp.py index 587d9e0..2314ebd 100644 --- a/Data/temp.py +++ b/Data/temp.py @@ -3,8 +3,8 @@ import cantools.database as db from Data.DataDecoding_N_CorrectionScripts.dataDecodingFunctions import * -from Data.AnalysisFunctions import * -from Data.integralsAndDerivatives import * +from Data.FSLib.AnalysisFunctions import * +from Data.FSLib.IntegralsAndDerivatives import * from scipy.interpolate import CubicSpline dbcPath = "../fs-3/CANbus.dbc" @@ -70,23 +70,21 @@ etcRTDButton = "ETC_STATUS_RTD_BUTTON" etcBrakeVoltage = "ETC_STATUS_BRAKE_SENSE_VOLTAGE" -df = read("C:/Projects/FormulaSlug/fs-data/FS-3/10112025/firstDriveMCError30.parquet") -df = df.with_columns( - df["timestamp"].alias("Time") -) +df = read("C:/Projects/FormulaSlug/fs-data/FS-3/10082025/fixed_wheels_nathaniel_inv_test_w_fault.parquet") + +plt.plot(df[busV]) +plt.show() + + +df = pl.read_parquet("C:/Projects/FormulaSlug/fs-data/FS-3/10112025/firstDriveMCError30-filled-null.parquet") + +df.select([a for a in df.columns if a.startswith("ACC") and a[9] == "T"]).mean_horizontal() -df = read("C:/Projects/FormulaSlug/fs-data/FS-3/10112025/firstDriveMCError30-filled-null.parquet") -df = df.with_columns( - simpleTimeCol(df) -) fig = plt.figure() ax = fig.add_subplot(111) ax.plot(df[t], df[frT], label=frT, c="blue") -ax.plot(df[t], df[flT], label=flT, c="red") -ax.plot(df[t], df[brT], label=brT, c="orange") -ax.plot(df[t], df[blT], label=blT, c="cyan") ax.set_title("Suspension Travel during First Drive with MC Fault") ax.set_xlabel("Time") ax.set_ylabel("Suspension Travel (mm)") @@ -94,14 +92,42 @@ plt.show() -dfNullless = df.drop_nulls(subset=[frT, flT, brT, blT]) +heFR = "TELEM_FR_WHEELSPEED" +heFL = "TELEM_FL_WHEELSPEED" +heBR = "TELEM_BR_WHEELSPEED" +heBL = "TELEM_BL_WHEELSPEED" + +df = read("C:/Projects/FormulaSlug/fs-data/FS-2/Parquet/2025-03-06-BrakingTests1.parquet") +df = df.with_columns( + simpleTimeCol(df) +) -cs = CubicSpline(dfNullless[t], dfNullless[frT]) fig = plt.figure() ax = fig.add_subplot(111) -ax.scatter(dfNullless[t], cs(dfNullless[t]), label=frT, s=0.5) -ax.scatter(dfNullless[t], in_place_derive(cs(dfNullless[t])), label=f"Derived {frT}", s=0.5) +ax.plot(df[t], df[heFR], label=heFR, c="orange") +ax.plot(df[t], df[heFL], label=heFL, c="red") +ax.plot(df[t], df[heBR], label=heBR, c="blue") +ax.plot(df[t], df[heBL], label=heBL, c="cyan") +ax.set_title("HE Sensors") +ax.set_xlabel("Time") +ax.set_ylabel("idk lol, 0-32767") ax.legend() plt.show() + + +fig = plt.figure() +ax = fig.add_subplot(111) +ax1 = ax.twinx() + +ax.plot(df[t], df[heFR], label="frontRight", c="red") +ax.plot(df[t], df.select([heBL, heBR]).mean_horizontal(), label="meanBack", c="blue") +ax1.plot(df[t],df.select([heBL, heBR]).mean_horizontal() / df[heFR], label="ratio", c="green") +# ax.plot(df[t], df[rpm]*11/41, label="RPM", c="orange") ## Cursed until we have proper wheel speed calibration +ax.set_title("HE Sensors") +ax.set_xlabel("Time") +ax.set_ylabel("idk lol, 0-32767") +ax.legend() +ax1.set_ylim(0,2) +plt.show() \ No newline at end of file diff --git a/Data/temp2.py b/Data/temp2.py new file mode 100644 index 0000000..e90480f --- /dev/null +++ b/Data/temp2.py @@ -0,0 +1,95 @@ +import polars as pl +from Data.FSLib.AnalysisFunctions import * +from scipy.stats import linearregress + +rpm = "SME_TRQSPD_Speed" +lat = "VDM_GPS_Latitude" +long = "VDM_GPS_Longitude" +xA = "VDM_X_AXIS_ACCELERATION" +yA = "VDM_Y_AXIS_ACCELERATION" +speed = "VDM_GPS_SPEED" ## mph + +df1 = readValid("C:/Projects/FormulaSlug/fs-data/FS-3/08102025/08102025Endurance1_FirstHalf.parquet") ## 276,1075 +df1 = df1.insert_column(0, simpleTimeCol(df1)) +df1 = df1.filter(pl.col("Time") >= 276).filter(pl.col("Time") <= 1075) +df1 = df1.with_columns( + pl.col("Time") - pl.col("Time").min() +) +df1 = df1.insert_column(0, lapSegmentation(df1)) + +df2 = readValid("C:/Projects/FormulaSlug/fs-data/FS-3/08102025/08102025Endurance1_SecondHalf.parquet") ## 77, 862 +df2 = df2.insert_column(0, simpleTimeCol(df2)) +df2 = df2.filter(pl.col("Time") >= 77).filter(pl.col("Time") <= 862) +df2 = df2.with_columns( + pl.col("Time") - pl.col("Time").min() +) +df2 = df2.insert_column(0, lapSegmentation(df2)) +df2 = df2.with_columns( + pl.col("Lap") + df1["Lap"].max() + 1 +) + +df3 = readValid("C:/Projects/FormulaSlug/fs-data/FS-3/08172025/08172025_20_Endurance1P1.parquet") ## 72, 1120 -- Drop laps 8 and 9 +df3 = df3.insert_column(0, simpleTimeCol(df3)) +df3 = df3.filter(pl.col("Time") >= 72).filter(pl.col("Time") <= 1120) +df3 = df3.with_columns( + pl.col("Time") - pl.col("Time").min() +) +df3 = df3.insert_column(0, lapSegmentation(df3)) +df3_1 = df3.filter(pl.col("Lap") < 8) +df3_2 = df3.filter(pl.col("Lap") > 9).with_columns( + pl.col("Lap") - 2 +) +df3 = df3_1.vstack(df3_2) +df3 = df3.with_columns( + pl.col("Lap") + df2["Lap"].max() + 1 +) + +df1 = df1.select(df3.columns) +df2 = df2.select(df3.columns) + +df1f = df1._cast_all_from_to(df1, frozenset((pl.Int8, pl.Int16, pl.Int32, pl.Int64, pl.UInt8, pl.UInt16, pl.UInt32, pl.UInt64, pl.Float32)), pl.Float64) +df2f = df2._cast_all_from_to(df2, frozenset((pl.Int8, pl.Int16, pl.Int32, pl.Int64, pl.UInt8, pl.UInt16, pl.UInt32, pl.UInt64, pl.Float32)), pl.Float64) +df3f = df3._cast_all_from_to(df3, frozenset((pl.Int8, pl.Int16, pl.Int32, pl.Int64, pl.UInt8, pl.UInt16, pl.UInt32, pl.UInt64, pl.Float32)), pl.Float64) + +df = df1f.vstack(df2f).vstack(df3f) +df = df.drop("Time").insert_column(0, simpleTimeCol(df)) + +fig = plt.figure(layout="constrained") +ax1 = fig.add_subplot(111) +ax2 = ax1.twinx() +ax1.plot(df["Time"], df[rpm], label="RPM") +ax2.plot(df["Time"], df["Lap"], label="Lap") +fig.legend() +plt.show() + +for i in df["Lap"].unique(): + plt.plot(df.filter(pl.col("Lap") == i)[lat], df.filter(pl.col("Lap") == i)[long], label=f"Lap {i}") +plt.legend() +plt.show() + +# df.drop("Time").write_parquet("C:/Projects/FormulaSlug/fs-data/FS-3/PreparedData/CombinedEndurance_0810_0817_2025.parquet") +# df["Lap"].max() + +df = pl.read_parquet("C:/Projects/FormulaSlug/fs-data/FS-3/PreparedData/CombinedEndurance_0810_0817_2025.parquet") +df = df.with_columns( + (((df["ETC_STATUS_BRAKE_SENSE_VOLTAGE"]/1000)-0.33)/2.64*2000).alias("Brake_Pedal_Pressure_PSI"), + (df[xA]*9.81*df[speed]*0.44704*300/1000).alias("Braking_Power_kW") + ) + +fig = plt.figure(layout="constrained") +ax1 = fig.add_subplot(111) +ax1.scatter(df[long], df[lat], c=df[xA]*9.81*df[speed]*0.44704*300*-1/1000, cmap='viridis', s=2, alpha=0.3, label="Brake Power (kW)") ## convert mph to m/s by multiplying by 0.44704 +ax1.set_aspect('equal', adjustable='datalim') +plt.colorbar(ax1.collections[0], ax=ax1, label="Braking Power (kW)") +ax1.set_title("Track Map Colored by Braking Power (kW)") +fig.show() + +dfBraking = df.filter(pl.col("Braking_Power_kW") > 20).filter(pl.col("Brake_Pedal_Pressure_PSI") > 100) + +fig2 = plt.figure(layout="constrained") +ax2 = fig2.add_subplot(111) +ax2.scatter(dfBraking["Brake_Pedal_Pressure_PSI"], dfBraking["Braking_Power_kW"]) +ax2.set_title("Brake Pedal Pressure vs Braking Power (kW)") +ax2.set_xlabel("Brake Pedal Pressure (PSI)") +ax2.set_ylabel("Braking Power (kW)") +fig2.show() \ No newline at end of file diff --git a/Data/tempBlueMaxAnalysis.py b/Data/tempBlueMaxAnalysis.py index 65d1939..2bded34 100644 --- a/Data/tempBlueMaxAnalysis.py +++ b/Data/tempBlueMaxAnalysis.py @@ -1,8 +1,8 @@ import polars as pl import matplotlib.pyplot as plt -from Data.integralsAndDerivatives import * -from Data.fftTools import * -from Data.AnalysisFunctions import * +from Data.FSLib.IntegralsAndDerivatives import * +from Data.FSLib.fftTools import * +from Data.FSLib.AnalysisFunctions import * t = "Time" @@ -59,10 +59,136 @@ file2 = "../fs-data/FS-3/11222025/11222025_19.parquet" file3 = "../fs-data/FS-3/11222025/11222025_20.parquet" file4 = "../fs-data/FS-3/11222025/11222025_21.parquet" -file5 = "../fs-data/FS-3/11222025/11222025_23.parquet" +file5 = "../fs-data/FS-3/11222025/11222025_22.parquet" +file6 = "../fs-data/FS-3/11222025/11222025_23.parquet" -df = read(file1).vstack(read(file2)).vstack(read(file3)).vstack(read(file4)).vstack(read(file5)) +df = read(file1).vstack(read(file2)).vstack(read(file3)).vstack(read(file4)).vstack(read(file5)).vstack(read(file6)) df.insert_column(0, simpleTimeCol(df)) -basicView(df, cellVoltages=False, tempsInsteadOfVoltages=True) \ No newline at end of file +basicView(df.filter(pl.col(t) > 1200).filter(pl.col(t) < 1350), cellVoltages=False, tempsInsteadOfVoltages=False, faults=True) +mcErrorView(df) + +dfNoRegen = read(file1).vstack(read(file2)).vstack(read(file3)).vstack(read(file4)) +dfNoRegen = dfNoRegen.insert_column(0, simpleTimeCol(dfNoRegen)) +dfWeakRegen = read(file5) +dfWeakRegen = dfWeakRegen.insert_column(0, simpleTimeCol(dfWeakRegen)) +dfStrongRegen = read(file6) +dfStrongRegen = dfStrongRegen.insert_column(0, simpleTimeCol(dfStrongRegen)) + +fig = plt.figure(layout="constrained") +ax = fig.add_subplot(111) +ax.scatter(dfNoRegen[pedalTravel], dfNoRegen[torqueDemand], s=1, label="No Regen") +ax.scatter(dfWeakRegen[pedalTravel], dfWeakRegen[torqueDemand], s=1, label="Weak Regen") +ax.scatter(dfStrongRegen[pedalTravel], dfStrongRegen[torqueDemand], s=1, label="Strong Regen") +ax.legend() +ax.set_xlabel("Pedal Travel (%)") +ax.set_ylabel("Torque Demand (Nm)") +plt.suptitle("Regen Effect on Torque Demand vs Pedal Travel") +plt.show() + +fig = plt.figure(layout="constrained") +ax = fig.add_subplot(111) +ax.scatter(dfWeakRegen[pedalTravel], dfWeakRegen[busC], s=1, label="Weak Regen", alpha=0.2) +ax.scatter(dfStrongRegen[pedalTravel], dfStrongRegen[busC], s=1, label="Strong Regen", alpha=0.2) +ax.legend() +ax.set_xlabel("Pedal Travel (%)") +ax.set_ylabel("Bus Current (A)") +plt.suptitle("Regen Effect on Bus Current vs Pedal Travel") +plt.show() + +file7 = "../fs-data/FS-3/11222025/11222025_11.parquet" +file8 = "../fs-data/FS-3/11222025/11222025_12.parquet" +file9 = "../fs-data/FS-3/11222025/11222025_13.parquet" +file10 = "../fs-data/FS-3/11222025/11222025_14.parquet" + +dfEarlyRegen = read(file7).vstack(read(file8)).vstack(read(file9)).vstack(read(file10)) +dfEarlyRegen = dfEarlyRegen.insert_column(0, simpleTimeCol(dfEarlyRegen)) + +basicView(dfEarlyRegen, title="Early Regen Testing", cellVoltages=False, tempsInsteadOfVoltages=True, faults=True) + +fig = plt.figure(layout="constrained") +ax = fig.add_subplot(111) +ax.scatter(dfEarlyRegen[pedalTravel], dfEarlyRegen[busC], s=1, label="Early Regen", alpha=0.2) +ax.legend() +ax.set_xlabel("Pedal Travel (%)") +ax.set_ylabel("Bus Current (A)") +plt.suptitle("Regen Effect on Bus Current vs Pedal Travel") +plt.show() + +dfWeakNeg = dfWeakRegen.filter(pl.col(busC) < 0) +dfWeakPos = dfWeakRegen.filter(pl.col(busC) > 0) + +plt.plot(dfWeakNeg[t], in_place_integrate(dfWeakNeg[busC]*dfWeakNeg[busV], dt=60/5035)) +plt.plot(dfWeakPos[t], in_place_integrate(dfWeakPos[busC]*dfWeakPos[busV], dt=60/5035)) +plt.show() + +np.min(in_place_integrate(dfWeakNeg[busC]*dfWeakNeg[busV], dt=60/5035))/np.max(in_place_integrate(dfWeakPos[busC]*dfWeakPos[busV], dt=60/5035))*-1 + +fig = plt.figure(layout="constrained") +ax = fig.add_subplot(111) +# ax.plot(df[t], df["ETC_STATUS_HE1"]/3300, label="HE1") +# ax.plot(df[t], df["ETC_STATUS_HE2"]/3300, label="HE2") + +# ax.plot(df[t], df["ETC_STATUS_HE1"]/3300 - df["ETC_STATUS_HE2"]/3300, label="diffHE") +ax.plot(df[t], df["ACC_STATUS_PRECHARGE_DONE"].cast(pl.Int32)*60, label="PCH Done") +# ax.plot(df[t], df[rpm]) +ax.plot(df[t], df[busV], label="Bus V") +ax.plot(df[t], df["ACC_STATUS_SHUTDOWN_STATE"].cast(pl.Int32)*50, label="Shutdown State") +ax.plot(df[t], df["ACC_STATUS_GLV_VOLTAGE"]/1000, label="GLV V") +ax.plot(df[t], df["ETC_STATUS_RTD"].cast(pl.Int32)*40, label="RTD") + +ax.set_xlabel("Time (s)") +# ax.set_ylabel("Hall Effect Travel") +# plt.suptitle("Hall Effect Sensor Travel during BlueMax Testing") +ax.legend() +plt.show() + +## Graphs to make + +# 1. Wheel speed vs traction control +# 2. Regen Bit swapping a lot + Regen Mapping +# 3. Regen Power usage +# 4. Potential MC error due to ACC Board not reading shutdown properly +# 5. Tray Temp Sensors +# 6. ACC Weird Temp data +# 7. ACC Cell Temp Distribution +# 8. ACC Cell Voltage Distribution + +dfa = df.filter(pl.col(t) < 825).filter(pl.col(t) > 815) + +fig = plt.figure(layout="constrained") +ax1 = fig.add_subplot(211) +ax2 = fig.add_subplot(212) + +ax1.plot(dfa[t], dfa[heFL], label="FL Wheel Speed") +ax1.plot(dfa[t], dfa[heFR], label="FR Wheel Speed") +ax1.plot(dfa[t], dfa[heBL], label="BL Wheel Speed") +ax1.plot(dfa[t], dfa[heBR], label="BR Wheel Speed") + +ax1.set_ylabel("Wheel Speed (rpm)") + +ax2.plot(dfa[t], dfa[pedalTravel]/dfa[torqueDemand], label="Traction Control Ratio") +ax2.set_xlabel("Time (s)") +ax1.legend() +fig.show() + +fig = plt.figure(layout="constrained") +ax = fig.add_subplot(111) +ax.plot(df[t], df[busC]*df[busV], label="Regen Power") +ax.set_xlabel("Time (s)") +ax.set_ylabel("Power (W)") +plt.suptitle("Regen Power during BlueMax Testing") +fig.show() + +fig = plt.figure(layout="constrained") +ax1 = fig.add_subplot(211) +ax2 = fig.add_subplot(212) +ax1.plot(dfWeakRegen[t], in_place_integrate(dfWeakRegen[busC]*dfWeakRegen[busV], dt=60/5035)/3600000, label="Weak Regen Energy") +ax1.set_xlabel("Time (s)") +ax1.set_ylabel("Energy (kWh)") +ax2.plot(dfStrongRegen[t], in_place_integrate(dfStrongRegen[busC]*dfStrongRegen[busV], dt=60/5035)/3600000, label="Strong Regen Energy") +ax2.set_xlabel("Time (s)") +ax2.set_ylabel("Energy (kWh)") +plt.suptitle("Regen Energy during BlueMax Testing") +fig.show() \ No newline at end of file diff --git a/Docs/Battery_schematics.png b/Docs/Battery_schematics.png new file mode 100644 index 0000000..8e5a1f3 Binary files /dev/null and b/Docs/Battery_schematics.png differ diff --git a/Docs/SimulationTodo.md b/Docs/SimulationTodo.md new file mode 100644 index 0000000..ef03388 --- /dev/null +++ b/Docs/SimulationTodo.md @@ -0,0 +1,82 @@ +# Simulation Todo + +1. Better drag model that takes into account aeropackage (FS-3) +1. Individual wheel models + 1. Suspension + 1. Travel (x) + 1. Velocity (v) + 1. How will this react under acceleration in any direction (steering causes lateral acceleration) (throttle/brakes causes longitudinal acceleration) + 1. Wheel + 1. Brake temp (more complex soon) + 1. Wheel temp (more complex soon) + 1. Wheel rpm/speed +1. Differential + Drivetrain losses + 1. Energy loss due to chain, tripods, axle, hub/upright rubbing + 1. Model rolling resistance better + 1. Model limited slip differential + 1. Log losses so we have an idea of energy loss in the drivetrain +1. Motor Efficiency + Heating + 1. Function of temp and current draw + 1. Keep track of losses + 1. Efficiency loss goes into heat of motor. Need an estimate of its thermal mass and then change in temp. (Motor temp new var) +1. Cleaner logging + 1. Log everything without having to add more rows constantly + 1. Keep efficiency in mind +1. Tractive system heat generation (Not acc) + 1. Estimate how much heat is generated in the accumulator + 1. Not high priority unless we can get more data. +1. Steering model +1. Suspension Model + + +# New simulation architecture idea + + +```python +# Dynamic Vars +posX = 0 +posY = 1 +velX = 2 +velY = 3 +accelX = 4 +accelY = 5 + +arr = np.array((simSteps, 6+1)) +arr[0] = step0 + +def step(): + newPosX = step[posX] + step[velX] * t + +for i in range(simSteps): + arr[i+1] = step(arr[i]) +``` + +```python +# Dictionary Idea #1 +# Array of dictionaries where each time step gets its own dictionary +# Trivial to access a specific thing from any row + +arr = np.array((simSteps)) +arr[0] = step0 + +def step(): + newPosX = arr[posX] + arr[velX] * t + +for i in range(simSteps): + arr[i+1] = step(arr[i]) +``` + +```python +# Dictionary Idea #2 +# Dictionary of arrays. Each key is a column and each array is the length of the simulation +# Trivial to access columns which is typically how we access data + +arr = np.array((simSteps)) +arr[0] = step0 + +def step(): + newPosX = arr[posX] + arr[velX] * t + +for i in range(simSteps): + arr[i+1] = step(arr[i]) +``` \ No newline at end of file diff --git a/Docs/fs3_batter_thermal_schematics.md b/Docs/fs3_batter_thermal_schematics.md new file mode 100644 index 0000000..ff558d8 --- /dev/null +++ b/Docs/fs3_batter_thermal_schematics.md @@ -0,0 +1,3 @@ +## Battery Thermal Schematics + +![alt text](Battery_schematics.png) \ No newline at end of file diff --git a/FullVehicleSim/Electrical/powertrain.py b/FullVehicleSim/Electrical/powertrain.py index 53d4bd9..f583df7 100644 --- a/FullVehicleSim/Electrical/powertrain.py +++ b/FullVehicleSim/Electrical/powertrain.py @@ -1,24 +1,23 @@ -from state import VehicleState -from paramLoader import Parameters, Magic +import numpy as np +from paramLoader import * -def calcMaxMotorTorque(worldPrev:VehicleState, resistiveForces:float, maxPower:float, maxTractionTorqueAtWheel:float): +def calcMaxMotorTorque(worldArray:np.ndarray, step:int, resistiveForces:float, maxPower:float, maxTractionTorqueAtWheel:float): ''' Motor Torque at the wheel minimum(rpm limited torque, power limited torque, perfect traction torque) ''' ## RPM Limited Torque (Motor Controller limits it to ~ this in practice. Maybe something more like 7490ish) - if worldPrev.motorRPM > 7490: + if worldArray[step-1, varMotorRPM] > 7490: return -1 * resistiveForces * Parameters["wheelRadius"] - if worldPrev.motorRotationsHZ != 0: ## If rolling, torque may be power limited. - maxPowerTorque = maxPower / worldPrev.motorRotationsHZ * Parameters["gearRatio"] + if worldArray[step-1, varMotorRotationsHZ] != 0: ## If rolling, torque may be power limited. + maxPowerTorque = maxPower / worldArray[step-1, varMotorRotationsHZ] * Parameters["gearRatio"] else: ## Avoid divide by 0 error but it's just the same as the max torque that the motor can deliver (180 Nm) maxPowerTorque = 180.0 # Nm at 0 rpm - perfectTractionTorque = Parameters["maxTorque"] - torque = min(perfectTractionTorque, maxPowerTorque, maxTractionTorqueAtWheel/Parameters["gearRatio"]) + torque = min(Parameters["maxTorque"], maxPowerTorque, maxTractionTorqueAtWheel/Parameters["gearRatio"]) return torque -def calcCurrent(power, voltage): +def calcCurrent(power:float, voltage:float) -> float: if (power / voltage) > Parameters["tractiveIMax"]: return Parameters["tractiveIMax"] return power / voltage @@ -29,10 +28,10 @@ def calcMaxWheelTorque(maxMotorTorque): ''' return maxMotorTorque * Parameters["gearRatio"] -def calcMotorForce(maxWheelTorque): +def calcMotorForce(maxWheelTorque:float) -> float: return (maxWheelTorque / Parameters["wheelRadius"]) -def calcMaxPower(voltage): +def calcMaxPower(voltage:float) -> float: return Parameters["tractiveIMax"] * voltage def calcVoltage(): diff --git a/FullVehicleSim/Mech/aero.py b/FullVehicleSim/Mech/aero.py index 8be586d..6b5ecbc 100644 --- a/FullVehicleSim/Mech/aero.py +++ b/FullVehicleSim/Mech/aero.py @@ -1,9 +1,8 @@ import numpy as np -from paramLoader import Parameters, Magic -from state import VehicleState +from paramLoader import * -def calcDrag(prevWorld:VehicleState) -> float: - return 0.5 * Parameters["airDensity"] * Parameters["dragCoeffAreaCombo"] * prevWorld.speed**2 +def calcDrag(worldArray:np.ndarray, step:int) -> float: + return 0.5 * Parameters["airDensity"] * Parameters["dragCoeffAreaCombo"] * worldArray[step-1, varSpeed]**2 -def calcDownForce(prevWorld:VehicleState) -> np.ndarray: +def calcDownForce(worldArray:np.ndarray, step:int) -> np.ndarray: return np.asarray([0,0,0,0], dtype=float) diff --git a/FullVehicleSim/Mech/braking.py b/FullVehicleSim/Mech/braking.py index 688d8e4..4a15070 100644 --- a/FullVehicleSim/Mech/braking.py +++ b/FullVehicleSim/Mech/braking.py @@ -1,7 +1,6 @@ from Mech import brakepadFrictionModel -from paramLoader import Parameters, Magic +from paramLoader import * import numpy as np -from state import VehicleState # Docs: # https://docs.google.com/document/d/1oGsGDnY0DEKWpE3S6481A9yZ0F9qUEwWkSXJwTSz4E4/edit?tab=t.2rmbsj26c7w # The goal of these functions are to calculate the net force on the brakes, applied reverse to heading @@ -9,46 +8,47 @@ def brakePSI_toNewtons(psi:float) -> float: return psi * Parameters["brakeCaliperArea"] * 4.448222 # lb force to Newtons -def calcBrakeForce(prevWorld:VehicleState, inputs) -> tuple[float,float]: +def calcBrakeForce(worldArray:np.ndarray, step:int) -> tuple[float,float]: """ Calculate the brake force. FrictionCoeff(temp) * maxBrakeForce * 4 (for 4 wheels) - :param prevWorld: World State Previous + :param worldArray: World State Array + :param step: Current step index :return: Brake Force """ - frontBrakePSI = inputs[1] - rearBrakePSI = inputs[2] + frontBrakePSI = worldArray[step, varBrakePressureFront] + rearBrakePSI = worldArray[step, varBrakePressureRear] frontBrakeForce = brakePSI_toNewtons(frontBrakePSI) rearBrakeForce = brakePSI_toNewtons(rearBrakePSI) # Calculate Brake Force - frontBrakeForce:float = brakepadFrictionModel.calcFrictionCoeff(prevWorld.frontBrakeTemperature) * frontBrakeForce * 2 * Parameters["brakeDiscRadius"] / Parameters["wheelRadius"] - rearBrakeForce:float = brakepadFrictionModel.calcFrictionCoeff(prevWorld.rearBrakeTemperature) * rearBrakeForce * 2 * Parameters["brakeDiscRadius"] / Parameters["wheelRadius"] + frontBrakeForce:float = brakepadFrictionModel.calcFrictionCoeff(worldArray[step-1, varFrontBrakeTemperature]) * frontBrakeForce * 2 * Parameters["brakeDiscRadius"] / Parameters["wheelRadius"] + rearBrakeForce:float = brakepadFrictionModel.calcFrictionCoeff(worldArray[step-1, varRearBrakeTemperature]) * rearBrakeForce * 2 * Parameters["brakeDiscRadius"] / Parameters["wheelRadius"] return frontBrakeForce, rearBrakeForce -def calcBrakeCooling(prevWorld:VehicleState) -> tuple[float,float]: +def calcBrakeCooling(worldArray:np.ndarray, step:int) -> tuple[float,float]: """ Calculate the cooled brake temperature. :param prevWorld: World State :return: Change in Temperature """ - frontBrakeCooling = Parameters["ambientTemperature"] + (prevWorld.frontBrakeTemperature - Parameters["ambientTemperature"]) * np.e ** (-1 / Parameters["stepsPerSecond"]/50.2) - rearBrakeCooling = Parameters["ambientTemperature"] + (prevWorld.rearBrakeTemperature - Parameters["ambientTemperature"]) * np.e ** (-1 / Parameters["stepsPerSecond"]/50.2) + frontBrakeCooling = Parameters["ambientTemperature"] + (worldArray[step-1, varFrontBrakeTemperature] - Parameters["ambientTemperature"]) * np.e ** (-1 / Parameters["stepsPerSecond"]/50.2) + rearBrakeCooling = Parameters["ambientTemperature"] + (worldArray[step-1, varRearBrakeTemperature] - Parameters["ambientTemperature"]) * np.e ** (-1 / Parameters["stepsPerSecond"]/50.2) return frontBrakeCooling, rearBrakeCooling #q = (initTemperature - parameters["ambientTemperature"]) * parameters["brakeMass"] * parameters["brakeSpecificHeatCapacity"] #change = (q * parameters["brakepadThickness"])/(initTemperature * parameters["brakeThermalConductivity"] * parameters["brakeSurfaceArea"] #return initTemperature - change -def calcBrakeHeating(prevWorld:VehicleState, inputs) -> tuple[float,float]: +def calcBrakeHeating(worldArray:np.ndarray, step:int) -> tuple[float,float]: # Calculate Brake Force - frontBrakeForce, rearBrakeForce = calcBrakeForce(prevWorld, inputs) + frontBrakeForce, rearBrakeForce = calcBrakeForce(worldArray, step) # Guess energy increase based on kinetic energy decrease of the vehicle. # Assumption is 100% of kinetic energy lost goes into brake heating. speedChange = (frontBrakeForce + rearBrakeForce) / Parameters["Mass"] / Parameters["stepsPerSecond"] # momentum impulse - energyChange = 0.5 * Parameters["Mass"] * (prevWorld.speed - (prevWorld.speed - speedChange)) + energyChange = 0.5 * Parameters["Mass"] * (worldArray[step-1, varSpeed] - (worldArray[step-1, varSpeed] - speedChange)) tempChange = energyChange/(Parameters["brakeMass"] * Parameters["brakeSpecificHeatCapacity"]) # While this doesn't seem physically intuitive, it is based on the idea that the front and rear brakes share heat based on their contribution to total braking force. diff --git a/FullVehicleSim/Mech/general.py b/FullVehicleSim/Mech/general.py index ea4bb5b..5bad33b 100644 --- a/FullVehicleSim/Mech/general.py +++ b/FullVehicleSim/Mech/general.py @@ -1,20 +1,19 @@ from Mech.traction import calcCorneringStiffness -from paramLoader import Parameters, Magic -from state import VehicleState +from paramLoader import * from Mech.braking import calcBrakeForce from Mech.aero import calcDrag from Mech.steering import calcSlipAngle, calcYawRate from Mech.tireLoad import calcLoadTransfer import numpy as np -def calcResistiveForces(worldPrev:VehicleState, inputs): - if worldPrev.speed <= 1e-5: # Floating point error +def calcResistiveForces(worldArray:np.ndarray, step:int): + if worldArray[step-1, varSpeed] <= 1e-5: # Floating point error return 0 else: - frontBrakeForce, rearBrakeForce = calcBrakeForce(worldPrev, inputs) - return -1 * (calcDrag(worldPrev) + frontBrakeForce + rearBrakeForce) + frontBrakeForce, rearBrakeForce = calcBrakeForce(worldArray, step) + return -1 * (calcDrag(worldArray, step) + frontBrakeForce + rearBrakeForce) -def calculateYawRate(prevWorld:VehicleState, steerAngle:float, initAcceleration:float, heading:np.ndarray, initYawRate:float, timeSinceLastSteer:float): +def calculateYawRate(worldArray:np.ndarray, step:int, initAcceleration:float, initYawRate:float, timeSinceLastSteer:float): """Calculate the yaw rate of the vehicle at the current state. This function computes the yaw rate by calculating tire loads, slip angles, cornering stiffness, and then applying the vehicle dynamics equations. @@ -32,9 +31,9 @@ def calculateYawRate(prevWorld:VehicleState, steerAngle:float, initAcceleration: ----- Slip ratio is fixed at 0.15. """ - tireLoad = calcLoadTransfer(initAcceleration * heading[0], initAcceleration * heading[1], initYawRate) - slipAngle = calcSlipAngle(initYawRate, prevWorld.velocity, steerAngle, Parameters) + tireLoad = calcLoadTransfer(initAcceleration * worldArray[step-1, varHeadingX], initAcceleration * worldArray[step-1, varHeadingY], initYawRate) + slipAngle = calcSlipAngle(worldArray, step) slipRatio = 0.15 - corneringStiffness = calcCorneringStiffness(tireLoad, slipAngle, slipRatio, prevWorld.speed, 80, 40, Parameters, Magic) # Works but unused - res = calcYawRate(initYawRate, prevWorld.speed, steerAngle, timeSinceLastSteer, corneringStiffness[0], corneringStiffness[1], Parameters) - return res \ No newline at end of file + corneringStiffness = calcCorneringStiffness(tireLoad, slipAngle, slipRatio, worldArray[step-1, varSpeed], 80, 40, Parameters, Magic) # Works but unused + res = calcYawRate(initYawRate, worldArray[step-1, varSpeed], worldArray[step, varSteerAngle], timeSinceLastSteer, corneringStiffness[0], corneringStiffness[1]) + return res diff --git a/FullVehicleSim/Mech/steering.py b/FullVehicleSim/Mech/steering.py index 44d4486..4c06198 100644 --- a/FullVehicleSim/Mech/steering.py +++ b/FullVehicleSim/Mech/steering.py @@ -1,9 +1,8 @@ # Steering model import numpy as np -from state import VehicleState -from paramLoader import Parameters, Magic +from paramLoader import * -def calcSlipAngle(prevWorld:VehicleState, inputs) -> tuple[float,float]: +def calcSlipAngle(worldArray:np.ndarray, step:int) -> tuple[float,float]: """ Calculate Slip Angle Based on yawRate, Velocity, and Steering Angle. @@ -15,15 +14,16 @@ def calcSlipAngle(prevWorld:VehicleState, inputs) -> tuple[float,float]: :param steerAngle: Description :return: (frontSlipAngle, rearSlipAngle) """ - steerAngle = inputs[3] - speed = np.sqrt(prevWorld.velocity[0] ** 2 + prevWorld.velocity[1] ** 2 + prevWorld.velocity[2]**2) - if prevWorld.yawRate == 0 or speed == 0: # WRONG. RELAXATION LENGTH. PROJECT + steerAngle = worldArray[step, varSteerAngle] + speed = worldArray[step-1, varSpeed] + yawRate = worldArray[step-1, varYawRate] + if yawRate == 0 or speed == 0: # WRONG. RELAXATION LENGTH. PROJECT return (0, 0) else: - bodySlip = np.arctan(prevWorld.velocity[1]/prevWorld.velocity[0]) + bodySlip = np.arctan(worldArray[step-1, varVelY]/worldArray[step-1, varVelX]) - frontSlipAngle = calcVirtualSlipAngle() + bodySlip + (Parameters["wheelBase"]*Parameters["frontWeightDist"]/100 * prevWorld.yawRate)/speed - steerAngle - rearSlipAngle = bodySlip - (Parameters["wheelBase"]*(100-Parameters["frontWeightDist"])/100 * prevWorld.yawRate)/speed + frontSlipAngle = calcVirtualSlipAngle() + bodySlip + (Parameters["wheelBase"]*Parameters["frontWeightDist"]/100 * yawRate)/speed - steerAngle + rearSlipAngle = bodySlip - (Parameters["wheelBase"]*(100-Parameters["frontWeightDist"])/100 * yawRate)/speed return (frontSlipAngle, rearSlipAngle) diff --git a/FullVehicleSim/Mech/tireLoad.py b/FullVehicleSim/Mech/tireLoad.py index 13fbe81..5eeb645 100644 --- a/FullVehicleSim/Mech/tireLoad.py +++ b/FullVehicleSim/Mech/tireLoad.py @@ -1,4 +1,4 @@ -from paramLoader import Parameters +from paramLoader import * def calcLoadTransfer(accelerationX, accelerationY, yawVelocity:float) -> tuple[float, float, float, float]: # TODO: add weight transfer for torsional compliancy diff --git a/FullVehicleSim/SimultionControlInputs/controls.json b/FullVehicleSim/SimulationControlInputs/controls.json similarity index 100% rename from FullVehicleSim/SimultionControlInputs/controls.json rename to FullVehicleSim/SimulationControlInputs/controls.json diff --git a/FullVehicleSim/SimulationControlInputs/simulationControls.csv b/FullVehicleSim/SimulationControlInputs/simulationControls.csv new file mode 100644 index 0000000..c19041a --- /dev/null +++ b/FullVehicleSim/SimulationControlInputs/simulationControls.csv @@ -0,0 +1,6 @@ +time,throttle,brakePressureFront,brakePressureRear,steerAngle +0.0,0.0,0.0,0.0,0.0 +10.0,1.0,0.0,0.0,0.0 +20.0,0.0,0.0,0.0,0.0 +30.0,0.0,300.0,300.0,0.0 +40.0,0.0,0.0,0.0,0.0 \ No newline at end of file diff --git a/FullVehicleSim/SimultionControlInputs/simulationControls.csv b/FullVehicleSim/SimultionControlInputs/simulationControls.csv deleted file mode 100644 index dd8b409..0000000 --- a/FullVehicleSim/SimultionControlInputs/simulationControls.csv +++ /dev/null @@ -1,6 +0,0 @@ -time,throttle,brakesFront,brakesRear,steerAngle -0.0,0.0,0.0,0.0,0.0 -10.0,1.0,0.0,0.0,0.0 -20.0,0.0,0.0,0.0,0.0 -30.0,0.0,500.0,1.0,0.0 -40.0,0.0,0.0,0.0,0.0 \ No newline at end of file diff --git a/FullVehicleSim/basicViewer.py b/FullVehicleSim/basicViewer.py index 07f28e7..1687a8e 100644 --- a/FullVehicleSim/basicViewer.py +++ b/FullVehicleSim/basicViewer.py @@ -2,22 +2,17 @@ import matplotlib.pyplot as plt df = pl.read_parquet("FullVehicleSim/simulation_output.parquet") +t = df["time"] -cols = ["x", "y", "z", "vX", "vY", "vZ", "speed", - "headingX", "headingY", "headingZ", - "yawRate", "frontBrakeTemperature", "rearBrakeTemperature", - "charge", "drag", "resistiveForces", - "motorTorque", "motorForce", "netForce", - "maxTraction", "wheelRotationsHZ", "motorRPM", - "motorRotationsHZ", "current", - "maxWheelTorque", "maxPower", "power", - "voltage", - "frontBrakeForce", "rearBrakeForce", - "frontBrakeHeating", "rearBrakeHeating", - "frontBrakeCooling", "rearBrakeCooling", - "frontSlipAngle", "rearSlipAngle"] -plt.plot(df["time"], df["motorForce"], label="motorForce") -plt.plot(df["time"], df["frontBrakeForce"] + df["rearBrakeForce"], label="Brake Force") +plt.plot(t, df["throttle"]*300, label="throttle") +plt.plot(t, df["brakePressureFront"], label="brakesF") +plt.plot(t, df["netForce"], label="netForce") +plt.plot(t, df["motorForce"], label="motorForce") +plt.plot(t, df["motorTorque"], label="motorTorque") +# plt.plot(t, df["motorRPM"], label="motorRPM") +plt.plot(t, df["speed"], label="speed") plt.legend() plt.show() + +df["speed"].max() \ No newline at end of file diff --git a/FullVehicleSim/engine.py b/FullVehicleSim/engine.py index 49e3077..0a766ec 100644 --- a/FullVehicleSim/engine.py +++ b/FullVehicleSim/engine.py @@ -1,6 +1,5 @@ -from paramLoader import Parameters, Magic +from paramLoader import * import numpy as np -from state import VehicleState from Mech.braking import calcBrakeCooling, calcBrakeHeating, calcBrakeForce from Mech.aero import calcDrag, calcDownForce from Mech.steering import calcSlipAngle @@ -8,9 +7,12 @@ from Electrical.powertrain import calcCurrent, calcMaxMotorTorque, calcMaxWheelTorque, calcMotorForce, calcMaxPower, calcVoltage from scipy.integrate import RK45 -def calculateHeading(heading, yaw_rate, time_increment): - initial_heading = heading[:2] - rotation_angle = yaw_rate * time_increment +# Vibe coded but it looks about right so idk. +# TODO: Verify that this is correct +def calculateHeading(worldArray:np.ndarray, step:int) -> np.ndarray: + time_increment = 1/Parameters["stepsPerSecond"] + initial_heading = worldArray[step-1, varHeadingX:varHeadingZ] # Yes this removes Z, we just want X and Y for this simplification + rotation_angle = worldArray[step-1, varYawRate] * time_increment cos_theta = np.cos(rotation_angle) sin_theta = np.sin(rotation_angle) @@ -24,87 +26,82 @@ def calculateHeading(heading, yaw_rate, time_increment): return np.append(new_heading, 0) -def stepState(worldPrev:VehicleState, inputs): +def stepState(worldArray:np.ndarray, step:int) -> np.ndarray: + """ + The order by which things get updated in this function is incredibly important. + If you calculate velocity before you calculate acceleration, + you would just wind up using the 0 that is present in the array at that index. + Update acceleration before you update velocity. This will also fail somewhat + silently so be cautious. - # Empirically we see that throttle can only go from about 0-.75. - # TODO: Update later - # Made it so you can just comment this out when it's fixed. - # Throttle, brakesFront, brakesRear, steering angle - # 0-1, PSI, PSI, Radians + The worldArray will also fail silently if it doen't contain a row before step. + The 0-1 evaluates to -1 so it just grabs the last row in the array instead of the + previous one. + + :param worldArray: The main world array. To work properly, the worldArray needs to contain a row (step) and a previous row (step-1) with the same format as the output of this function. The function will read from the previous row to calculate the new values for the current step. + :type worldArray: np.ndarray + :param step: The current step in the simulation + :type step: int + :return: The updated state array for the current step + :rtype: ndarray[Any, Any] + """ + + # Empirically we see that throttle can only go from about 0-.75. Currently not accounted for. + arr = np.copy(worldArray[step, :]) # This is the array that is updated and then returned. delta = 1/Parameters["stepsPerSecond"] - maxTraction = 180.0 # Needs a more complex implementation before being used. Potentially something akin to the gaussian kernel of the voltage histeresis model but for acceleration? Or literally based on the suspension travel. - voltage = calcVoltage() # Not yet implemented. Returns 120 for now. - maxPower = calcMaxPower(voltage) # Watts + arr[varMaxTraction] = 180.0 # Needs a more complex implementation before being used. Potentially something akin to the gaussian kernel of the voltage histeresis model but for acceleration? Or literally based on the suspension travel. + arr[varVoltage] = calcVoltage() # Not yet implemented. Returns 120 for now. + arr[varMaxPower] = calcMaxPower(arr[varVoltage]) # Watts - resistiveForces = calcResistiveForces(worldPrev, inputs) - frontBrakeHeating, rearBrakeHeating = calcBrakeHeating(worldPrev, inputs) - frontBrakeCooling, rearBrakeCooling = calcBrakeCooling(worldPrev) - frontBrakeTemperature = worldPrev.frontBrakeTemperature + frontBrakeHeating - frontBrakeCooling - rearBrakeTemperature = worldPrev.rearBrakeTemperature + rearBrakeHeating - rearBrakeCooling + arr[varResistiveForces] = calcResistiveForces(worldArray, step) + arr[varFrontBrakeHeating], arr[varRearBrakeHeating] = calcBrakeHeating(worldArray, step) + arr[varFrontBrakeCooling], arr[varRearBrakeCooling] = calcBrakeCooling(worldArray, step) + arr[varFrontBrakeForce], arr[varRearBrakeForce] = calcBrakeForce(worldArray, step) + arr[varFrontBrakeTemperature] = worldArray[step-1, varFrontBrakeTemperature] + arr[varFrontBrakeHeating] - arr[varFrontBrakeCooling] + arr[varRearBrakeTemperature] = worldArray[step-1, varRearBrakeTemperature] + arr[varRearBrakeHeating] - arr[varRearBrakeCooling] - maxMotorTorque = calcMaxMotorTorque(worldPrev, resistiveForces, maxPower, maxTraction) - motorTorque = min(Parameters["maxTorque"]*inputs[0], maxMotorTorque) # Nm + arr[varMaxMotorTorque] = calcMaxMotorTorque(worldArray, step, arr[varResistiveForces], arr[varMaxPower], arr[varMaxTraction]) + arr[varMotorTorque] = min(Parameters["maxTorque"]*arr[varThrottle], arr[varMaxMotorTorque]) # Nm - power = motorTorque * worldPrev.motorRotationsHZ # Watts - motorForce = calcMotorForce(motorTorque) # Newtons - netForce = motorForce + resistiveForces # Newtons + arr[varPower] = arr[varMotorTorque] * worldArray[step-1, varMotorRotationsHZ] # Watts + arr[varMotorForce] = calcMotorForce(arr[varMotorTorque]) # Newtons + arr[varNetForce] = arr[varMotorForce] + arr[varResistiveForces] # Newtons - acceleration = netForce / Parameters["Mass"] # m/s^2 + arr[varAcceleration] = arr[varNetForce] / Parameters["Mass"] # m/s^2 - current = power/voltage # Amps - - charge = worldPrev.charge - current * delta / 3600.0 - position = worldPrev.position + worldPrev.velocity * delta - speed = max(0, worldPrev.speed + acceleration * delta) # Sometimes braking falls a tad below 0 so we just correct that because otherwise everything breaks - yawRate = worldPrev.yawRate - if inputs[2] == 0: - yawRate = 0 + arr[varCurrent] = arr[varPower] / arr[varVoltage] # Amps - heading = calculateHeading(worldPrev.heading, yawRate, delta) + arr[varCharge] = worldArray[step-1, varCharge] - worldArray[step, varCurrent] * delta / 3600.0 + arr[varPosX:varPosZ+1] = worldArray[step-1, varPosX:varPosZ+1] + worldArray[step-1, varVelX:varVelZ+1] * delta + arr[varSpeed] = max(0, worldArray[step-1, varSpeed] + arr[varAcceleration] * delta) # Sometimes braking falls a tad below 0 so we just correct that because otherwise everything breaks + arr[varYawRate] = worldArray[step-1, varYawRate] + if worldArray[step, varSteerAngle] == 0: + arr[varYawRate] = 0 + arr[varVelX:varVelZ+1] = arr[varSpeed] * worldArray[step-1, varHeadingX:varHeadingZ+1] + arr[varHeadingX:varHeadingZ+1] = calculateHeading(worldArray, step) - drag = calcDrag(worldPrev) - frontBrakeForce, rearBrakeForce = calcBrakeForce(worldPrev, inputs) - frontSlipAngle, rearSlipAngle = calcSlipAngle(worldPrev, inputs) - maxWheelTorque = calcMaxWheelTorque(maxMotorTorque) - - # cols = ["x", "y", "z", "vX", "vY", "vZ", "speed", - # "headingX", "headingY", "headingZ", - # "yawRate", "frontBrakeTemperature", "rearBrakeTemperature", - # "charge", "drag", "resistiveForces", - # "motorTorque", "motorForce", "netForce", - # "maxTraction", "wheelRotationsHZ", "motorRPM", - # "motorRotationsHZ", "current", - # "maxWheelTorque", "maxPower", "power", - # "voltage", "downForce", - # "frontBrakeForce", "rearBrakeForce", - # "frontBrakeHeating", "rearBrakeHeating", - # "frontBrakeCooling", "rearBrakeCooling", - # "frontSlipAngle", "rearSlipAngle"] + arr[varDrag] = calcDrag(worldArray, step) - log:list[float] = [position[0], position[1], position[2], - worldPrev.velocity[0], worldPrev.velocity[1], worldPrev.velocity[2], - worldPrev.speed, - worldPrev.heading[0], worldPrev.heading[1], worldPrev.heading[2], - worldPrev.yawRate, frontBrakeTemperature, rearBrakeTemperature, - charge, drag, resistiveForces, - motorTorque, motorForce, netForce, - maxTraction, worldPrev.wheelRotationsHZ, worldPrev.motorRPM, - worldPrev.motorRotationsHZ, current, - maxWheelTorque, maxPower, power, - voltage, - frontBrakeForce, rearBrakeForce, - frontBrakeHeating, rearBrakeHeating, - frontBrakeCooling, rearBrakeCooling, - frontSlipAngle, rearSlipAngle] + arr[varFrontSlipAngle], arr[varRearSlipAngle] = calcSlipAngle(worldArray, step) + arr[varMaxWheelTorque] = calcMaxWheelTorque(arr[varMaxMotorTorque]) + arr[varWheelRotationsHZ] = arr[varSpeed] / Parameters["wheelCircumferance"] * 2.0 * np.pi + arr[varMotorRotationsHZ] = arr[varWheelRotationsHZ] / Parameters["gearRatio"] + arr[varWheelRPM] = arr[varWheelRotationsHZ] * 60.0 + arr[varMotorRPM] = arr[varWheelRPM] / Parameters["gearRatio"] + return arr - worldNext = VehicleState( - position=position, - speed=speed, - heading = heading, - charge=charge, - frontBrakeTemperature = frontBrakeTemperature, - rearBrakeTemperature = rearBrakeTemperature, - yawRate = worldPrev.yawRate - ) - return worldNext, log +def dynamicStepState(step:np.ndarray) -> np.ndarray: + """ + Interface for simulation step state that takes a single row with inputs and a previous step. + Separates this into 2 rows with the first row being the previous step and the second row being the current inputs. + Then calls stepState to get the new state for the current step. + + :param step: Step you wish to input (Array of all features) + :type step: np.ndarray + :return: Next step in the simulation given your contorl input and vehicle state. + :rtype: ndarray[Any, Any] + """ + arr = np.array([step, step]) + arr[1, 5:] = np.zeros((step.shape[0]-5)) + return stepState(arr, 1) \ No newline at end of file diff --git a/FullVehicleSim/main.py b/FullVehicleSim/main.py index 7786715..62d0f5f 100644 --- a/FullVehicleSim/main.py +++ b/FullVehicleSim/main.py @@ -4,8 +4,7 @@ import argparse import time -from paramLoader import Magic, Parameters -from state import * +from paramLoader import * from engine import * if __name__ == "__main__": @@ -29,31 +28,20 @@ raise Exception("Please provide a valid simulation controls file path using --simulation_controls or -c") ## Double check it has the correct columns - if df_controls.columns != ['time', 'throttle', 'brakesFront','brakesRear', 'steerAngle']: - raise Exception("Simulation controls file must contain the following columns: 'time', 'throttle', 'brakesFront', 'brakesRear', 'steerAngle'") + if df_controls.columns != ['time', 'throttle', 'brakePressureFront','brakePressureRear', 'steerAngle']: + raise Exception("Simulation controls file must contain the following columns: 'time', 'throttle', 'brakePressureFront', 'brakePressureRear', 'steerAngle'") totalSteps = int(Parameters["stepsPerSecond"] * Parameters["simulationDuration"]) steps = np.arange(0, Parameters["simulationDuration"], 1/Parameters["stepsPerSecond"]) - cols = ["x", "y", "z", "vX", "vY", "vZ", "speed", - "headingX", "headingY", "headingZ", - "yawRate", "frontBrakeTemperature", "rearBrakeTemperature", - "charge", "drag", "resistiveForces", - "motorTorque", "motorForce", "netForce", - "maxTraction", "wheelRotationsHZ", "motorRPM", - "motorRotationsHZ", "current", - "maxWheelTorque", "maxPower", "power", - "voltage", - "frontBrakeForce", "rearBrakeForce", - "frontBrakeHeating", "rearBrakeHeating", - "frontBrakeCooling", "rearBrakeCooling", - "frontSlipAngle", "rearSlipAngle"] - - log = np.zeros((totalSteps + 1, len(cols))) - worldArray = np.zeros(totalSteps + 1, dtype=VehicleState) - # Set the inital time to 0 if not already 0 - timeSeries = df_controls['time'] - df_controls['time'][0] + ## This is structured so the first row is the initial conditions (inputs don't matter and will just be left to 0), and the + ## rest are generated as the simulation progresses. This means that a simulation array will always be 1 longer than just the time steps + ## and duration would indicate. + worldArray = np.zeros((totalSteps + 1, len(VARIABLE_NAMES)), dtype=np.float32) + + # Set the inital time to 0 if not already 0. Eg. [1.79, 2.36, 3.13] becomes [0.0, 0.57, 1.34] + timeSeries = df_controls['time'] - df_controls['time'][0] # Normalize to start at 0 # This takes the last time step and copies it out to the end of the simulation duration. # This has the effect of holding the last command constant until the end of the simulation duration. @@ -61,8 +49,8 @@ df_controls = df_controls.vstack(pl.DataFrame({ 'time': [Parameters["simulationDuration"]], 'throttle': df_controls["throttle"][-1], - 'brakesFront': df_controls["brakesFront"][-1], - 'brakesRear': df_controls["brakesRear"][-1], + 'brakePressureFront': df_controls["brakePressureFront"][-1], + 'brakePressureRear': df_controls["brakePressureRear"][-1], 'steerAngle': df_controls["steerAngle"][-1]})) timeSeries = df_controls['time'] @@ -76,28 +64,29 @@ elif Parameters["interpolationMethod"] == "linear": controlInputs = np.zeros((len(steps), 4)) controlInputs[:,0] = np.interp(steps, timeSeries, df_controls['throttle']) - controlInputs[:,1] = np.interp(steps, timeSeries, df_controls['brakesFront']) - controlInputs[:,2] = np.interp(steps, timeSeries, df_controls['brakesRear']) + controlInputs[:,1] = np.interp(steps, timeSeries, df_controls['brakePressureFront']) + controlInputs[:,2] = np.interp(steps, timeSeries, df_controls['brakePressureRear']) controlInputs[:,3] = np.interp(steps, timeSeries, df_controls['steerAngle']) else: raise Exception("Unsupported interpolation method. Please use 'cubic' or 'linear'.") - worldArray[0] = VehicleState( - position=np.asarray([0,0,0], dtype=np.float32), - speed=0, - heading = np.asarray([1,0,0], dtype=np.float32), - charge=Parameters["vehicleSOC"], - yawRate = 0, - frontBrakeTemperature = Parameters["initialBrakeTemperature"], - rearBrakeTemperature= Parameters["initialBrakeTemperature"], - tractiveBatteryTemperature = Parameters["initialBatteryTemperature"] - ) - - timeCol = np.arange(0, Parameters["simulationDuration"] + 1/Parameters["stepsPerSecond"], 1/Parameters["stepsPerSecond"]) + ## Setup initial conditions. Leaves row 0 with no inputs (don't matter anyway since sim runs from 1 -> end) + ## Some other initial conditions based on input parameters. + worldArray[1:, varThrottle] = controlInputs[:,0] + worldArray[1:, varBrakePressureFront] = controlInputs[:,1] + worldArray[1:, varBrakePressureRear] = controlInputs[:,2] + worldArray[1:, varSteerAngle] = controlInputs[:,3] + worldArray[0,varCharge] = Parameters["vehicleSOC"] + worldArray[0,varFrontBrakeTemperature] = Parameters["initialBrakeTemperature"] + worldArray[0,varRearBrakeTemperature] = Parameters["initialBrakeTemperature"] + worldArray[0, varHeadingX:varHeadingZ+1] = Parameters["initHeading"] + worldArray[0, varPosX:varPosZ+1] = Parameters["initPosition"] + worldArray[0, varVelX:varVelZ+1] = Parameters["initVelocity"] + worldArray[:, varTime] = np.arange(0, Parameters["simulationDuration"] + 1/Parameters["stepsPerSecond"], 1/Parameters["stepsPerSecond"]) start = time.time() - for i in range(totalSteps): - worldArray[i+1], log[i+1] = stepState(worldArray[i], controlInputs[i]) # Step forward!! + for i in range(1, totalSteps): + worldArray[i, :] = stepState(worldArray, i) # Step forward!! ## This was above the stepState but I moved it down to make it clearer to read. # timeRunning += 1/stepsPerSecond # timeSinceLastSteer += 1/stepsPerSecond @@ -117,18 +106,12 @@ # 'cooledBrakeTemperature', 'wheelRPM', 'wheelRotationsHZ', # 'rpm', 'motorRotationsHZ', 'charge', 'voltage', 'current', # 'power', 'maxPower', 'stepSize', 'timeSinceLastSteer'] + # print(VARIABLE_NAMES) - df = pl.DataFrame(log, schema=cols, orient="row") + df = pl.DataFrame(worldArray, schema=VARIABLE_NAMES, orient="row") # print(f"df shape: {df.shape}") # print(f"control inputs shape: {controlInputs.shape}") # print(f"timeCol shape: {timeCol.shape}") - df = df[1:].with_columns( - pl.Series(timeCol[1:]).alias("time"), - pl.Series(controlInputs[:,0]).alias("throttle"), - pl.Series(controlInputs[:,1]).alias("brakesFront"), - pl.Series(controlInputs[:,2]).alias("brakesRear"), - pl.Series(controlInputs[:,3]).alias("steerAngle") - ) df.write_parquet("simulation_output.parquet") @@ -139,30 +122,32 @@ torque = df['motorTorque'] yawRate = df['yawRate'] frontBrakeTemperature = df['frontBrakeTemperature'] - ax1 = plt.subplot(1,4,1) - ax2 = plt.subplot(1,4,2) - ax3 = plt.subplot(1,4,3) - ax4 = plt.subplot(1,4,4) - - ax1.set_title("Current vs Time") + ax1 = plt.subplot(411) + ax11 = ax1.twinx() + ax2 = plt.subplot(412) + ax22 = ax2.twinx() + ax3 = plt.subplot(413) + ax33 = ax3.twinx() + ax4 = plt.subplot(414) + ax44 = ax4.twinx() + + ax1.set_title("I (Blue)/V (Orange) vs Time") ax1.set_xlabel("Time (s)") - ax1.set_ylabel("Current (A)") - ax1.plot(t, current) + ax1.set_ylabel("Current (A) / Voltage (V)") + ax1.plot(t, current, label="Current") + ax11.plot(t, voltage, label="Voltage", color='orange') ax2.set_title("Speed vs Time") ax2.set_xlabel("Time (s)") ax2.set_ylabel("Speed (m/s)") ax2.plot(t, speed) - ax3.set_title("Voltage vs Time") - ax3.set_xlabel("Time (s)") - ax3.set_ylabel("Voltage (V)") - ax3.plot(t, voltage) - - ax3.set_title("Voltage vs Time") + ax3.set_title("Throttle (Blue)/Brakes (Orange) vs Time") ax3.set_xlabel("Time (s)") - ax3.set_ylabel("Voltage (V)") - ax3.plot(t, voltage) + ax3.set_ylabel("Throttle (0-1)") + ax33.set_ylabel("Brake Pressure (PSI)") + ax3.plot(t, df["throttle"], label="Throttle") + ax33.plot(t, df["brakePressureFront"], color='orange') ax4.set_title("rvt") ax4.plot(t, yawRate) diff --git a/FullVehicleSim/paramLoader.py b/FullVehicleSim/paramLoader.py index 8e06320..5249d9d 100644 --- a/FullVehicleSim/paramLoader.py +++ b/FullVehicleSim/paramLoader.py @@ -1,9 +1,101 @@ import json5 -Magic:dict -Parameters:dict +from typing import Dict, List, Tuple + +Magic: dict +Parameters: dict with open('params.json5', 'r') as file: params = json5.load(file) Magic = params["Magic"] Parameters = params["Parameters"] del params + +# Variable definitions - maintain original order for compatibility + +varTime = 0 +varThrottle = 1 +varBrakePressureFront = 2 +varBrakePressureRear = 3 +varSteerAngle = 4 +varPosX = 5 +varPosY = 6 +varPosZ = 7 +varVelX = 8 +varVelY = 9 +varVelZ = 10 +varSpeed = 11 +varHeadingX = 12 +varHeadingY = 13 +varHeadingZ = 14 +varYawRate = 15 +varFrontBrakeTemperature = 16 +varRearBrakeTemperature = 17 +varCharge = 18 +varDrag = 19 +varResistiveForces = 20 +varMotorTorque = 21 +varMotorForce = 22 +varNetForce = 23 +varMaxTraction = 24 +varWheelRotationsHZ = 25 +varMotorRPM = 26 +varMotorRotationsHZ = 27 +varCurrent = 28 +varMaxWheelTorque = 29 +varMaxPower = 30 +varPower = 31 +varVoltage = 32 +varFrontBrakeForce = 33 +varRearBrakeForce = 34 +varFrontBrakeHeating = 35 +varRearBrakeHeating = 36 +varFrontBrakeCooling = 37 +varRearBrakeCooling = 38 +varFrontSlipAngle = 39 +varRearSlipAngle = 40 +varMaxMotorTorque = 41 +varAcceleration = 42 +varWheelRPM = 43 + +# Automatically generate schema from defined variables +def generate_variable_schema() -> Dict[int, str]: + """ + Generate a schema mapping variable indices to their names. + Preserves the order of definition in the file. + """ + schema = {} + + # Get all variables that start with 'var' from the current module + current_module = globals() + var_items = [(name, value) for name, value in current_module.items() + if name.startswith('var') and isinstance(value, int)] + + # Sort by value to maintain order + var_items.sort(key=lambda x: x[1]) + + # Create the schema + for name, index in var_items: + # Convert variable name to readable format + readable_name = name[3].lower() + name[4:] # Remove 'var' prefix and lowercase first letter + schema[index] = readable_name + + return schema + +def get_variable_names() -> List[str]: + """ + Get ordered list of variable names (without 'var' prefix). + """ + schema = generate_variable_schema() + return [schema[i] for i in range(len(schema))] + +def get_variable_mapping() -> Dict[str, int]: + """ + Get mapping from variable names to indices. + """ + schema = generate_variable_schema() + return {name: index for index, name in schema.items()} + +# Generate the schema on module load +VARIABLE_SCHEMA = generate_variable_schema() +VARIABLE_NAMES = get_variable_names() +VARIABLE_MAPPING = get_variable_mapping() print("Parameters loaded...") diff --git a/FullVehicleSim/params.json5 b/FullVehicleSim/params.json5 index a9edabe..030bcec 100644 --- a/FullVehicleSim/params.json5 +++ b/FullVehicleSim/params.json5 @@ -3,7 +3,7 @@ "Parameters": { "stepsPerSecond": 300, "simulationDuration": 40.0, - "interpolationMethod": "linear", // "linear" or "cubic" + "interpolationMethod": "linear", // "linear" or "cubic", this is for interpolating the simultion controls. "ambientTemperature": 20, // °C "tractiveIMax": 300, // A @@ -11,6 +11,9 @@ "initialBatteryTemperature": 20, // °C "vehicleSOC": 1.0, // Out of 1 "initSpeed": 0.0, // m/s + "initHeading": [1.0, 0.0, 0.0], // Vector pointing in the direction of the front of the car + "initPosition": [0.0, 0.0, 0.0], // Starting position of the car in the world frame + "initVelocity": [0.0, 0.0, 0.0], // Initial velocity vector in the world frame "airDensity": 1.230, "dragCoeffAreaCombo": 1.0858790012112278, // Combines area and drag coeff into one constant @@ -28,7 +31,7 @@ "p_0": 82000, "load_0": 300, - "Mass": 300, + "Mass": 300, // kg "wheelBase": 1.65471, "a": 0.853506, "frontWeightDist": 46.46, @@ -48,7 +51,7 @@ "brakeMass": 0.408, "maxBrakeForce": 1500, - "seriesCells": 30, + "seriesCells": 30,s "cellCapacity_Ah": 2.6 }, "Magic": { @@ -156,6 +159,6 @@ "pressureYA": -3.086921788053587e-05, "pressureYB": -9.812999633140862e-05, "pressureYC": 0.9998338222503662, - "gysign": -0.10000000149011612 + "gysign": -0.10000000149011612, } } diff --git a/FullVehicleSim/state.py b/FullVehicleSim/state.py deleted file mode 100644 index 89d047d..0000000 --- a/FullVehicleSim/state.py +++ /dev/null @@ -1,62 +0,0 @@ -import numpy as np -from paramLoader import Parameters, Magic -from dataclasses import dataclass - -@dataclass -class VehicleState: - def __init__(self, position, speed, heading, charge, yawRate, frontBrakeTemperature, rearBrakeTemperature, tractiveBatteryTemperature): - self.position:np.ndarray = position - self.speed:float = speed - self.heading:np.ndarray = heading - self.charge:float = charge - self.frontBrakeTemperature:float = frontBrakeTemperature - self.rearBrakeTemperature:float = rearBrakeTemperature - self.yawRate:float = yawRate - self.tractiveBatteryTemperature:float = tractiveBatteryTemperature - - # ---- current history buffer (Gaussian uses this) ---- - self.history_steps = int(10.0 * Parameters["stepsPerSecond"]) - - if current_history is not None: - self.current_history = np.asarray(current_history, dtype=float) - # ensure correct length (optional but helps stability) - if self.current_history.size != self.history_steps: - # resize preserving most recent samples - tmp = np.zeros(self.history_steps, dtype=float) - n = min(self.history_steps, self.current_history.size) - tmp[-n:] = self.current_history[-n:] - self.current_history = tmp - else: - self.current_history = np.zeros(self.history_steps, dtype=float) - - # ---- NEW: store battery model states (persist across steps) ---- - # self._cell_voltage = float(cell_voltage) - # self.batt_v_rc = float(batt_v_rc) - # self.batt_hyst = float(batt_hyst) - # self.batt_temp_c = float(batt_temp_c) - - # tires container - self.tires: np.ndarray = np.asarray([None, None, None, None]) # [FL, FR, BL, BR] - - @property - def velocity(self): - return self.heading * self.speed - - @property - def wheelRPM(self): - return self.speed / Parameters["wheelCircumferance"] * 60.0 - - @property - def wheelRotationsHZ(self): - return self.speed / Parameters["wheelCircumferance"] * 2.0 * np.pi - @property - def motorRPM(self): - return self.wheelRPM * Parameters["gearRatio"] - - @property - def motorRotationsHZ(self): - return self.wheelRotationsHZ * Parameters["gearRatio"] - - @property - def maxTractionTorqueAtWheel(self): - return Parameters["maxTractionTorque"] * Parameters["wheelRadius"] diff --git a/FullVehicleSim/visualizeManual.py b/FullVehicleSim/visualizeManual.py index 107e30e..5318590 100644 --- a/FullVehicleSim/visualizeManual.py +++ b/FullVehicleSim/visualizeManual.py @@ -3,7 +3,6 @@ import numpy as np import asyncio import platform -from state import VehicleState from engine import stepState #Simulated control inputs (since no file I/O in Pyodide) diff --git a/FullVehicleSim/yaw_rate_model/double_bicycle_model.py b/FullVehicleSim/yaw_rate_model/double_bicycle_model.py new file mode 100644 index 0000000..b0a78f2 --- /dev/null +++ b/FullVehicleSim/yaw_rate_model/double_bicycle_model.py @@ -0,0 +1,419 @@ +""" +Double Bicycle Yaw Rate Model for Formula Slug + +2DOF model (lateral velocity + yaw rate) for basic vehicle dynamics. +Based on Rajamani's bicycle model. +""" + +import numpy as np +from dataclasses import dataclass +from typing import Tuple, List +import matplotlib.pyplot as plt + + +@dataclass +class VehicleParameters: + """Vehicle parameters from 2025 FSAE Design Spec Sheet (Car #216)""" + + # Geometry (meters) - from 2025 FSAE Design EV Spec Sheet + wheelbase: float = 1.589989 # 1589.989 mm + Lf: float = 0.8535 # Calculated from 46.32% front weight distribution + Lr: float = 0.7365 # Calculated from weight distribution + track_width_front: float = 1.234008 # 1234.008 mm + track_width_rear: float = 1.186 # 1186 mm + track_width: float = 1.234008 # Using front track for compatibility + + # Mass properties (with driver) + mass: float = 300.0 # Vehicle + driver (from Formula Slug FS-4 specs) + mass_without_driver: float = 232.0 + cg_height: float = 0.3048 # 304.8 mm + yaw_inertia: float = 658.09 # Not in spec sheet, using previous value + + # Tire model (stiffness values not in spec sheet, using estimates) + # Tires: Front - Hoosier 16.0x6.0-10 LC0, Rear - Hoosier 16.0x7.5-10 LC0 + C_alpha: float = 180000.0 # Cornering stiffness estimate + C_alpha_front: float = 180000.0 + C_alpha_rear: float = 180000.0 + mu: float = 1.733 + longitudinal_inertia: float = 0.0 + + # Additional spec sheet info (for reference) + # Wheel rates: Front 30.647 N/mm, Rear 35.025 N/mm + # Roll rates: Front 407.25 Nm/deg, Rear 429.93 Nm/deg + # Natural frequency: Front 3.55 Hz, Rear 3.53 Hz + + def __post_init__(self): + tolerance = 0.0001 + wheelbase_check = self.Lf + self.Lr + assert abs(wheelbase_check - self.wheelbase) < tolerance, \ + f"Wheelbase math is wrong: {self.Lf} + {self.Lr} = {wheelbase_check} != {self.wheelbase}" + assert self.mass > 0, "Mass has to be positive" + assert self.yaw_inertia > 0, "Yaw inertia has to be positive" + assert self.C_alpha > 0, "Tire stiffness has to be positive" + + +@dataclass +class TireModel: + stiffness: float + max_slip_angle: float = 0.3 + saturation_method: str = "linear" + friction_coeff: float = 1.733 # Peak friction coefficient + + def lateral_force(self, slip_angle: float, vertical_load: float) -> float: + if self.saturation_method == "linear": + return self.stiffness * slip_angle + + elif self.saturation_method == "nonlinear": + # tanh saturation at high slip angles + # Saturates at F_max = mu * Fz (friction limit) + F_linear = self.stiffness * slip_angle + F_max = self.friction_coeff * vertical_load + return F_max * np.tanh(F_linear / F_max) + + return 0.0 + + +class DoubleBicycleModel: + """2DOF bicycle model: v_y (lateral velocity) and r (yaw rate)""" + + def __init__(self, params: VehicleParameters, tire_model: str = "linear"): + self.params = params + self.tire_front = TireModel(params.C_alpha_front, saturation_method=tire_model, friction_coeff=params.mu) + self.tire_rear = TireModel(params.C_alpha_rear, saturation_method=tire_model, friction_coeff=params.mu) + + self.state = np.array([0.0, 0.0]) + self.time_history = [] + self.state_history = [] + self.input_history = [] + + def get_slip_angles(self, v_y: float, r: float, v_x: float, delta: float) \ + -> Tuple[float, float]: + """Calculate front and rear slip angles""" + if abs(v_x) < 0.1: + return delta, 0.0 + + alpha_f = delta - np.arctan2(v_y + self.params.Lf * r, v_x) + alpha_r = -np.arctan2(v_y - self.params.Lr * r, v_x) + + return alpha_f, alpha_r + + def dynamics(self, state: np.ndarray, v_x: float, delta: float, + ax: float = 0.0) -> np.ndarray: + """Compute state derivatives [dv_y/dt, dr/dt]""" + v_y, r = state + + alpha_f, alpha_r = self.get_slip_angles(v_y, r, v_x, delta) + + # Static weight distribution (no load transfer) + # Front axle load: weight farther back (at distance Lr from front) + # Rear axle load: weight farther forward (at distance Lf from rear) + Fz_f = self.params.mass * 9.81 * self.params.Lr / self.params.wheelbase / 2.0 + Fz_r = self.params.mass * 9.81 * self.params.Lf / self.params.wheelbase / 2.0 + + Fy_f = self.tire_front.lateral_force(alpha_f, Fz_f) + Fy_r = self.tire_rear.lateral_force(alpha_r, Fz_r) + + # Account for both tires per axle + Fy_f_total = 2.0 * Fy_f + Fy_r_total = 2.0 * Fy_r + + # Lateral and yaw accelerations + a_y = (Fy_f_total * np.cos(delta) + Fy_r_total) / self.params.mass + v_x * r + dv_y = a_y + + M_yaw = self.params.Lf * Fy_f_total * np.cos(delta) - self.params.Lr * Fy_r_total + dr = M_yaw / self.params.yaw_inertia + + return np.array([dv_y, dr]) + + def integrate_step(self, v_x: float, delta: float, dt: float, + ax: float = 0.0, method: str = "rk4") -> None: + """Integrate one timestep using euler, rk2, or rk4""" + if method == "euler": + k1 = self.dynamics(self.state, v_x, delta, ax) + self.state = self.state + dt * k1 + + elif method == "rk2": + k1 = self.dynamics(self.state, v_x, delta, ax) + k2 = self.dynamics(self.state + 0.5*dt*k1, v_x, delta, ax) + self.state = self.state + dt * k2 + + elif method == "rk4": + k1 = self.dynamics(self.state, v_x, delta, ax) + k2 = self.dynamics(self.state + 0.5*dt*k1, v_x, delta, ax) + k3 = self.dynamics(self.state + 0.5*dt*k2, v_x, delta, ax) + k4 = self.dynamics(self.state + dt*k3, v_x, delta, ax) + self.state = self.state + (dt/6.0) * (k1 + 2*k2 + 2*k3 + k4) + + else: + raise ValueError(f"Unknown integration method: {method}") + + def simulate(self, v_x: float, steering_inputs: List[float], + dt: float = 0.01, method: str = "rk4") -> Tuple[np.ndarray, np.ndarray]: + """Run simulation with given steering input sequence""" + self.state = np.array([0.0, 0.0]) + self.time_history = [0.0] + self.state_history = [self.state.copy()] + self.input_history = [(0.0, v_x)] + + for i, delta in enumerate(steering_inputs): + t = (i + 1) * dt + self.integrate_step(v_x, delta, dt, ax=0.0, method=method) + + self.time_history.append(t) + self.state_history.append(self.state.copy()) + self.input_history.append((delta, v_x)) + + return np.array(self.time_history), np.array(self.state_history) + + def reset(self): + self.state = np.array([0.0, 0.0]) + self.time_history = [] + self.state_history = [] + self.input_history = [] + + +def validate_against_telemetry(csv_path: str, model: DoubleBicycleModel, + sample_window: int = 1000) -> dict: + """Load telemetry and extract stats for model comparison""" + try: + import pandas as pd + except ImportError: + print("Pandas required. Install with: pip install pandas") + return {} + + try: + df = pd.read_csv(csv_path, low_memory=False) + except Exception as e: + print(f"Failed to load {csv_path}: {e}") + return {} + + results = { + 'samples_loaded': len(df), + 'columns_found': [], + 'validation_data': {} + } + + if 'VDM_Z_AXIS_YAW_RATE' in df.columns: + results['columns_found'].append('yaw_rate') + yaw_telem = df['VDM_Z_AXIS_YAW_RATE'].dropna() + + if len(yaw_telem) > 0: + results['validation_data']['yaw_rate_min'] = yaw_telem.min() + results['validation_data']['yaw_rate_max'] = yaw_telem.max() + results['validation_data']['yaw_rate_mean'] = yaw_telem.mean() + results['validation_data']['yaw_rate_std'] = yaw_telem.std() + + if 'VDM_Y_AXIS_ACCELERATION' in df.columns: + results['columns_found'].append('lateral_accel') + lat_accel = df['VDM_Y_AXIS_ACCELERATION'].dropna() + + if len(lat_accel) > 0: + results['validation_data']['lat_accel_min'] = lat_accel.min() + results['validation_data']['lat_accel_max'] = lat_accel.max() + results['validation_data']['lat_accel_mean'] = lat_accel.mean() + results['validation_data']['lat_accel_max_g'] = lat_accel.max() / 9.81 + + if 'SME_TRQSPD_Speed' in df.columns: + results['columns_found'].append('speed') + speed = df['SME_TRQSPD_Speed'].dropna() + + if len(speed) > 0: + results['validation_data']['speed_min'] = speed.min() + results['validation_data']['speed_max'] = speed.max() + results['validation_data']['speed_mean'] = speed.mean() + + return results + + +def plot_response(model: DoubleBicycleModel, title: str = "Model Response"): + if not model.time_history: + print("No data. Run simulate() first.") + return + + time = np.array(model.time_history) + states = np.array(model.state_history) + steering = [inp[0] for inp in model.input_history] + + fig, axes = plt.subplots(3, 1, figsize=(12, 9)) + fig.suptitle(title, fontsize=14, fontweight='bold') + + axes[0].plot(time, np.rad2deg(steering), 'b-', linewidth=2) + axes[0].set_ylabel('Steering Angle (°)', fontsize=11) + axes[0].grid(True, alpha=0.3) + axes[0].set_title('Steering Input') + + axes[1].plot(time, states[:, 0], 'g-', linewidth=2, label='Lateral Velocity') + axes[1].set_ylabel('Lateral Velocity (m/s)', fontsize=11) + axes[1].grid(True, alpha=0.3) + axes[1].legend() + axes[1].set_title('Lateral Velocity') + + axes[2].plot(time, np.rad2deg(states[:, 1]), 'r-', linewidth=2, label='Yaw Rate') + axes[2].set_ylabel('Yaw Rate (°/s)', fontsize=11) + axes[2].set_xlabel('Time (s)', fontsize=11) + axes[2].grid(True, alpha=0.3) + axes[2].legend() + axes[2].set_title('Yaw Rate') + + plt.tight_layout() + return fig + + +if __name__ == "__main__": + print("\n--- FORMULA SLUG - DOUBLE BICYCLE YAW RATE MODEL ---\n") + + params = VehicleParameters() + print("Vehicle Parameters:") + print(f" Wheelbase: {params.wheelbase:.3f} m (Lf={params.Lf:.3f}, Lr={params.Lr:.3f})") + print(f" Track width: {params.track_width:.3f} m") + print(f" Mass: {params.mass:.1f} kg") + print(f" Yaw inertia: {params.yaw_inertia:.1f} kg·m²") + print(f" Tire stiffness: {params.C_alpha:.0f} N/rad") + + model = DoubleBicycleModel(params, tire_model="linear") + + # Test 1: Step steer + print("\nTEST 1: Step Steer") + v_x = 10.0 + duration = 3.0 + dt = 0.01 + num_steps = int(duration / dt) + + steering_step = np.concatenate([ + np.zeros(int(0.2 / dt)), + np.full(num_steps - int(0.2 / dt), np.deg2rad(5.7)) + ]) + steering_step = steering_step[:num_steps] + + time_sim, states_sim = model.simulate(v_x, steering_step, dt=dt, method="rk4") + + print(f"\nSpeed: {v_x:.1f} m/s ({v_x*3.6:.1f} km/h)") + print(f"Duration: {duration:.2f} s") + print(f"Results at end:") + print(f" Lateral velocity: {states_sim[-1, 0]:.3f} m/s") + print(f" Yaw rate: {np.rad2deg(states_sim[-1, 1]):.2f} °/s") + print(f" G-force: {v_x * states_sim[-1, 1] / 9.81:.3f}g") + + fig1 = plot_response(model, "Test 1: Step Steering") + fig1.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test1_step_steer.png', dpi=150) + print("Saved to test1_step_steer.png") + + # Test 2: Ramp steer + print("\nTEST 2: Ramp Steer") + model.reset() + v_x = 8.0 + duration = 5.0 + num_steps = int(duration / dt) + + steering_ramp = np.concatenate([ + np.linspace(0, np.deg2rad(10), int(1.0 / dt)), + np.full(num_steps - int(1.0 / dt), np.deg2rad(10)) + ]) + steering_ramp = steering_ramp[:num_steps] + + time_sim, states_sim = model.simulate(v_x, steering_ramp, dt=dt, method="rk4") + + print(f"\nSpeed: {v_x:.1f} m/s ({v_x*3.6:.1f} km/h)") + print(f"Ramp rate: {np.rad2deg(10):.1f}°/s") + print(f"Results at end:") + print(f" Lateral velocity: {states_sim[-1, 0]:.3f} m/s") + print(f" Yaw rate: {np.rad2deg(states_sim[-1, 1]):.2f} °/s") + print(f" Steady-state G: {v_x * states_sim[-1, 1] / 9.81:.3f}g") + + fig2 = plot_response(model, "Test 2: Ramp Steering") + fig2.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test2_ramp_steer.png', dpi=150) + print("Saved to test2_ramp_steer.png") + + # Test 3: Lane change + print("\nTEST 3: Double Lane Change") + model.reset() + v_x = 10.0 + duration = 4.0 + num_steps = int(duration / dt) + + freq = 0.5 + steering_dlc = np.deg2rad(10) * np.sin(2 * np.pi * freq * np.linspace(0, duration, num_steps)) + + time_sim, states_sim = model.simulate(v_x, steering_dlc, dt=dt, method="rk4") + + print(f"\nSpeed: {v_x:.1f} m/s ({v_x*3.6:.1f} km/h)") + print(f"Frequency: {freq:.1f} Hz (±10° amplitude)") + print(f"Results:") + print(f" Max lateral velocity: {np.max(np.abs(states_sim[:, 0])):.3f} m/s") + print(f" Max yaw rate: {np.rad2deg(np.max(np.abs(states_sim[:, 1]))):.2f} °/s") + print(f" Max G-force: {v_x * np.max(np.abs(states_sim[:, 1])) / 9.81:.3f}g") + + fig3 = plot_response(model, "Test 3: Double Lane Change") + fig3.savefig('/Users/brianlee/vscode_projects/formula_slug/fs_yawratemodel/test3_double_lanechange.png', dpi=150) + print("Saved to test3_double_lanechange.png") + + print("\n--- Done! ---") + + # Telemetry validation + print("\nTELEMETRY VALIDATION") + + try: + import pandas as pd + + print("\nLoading telemetry from 08102025Endurance1_FirstHalf.csv...") + telemetry_file = '/Users/brianlee/vscode_projects/formula_slug/workshops/data_for_dwshop/08102025Endurance1_FirstHalf.csv' + + df = pd.read_csv(telemetry_file, low_memory=False) + print(f"Loaded {len(df)} samples") + + columns_needed = [ + 'VDM_X_AXIS_ACCELERATION', + 'VDM_Y_AXIS_ACCELERATION', + 'VDM_Z_AXIS_YAW_RATE', + 'SME_TRQSPD_Speed', + 'TPERIPH_FL_DATA_WHEELSPEED', + 'TPERIPH_FR_DATA_WHEELSPEED', + 'TPERIPH_BL_DATA_WHEELSPEED', + 'TPERIPH_BR_DATA_WHEELSPEED', + ] + + available_cols = [col for col in columns_needed if col in df.columns] + print(f"Found {len(available_cols)}/{len(columns_needed)} expected columns") + + if 'VDM_Z_AXIS_YAW_RATE' not in df.columns: + print(" x VDM_Z_AXIS_YAW_RATE not found") + else: + print(" + VDM_Z_AXIS_YAW_RATE found") + + if 'VDM_Y_AXIS_ACCELERATION' not in df.columns: + print(" x VDM_Y_AXIS_ACCELERATION not found") + else: + print(" + VDM_Y_AXIS_ACCELERATION found") + + if 'SME_TRQSPD_Speed' not in df.columns: + print(" x SME_TRQSPD_Speed not found") + else: + print(" + SME_TRQSPD_Speed found") + + if 'VDM_Z_AXIS_YAW_RATE' in df.columns: + valid_yaw = df[df['VDM_Z_AXIS_YAW_RATE'].notna()]['VDM_Z_AXIS_YAW_RATE'] + + if len(valid_yaw) > 0: + print(f"\nYaw rate statistics from telemetry:") + print(f" Min: {valid_yaw.min():.1f} °/s ({np.deg2rad(valid_yaw.min()):.3f} rad/s)") + print(f" Max: {valid_yaw.max():.1f} °/s ({np.deg2rad(valid_yaw.max()):.3f} rad/s)") + print(f" Mean: {valid_yaw.mean():.1f} °/s ({np.deg2rad(valid_yaw.mean()):.3f} rad/s)") + print(f" Std: {valid_yaw.std():.1f} °/s ({np.deg2rad(valid_yaw.std()):.3f} rad/s)") + + high_yaw_mask = np.abs(valid_yaw) > 10.0 # 10 °/s threshold for turning + if high_yaw_mask.any(): + high_idx = np.where(high_yaw_mask)[0][0] + print(f"\nFound turning maneuver at sample {high_idx}") + yaw_val = valid_yaw.iloc[high_idx] + print(f" Yaw rate: {yaw_val:.1f} °/s ({np.deg2rad(yaw_val):.3f} rad/s)") + + print("\nTelemetry validation framework ready") + + except ImportError: + print("Pandas not installed - can't load CSV") + print("Install with: pip install pandas") + except Exception as e: + print(f"Error loading telemetry: {e}") + + plt.show() diff --git a/endur1Curr.csv b/endur1Curr.csv new file mode 100644 index 0000000..322bda2 --- /dev/null +++ b/endur1Curr.csv @@ -0,0 +1,72 @@ +Current,Time +24.611364,0.0 +32.075455,1.0 +83.82,2.0 +103.79028,3.0 +1.6792728,4.0 +47.380276,5.0 +156.73936,6.0 +181.45502,7.0 +178.01755,8.0 +266.4711,9.0 +314.56467,10.0 +337.80002,11.0 +123.42737,12.0 +18.743092,13.0 +41.31073,14.0 +23.57373,15.0 +71.3661,16.0 +147.76573,17.0 +38.92582,18.0 +21.131275,19.0 +15.494909,20.0 +40.06673,21.0 +118.78701,22.0 +115.37664,23.0 +147.45909,24.0 +223.76947,25.0 +148.38336,26.0 +78.637184,27.0 +23.648548,28.0 +43.008003,29.0 +58.465275,30.0 +91.7551,31.0 +77.75927,32.0 +24.253273,33.0 +37.92873,34.0 +42.02046,35.0 +48.948368,36.0 +88.985916,37.0 +89.52327,38.0 +99.00618,39.0 +67.61691,40.0 +23.409546,41.0 +59.80364,42.0 +79.542,43.0 +83.21628,44.0 +128.63428,45.0 +151.5899,46.0 +113.44146,47.0 +91.6961,48.0 +24.420546,49.0 +25.824457,50.0 +60.09946,51.0 +69.13337,52.0 +98.356,53.0 +57.09346,54.0 +55.792725,55.0 +43.36391,56.0 +39.913273,57.0 +53.080276,58.0 +73.36591,59.0 +83.82373,60.0 +54.693638,61.0 +52.09064,62.0 +48.891186,63.0 +48.31355,64.0 +57.849186,65.0 +29.795094,66.0 +37.14237,67.0 +22.748,68.0 +34.09791,69.0 +30.930456,70.0