diff --git a/arm/.wpilib/wpilib_preferences.json b/arm/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..9629046 --- /dev/null +++ b/arm/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 4774} diff --git a/arm/constants.py b/arm/constants.py new file mode 100644 index 0000000..80eff32 --- /dev/null +++ b/arm/constants.py @@ -0,0 +1,10 @@ +import enum + + +@enum.unique +class SparkId(enum.IntEnum): + wrist_motor = 5 + + +class OIConstants: + kDriverControllerPort = 0 diff --git a/arm/robot.py b/arm/robot.py new file mode 100644 index 0000000..9ac757f --- /dev/null +++ b/arm/robot.py @@ -0,0 +1,48 @@ +from commands2 import CommandScheduler, TimedCommandRobot +from urcl import URCL + +from sysidroutinebot import SysIdRoutineBot + + +class MyRobot(TimedCommandRobot): + """The VM is configured to automatically run this class, and to call the functions corresponding to + each mode, as described in the TimedRobot documentation. If you change the name of this class or + the package after creating this project, you must also update the build.gradle file in the + project. + """ + + def robotInit(self) -> None: + """This function is run when the robot is first started up and should be used for any + initialization code. + """ + URCL.start() + + self.robot = SysIdRoutineBot() + self.robot.configureBindings() + + self.autonomous_command = self.robot.getAutonomousCommand() + + def disabledInit(self) -> None: + """This function is called once each time the robot enters Disabled mode.""" + pass + + def disabledPeriodic(self) -> None: + pass + + def autonomousInit(self) -> None: + self.autonomous_command.schedule() + + def teleopInit(self) -> None: + # This makes sure that the autonomous stops running when + # teleop starts running. If you want the autonomous to + # continue until interrupted by another command, remove + # this line or comment it out. + self.autonomous_command.cancel() + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll() + + def testPeriodic(self) -> None: + """This function is called periodically during test mode.""" + pass diff --git a/arm/subsystems/arm.py b/arm/subsystems/arm.py new file mode 100644 index 0000000..ea90a9e --- /dev/null +++ b/arm/subsystems/arm.py @@ -0,0 +1,103 @@ +import math + +import rev +from commands2 import Command, Subsystem +from commands2.sysid import SysIdRoutine +from wpilib import sysid + + +class Arm(Subsystem): + def __init__( + self, + motor: rev.SparkMax, + follower: rev.SparkMax | None, + *, + oppose_leader: bool, + gearing: float, + lower_limit: float, + upper_limit: float, + motor_inverted: bool, + name: str | None = None, + ) -> None: + self.motor = motor + config = self._create_smax_config(gearing, upper_limit, lower_limit, motor_inverted) + self._configure_ephemeral(motor, config) + self.encoder = motor.getEncoder() + self.upper_limit = upper_limit + self.lower_limit = lower_limit + if name is not None: + self.setName(name) + + self.follower = follower + + if self.follower is not None: + + self.follower_encoder = self.follower.getEncoder() + follower_config = self._create_smax_config(gearing, upper_limit, lower_limit, motor_inverted) + follower_config.follow(motor, invert=oppose_leader) + self._configure_ephemeral(self.follower, follower_config) + + # Tell SysId to make generated commands require this subsystem, suffix test state in + # WPILog with this subsystem's name ("drive") + self.sys_id_routine = SysIdRoutine( + SysIdRoutine.Config(rampRate=3, stepVoltage=7), + SysIdRoutine.Mechanism(self.motor.setVoltage, self.log, self), + ) + + @classmethod + def _create_smax_config(cls, gearing: float, upper_limit: float, lower_limit: float, inverted: bool) -> rev.SparkMaxConfig: + config = rev.SparkMaxConfig() + config.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake) + config.softLimit.reverseSoftLimitEnabled(True) + config.softLimit.reverseSoftLimit(lower_limit) + config.softLimit.forwardSoftLimitEnabled(True) + config.softLimit.forwardSoftLimit(upper_limit) + config.inverted(inverted) + + # Measure position in rad and velocity in rad/s. + output_rad_per_motor_rev = (1/gearing) * math.tau + config.encoder.positionConversionFactor(output_rad_per_motor_rev) + config.encoder.velocityConversionFactor(output_rad_per_motor_rev / 60) + + return config + + @classmethod + def _configure_ephemeral(cls, motor: rev.SparkBase, config: rev.SparkBaseConfig): + motor.configure( + config, + rev.SparkBase.ResetMode.kNoResetSafeParameters, + rev.SparkBase.PersistMode.kNoPersistParameters, + ) + + # Tell SysId how to record a frame of data for each motor on the mechanism being + # characterized. + def log(self, sys_id_routine: sysid.SysIdRoutineLog) -> None: + ( + sys_id_routine.motor("leader") + .voltage(self.motor.get() * self.motor.getBusVoltage()) + .position(self.encoder.getPosition()) + .velocity(self.encoder.getVelocity()) + ) + if self.follower is not None: + ( + sys_id_routine.motor("follower") + .voltage(self.follower.get() * self.follower.getBusVoltage()) + .position(self.follower_encoder.getPosition()) + .velocity(self.follower_encoder.getVelocity()) + ) + + + def below_upper_limit(self) -> bool: + return self.encoder.getPosition() < self.upper_limit + + def above_lower_limit(self) -> bool: + return self.encoder.getPosition() > self.lower_limit + + def defaultCommand(self) -> Command: + return self.run(lambda: None) + + def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command: + return self.sys_id_routine.quasistatic(direction) + + def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command: + return self.sys_id_routine.dynamic(direction) diff --git a/arm/sysidroutinebot.py b/arm/sysidroutinebot.py new file mode 100644 index 0000000..be73096 --- /dev/null +++ b/arm/sysidroutinebot.py @@ -0,0 +1,72 @@ +import rev +import math +from commands2 import Command +from commands2.button import CommandXboxController +from commands2.sysid import SysIdRoutine + +from subsystems.arm import Arm + +from constants import OIConstants, SparkId + +class SysIdRoutineBot: + """This class is where the bulk of the robot should be declared. Since Command-based is a + "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` + periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + subsystems, commands, and button mappings) should be declared here. + """ + + def __init__(self) -> None: + # The robot's subsystems + self.arm = Arm( + self._create_neo(SparkId.wrist_motor), + None, + oppose_leader=True, + gearing=432, + upper_limit=math.radians(-10.0), + lower_limit=math.radians(-113.0), + motor_inverted=False + ) + + # The driver's controller + self.controller = CommandXboxController(OIConstants.kDriverControllerPort) + + @staticmethod + def _create_neo(id: int) -> rev.SparkMax: + return rev.SparkMax(id, rev.SparkMax.MotorType.kBrushless) + + def configureBindings(self) -> None: + """Use this method to define bindings between conditions and commands. These are useful for + automating robot behaviors based on button and sensor input. + + Should be called during :meth:`.Robot.robotInit`. + + Event binding methods are available on the :class:`.Trigger` class. + """ + + # Control the drive with split-stick arcade controls + self.arm.setDefaultCommand(self.arm.defaultCommand()) + + # Bind full set of SysId routine tests to buttons; a complete routine should run each of these + # once. + + self.controller.a().whileTrue( + self.arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward).onlyWhile(self.arm.below_upper_limit) + ) + self.controller.b().whileTrue( + self.arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse).onlyWhile(self.arm.above_lower_limit) + ) + self.controller.x().whileTrue( + self.arm.sysIdDynamic(SysIdRoutine.Direction.kForward).onlyWhile(self.arm.below_upper_limit) + ) + self.controller.y().whileTrue( + self.arm.sysIdDynamic(SysIdRoutine.Direction.kReverse).onlyWhile(self.arm.above_lower_limit) + ) + + def getAutonomousCommand(self) -> Command: + """Use this to define the command that runs during autonomous. + + Scheduled during :meth:`.Robot.autonomousInit`. + """ + + # Do nothing + return self.arm.run(lambda: None) diff --git a/arm/tests/pyfrc_test.py b/arm/tests/pyfrc_test.py new file mode 100644 index 0000000..03d3a1b --- /dev/null +++ b/arm/tests/pyfrc_test.py @@ -0,0 +1,6 @@ +""" +This test module imports tests that come with pyfrc, and can be used +to test basic functionality of just about any robot. +""" + +from pyfrc.tests import * # noqa: F403