Hatchbot inlined

commands-v2/hatchbot-inlined/commands/autos.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+#
+# 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.
+#
+
+import commands2
+import commands2.cmd
+
+import subsystems.drivesubsystem
+import subsystems.hatchsubsystem
+
+import constants
+
+class Autos():
+    """Container for auto command factories."""
+
+    def __init__(self) -> None:
+        raise Exception("This is a utility class!")
+
+    def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem):
+        """A simple auto routine that drives forward a specified distance, and then stops."""
+        return commands2.FunctionalCommand(
+            # Reset encoders on command start
+            drive.resetEncoders,
+            # Drive forward while the command is executing,
+            lambda: drive.arcadeDrive(constants.kAutoDriveSpeed, 0),
+            # Stop driving at the end of the command
+            lambda interrupt: drive.arcadeDrive(0, 0),
+            # End the command when the robot's driven distance exceeds the desired value
+            lambda: drive.getAverageEncoderDistance() >= constants.kAutoDriveDistanceInches,
+            # Require the drive subsystem
+        )
+
+    def complexAuto(driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem):
+        """A complex auto routine that drives forward, drops a hatch, and then drives backward."""
+        return commands2.cmd.sequence([
+            # Drive forward up to the front of the cargo ship
+            commands2.FunctionalCommand(
+                # Reset encoders on command start
+                driveSubsystem.resetEncoders,
+                # Drive forward while the command is executing
+                lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0),
+                # Stop driving at the end of the command
+                lambda interrupt: driveSubsystem.arcadeDrive(0,0),
+                # End the command when the robot's driven distance exceeds the desired value
+                lambda: driveSubsystem.getAverageEncoderDistance() >= constants.kAutoDriveDistanceInches,
+                # Require the drive subsystem
+                [driveSubsystem]
+            ),
+
+            # Release the hatch
+            hatchSubsystem.releaseHatch(),
+
+            # Drive backward the specified distance
+            commands2.FunctionalCommand(
+                # Reset encoders on command start
+                driveSubsystem.resetEncoders,
+                # Drive backwards while the command is executing
+                lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0),
+                # Stop driving at the end of the command
+                lambda interrupt: driveSubsystem.arcadeDrive(0,0),
+                # End the command when the robot's driven distance exceeds the desired value
+                lambda: abs(driveSubsystem.getAverageEncoderDistance()) >= constants.kAutoBackupDistanceInches,
+                # Require the drive subsystem
+                [driveSubsystem]
+            )
+        ])

commands-v2/hatchbot-inlined/constants.py

@@ -0,0 +1,45 @@
+#
+# The constants module is a convenience place for teams to hold robot-wide
+# numerical or boolean constants. Don't use this for any other purpose!
+#
+
+import math
+import wpilib
+
+# Motors
+kLeftMotor1Port = 0
+kLeftMotor2Port = 1
+kRightMotor1Port = 2
+kRightMotor2Port = 3
+
+# Encoders
+kLeftEncoderPorts = (0, 1)
+kRightEncoderPorts = (2, 3)
+kLeftEncoderReversed = False
+kRightEncoderReversed = True
+
+kEncoderCPR = 1024
+kWheelDiameterInches = 6
+# Assumes the encoders are directly mounted on the wheel shafts
+kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR
+
+# Hatch
+kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM
+kHatchSolenoidModule = 0
+kHatchSolenoidPorts = (0, 1)
+
+# Autonomous
+kAutoDriveDistanceInches = 60
+kAutoBackupDistanceInches = 20
+kAutoDriveSpeed = 0.5
+
+# Operator Interface
+kDriverControllerPort = 0
+
+# Physical parameters
+kDriveTrainMotorCount = 2
+kTrackWidth = 0.381 * 2
+kGearingRatio = 8
+kWheelRadius = 0.0508
+
+# kEncoderResolution = -

commands-v2/hatchbot-inlined/physics.py

@@ -0,0 +1,85 @@
+#
+# See the documentation for more details on how this works
+#
+# The idea here is you provide a simulation object that overrides specific
+# pieces of WPILib, and modifies motors/sensors accordingly depending on the
+# state of the simulation. An example of this would be measuring a motor
+# moving for a set period of time, and then changing a limit switch to turn
+# on after that period of time. This can help you do more complex simulations
+# of your robot code without too much extra effort.
+#
+
+import wpilib
+import wpilib.simulation
+from wpimath.system import LinearSystemId
+from wpimath.system.plant import DCMotor
+
+import constants
+
+from pyfrc.physics.core import PhysicsInterface
+
+import typing
+
+if typing.TYPE_CHECKING:
+    from robot import MyRobot
+
+
+class PhysicsEngine:
+    """
+    Simulates a motor moving something that strikes two limit switches,
+    one on each end of the track. Obviously, this is not particularly
+    realistic, but it's good enough to illustrate the point
+    """
+
+    def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"):
+
+        self.physics_controller = physics_controller
+
+        # Motors
+        self.l_motor = wpilib.simulation.PWMSim(
+            robot.container.driveSubsystem.left1.getChannel()
+        )
+        self.r_motor = wpilib.simulation.PWMSim(
+            robot.container.driveSubsystem.right1.getChannel()
+        )
+
+        self.system = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3)
+        self.drivesim = wpilib.simulation.DifferentialDrivetrainSim(
+            self.system,
+            constants.kTrackWidth,
+            DCMotor.CIM(constants.kDriveTrainMotorCount),
+            constants.kGearingRatio,
+            constants.kWheelRadius,
+        )
+
+        self.leftEncoderSim = wpilib.simulation.EncoderSim(
+            robot.container.driveSubsystem.leftEncoder
+        )
+        self.rightEncoderSim = wpilib.simulation.EncoderSim(
+            robot.container.driveSubsystem.rightEncoder
+        )
+
+    def update_sim(self, now: float, tm_diff: float) -> None:
+        """
+        Called when the simulation parameters for the program need to be
+        updated.
+
+        :param now: The current time as a float
+        :param tm_diff: The amount of time that has passed since the last
+                        time that this function was called
+        """
+
+        # Simulate the drivetrain
+        l_motor = self.l_motor.getSpeed()
+        r_motor = self.r_motor.getSpeed()
+
+        voltage = wpilib.RobotController.getInputVoltage()
+        self.drivesim.setInputs(l_motor * voltage, r_motor * voltage)
+        self.drivesim.update(tm_diff)
+
+        self.leftEncoderSim.setDistance(self.drivesim.getLeftPosition() * 39.37)
+        self.leftEncoderSim.setRate(self.drivesim.getLeftVelocity() * 39.37)
+        self.rightEncoderSim.setDistance(self.drivesim.getRightPosition() * 39.37)
+        self.rightEncoderSim.setRate(self.drivesim.getRightVelocity() * 39.37)
+
+        self.physics_controller.field.setRobotPose(self.drivesim.getPose())

commands-v2/hatchbot-inlined/robot.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+
+import typing
+import wpilib
+import commands2
+
+from robotcontainer import RobotContainer
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    Our default robot class, pass it to wpilib.run
+
+    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
+    has an implementation of robotPeriodic which runs the scheduler for you
+    """
+
+    autonomousCommand: typing.Optional[commands2.Command] = None
+
+    def robotInit(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = RobotContainer()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.container.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous"""
+
+    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.
+        if self.autonomousCommand:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+
+    def testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

commands-v2/hatchbot-inlined/robotcontainer.py

@@ -0,0 +1,90 @@
+import wpilib
+from wpilib.interfaces import GenericHID
+
+import commands2
+import commands2.button
+import commands2.cmd
+
+import constants
+
+import commands.autos
+
+from subsystems.drivesubsystem import DriveSubsystem
+from subsystems.hatchsubsystem import HatchSubsystem
+
+
+class RobotContainer:
+    """
+    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.driveSubsystem = DriveSubsystem()
+        self.hatchSubsystem = HatchSubsystem()
+
+        # Retained command handles
+
+        # A simple auto routine that drives forward a specified distance, and then stops.
+        self.simpleAuto = commands.autos.Autos.simpleAuto(self.driveSubsystem)
+
+        # A complex auto routine that drives forward, drops a hatch, and then drives backward.
+        self.complexAuto = commands.autos.Autos.complexAuto(
+            self.driveSubsystem, self.hatchSubsystem
+        )
+
+        # The driver's controller
+        self.driverController = commands2.button.CommandPS4Controller(
+            constants.kDriverControllerPort
+        )
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+        # Configure default commands
+        # Set the default drive command to split-stick arcade drive
+        self.driveSubsystem.setDefaultCommand(
+            # A split-stick arcade command, with forward/backward controlled by the left
+            # hand, and turning controlled by the right.
+            commands2.cmd.run(
+                lambda: self.driveSubsystem.arcadeDrive(
+                    -self.driverController.getLeftY(),
+                    -self.driverController.getRightX(),
+                ),
+                [self.driveSubsystem],
+            )
+        )
+
+        # Chooser
+        self.chooser = wpilib.SendableChooser()
+
+        # Add commands to the autonomous command chooser
+        self.chooser.setDefaultOption("Simple Auto", self.simpleAuto)
+        self.chooser.addOption("Complex Auto", self.complexAuto)
+
+        # Put the chooser on the dashboard
+        wpilib.SmartDashboard.putData("Autonomous", self.chooser)
+
+    def configureButtonBindings(self):
+        """
+        Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a :GenericHID or one of its subclasses (Joystick or XboxController),
+        and then passing it to a JoystickButton.
+        """
+
+        # Grab the hatch when the Circle button is pressed.
+        self.driverController.circle().onTrue(self.hatchSubsystem.grabHatch())
+
+        # Release the hatch when the Square button is pressed.
+        self.driverController.square().onTrue(self.hatchSubsystem.releaseHatch())
+
+        # While holding R1, drive at half speed
+        self.driverController.R1().onTrue(
+            commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(0.5))
+        ).onFalse(commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(1)))
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        return self.chooser.getSelected()