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()