diff --git a/cones.json b/cones.json new file mode 100644 index 0000000..a5eacf1 --- /dev/null +++ b/cones.json @@ -0,0 +1,12 @@ +{ + "cones": [ + { "id": 0, "color": "red", "position": [5.0, 0.0, 0.0] }, + { "id": 1, "color": "blue", "position": [3.54, 3.54, 0.0] }, + { "id": 2, "color": "yellow","position": [0.0, 5.0, 0.0] }, + { "id": 3, "color": "red", "position": [-3.54, 3.54, 0.0] }, + { "id": 4, "color": "blue", "position": [-5.0, 0.0, 0.0] }, + { "id": 5, "color": "yellow","position": [-3.54, -3.54, 0.0] }, + { "id": 6, "color": "red", "position": [0.0, -5.0, 0.0] }, + { "id": 7, "color": "blue", "position": [3.54, -3.54, 0.0] } + ] +} diff --git a/cones_plot.py b/cones_plot.py new file mode 100644 index 0000000..dcd5762 --- /dev/null +++ b/cones_plot.py @@ -0,0 +1,95 @@ +import json +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d.art3d import Poly3DCollection + + +## loading the json file +with open("/Users/atvyas/Desktop/coding/FS/sim_int/cones.json", "r") as f: + data = json.load(f) + +cones = data["cones"] +positions = np.array([cone["position"] for cone in cones]) +colors = [cone["color"] for cone in cones] + + +## Drawing the cones: +def draw_cone(ax, x, y, z, height=0.4, radius=0.5, color="orange"): + # sqaure base to the cones + half = radius + square = [ + [x - half, y - half, z], + [x + half, y - half, z], + [x + half, y + half, z], + [x - half, y + half, z] + ] + + base = Poly3DCollection( + [square], + facecolors=color, + alpha=0.6 + ) + ax.add_collection3d(base) + + # the actual cones + theta = np.linspace(0, 2 * np.pi, 1) + r = np.linspace(0, radius, 2) + T, R = np.meshgrid(theta, r) + + X = x + R * np.cos(T) + Y = y + R * np.sin(T) + Z = z + height * (1 - R / radius) + + ax.plot_surface(X, Y, Z, color=color, alpha=0.7, linewidth=0) + + + # Cone surface + theta = np.linspace(0, 2*np.pi, 5) + r = np.linspace(0, radius, 2) + T, R = np.meshgrid(theta, r) + + X = x + R * np.cos(T) + Y = y + R * np.sin(T) + Z = z + height * (1 - R / radius) + + ax.plot_surface(X, Y, Z, color=color, alpha=0.7, linewidth=0) + + +## Plotting the cones in 3-D +fig = plt.figure(figsize=(14, 7)) + +# 3d +ax3d = fig.add_subplot(121, projection="3d") + +for (x, y, z), color in zip(positions, colors): + draw_cone(ax3d, x, y, z, height=0.5, radius=0.5, color=color) + +ax3d.set_title("3D Cone View") +ax3d.set_xlabel("X") +ax3d.set_ylabel("Y") +ax3d.set_zlabel("Z") + +ax3d.set_xlim(-6, 6) +ax3d.set_ylim(-6, 6) +ax3d.set_zlim(0, 3) + +# top down +ax2d = fig.add_subplot(122) + +x = positions[:, 0] +y = positions[:, 1] + +ax2d.scatter(x, y, c=colors, s=120, edgecolors="black") + +# setting up the grid +ax2d.set_title("Top-Down View (X-Y)") +ax2d.set_xlabel("X") +ax2d.set_ylabel("Y") +ax2d.set_aspect("equal") +ax2d.set_xlim(-6, 6) +ax2d.set_ylim(-6, 6) +ax2d.grid(True) + +# to display the graphs +plt.tight_layout() +plt.show() diff --git a/data_rep.py b/data_rep.py new file mode 100644 index 0000000..6223c36 --- /dev/null +++ b/data_rep.py @@ -0,0 +1,56 @@ +import numpy as np +import matplotlib.pyplot as plt +from state_test import ( + position, speed, heading, + acceleration, throttle, brakes, lastCurrent +) + +# sim time set up +dt = 0.1 # time step (seconds) +t_end = 10.0 # total simulation time +time = np.arange(0, t_end, dt) + +# sim values over sim time +speed_t = speed + acceleration * time +heading_t = heading + 0.02 * time # slow turn +throttle_t = np.full_like(time, throttle) +brakes_t = np.full_like(time, brakes) +current_t = lastCurrent + 0.5 * time # example trend + +# Position integration +x = position[0] + np.cumsum(speed_t * np.cos(heading_t) * dt) +y = position[1] + np.cumsum(speed_t * np.sin(heading_t) * dt) + +## plots +fig, axs = plt.subplots(3, 2, figsize=(12, 10)) +axs = axs.flatten() + +axs[0].plot(time, speed_t) +axs[0].set_title("Speed vs Time") +axs[0].set_ylabel("m/s") + +axs[1].plot(time, heading_t) +axs[1].set_title("Heading vs Time") +axs[1].set_ylabel("radians") + +axs[2].plot(time, throttle_t) +axs[2].set_title("Throttle vs Time") + +axs[3].plot(time, brakes_t) +axs[3].set_title("Brakes vs Time") + +axs[4].plot(time, current_t) +axs[4].set_title("Current vs Time") +axs[4].set_ylabel("Amps") + +axs[5].plot(x, y) +axs[5].set_title("Position (X vs Y)") +axs[5].set_xlabel("X") +axs[5].set_ylabel("Y") + +for ax in axs: + ax.set_xlabel("Time (s)") + ax.grid(True) + +plt.tight_layout() +plt.show() diff --git a/sim-int-issue1.py b/sim-int-issue1.py new file mode 100644 index 0000000..3a7baf8 --- /dev/null +++ b/sim-int-issue1.py @@ -0,0 +1,85 @@ + +'''Take vechile postion, speed, and last current as input from state.py and use numpy to find the + postions of the cones and the car and graph it on a coordinate system.''' + +import numpy as np +import matplotlib.pyplot as plt + +# Veichel state form state.py +stepSize = 0.1 +position = np.array([12.5, 7.8]) # (x, y) +speed = 18.2 # m/s +heading = np.deg2rad(15) # radians +acceleration = 1.3 # m/s^2 +throttle = 0.65 # 0–1 +brakes = 0.1 # 0–1 +lastCurrent = 42.0 # amps + +# cones postions coming from a json file +cones = np.array([ + [5, 5], + [8, 6], + [10, 7], + [13, 8], + [16, 9], + [18, 10] +]) + +# finding velcoity from heading +velocity = np.array([ + np.cos(heading), + np.sin(heading) +]) * speed + +#creating layout +fig, axs = plt.subplots(3, 1, figsize=(9, 12)) + +# cones and psition +axs[0].scatter(cones[:, 0], cones[:, 1], + marker="^", s=150, label="Cones") + +axs[0].scatter(position[0], position[1], + marker="o", s=200, label="Vehicle") + +axs[0].arrow( + position[0], position[1], + velocity[0] * 0.2, velocity[1] * 0.2, + head_width=0.4, + length_includes_head=True, + label="Velocity" +) + +axs[0].text( + position[0] + 0.5, + position[1] + 0.5, + f"Speed: {speed:.1f} m/s", + fontsize=10 +) + +axs[0].set_title("Vehicle & Cone Map") +axs[0].set_xlabel("X Position (m)") +axs[0].set_ylabel("Y Position (m)") +axs[0].axis("equal") +axs[0].grid(True) +axs[0].legend() + +#Motion data +axs[1].bar( + ["Speed (m/s)", "Acceleration (m/s²)"], + [speed, acceleration] +) + +axs[1].set_title("Vehicle Motion State") +axs[1].grid(True) + +#electrical data +axs[2].bar( + ["Throttle", "Brakes", "Motor Current (A)"], + [throttle, brakes, lastCurrent] +) + +axs[2].set_title("Control & Electrical State") +axs[2].grid(True) + +plt.tight_layout() +plt.show() diff --git a/state_test.py b/state_test.py new file mode 100644 index 0000000..896ae62 --- /dev/null +++ b/state_test.py @@ -0,0 +1,11 @@ +import numpy as np + + +# Vehicle state +position = np.array([12.0, 7.0]) +speed = 18.2 +heading = np.deg2rad(15) +acceleration = 1.3 +throttle = 0.65 +brakes = 0.1 +lastCurrent = 42.0 \ No newline at end of file