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..2d2d12a --- /dev/null +++ b/arm/constants.py @@ -0,0 +1,11 @@ +import enum + + +@enum.unique +class SparkId(enum.IntEnum): + intake_deploy_leader = 4 + intake_deploy_follower = 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..75a4c1c --- /dev/null +++ b/arm/subsystems/arm.py @@ -0,0 +1,76 @@ +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.CANSparkBase, + follower: rev.CANSparkBase, + *, + oppose_leader: bool, + gearing: float, + name: str | None = None, + ) -> None: + self.motor = motor + self.encoder = self._init_encoder(self.motor, gearing) + if name is not None: + self.setName(name) + + self.motor.setIdleMode(rev.CANSparkBase.IdleMode.kBrake) + if True: + self.follower = follower + self.follower_encoder = self._init_encoder(follower, gearing) + follower.setIdleMode(rev.CANSparkBase.IdleMode.kBrake) + # follower.setInverted(oppose_leader) + follower.follow(self.motor, invert=oppose_leader) + + # 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(), + SysIdRoutine.Mechanism(self.motor.setVoltage, self.log, self), + ) + + def _init_encoder( + self, motor: rev.CANSparkBase, gearing: float + ) -> rev.SparkRelativeEncoder: + encoder = motor.getEncoder() + + # Measure position in rad and velocity in rad/s. + output_rad_per_motor_rev = gearing * math.tau + encoder.setPositionConversionFactor(output_rad_per_motor_rev) + encoder.setVelocityConversionFactor(output_rad_per_motor_rev / 60) + + return encoder + + # 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 True: + ( + sys_id_routine.motor("follower") + .voltage(self.follower.get() * self.follower.getBusVoltage()) + .position(self.follower_encoder.getPosition()) + .velocity(self.follower_encoder.getVelocity()) + ) + + 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..8052ba2 --- /dev/null +++ b/arm/sysidroutinebot.py @@ -0,0 +1,69 @@ +import rev +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.intake_deploy_leader), + self._create_neo(SparkId.intake_deploy_follower), + oppose_leader=True, + gearing=(1 / 5) * (1 / 3) * (24 / 72), + ) + + # The driver's controller + self.controller = CommandXboxController(OIConstants.kDriverControllerPort) + + @staticmethod + def _create_neo(id: int) -> rev.CANSparkMax: + return rev.CANSparkMax(id, rev.CANSparkMax.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) + ) + self.controller.b().whileTrue( + self.arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + ) + self.controller.x().whileTrue( + self.arm.sysIdDynamic(SysIdRoutine.Direction.kForward) + ) + self.controller.y().whileTrue( + self.arm.sysIdDynamic(SysIdRoutine.Direction.kReverse) + ) + + 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