mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
126 lines
5.0 KiB
Python
126 lines
5.0 KiB
Python
#
|
|
# Copyright (c) FIRST and other WPILib contributors.
|
|
# Open Source Software; you can modify and/or share it under the terms of
|
|
# the WPILib BSD license file in the root directory of this project.
|
|
#
|
|
|
|
from typing import Callable
|
|
|
|
from commands2 import Command, Subsystem
|
|
from commands2.sysid import SysIdRoutine
|
|
from wpilib import DifferentialDrive, Encoder, PWMSparkMax, RobotController
|
|
from wpilib.sysid import SysIdRoutineLog
|
|
|
|
from constants import DriveConstants
|
|
|
|
|
|
class Drive(Subsystem):
|
|
"""Represents the drive subsystem."""
|
|
|
|
def __init__(self) -> None:
|
|
"""Creates a new Drive subsystem."""
|
|
super().__init__()
|
|
|
|
# The motors on the left side of the drive.
|
|
self.left_motor = PWMSparkMax(DriveConstants.kLeftMotor1Port)
|
|
|
|
# The motors on the right side of the drive.
|
|
self.right_motor = PWMSparkMax(DriveConstants.kRightMotor1Port)
|
|
|
|
# The robot's drive
|
|
self.drive = DifferentialDrive(self.left_motor, self.right_motor)
|
|
|
|
# The left-side drive encoder
|
|
self.left_encoder = Encoder(
|
|
DriveConstants.kLeftEncoderPorts[0],
|
|
DriveConstants.kLeftEncoderPorts[1],
|
|
DriveConstants.kLeftEncoderReversed,
|
|
)
|
|
|
|
# The right-side drive encoder
|
|
self.right_encoder = Encoder(
|
|
DriveConstants.kRightEncoderPorts[0],
|
|
DriveConstants.kRightEncoderPorts[1],
|
|
DriveConstants.kRightEncoderReversed,
|
|
)
|
|
|
|
# Create a new SysId routine for characterizing the drive.
|
|
self.sys_id_routine = SysIdRoutine(
|
|
# Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
|
|
SysIdRoutine.Config(),
|
|
SysIdRoutine.Mechanism(
|
|
# Tell SysId how to plumb the driving voltage to the motors.
|
|
self._driveVoltage,
|
|
# Tell SysId how to record a frame of data for each motor on the mechanism being
|
|
# characterized.
|
|
self._log,
|
|
# Tell SysId to make generated commands require this subsystem, suffix test state in
|
|
# WPILog with this subsystem's name ("drive")
|
|
self,
|
|
),
|
|
)
|
|
|
|
# Add the second motors on each side of the drivetrain
|
|
self.left_motor.addFollower(PWMSparkMax(DriveConstants.kLeftMotor2Port))
|
|
self.right_motor.addFollower(PWMSparkMax(DriveConstants.kRightMotor2Port))
|
|
|
|
# We need to invert one side of the drivetrain so that positive voltages
|
|
# result in both sides moving forward. Depending on how your robot's
|
|
# gearbox is constructed, you might have to invert the left side instead.
|
|
self.right_motor.setInverted(True)
|
|
|
|
# Sets the distance per pulse for the encoders
|
|
self.left_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
|
|
self.right_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
|
|
|
|
def _driveVoltage(self, voltage: float) -> None:
|
|
self.left_motor.setVoltage(voltage)
|
|
self.right_motor.setVoltage(voltage)
|
|
|
|
def _log(self, sys_id_routine: SysIdRoutineLog) -> None:
|
|
# Record a frame for the left motors. Since these share an encoder, we consider
|
|
# the entire group to be one motor.
|
|
sys_id_routine.motor("drive-left").voltage(
|
|
self.left_motor.get() * RobotController.getBatteryVoltage()
|
|
).position(self.left_encoder.getDistance()).velocity(
|
|
self.left_encoder.getRate()
|
|
)
|
|
# Record a frame for the right motors. Since these share an encoder, we consider
|
|
# the entire group to be one motor.
|
|
sys_id_routine.motor("drive-right").voltage(
|
|
self.right_motor.get() * RobotController.getBatteryVoltage()
|
|
).position(self.right_encoder.getDistance()).velocity(
|
|
self.right_encoder.getRate()
|
|
)
|
|
|
|
def arcadeDriveCommand(
|
|
self, fwd: Callable[[], float], rot: Callable[[], float]
|
|
) -> Command:
|
|
"""Returns a command that drives the robot with arcade controls.
|
|
|
|
:param fwd: the commanded forward movement
|
|
:param rot: the commanded rotation
|
|
"""
|
|
|
|
# A split-stick arcade command, with forward/backward controlled by the left
|
|
# hand, and turning controlled by the right.
|
|
return self.run(lambda: self.drive.arcadeDrive(fwd(), rot())).withName(
|
|
"arcadeDrive"
|
|
)
|
|
|
|
def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
|
|
"""Returns a command that will execute a quasistatic test in the given direction.
|
|
|
|
:param direction: The direction (forward or reverse) to run the test in
|
|
"""
|
|
|
|
return self.sys_id_routine.quasistatic(direction)
|
|
|
|
def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
|
|
"""Returns a command that will execute a dynamic test in the given direction.
|
|
|
|
:param direction: The direction (forward or reverse) to run the test in
|
|
"""
|
|
|
|
return self.sys_id_routine.dynamic(direction)
|