From 6ff04f3caeeb1d6559c4a0780fc1c5c29c03f5de Mon Sep 17 00:00:00 2001
From: Arcmyx Official <atharvna12@gmail.com>
Date: Wed, 6 May 2026 20:56:52 +0000
Subject: [PATCH] Add experiments/trajectory_drive.py

---
 experiments/trajectory_drive.py | 40 +++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)
 create mode 100644 experiments/trajectory_drive.py

diff --git a/experiments/trajectory_drive.py b/experiments/trajectory_drive.py
new file mode 100644
index 0000000..47948d7
--- /dev/null
+++ b/experiments/trajectory_drive.py
@@ -0,0 +1,40 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+import umath
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
+left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
+right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
+lazer_ranger = UltrasonicSensor(Port.E)
+color_sensor = ColorSensor(Port.F)
+# DriveBase configuration
+WHEEL_DIAMETER = 68.8  # mm (adjust for your wheels)
+AXLE_TRACK = 180  # mm (distance between wheels)
+drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
+drive_base.settings(600, 500, 300, 200)
+drive_base.use_gyro(True)
+
+transformations = {
+    "d": [2, 3],
+    "a": [0, 90]
+}
+current = {
+    "x": 0,
+    "y": 0,
+    "a": 0
+}
+# Extra useless code
+#for transformation in transformations:
+#    current.x += transformation.d * umath.cos(umath.radians(transformation.a))
+#    current.y += transformation.d * umath.sin(umath.radians(transformation.a))  
+def addtoposition(a, r):
+    current.x += r*cos(a)
+    current.y += r*sin(a)
+    current.a = a
+
+drive_base.drive()
\ No newline at end of file