Add GearsBot example

GearsBot/commands/autonomous.py

@@ -0,0 +1,42 @@
+#
+# 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
+from . import preparetopickup
+import constants
+from subsystems.drivetrain import Drivetrain
+from subsystems.claw import Claw
+from subsystems.elevator import Elevator
+from subsystems.wrist import Wrist
+
+class Autonomous(commands2.SequentialCommandGroup):
+    """The main autonomous command to pickup and deliver the soda to the box."""
+
+    def __init__(self, drive: Drivetrain, claw: Claw, wrist: Wrist, elevator:
+                 Elevator) -> None:
+        """Create a new autonomous command."""
+        super().__init__()
+
+        self.addCommands(
+            preparetopickup.PrepareToPickup(claw, wrist, elevator),
+            pickup.Pickup(claw, wrist, elevator),
+            setdistancetobox.SetDistanceToBox(
+                constants.AutoConstants.kDistToBox1, drive
+            ),
+            # drivestraight.DriveStraight(4) # Use encoders if ultrasonic is broken
+            place.Place(claw, wrist, elevator),
+            setdistancetobox.SetDistanceToBox(
+                constants.AutoConstants.kDistToBox2, drive
+            ),
+            # drivestraight.DriveStraight(-2) # Use encoders if ultrasonic is broken
+            commands2.parallel(
+                setwristsetpoint.SetWristSetpoint(
+                    constants.AutoConstants.kWristSetpoint, wrist
+                ),
+                closeclaw.CloseClaw(claw)
+            )
+        )

GearsBot/commands/closeclaw.py

@@ -0,0 +1,34 @@
+#
+# 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
+from subsystems.claw import Claw
+import robot
+
+class CloseClaw(commands2.Command):
+    """Closes the claw until the limit switch is tripped."""
+
+    def __init__(self, claw: Claw) -> None:
+        self.claw = claw
+        self.addRequirements(self.claw)
+
+    def initialize(self) -> None:
+        # Called just before this Command runs the first time
+        self.claw.close()
+
+    def isFinished(self) -> bool:
+        # Make this return true when this Command no longer needs to run execute()
+        return self.claw.isGrabbing()
+
+    def end(self, interrupted: bool) -> None:
+        # Called once after isFinished returns true
+
+        # NOTE: Doesn't stop in simulation due to lower friction causing the
+        # can to fall out + there is no need to worry about stalling the motor 
+        # or crushing the can.
+
+        if not robot.MyRobot.isSimulation():
+            self.claw.stop()

GearsBot/commands/drivestraight.py

@@ -0,0 +1,47 @@
+#
+# 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 wpimath.controller
+import ..constants2
+from subsystems.drivetrain import Drivetrain
+
+class DriveStraight(commands2.PIDCommand):
+    """Drive the given distance straight (negative values go backwards). 
+    Uses a local PID controller to run a simple PID loop that is only 
+    enabled while this command is running. The input is the
+    averaged values of the left and right encoders.
+    """
+
+    def __init__(self, distance: float, drivetrain: Drivetrain) -> None:
+        """Create a new DriveStraight command.
+
+        :param distance: The distance to drive
+        """
+        super().__init__(
+            wpimath.controller.PIDController(
+                constants.DriveStraightConstants.kP,
+                constants.DriveStraightConstants.kI,
+                constants.DriveStraightConstants.kD,
+            ),
+            drivetrain.getDistance(),
+            distance,
+            lambda d: drivetrain.drive(d, d)
+        )
+
+        self.drivetrain = drivetrain
+        self.addRequirements(self.drivetrain)
+        self.getController().setTolerance(0.01)
+
+    def initialize(self) -> None:
+        # Called just before this Command runs the first time
+        # Get everything in a safe starting state.
+        self.drivetrain.reset()
+        super().initialize()
+
+    def isFinished(self) -> bool:
+        # Make this return true when this Command no longer needs to run execute()
+        return self.getController().atSetpoint()

GearsBot/commands/openclaw.py

@@ -0,0 +1,28 @@
+#
+# 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
+from subsystems.claw import Claw
+
+class OpenClaw(commands2.WaitCommand):
+    """Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!"""
+    def __init__(self, claw: Claw) -> None:
+        """Creates a new OpenClaw command.
+
+        :param claw: The claw to use
+        """
+        super().__init__(1)
+        self.claw = claw
+        self.addRequirements(self.claw)
+
+    def initialize(self) -> None:
+        # Called just before this Command runs the first time
+        self.claw.open()
+        super().initialize()
+
+    def end(self, interrupted: bool) -> None:
+        # Called once after isFinished returns true
+        self.claw.stop()

GearsBot/commands/pickup.py

@@ -0,0 +1,36 @@
+#
+# 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 constants
+from . import closeclaw
+from . import setwristsetpoint
+from . import setelevatorsetpoint
+from subsystems.claw import Claw
+from subsystems.elevator import Elevator
+from subsystems.wrist import Wrist
+
+class Pickup(commands2.SequentialCommandGroup):
+    """Pickup a soda can (if one is between the open claws) and get it in a
+    safe state to drive around."""
+
+    def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None:
+        """Create a new pickup command.
+
+        :param claw:     The claw subsystem to use
+        :param wrist:    The wrist subsystem to use
+        :param elevator: The elevator subsystem to use
+        """
+        self.addCommands(closeclaw.CloseClaw(claw))
+        commands2.cmd.parallel(
+            setwristsetpoint.SetWristSetpoint(
+                constants.Position.Pickup.kWristSetpoint, wrist
+            ),
+            setelevatorsetpoint.SetElevatorSetpoint(
+                constants.Position.Pickup.kElevatorSetpoint, elevator
+            ),
+        )

GearsBot/commands/place.py

@@ -0,0 +1,35 @@
+#
+# 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 constants
+from . import openclaw
+from . import setelevatorsetpoint
+from . import setwristsetpoint
+from subsystems.claw import Claw
+from subsystems.elevator import Elevator
+from subsystems.wrist import Wrist
+
+class Place(commands2.SequentialCommandGroup):
+    """Place a held soda can onto the platform."""
+
+    def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None:
+        """Create a new place command.
+
+        :param claw:     The claw subsystem to use
+        :param wrist:    The wrist subsystem to use
+        :param elevator: The elevator subsystem to use
+        """
+        super().__init__()
+        self.addCommands(
+            setelevatorsetpoint.SetElevatorSetpoint(
+                constants.Positions.Place.kElevatorSetpoint, elevator
+            ),
+            setwristsetpoint.SetWristSetpoint(
+                constants.Positions.Place.kWristSetpoint, wrist
+            ),
+            openclaw.OpenClaw(claw)
+        )

GearsBot/commands/preparetopickup.py

@@ -0,0 +1,35 @@
+#
+# 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
+from . import setwristsetpoint
+from . import setelevatorsetpoint
+from . import openclaw
+from subsystems.elevator import Elevator
+from subsystems.wrist import Wrist
+from subsystems.claw import Claw
+
+class PrepareToPickup(commands2.SequentialCommandGroup):
+    def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None:
+        """Create a new prepare to pickup command.
+
+        :param claw:     The claw subsystem to use
+        :param wrist:    The wrist subsystem to use
+        :param elevator: The elevator subsystem to use
+        """
+        super().__init__()
+        self.addCommands(
+            openclaw.OpenClaw(claw),
+            commands2.cmd.parallel(
+                setwristsetpoint.SetWristSetpoint(
+                    constants.Positions.Place.kWristSetpoint, wrist
+                ),
+                setelevatorsetpoint.SetElevatorSetpoint(
+                    constants.Positions.Place.kElevatorSetpoint, elevator
+                ),
+            )
+        )

GearsBot/commands/setdistancetobox.py

@@ -0,0 +1,40 @@
+#
+# 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 wpimath.controller
+from subsystems.drivetrain import Drivetrain
+
+class SetDistanceToBox(commands2.PIDCommand):
+    """Drive until the robot is the given distance away from the box. Uses a local 
+    PID controller to run a simple PID loop that is only enabled while this command is
+    running. The input is the averaged values of the left and right encoders.
+    """
+
+    def __init__(self, distance: float, drivetrain: Drivetrain) -> None:
+        """Create a new set distance to box command.
+
+        :param distance: The distance away from the box to drive to
+        """
+        super().__init__(
+            wpimath.controller.PIDController(-2, 0, 0),
+            drivetrain.getDistanceToObstacle(),
+            distance,
+            lambda d : drivetrain.drive(d, d)
+        )
+        self.drivetrain = drivetrain
+        self.addRequirements(self.drivetrain)
+        self.getController().setTolerance(0.01)
+
+    def initialize(self) -> None:
+        # Called just before this Command runs the first time
+        # Get everything in a safe starting state.
+        self.drivetrain.reset()
+        super().initialize()
+
+    def isFinished(self) -> bool:
+        # Make this return true when this Command no longer needs to run execute()
+        return self.getController().atSetpoint()

GearsBot/commands/setelevatorsetpoint.py

@@ -0,0 +1,33 @@
+#
+# 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
+from subsystems.elevator import Elevator
+
+class SetElevatorSetpoint(commands2.Command):
+    """Move the elevator to a given location. This command finishes when it is 
+    within the tolerance, but leaves the PID loop running to maintain the position. 
+    Other commands using the elevator should make sure they disable PID!
+    """
+    def __init__(self, setpoint: float, elevator: Elevator) -> None:
+        """Create a new SetElevatorSetpoint command.
+
+        :param setpoint: The setpoint to set the elevator to
+        :param elevator: The elevator to use
+        """
+        super().__init__()
+        self.elevator = elevator
+        self.setpoint = setpoint
+        self.addRequirements(self.elevator)
+
+    def initialize(self) -> None:
+        # Called just before this Command runs the first time
+        self.elevator.setSetpoint(self.setpoint)
+        self.elevator.enable()
+
+    def isFinished(self) -> bool:
+        # Make this return true when this Command no longer needs to run execute()
+        return self.elevator.getController().atSetpoint()

GearsBot/commands/setwristsetpoint.py

@@ -0,0 +1,29 @@
+#
+# 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
+from subsystems.wrist import Wrist
+
+class SetWristSetpoint(commands2.Command):
+    def __init__(self, setpoint: float, wrist: Wrist) -> None:
+        """Create a new SetWristSetpoint command.
+
+        :param setpoint: The setpoint to set the wrist to
+        :param wrist:    The wrist to use
+        """
+        super().__init__()
+        self.wrist = wrist
+        self.setpoint = setpoint
+        self.addRequirements(self.wrist)
+
+    def initialize(self) -> None:
+        # Called just before this Command runs the first time
+        self.wrist.setSetpoint(self.setpoint)
+        self.wrist.enable()
+
+    def isFinished(self) -> bool:
+        # Make this return true when this Command no longer needs to run execute()
+        return self.wrist.getController().atSetpoint()

GearsBot/commands/tankdrive.py

@@ -0,0 +1,39 @@
+#
+# 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 typing
+import commands2
+from subsystems.drivetrain import Drivetrain
+
+class TankDrive(commands2.Command):
+    """Have the robot drive tank style."""
+
+    def __init__(self, left: typing.Callable[[], float], right:
+                 typing.Callable[[], float], drivetrain: Drivetrain) -> None:
+        """Creates a new TankDrive command.
+
+        :param left:       The control input for the left side of the drive
+        :param right:      The control input for the right sight of the drive
+        :param drivetrain: The drivetrain subsystem to drive
+        """
+        super().__init__()
+
+        self.drivetrain = drivetrain
+        self.left = left
+        self.right = right
+        self.addRequirements([self.drivetrain])
+
+    def execute(self) -> None:
+        # Called repeatedly when this Command is scheduled to run
+        self.drivetrain.drive(self.left(), self.right())
+
+    def isFinished(self) -> bool:
+        # Make this return true when this Command no longer needs to run execute()
+        return False
+
+    def end(self, interrupted: bool) -> None:
+        # Called once after isFinished returns true
+        self.drivetrain.drive(0, 0)

GearsBot/constants.py

@@ -0,0 +1,88 @@
+#
+# 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.
+#
+
+"""
+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
+
+class DriveConstants:
+    # The PWM IDs for the drivetrain motor controllers.
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    # Encoders and their respective motor controllers.
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    kRangeFinderPort = 6
+    kAnalogGyroPort = 1
+
+    # Encoder counts per revolution/rotation.
+    kEncoderCPR = 1024
+    kWheelDiameterInches = 6
+
+    # Assumes the encoders are directly mounted on the wheel shafts
+    kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR
+
+class ClawConstants:
+    kMotorPort = 7
+    kContactPort = 5
+
+class WristConstants:
+    kMotorPort = 6
+
+    # These pid constants are not real, and will need to be tuned
+    kP = 0.1
+    kI = 0.0
+    kD = 0.0
+
+    kTolerance = 2.5
+    kPotentiometerPort = 3;
+
+class ElevatorConstants:
+    kMotorPort = 5
+    kPotentiometerPort = 2
+
+    # These pid constants are not real, and will need to be tuned
+    kP_real = 4
+    kI_real = 0.007
+
+    kP_simulation = 18
+    kI_simulation = 0.2
+
+    kD = 0.0
+
+    kTolerance = 0.005
+
+class AutoConstants:
+    kDistToBox1 = 0.10
+    kDistToBox2 = 0.60
+    kWristSetpoint = -45.0
+
+class DriveStraightConstants:
+    kP = 4.0
+    kI = 0.0
+    kD = 0.0
+
+class Positions:
+    class Pickup:
+        kWristSetpoint = -45.0
+        kElevatorSetpoint = 0.2
+
+    class Place:
+        kWristSetpoint = 0.0
+        kElevatorSetpoint = 0.25
+
+    class PrepareToPickup:
+        kWristSetpoint = 0.0
+        kElevatorSetpoint = 0.0

GearsBot/robot.py

@@ -0,0 +1,66 @@
+#!/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 wpilib
+import commands2
+import typing
+
+from robotcontainer import RobotContainer
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    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."""
+        pass
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+        pass
+
+    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"""
+        pass
+
+    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"""
+        pass
+
+    def testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()

GearsBot/robotcontainer.py

@@ -0,0 +1,118 @@
+#
+# 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 wpilib
+import constants
+import commands2
+import commands2.button
+from subsystems.drivetrain import Drivetrain
+from subsystems.elevator import Elevator
+from subsystems.wrist import Wrist
+from subsystems.claw import Claw
+from commands.autonomous import Autonomous
+from commands.setelevatorsetpoint import SetElevatorSetpoint
+from commands.setwristsetpoint import SetWristSetpoint
+from commands.openclaw import OpenClaw
+from commands.closeclaw import CloseClaw
+from commands.tankdrive import TankDrive
+from commands.pickup import Pickup
+from commands.place import Place
+
+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 container for the robot. Contains subsystems, OI devices, and commands."""
+        # The robot's subsystems and commands are defined here...
+        self.drivetrain = Drivetrain()
+        self.elevator = Elevator()
+        self.wrist = Wrist()
+        self.claw = Claw()
+
+        self.joystick = wpilib.XboxController(0)
+
+        self.autonomousCommand = Autonomous(
+            self.drivetrain, self.claw, self.wrist, self.elevator
+        )
+
+        # Put Some buttons on the SmartDashboard
+        wpilib.SmartDashboard.putData(
+            "Elevator Bottom", SetElevatorSetpoint(0, self.elevator)
+        )
+        wpilib.SmartDashboard.putData(
+            "Elevator Top", SetElevatorSetpoint(0.25, self.elevator)
+        )
+        wpilib.SmartDashboard.putData(
+            "Wrist Horizontal", SetWristSetpoint(0, self.wrist)
+        )
+        wpilib.SmartDashboard.putData(
+            "Raise Wrist", SetWristSetpoint(-45, self.wrist)
+        )
+        wpilib.SmartDashboard.putData(
+            "Open Claw", OpenClaw(self.claw)
+        )
+        wpilib.SmartDashboard.putData(
+            "Close Claw", CloseClaw(self.claw)
+        )
+        wpilib.SmartDashboard.putData(
+            "Deliver Soda", 
+            Autonomous(self.drivetrain, self.claw, self.wrist, self.elevator)
+        )
+
+        # Assign default commands
+        self.drivetrain.setDefaultCommand(
+            TankDrive(
+                lambda: -self.joystick.getLeftY(),
+                lambda: -self.joystick.getRightY(),
+                self.drivetrain
+            ),
+        )
+
+        # Show what command your subsystem is running on the SmartDashboard
+        wpilib.SmartDashboard.putData(self.drivetrain)
+        wpilib.SmartDashboard.putData(self.elevator)
+        wpilib.SmartDashboard.putData(self.wrist)
+        wpilib.SmartDashboard.putData(self.claw)
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+    def configureButtonBindings(self) -> None:
+        """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.
+        """
+        # Create some buttons
+        dpadUp = commands2.button.JoystickButton(self.joystick, 5)
+        dpadRight = commands2.button.JoystickButton(self.joystick, 6)
+        dpadDown = commands2.button.JoystickButton(self.joystick, 7)
+        dpadLeft = commands2.button.JoystickButton(self.joystick, 8)
+        l2 = commands2.button.JoystickButton(self.joystick, 9)
+        r2 = commands2.button.JoystickButton(self.joystick, 10)
+        l1 = commands2.button.JoystickButton(self.joystick, 11)
+        r1 = commands2.button.JoystickButton(self.joystick, 12)
+
+        # Connect the buttons to commands
+        dpadUp.onTrue(SetElevatorSetpoint(0.25, self.elevator))
+        dpadDown.onTrue(SetElevatorSetpoint(0.0, self.elevator))
+        dpadRight.onTrue(CloseClaw(self.claw))
+        dpadLeft.onTrue(OpenClaw(self.claw))
+
+        r1.onTrue(PrepareToPickup(self.claw, self.wrist, self.elevator))
+        r2.onTrue(Pickup(self.claw, self.wrist, self.elevator))
+        l1.onTrue(Place(self.claw, self.wrist, self.elevator))
+        l2.onTrue(Autonomous(self.drivetrain, self.claw, self.wrist, self.elevator))
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """Use this to pass the autonomous command to the main :class:`.Robot` class.
+
+        :returns: the command to run in autonomous
+        """
+        return commands2.cmd.none()

GearsBot/subsystems/claw.py

@@ -0,0 +1,48 @@
+#
+# 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 wpilib
+import commands2
+import constants
+
+class Claw(commands2.Subsystem):
+    """The claw subsystem is a simple system with a motor for opening and closing. 
+    If using stronger motors, you should probably use a sensor so that the motors don't stall."""
+
+    def __init__(self) -> None:
+        """Create a new claw subsystem."""
+        super().__init__()
+
+        self.motor = wpilib.Victor(constants.ClawConstants.kMotorPort)
+        self.contact = wpilib.DigitalInput(constants.ClawConstants.kContactPort)
+
+        # Let's name everything on the LiveWindow
+        addChild("Motor", self.motor)
+        addChild("Limit Switch", self.contact)
+
+    def log(self) -> None:
+        wpilib.SmartDashboard.putData("Claw switch", self.contact)
+
+    def open(self) -> None:
+        """Set the claw motor to move in the open direction."""
+        self.motor.set(-1)
+
+    def close(self) -> None:
+        """Set the claw motor to move in the close direction."""
+        self.motor.set(1)
+
+    def stop(self) -> None:
+        """Stops the claw motor from moving."""
+        self.motor.set(0)
+
+    def isGrabbing(self) -> bool:
+        """Return true when the robot is grabbing an object hard enough to
+        trigger the limit switch."""
+        return self.contact.get()
+
+    def periodic(self) -> None:
+        """Call log method every loop."""
+        self.log()

GearsBot/subsystems/drivetrain.py

@@ -0,0 +1,126 @@
+#
+# 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 wpilib
+import wpilib.drive
+import wpiutil
+import commands2
+import constants
+import robot
+
+class Drivetrain(commands2.SubsystemBase):
+    """The Drivetrain subsystem incorporates the sensors and actuators
+    attached to the robots chassis. These include four drive motors, 
+    a left and right encoder and a gyro."""
+
+    def __init__(self) -> None:
+        """Create a new drivetrain subsystem."""
+        super().__init__()
+
+        self.leftLeader = wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port)
+        self.leftFollower = wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port)
+        self.rightLeader = wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port)
+        self.rightFollower = wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port)
+
+        self.drive = wpilib.drive.DifferentialDrive(
+            lambda s : self.leftLeader.set(s), 
+            lambda s : self.rightLeader.set(s)
+        )
+
+        self.leftEncoder = wpilib.Encoder(
+            constants.DriveConstants.kLeftEncoderPorts[0],
+            constants.DriveConstants.kLeftEncoderPorts[1],
+            constants.DriveConstants.kLeftEncoderReversed
+        )
+
+        self.rightEncoder = wpilib.Encoder(
+            constants.DriveConstants.kRightEncoderPorts[0],
+            constants.DriveConstants.kRightEncoderPorts[1],
+            constants.DriveConstants.kRightEncoderReversed
+        )
+
+        self.rangefinder = wpilib.AnalogInput(constants.DriveConstants.kRangeFinderPort)
+        self.gyro = wpilib.AnalogGyro(constants.DriveConstants.kAnalogGyroPort)
+
+        wpiutil.SendableRegistry.addChild(self.drive, self.leftLeader)
+        wpiutil.SendableRegistry.addChild(self.drive, self.rightLeader)
+
+        self.leftLeader.addFollower(self.leftFollower)
+        self.rightLeader.addFollower(self.rightFollower)
+
+        # 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.rightLeader.setInverted(true)
+
+        # Encoders may measure differently in the real world and in
+        # simulation. In this example the robot moves 0.042 barleycorns
+        # per tick in the real world, but the simulated encoders
+        # simulate 360 tick encoders. This if statement allows for the
+        # real robot to handle this difference in devices.
+        if robot.MyRobot.isReal():
+            self.leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+            self.rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+        else:
+            # Circumference = diameter in feet * pi. 360 tick simulated encoders.
+            self.leftEncoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
+            self.rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0)
+
+        # Let's name the sensors on the LiveWindow
+        self.addChild("Drive", self.drive);
+        self.addChild("Left Encoder", self.leftEncoder);
+        self.addChild("Right Encoder", self.rightEncoder);
+        self.addChild("Rangefinder", self.rangefinder);
+        self.addChild("Gyro", self.gyro);
+
+    def log(self) -> None:
+        """The log method puts interesting information to the SmartDashboard."""
+        wpilib.SmartDashboard.putNumber("Left Distance", self.leftEncoder.getDistance())
+        wpilib.SmartDashboard.putNumber("Right Distance", self.rightEncoder.getDistance())
+        wpilib.SmartDashboard.putNumber("Left Speed", self.leftEncoder.getRate())
+        wpilib.SmartDashboard.putNumber("Right Speed", self.rightEncoder.getRate())
+        wpilib.SmartDashboard.putNumber("Gyro", self.gyro.getAngle())
+
+    def drive(self, left: float, right: float) -> None:
+        """Tank style driving for the Drivetrain.
+
+        :param left:  Speed in range [-1,1]
+        :param right: Speed in range [-1,1]
+        """
+        self.drive.tankDrive(left, right)
+
+    def getHeading(self) -> float:
+        """Get the robot's heading.
+
+        :returns: The robots heading in degrees.
+        """
+        return self.gyro.getAngle()
+
+    def reset(self) -> None:
+        """Reset the robots sensors to the zero states."""
+        self.gyro.reset()
+        self.leftEncoder.reset()
+        self.rightEncoder.reset()
+
+    def getDistance(self) -> float:
+        """Get the average distance of the encoders since the last reset.
+
+        :returns: The distance driven (average of left and
+        right encoders).
+        """
+        return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2
+
+    def getDistanceToObstacle(self) -> float:
+        """Get the distance to the obstacle.
+
+        :returns: The distance to the obstacle detected by the rangefinder.
+        """
+        # Really meters in simulation since it's a rangefinder...
+        return self.rangefinder.getAverageVoltage()
+
+    def periodic(self) -> None:
+        """Call log method every loop."""
+        self.log()

GearsBot/subsystems/elevator.py

@@ -0,0 +1,65 @@
+#
+# 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 wpilib
+import wpimath.controller
+import commands2
+import constants
+import robot
+
+class Elevator(commands2.PIDSubsystem):
+    def __init__(self) -> None:
+        """Create a new elevator subsystem."""
+        super().__init__(
+            wpimath.controller.PIDController(
+                constants.ElevatorConstants.kP_real,
+                constants.ElevatorConstants.kI_real,
+                constants.ElevatorConstants.kD_real,
+            )
+        )
+
+        self.motor = wpilib.Victor(constants.ElevatorConstants.kMotorPort)
+
+        if robot.MyRobot.isSimulation(): # Check for simulation and update PID values
+            self.getController().setPID(
+                constants.ElevatorConstants.kP_simulation,
+                constants.ElevatorConstants.kI_simulation,
+                constants.ElevatorConstants.kD,
+            )
+        self.getController().setTolerance(constants.ElevatorConstants.kTolerance)
+
+        # Conversion value of potentiometer varies between the real world and simulation
+        if robot.MyRobot.isReal():
+            self.pot = wpilib.AnalogPotentiometer(
+                constants.ElevatorConstants.kPotentiometerPort, -2.0 / 5
+            )
+        else:
+            # Defaults to meters
+            self.pot = wpilib.AnalogPotentiometer(
+                constants.ElevatorConstants.kPotentiometerPort
+            )
+
+        # Let's name everything on the LiveWindow
+        self.addChild("Motor", self.motor);
+        self.addChild("Pot", self.pot);
+
+    def log(self) -> None:
+        """The log method puts interesting information to the SmartDashboard."""
+        wpilib.SmartDashboard.putData("Elevator Pot", self.pot)
+
+    def getMeasurement(self) -> None:
+        """Use the potentiometer as the PID sensor. This method is automatically 
+        called by the subsystem."""
+        return self.pot.get()
+
+    def useOutput(self, output: float, setpoint: float) -> None:
+        """Use the motor as the PID output. This method is automatically called
+        by the subsystem."""
+        self.motor.set(output)
+
+    def periodic(self) -> None:
+        """Call log method every loop."""
+        self.log()

GearsBot/subsystems/wrist.py

@@ -0,0 +1,62 @@
+#
+# 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 wpilib
+import wpimath.controller
+import constants
+import robot
+
+class Wrist(commands2.PIDSubsystem):
+    """The wrist subsystem is like the elevator, but with a rotational
+    joint instead of a linear joint."""
+
+    def __init__(self) -> None:
+        """Create a new wrist subsystem."""
+        super().__init__(
+            wpimath.controller.PIDController(
+                constants.WristConstants.kP,
+                constants.WristConstants.kI,
+                constants.WristConstants.kD,
+            )
+        )
+
+        self.getController().setTolerance(constants.WristConstants.kTolerance)
+
+        self.motor = wpilib.Victor(constants.WristConstants.kMotorPort)
+
+        # Conversion value of potentiometer varies between the real world and simulation
+        if robot.MyRobot.isReal():
+            self.pot = wpilib.AnalogPotentiometer(
+                constants.ElevatorConstants.kPotentiometerPort, -270.0 / 5
+            )
+        else:
+            # Defaults to meters
+            self.pot = wpilib.AnalogPotentiometer(
+                constants.ElevatorConstants.kPotentiometerPort
+            )
+
+        # Let's name everything on the LiveWindow
+        self.addChild("Motor", self.motor)
+        self.addChild("Pot", self.pot)
+
+    def log(self) -> None:
+        """The log method puts interesting information to the SmartDashboard."""
+        wpilib.SmartDashboard.putData("Wrist Angle", self.pot)
+
+    def getMeasurement(self) -> float:
+        """Use the potentiometer as the PID sensor. This method is automatically 
+        called by the subsystem."""
+        return self.pot.get()
+
+    def useOutput(self, output: float, setpoint: float) -> None:
+        """Use the motor as the PID output. This method is automatically called
+        by the subsystem."""
+        self.motor.set(output)
+
+    def periodic(self) -> None:
+        """Call log method every loop."""
+        self.log();

run_tests.sh

@@ -25,6 +25,7 @@ BASE_TESTS="
   FlywheelBangBangController
   FrisbeeBot
   GameData
+  GearsBot
   GettingStarted
   Gyro
   GyroDriveCommands