Add diagnostics/DriveBaseDiagnostics.py

diagnostics/DriveBaseDiagnostics.py

@@ -0,0 +1,49 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+
+from usys import stdin
+from uselect import poll
+
+hub = PrimeHub()
+port_map = {
+    "A": Port.A,
+    "B": Port.B,
+    "C": Port.C,
+    "D": Port.D,
+    "E": Port.E,
+    "F": Port.F,
+    }
+leftmotorport = input("Enter the left motor port: ")
+left_motor = Motor(port_map[leftmotorport], Direction.COUNTERCLOCKWISE)
+rightmotorport = input("Enter the right motor port: ")
+right_motor = Motor(port_map[rightmotorport],Direction.CLOCKWISE) # Specify default direction
+
+# 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)
+# Register the standard input so we can read keyboard presses.
+keyboard = poll()
+keyboard.register(stdin)
+
+while True:
+    # Check if a key has been pressed.
+    if keyboard.poll(0):
+        # Read the key and print it.
+        key = stdin.read(1)
+        print("You pressed:", key)
+        if(key == "W"):
+            print("Insert robot go forward code here")
+        elif(key == "A"):
+            print("Insert robot go left code here")
+        elif(key == "S"):
+            print("Insert robot go backwards code here")
+        elif(key == "D"):
+            print("Insert robot go right code here")
+        else:
+            print("That key doesn't do anything.")