Add gyro-drive-commands example (#63)

BerkeSinanYetkin · web-flow · commit 319caaa0260e · 2022-12-27T00:34:12.000-05:00
* added elevator trapezoid profile example

* updated tests

* No self parameter in setSetPoint function

* format with black

* license and enviroment stuff

* Add gyro-drive-commands example

* format with black
diff --git a/commands-v2/gyro-drive-commands/commands/turntoangle.py b/commands-v2/gyro-drive-commands/commands/turntoangle.py
@@ -0,0 +1,53 @@
+# 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 commands2.cmd
+import wpimath.controller
+
+from subsystems.drivesubsystem import DriveSubsystem
+
+import constants
+
+
+class TurnToAngle(commands2.PIDCommand):
+    """A command that will turn the robot to the specified angle."""
+
+    def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None:
+        """
+        Turns to robot to the specified angle.
+
+        :param: targetAngleDegrees The angle to turn to
+        :param: drive The drive subsystem to
+        """
+        super().__init__(
+            wpimath.controller.PIDController(
+                constants.DriveConstants.kTurnP,
+                constants.DriveConstants.kTurnI,
+                constants.DriveConstants.kTurnD,
+            ),
+            # Close loop on heading
+            drive.getHeading,
+            # Set reference to target
+            targetAngleDegrees,
+            # Pipe output to turn robot
+            lambda output: drive.arcadeDrive(0, output),
+            # Require the drive
+            [drive],
+        )
+
+        # Set the controller to be continuous (because it is an angle controller)
+        self.getController().enableContinuousInput(-180, 180)
+
+        # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+        # setpoint before it is considered as having reached the reference
+        self.getController().setTolerance(
+            constants.DriveConstants.kTurnToleranceDeg,
+            constants.DriveConstants.kTurnRateToleranceDegPerS,
+        )
+
+    def isFinished(self) -> bool:
+        # End when the controller is at the reference.
+        return self.getController().atSetpoint()
diff --git a/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py b/commands-v2/gyro-drive-commands/commands/turntoangleprofiled.py
@@ -0,0 +1,58 @@
+# 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 commands2.cmd
+import wpimath.controller
+import wpimath.trajectory
+
+from subsystems.drivesubsystem import DriveSubsystem
+
+import constants
+
+
+class TurnToAngleProfiled(commands2.ProfiledPIDCommand):
+    """A command that will turn the robot to the specified angle using a motion profile."""
+
+    def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None:
+        """
+        Turns to robot to the specified angle.
+
+        :param: targetAngleDegrees The angle to turn to
+        :param: drive The drive subsystem to
+        """
+        super().__init__(
+            wpimath.controller.ProfiledPIDController(
+                constants.DriveConstants.kTurnP,
+                constants.DriveConstants.kTurnI,
+                constants.DriveConstants.kTurnD,
+                wpimath.trajectory.TrapezoidProfile.Constraints(
+                    constants.DriveConstants.kMaxTurnRateDegPerS,
+                    constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared,
+                ),
+            ),
+            # Close loop on heading
+            drive.getHeading,
+            # Set reference to target
+            targetAngleDegrees,
+            # Pipe output to turn robot
+            lambda output, setpoint: drive.arcadeDrive(0, output),
+            # Require the drive
+            [drive],
+        )
+
+        # Set the controller to be continuous (because it is an angle controller)
+        self.getController().enableContinuousInput(-180, 180)
+
+        # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+        # setpoint before it is considered as having reached the reference
+        self.getController().setTolerance(
+            constants.DriveConstants.kTurnToleranceDeg,
+            constants.DriveConstants.kTurnRateToleranceDegPerS,
+        )
+
+    def isFinished(self) -> bool:
+        # End when the controller is at the reference.
+        return self.getController().atSetpoint()
diff --git a/commands-v2/gyro-drive-commands/constants.py b/commands-v2/gyro-drive-commands/constants.py
@@ -0,0 +1,49 @@
+# 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.
+
+"""
+A place for the constant values in the code that may be used in more than one place. 
+This offers a convenient resources to teams who need to make both quick and universal
+changes.
+"""
+
+import math
+
+
+class DriveConstants:
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    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
+
+    kGyroReversed = False
+
+    kStabilizationP = 1
+    kStabilizationI = 0.5
+    kStabilizationD = 0
+
+    kTurnP = 1
+    kTurnI = 0
+    kTurnD = 0
+
+    kMaxTurnRateDegPerS = 100
+    kMaxTurnAccelerationDegPerSSquared = 300
+
+    kTurnToleranceDeg = 5
+    kTurnRateToleranceDegPerS = 10  # degrees per second
+
+
+class OIConstants:
+    kDriverControllerPort = 0
diff --git a/commands-v2/gyro-drive-commands/robot.py b/commands-v2/gyro-drive-commands/robot.py
@@ -0,0 +1,78 @@
+#!/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 typing
+
+import wpilib
+import commands2
+import commands2.cmd
+
+import robotcontainer
+
+"""
+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.
+"""
+
+
+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
+    """
+
+    def robotInit(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        self.autonomousCommand: typing.Optional[commands2.Command] = None
+
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = robotcontainer.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()
+
+        # schedule the autonomous command (example)
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.schedule()
+        else:
+            print("no auto command?")
+
+    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 is not None:
+            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)
diff --git a/commands-v2/gyro-drive-commands/robotcontainer.py b/commands-v2/gyro-drive-commands/robotcontainer.py
@@ -0,0 +1,116 @@
+# 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 commands2.cmd
+import commands2.button
+
+import constants
+import subsystems.drivesubsystem
+import commands.turntoangle
+import commands.turntoangleprofiled
+
+
+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):
+        """The container for the robot. Contains subsystems, OI devices, and commands."""
+        # The robot's subsystems
+        self.robotDrive = subsystems.drivesubsystem.DriveSubsystem()
+
+        # The driver's controller
+        self.driverController = commands2.button.CommandPS4Controller(
+            constants.OIConstants.kDriverControllerPort
+        )
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+        # Configure default commands
+        # Set the default drive command to split-stick arcade drive
+        self.robotDrive.setDefaultCommand(
+            # A split-stick arcade command, with forward/backward controlled by the left
+            # hand, and turning controlled by the right.
+            commands2.RunCommand(
+                lambda: self.robotDrive.arcadeDrive(
+                    -self.driverController.getLeftY(),
+                    -self.driverController.getRightX(),
+                ),
+                [self.robotDrive],
+            )
+        )
+
+    def configureButtonBindings(self):
+        """
+        Use this method to define your button->command mappings. Buttons can be created via the button
+        factories on commands2.button.CommandGenericHID or one of its
+        subclasses (commands2.button.CommandJoystick or command2.button.CommandXboxController).
+        """
+        # Drive at half speed when the right bumper is held
+        commands2.button.JoystickButton(
+            self.driverController, wpilib.PS4Controller.Button.kR1
+        ).onTrue(
+            commands2.InstantCommand(
+                (lambda: self.robotDrive.setMaxOutput(0.5)), [self.robotDrive]
+            )
+        ).onFalse(
+            commands2.InstantCommand(
+                (lambda: self.robotDrive.setMaxOutput(1)), [self.robotDrive]
+            )
+        )
+
+        # Stabilize robot to drive straight with gyro when left bumper is held
+        commands2.button.JoystickButton(
+            self.driverController, wpilib.PS4Controller.Button.kL1
+        ).whileTrue(
+            commands2.PIDCommand(
+                wpimath.controller.PIDController(
+                    constants.DriveConstants.kStabilizationP,
+                    constants.DriveConstants.kStabilizationI,
+                    constants.DriveConstants.kStabilizationD,
+                ),
+                # Close the loop on the turn rate
+                self.robotDrive.getTurnRate,
+                # Setpoint is 0
+                0,
+                # Pipe the output to the turning controls
+                lambda output: self.robotDrive.arcadeDrive(
+                    -self.driverController.getLeftY(), output
+                ),
+                # Require the robot drive
+                [self.robotDrive],
+            )
+        )
+
+        # Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
+        commands2.button.JoystickButton(
+            self.driverController, wpilib.PS4Controller.Button.kCross
+        ).onTrue(commands.turntoangle.TurnToAngle(90, self.robotDrive).withTimeout(5))
+
+        # Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout
+        commands2.button.JoystickButton(
+            self.driverController, wpilib.PS4Controller.Button.kCircle
+        ).onTrue(
+            commands.turntoangleprofiled.TurnToAngleProfiled(
+                -90, self.robotDrive
+            ).withTimeout(5)
+        )
+
+    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.InstantCommand()
diff --git a/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py
diff --git a/run_tests.sh b/run_tests.sh
