51 lines
1.9 KiB
Python
51 lines
1.9 KiB
Python
|
from pybricks.parameters import Direction, Port, Side, Stop
|
||
|
|
from pybricks.robotics import DriveBase
|
||
|
|
from pybricks.tools import wait, StopWatch
|
||
|
|
|
||
|
|
from usys import stdin
|
||
|
|
from uselect import poll
|
||
|
|
|
||
|
|
class DriveBaseDiagnostics:
|
||
|
|
def __init__(self, hub, motorclass, dbclass):
|
||
|
|
self.PORT_MAP = {
|
||
|
|
"A": Port.A,
|
||
|
|
"B": Port.B,
|
||
|
|
"C": Port.C,
|
||
|
|
"D": Port.D,
|
||
|
|
"E": Port.E,
|
||
|
|
"F": Port.F,
|
||
|
|
}
|
||
|
|
def initializeDriveBase():
|
||
|
|
leftmotorport = input("Enter the left motor port: ")
|
||
|
|
left_motor = self.motorclass(port_map[leftmotorport], Direction.COUNTERCLOCKWISE)
|
||
|
|
rightmotorport = input("Enter the right motor port: ")
|
||
|
|
right_motor = self.motorclass(port_map[rightmotorport],Direction.CLOCKWISE)
|
||
|
|
# DriveBase configuration
|
||
|
|
WHEEL_DIAMETER = 68.8 # mm
|
||
|
|
AXLE_TRACK = 180 # mm
|
||
|
|
drive_base = self.dbclass(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||
|
|
drive_base.settings(600, 500, 300, 200)
|
||
|
|
drive_base.use_gyro(True)
|
||
|
|
def driveRobot():
|
||
|
|
# 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")
|
||
|
|
elif(key == "X")
|
||
|
|
break
|
||
|
|
else:
|
||
|
|
print("That key doesn't do anything.")
|