40 lines
1.4 KiB
Python
40 lines
1.4 KiB
Python
|
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()
|