Noah main sim maintenence

FullVehicleSim/Electrical/powertrain.py

@@ -1,7 +1,8 @@
 import numpy as np
+from numpy import ndarray
 from paramLoader import *
 
-def calcMaxMotorTorque(worldArray:np.ndarray, step:int, resistiveForces:float, maxPower:float, maxTractionTorqueAtWheel:float):
+def calcMaxMotorTorque(worldArray:ndarray[np.float64], step:int, resistiveForces:float, maxPower:float, maxTractionTorqueAtWheel:float):
         '''
         Motor Torque at the wheel
 
@@ -35,7 +36,7 @@ def calcMaxPower(voltage:float) -> float:
         return Parameters["tractiveIMax"] * voltage
 
 
-def calcVoltage(worldArray:np.ndarray, step:int) -> float:
+def calcVoltage(worldArray:ndarray[np.float64], step:int) -> float:
 
     F = Parameters["FaradaysConstant"]
     R = Parameters["GasConstant"]

FullVehicleSim/Mech/aero.py

@@ -1,8 +1,9 @@
 import numpy as np
+from numpy import ndarray
 from paramLoader import *
 
-def calcDrag(worldArray:np.ndarray, step:int) -> float:
+def calcDrag(worldArray:ndarray[np.float64], step:int) -> float:
     return  0.5 * Parameters["airDensity"] * Parameters["dragCoeffAreaCombo"] * worldArray[step-1, varSpeed]**2
 
-def calcDownForce(worldArray:np.ndarray, step:int) -> np.ndarray:
+def calcDownForce(worldArray:ndarray[np.float64], step:int) -> np.ndarray:
     return np.asarray([0,0,0,0], dtype=float)

FullVehicleSim/Mech/braking.py

@@ -1,14 +1,15 @@
 from Mech import brakepadFrictionModel
 from paramLoader import *
 import numpy as np
+from numpy import ndarray
 # 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
 
 def brakePSI_toNewtons(psi:float) -> float:
     return psi * Parameters["brakeCaliperArea"] * 4.448222 # lb force to Newtons
 
-def calcBrakeForce(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcBrakeForce(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     """
     Calculate the brake force.
 
@@ -28,7 +29,7 @@ def calcBrakeForce(worldArray:np.ndarray, step:int) -> tuple[float,float]:
     rearBrakeForce:float = brakepadFrictionModel.calcFrictionCoeff(worldArray[step-1, varRearBrakeTemperature]) * rearBrakeForce * 2 * Parameters["brakeDiscRadius"] / Parameters["wheelRadius"]
     return frontBrakeForce, rearBrakeForce
 
-def calcBrakeCooling(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcBrakeCooling(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     """
     Calculate the cooled brake temperature.
 
@@ -42,7 +43,7 @@ def calcBrakeCooling(worldArray:np.ndarray, step:int) -> tuple[float,float]:
     #change = (q * parameters["brakepadThickness"])/(initTemperature * parameters["brakeThermalConductivity"] * parameters["brakeSurfaceArea"]
     #return initTemperature - change
 
-def calcBrakeHeating(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcBrakeHeating(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     # Calculate Brake Force
     frontBrakeForce, rearBrakeForce = calcBrakeForce(worldArray, step)
     # Guess energy increase based on kinetic energy decrease of the vehicle.

FullVehicleSim/Mech/general.py

@@ -5,15 +5,16 @@
 from Mech.steering import calcSlipAngle, calcYawRate
 from Mech.tireLoad import calcLoadTransfer
 import numpy as np
+from numpy import ndarray
 
-def calcResistiveForces(worldArray:np.ndarray, step:int):
+def calcResistiveForces(worldArray:ndarray[np.float64], step:int):
         if worldArray[step-1, varSpeed] <= 1e-5: # Floating point error
             return 0
         else:
             frontBrakeForce, rearBrakeForce = calcBrakeForce(worldArray, step)
             return -1 * (calcDrag(worldArray, step) + frontBrakeForce + rearBrakeForce)
 
-def calculateYawRate(worldArray:np.ndarray, step:int, initAcceleration:float, initYawRate:float, timeSinceLastSteer:float):
+def calculateYawRate(worldArray:ndarray[np.float64], 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.

FullVehicleSim/Mech/steering.py

@@ -1,8 +1,9 @@
 # Steering model
 import numpy as np
+from numpy import ndarray
 from paramLoader import *
 
-def calcSlipAngle(worldArray:np.ndarray, step:int) -> tuple[float,float]:
+def calcSlipAngle(worldArray:ndarray[np.float64], step:int) -> tuple[float,float]:
     """
     Calculate Slip Angle Based on yawRate, Velocity, and Steering Angle.
 

FullVehicleSim/SimulationControlInputs/simulationControls.csv

@@ -1,6 +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
+10.0,1.0,0.0,0.0,0.1
+20.0,0.0,0.0,0.0,0.1
+30.0,0.0,300.0,300.0,0.1
 40.0,0.0,0.0,0.0,0.0

FullVehicleSim/SimulationControlInputs/testing.csv

@@ -0,0 +1,17 @@
+time,throttle,brakePressureFront,brakePressureRear,steerAngle
+0.0,0.0,0.0,0.0,0.0
+5.0,1.0,0.0,0.0,0.0
+10.0,1.0,0.0,0.0,0.0
+15.0,0.7,0.0,0.0,0.1
+30.0,0.7,0.0,0.0,0.1
+35.0,0.7,0.0,0.0,-0.1
+45.0,0.7,0.0,0.0,0.1
+50.0,0.7,0.0,0.0,0.0
+52.0,0.7,0.0,0.0,0.15
+55.0,0.7,0.0,0.0,0.15
+60.0,0.7,0.0,0.0,-0.1
+75.0,0.7,0.0,0.0,-0.1
+80.0,0.0,200.0,200.0,-0.1
+90.0,0.0,200.0,200.0,0.0
+100.0,0.0,0.0,0.0,0.0
+110.0,0.0,0.0,0.0,0.0

FullVehicleSim/engine.py

@@ -1,15 +1,17 @@
 from paramLoader import *
 import numpy as np
+from numpy import ndarray
 from Mech.braking import calcBrakeCooling, calcBrakeHeating, calcBrakeForce
 from Mech.aero import calcDrag, calcDownForce
 from Mech.steering import calcSlipAngle
 from Mech.general import calcResistiveForces
 from Electrical.powertrain import calcCurrent, calcMaxMotorTorque, calcMaxWheelTorque, calcMotorForce, calcMaxPower, calcVoltage
+from yaw_rate_model.double_bicycle_model import calcYawRate
 from scipy.integrate import RK45
 
 # Vibe coded but it looks about right so idk.
 # TODO: Verify that this is correct
-def calculateHeading(worldArray:np.ndarray, step:int) -> np.ndarray:
+def calculateHeading(worldArray:ndarray[np.float64], 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
@@ -26,7 +28,7 @@ def calculateHeading(worldArray:np.ndarray, step:int) -> np.ndarray:
 
     return np.append(new_heading, 0)
 
-def stepState(worldArray:np.ndarray, step:int) -> np.ndarray:
+def stepState(worldArray:ndarray[np.float64], 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, 
@@ -75,9 +77,7 @@ def stepState(worldArray:np.ndarray, step:int) -> np.ndarray:
     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[varLateralVelocty], arr[varYawRate] = calcYawRate(worldArray, step)
     arr[varVelX:varVelZ+1] = arr[varSpeed] * worldArray[step-1, varHeadingX:varHeadingZ+1]
     arr[varHeadingX:varHeadingZ+1] = calculateHeading(worldArray, step)
 

FullVehicleSim/main.py

@@ -82,6 +82,8 @@
     worldArray[0, varHeadingX:varHeadingZ+1] = Parameters["initHeading"]
     worldArray[0, varPosX:varPosZ+1] = Parameters["initPosition"]
     worldArray[0, varVelX:varVelZ+1] = Parameters["initVelocity"]
+    worldArray[0, varLateralVelocty] = Parameters["InitLateralVelocity"] # velocity in y direction (needed for yaw rate)
+    worldArray[0, varYawRate] = Parameters["InitYawRate"]
     worldArray[:, varTime] = np.arange(0, Parameters["simulationDuration"] + 1/Parameters["stepsPerSecond"], 1/Parameters["stepsPerSecond"])
 
     start = time.time()
@@ -122,35 +124,37 @@
     torque = df['motorTorque']
     yawRate = df['yawRate']
     frontBrakeTemperature = df['frontBrakeTemperature']
-    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)
+    # ax1 = plt.subplot(411)
+    # ax11 = ax1.twinx()
+    # ax2 = plt.subplot(412)
+    # ax22 = ax2.twinx()
+    # ax3 = plt.subplot(413)
+    # ax33 = ax3.twinx()
+    ax4 = plt.subplot(111)
     ax44 = ax4.twinx()
 
-    ax1.set_title("I (Blue)/V (Orange) vs Time")
-    ax1.set_xlabel("Time (s)")
-    ax1.set_ylabel("Current (A) / Voltage (V)")
-    ax1.plot(t, current, label="Current")
-    ax11.plot(t, voltage, label="Voltage", color='orange')
-    ax1.legend()
-
-    ax2.set_title("Speed vs Time")
-    ax2.set_xlabel("Time (s)")
-    ax2.set_ylabel("Speed (m/s)")
-    ax2.plot(t, speed)
-
-    ax3.set_title("Throttle (Blue)/Brakes (Orange) vs Time")
-    ax3.set_xlabel("Time (s)")
-    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")
+    # ax1.set_title("I (Blue)/V (Orange) vs Time")
+    # ax1.set_xlabel("Time (s)")
+    # ax1.set_ylabel("Current (A) / Voltage (V)")
+    # ax1.plot(t, current, label="Current")
+    # ax11.plot(t, voltage, label="Voltage", color='orange')
+    # ax1.legend()
+
+    # ax2.set_title("Speed vs Time")
+    # ax2.set_xlabel("Time (s)")
+    # ax2.set_ylabel("Speed (m/s)")
+    # ax2.plot(t, speed)
+
+    # ax3.set_title("Throttle (Blue)/Brakes (Orange) vs Time")
+    # ax3.set_xlabel("Time (s)")
+    # 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_xlabel("Time (s)")
+    ax4.set_ylabel("Yaw Rate (radians)")
+    ax4.set_title("yawRate")
     ax4.plot(t, yawRate)
 
     #ax4.set_ylim([0, 190])

FullVehicleSim/paramLoader.py

@@ -67,6 +67,7 @@
 varMaxMotorTorque = 41
 varAcceleration = 42
 varWheelRPM = 43
+varLateralVelocty = 44
 
 # Automatically generate schema from defined variables
 def generate_variable_schema() -> Dict[int, str]:

FullVehicleSim/params.json5

@@ -57,6 +57,16 @@
 
         "GasConstant": 8.31446261815324,
         "FaradaysConstant": 96485.33212,
+
+        "InitLateralVelocity": 0,
+        "InitYawRate": 0,
+        "tw": 1083.3862, // mm (simplified track width from steering axis to steering axis [steering axis is also simplified to be A-arm knuckle to A-arm knuckle])
+        "rackRatio": 0.332862903226, // 82.55/248 [4] mm rack displacement/deg pinion rotation
+        "rackShift": 0.0, //  mm of movement of the rack from left to right (left is - right is +)
+        "l_rack": 292.1, // [4] mm (width of steering rack casing)
+        "l_rod": 378.9426, // [3] mm (length of tie rod as left in FS-3 master CAD)
+        "d": 109.7788, // [3] mm (plan view distance between front axis and rack. negative because we have a front steer setup)
+        "l_arm": 71.628 // [3] mm (length of "steer arm", which is the distance from the center of the upright toe rod pickup to the KPA)
     },
     "Magic": {
 
@@ -171,5 +181,129 @@
         "pressureYB": -9.812999633140862e-05,
         "pressureYC": 0.9998338222503662,
         "gysign": -0.10000000149011612,
-    }
+
+        "q_v2": 1.9079276329655327e-36,
+        "loadA": -0.08525806665420532,
+        "loadB": 0.8007363080978394,
+        "loadC": 3.823115110397339,
+        "q_Fcx": 0.40706977248191833,
+        "q_Fcy": 0.40706977248191833,
+        "Omega": 1.9079276329655327e-36,
+        "q_Fz1": 0.10348843783140182,
+        "q_Fz2": 0.10000000149011612,
+        "P_pFz1": -0.38212499022483826
+    },
+    "Magic-Parameters": {
+        "q_v2": 1.9079276329655327e-36,
+        "Omega": 1.9079276329655327e-36,
+        "q_Fcx": 0.40706977248191833,
+        "q_Fcy": 0.40706977248191833,
+        "q_Fz1": 0.10348843783140182,
+        "q_Fz2": 0.10000000149011612,
+        "P_pFz1": -0.38212499022483826,
+        "shape-factor": 0.696268618106842,
+        "curvature-scaling-factor": 1.7177082300186157,
+        "horizontal-shift-factor": 1.0,
+        "vertical-shift-factor": 0.9601083397865295,
+        "zeta_1": 0.6045444011688232,
+        "p_px1": 0.013196180574595928,
+        "p_px2": 0.8705743551254272,
+        "p_px3": -0.9542407989501953,
+        "p_px4": -0.9245550036430359,
+        "p_dx1": 0.642947793006897,
+        "p_dx2": 0.9363532066345215,
+        "p_dx3": 0.0,
+        "p_ex1": 0.5635436177253723,
+        "p_ex2": -0.5660403966903687,
+        "p_ex3": 0.8500840663909912,
+        "p_ex4": -0.005640119314193726,
+        "p_kx1": 21.55539894104004,
+        "p_kx2": 13.510042190551758,
+        "p_kx3": -0.5750095248222351,
+        "p_hx1": 0.0021615000441670418,
+        "p_hx2": 0.0011597999837249517,
+        "p_vx1": 0.41343268752098083,
+        "p_vx2": -0.28164142370224,
+        "lambda_loadscalarlong": 1.1716148853302002,
+        "lambda_pressurescalarlong": 0.4518716633319855,
+        "tempXAPure": -0.01220773346722126,
+        "tempXBPure": 0.9834595322608948,
+        "tempXCPure": 3.0494847297668457,
+        "r_bx1": 13.225804328918457,
+        "r_bx2": 9.537531852722168,
+        "r_bx3": 0.0,
+        "r_cx1": 1.148189902305603,
+        "r_ex1": -0.23479725420475006,
+        "r_ex2": -0.27553272247314453,
+        "r_hx1": -0.11146939545869827,
+        "lambda_alphastar": 1.14445960521698,
+        "lambda_xalpha": 1.1735954284667969,
+        "lambda_combinedslipcoeff": 0.8948060870170593,
+        "combined_long_offset": 0.8635340929031372,
+        "tempXA": -0.001096199150197208,
+        "tempXB": -0.004315061029046774,
+        "tempXC": 0.9925193786621094,
+        "By_pure": 0.17780134081840515,
+        "p_cy1": 1.2041044235229492,
+        "p_dy1": -0.31575706601142883,
+        "p_dy2": -1.0540627241134644,
+        "p_dy3": 0.0,
+        "p_ey1": -1.6930402517318726,
+        "p_ey2": -2.0134072303771973,
+        "p_ey3": 0.21373338997364044,
+        "p_ey4": -6.697000026702881,
+        "p_ey5": 0.0,
+        "p_ky1": -15.324000358581543,
+        "p_ky2": 1.715000033378601,
+        "p_ky3": 0.3695000112056732,
+        "p_ky4": -6.697000026702881,
+        "p_ky5": 0.0,
+        "p_py1": -0.6255000233650208,
+        "p_py2": -0.06522999703884125,
+        "p_py3": 1.8217451724922284e-06,
+        "p_py4": -0.26475754380226135,
+        "Svy": 31.63314437866211,
+        "zeta3": 1.0,
+        "epsilon_y": 0.009999999776482582,
+        "lambda_alphastarypure": 0.49190351366996765,
+        "lambda_ey": -1.1078534126281738,
+        "lambda_kyalpha": 1.0,
+        "lambda_nominalload": 1.0,
+        "lambda_coeffscalary": 0.8558508157730103,
+        "lambda_loadscalarlat": 0.7529077529907227,
+        "lambda_pressurescalarlat": 1.0000001192092896,
+        "tempYAPure": 0.035267509520053864,
+        "tempYBPure": -4.318601608276367,
+        "tempYCPure": 0.130745992064476,
+        "Byk": 0.7075067758560181,
+        "Cyk": 1.7348066568374634,
+        "Eyk": 0.9768351912498474,
+        "Shyk": 0.6505736708641052,
+        "Svyk": -6.833913803100586,
+        "r_by1": 9.446654319763184,
+        "r_by2": 7.820000171661377,
+        "r_by3": 0.00203699991106987,
+        "r_by4": 0.0,
+        "r_cy1": 0.8584131598472595,
+        "r_ey1": -1.0929341316223145,
+        "r_ey2": -1.7355691194534302,
+        "r_hy1": 0.11233723163604736,
+        "r_hy2": -0.23296165466308594,
+        "r_vy1": 0.534748911857605,
+        "r_vy2": 0.48336413502693176,
+        "r_vy3": 0.0,
+        "r_vy4": 93.80660247802734,
+        "r_vy5": 1.9759562015533447,
+        "r_vy6": 23.584060668945312,
+        "lambda_yk": 1.0,
+        "lambda_vyk": 1.7570732831954956,
+        "zeta_2": 1.7570732831954956,
+        "pressureYA": -3.086921788053587e-05,
+        "pressureYB": -9.812999633140862e-05,
+        "pressureYC": 0.9998338222503662,
+        "gysign": -0.10000000149011612,
+        "loadA": -0.08525806665420532,
+        "loadB": 0.8007363080978394,
+        "loadC": 3.823115110397339
+  }
 }