130 lines
4.1 KiB
Python
130 lines
4.1 KiB
Python
from pybricks.hubs import PrimeHub
|
||
from pybricks.parameters import Axis, Direction, Port, Stop
|
||
from pybricks.pupdevices import Motor
|
||
from pybricks.robotics import DriveBase
|
||
from pybricks.tools import wait
|
||
|
||
from assi import A
|
||
|
||
DEFAULT_STRAIGHT_SPEED: int = 100 # mm/s
|
||
DEFAULT_STRAIGHT_ACCELERATION: int = 100 # mm/s²
|
||
DEFAULT_TURN_RATE: int = 90 # deg/s
|
||
DEFAULT_TURN_ACCELERATION: int = 50 # deg/s²
|
||
DEFAULT_GYRO_SETTLE_MS: int = 300 # ms
|
||
|
||
|
||
class Robot:
|
||
"""Inizializza Hub, Motori e DriveBase con impostazioni coerenti.
|
||
|
||
Parametri:
|
||
name: Nome logico del robot (per logging/diagnostica).
|
||
top_side: Faccia dell'hub che punta verso l'alto (Axis).
|
||
front_side: Faccia dell'hub che definisce l'“avanti” del robot (Axis).
|
||
left_port: Porta del motore sinistro (guardando verso l'“avanti”).
|
||
right_port: Porta del motore destro.
|
||
left_direction: Verso del motore sinistro (Direction).
|
||
right_direction: Verso del motore destro (Direction).
|
||
wheel_diameter: Diametro ruote in millimetri (mm).
|
||
axle_track: Distanza tra i punti di contatto a terra delle due ruote (mm).
|
||
use_gyro: Se True abilita il giroscopio per turn/straight.
|
||
straight_speed: Velocità lineare predefinita (mm/s).
|
||
straight_acceleration: Accelerazione lineare predefinita (mm/s²).
|
||
turn_rate: Velocità di rotazione predefinita (deg/s).
|
||
turn_acceleration: Accelerazione di rotazione predefinita (deg/s²).
|
||
settle_ms: Millisecondi di attesa dopo l’abilitazione del gyro.
|
||
|
||
Attributi:
|
||
hub: Istanza di PrimeHub configurata con l’orientamento dato.
|
||
left: Motore sinistro.
|
||
right: Motore destro.
|
||
db: DriveBase pronta all’uso (settings applicati).
|
||
"""
|
||
|
||
name: str
|
||
hub: PrimeHub
|
||
left: Motor
|
||
right: Motor
|
||
db: DriveBase
|
||
|
||
def __init__(
|
||
self,
|
||
name: str,
|
||
top_side: Axis,
|
||
front_side: Axis,
|
||
left_port: Port,
|
||
right_port: Port,
|
||
left_direction: Direction,
|
||
right_direction: Direction,
|
||
wheel_diameter: float,
|
||
axle_track: int,
|
||
use_gyro: bool,
|
||
straight_speed: int = DEFAULT_STRAIGHT_SPEED,
|
||
straight_acceleration: int = DEFAULT_STRAIGHT_ACCELERATION,
|
||
turn_rate: int = DEFAULT_TURN_RATE,
|
||
turn_acceleration: int = DEFAULT_TURN_ACCELERATION,
|
||
settle_ms: int = DEFAULT_GYRO_SETTLE_MS,
|
||
) -> None:
|
||
self.name = name
|
||
|
||
self.hub = PrimeHub(top_side=top_side, front_side=front_side)
|
||
|
||
self.left = Motor(left_port, left_direction)
|
||
self.right = Motor(right_port, right_direction)
|
||
|
||
self.db = DriveBase(
|
||
self.left,
|
||
self.right,
|
||
wheel_diameter=wheel_diameter,
|
||
axle_track=axle_track,
|
||
)
|
||
|
||
self.db.settings(
|
||
straight_speed,
|
||
straight_acceleration,
|
||
turn_rate,
|
||
turn_acceleration,
|
||
)
|
||
|
||
if use_gyro:
|
||
self.db.use_gyro(True)
|
||
wait(settle_ms)
|
||
self.db.reset(angle=0)
|
||
else:
|
||
self.db.reset()
|
||
|
||
|
||
LEOHUB: Robot = Robot(
|
||
name="leohub",
|
||
top_side=A.UP,
|
||
front_side=A.BACKWARD,
|
||
left_port=Port.B,
|
||
right_port=Port.A,
|
||
left_direction=Direction.COUNTERCLOCKWISE,
|
||
right_direction=Direction.CLOCKWISE,
|
||
wheel_diameter=56,
|
||
axle_track=105,
|
||
use_gyro=True,
|
||
straight_speed=DEFAULT_STRAIGHT_SPEED,
|
||
straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
|
||
turn_rate=DEFAULT_TURN_RATE,
|
||
turn_acceleration=DEFAULT_TURN_ACCELERATION,
|
||
settle_ms=DEFAULT_GYRO_SETTLE_MS,
|
||
)
|
||
|
||
CBHUB: Robot = Robot(
|
||
name="cbhub",
|
||
top_side=A.UP,
|
||
front_side=A.RIGHT,
|
||
left_port=Port.B,
|
||
right_port=Port.A,
|
||
left_direction=Direction.COUNTERCLOCKWISE,
|
||
right_direction=Direction.CLOCKWISE,
|
||
wheel_diameter=49.5,
|
||
axle_track=88,
|
||
use_gyro=True,
|
||
straight_speed=DEFAULT_STRAIGHT_SPEED,
|
||
straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
|
||
turn_rate=DEFAULT_TURN_RATE,
|
||
turn_acceleration=DEFAULT_TURN_ACCELERATION,
|
||
settle_ms=DEFAULT_GYRO_SETTLE_MS,
|
||
)
|