Added example SwerveControllerCommand

SwerveControllerCommand/constants.py

@@ -0,0 +1,95 @@
+#
+# 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.
+#
+
+from wpilib import TimedRobot
+from wpimath.geometry import Translation2d
+from wpimath.kinematics import SwerveDrive4Kinematics
+from wpimath.trajectory import TrapezoidProfile
+import math
+
+
+class DriveConstants:
+    kFrontLeftDriveMotorPort = 0
+    kRearLeftDriveMotorPort = 2
+    kFrontRightDriveMotorPort = 4
+    kRearRightDriveMotorPort = 6
+
+    kFrontLeftTurningMotorPort = 1
+    kRearLeftTurningMotorPort = 3
+    kFrontRightTurningMotorPort = 5
+    kRearRightTurningMotorPort = 7
+
+    kFrontLeftTurningEncoderPorts = [0, 1]
+    kRearLeftTurningEncoderPorts = [2, 3]
+    kFrontRightTurningEncoderPorts = [4, 5]
+    kRearRightTurningEncoderPorts = [6, 7]
+
+    kFrontLeftTurningEncoderReversed = False
+    kRearLeftTurningEncoderReversed = True
+    kFrontRightTurningEncoderReversed = False
+    kRearRightTurningEncoderReversed = True
+
+    kFrontLeftDriveEncoderPorts = [8, 9]
+    kRearLeftDriveEncoderPorts = [10, 11]
+    kFrontRightDriveEncoderPorts = [12, 13]
+    kRearRightDriveEncoderPorts = [14, 15]
+
+    kFrontLeftDriveEncoderReversed = False
+    kRearLeftDriveEncoderReversed = True
+    kFrontRightDriveEncoderReversed = False
+    kRearRightDriveEncoderReversed = True
+
+    kDrivePeriod = TimedRobot.kDefaultPeriod
+
+    kTrackWidth = 0.5
+    kWheelBase = 0.7
+    kDriveKinematics = SwerveDrive4Kinematics(
+        Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        Translation2d(-kWheelBase / 2, -kTrackWidth / 2),
+    )
+
+    kGyroReversed = False
+
+    ksVolts = 1
+    kvVoltSecondsPerMeter = 0.8
+    kaVoltSecondsSquaredPerMeter = 0.15
+
+    kMaxSpeedMetersPerSecond = 3
+
+
+class ModuleConstants:
+    kMaxModuleAngularSpeedRadiansPerSecond = 2 * math.pi
+    kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * math.pi
+
+    kEncoderCPR = 1024
+    kWheelDiameterMeters = 0.15
+    kDriveEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR
+
+    kTurningEncoderDistancePerPulse = (2 * math.pi) / kEncoderCPR
+
+    kPModuleTurningController = 1
+    kPModuleDriveController = 1
+
+
+class OIConstants:
+    kDriverControllerPort = 0
+
+
+class AutoConstants:
+    kMaxSpeedMetersPerSecond = 3
+    kMaxAccelerationMetersPerSecondSquared = 3
+    kMaxAngularSpeedRadiansPerSecond = math.pi
+    kMaxAngularSpeedRadiansPerSecondSquared = math.pi
+
+    kPXController = 1
+    kPYController = 1
+    kPThetaController = 1
+
+    kThetaControllerConstraints = TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared
+    )

SwerveControllerCommand/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.
+#
+
+from commands2 import TimedCommandRobot, CommandScheduler
+from robotcontainer import RobotContainer
+
+
+class Robot(TimedCommandRobot):
+    def robotInit(self):
+        """
+        Instantiate our RobotContainer. This will perform all our button bindings, and put our
+        autonomous chooser on the dashboard.
+        """
+        self.robotContainer = RobotContainer()
+
+    def robotPeriodic(self):
+        """
+        Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+        commands, running already-scheduled commands, removing finished or interrupted commands,
+        and running subsystem periodic() methods. This must be called from the robot's periodic
+        block in order for anything in the Command-based framework to work.
+        """
+
+    def disabledInit(self):
+        pass
+
+    def disabledPeriodic(self):
+        pass
+
+    def autonomousInit(self):
+        self.autonomousCommand = self.robotContainer.getAutonomousCommand()
+
+        """
+        schedule the autonomous command (example)
+        """
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self):
+        pass
+
+    def teleopInit(self):
+        """
+        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):
+        pass
+
+    def testInit(self):
+        """
+        Cancels all running commands at the start of test mode.
+        """
+        CommandScheduler.getInstance().cancelAll()
+
+    def testPeriodic(self):
+        pass

SwerveControllerCommand/robotcontainer.py

@@ -0,0 +1,109 @@
+#
+# 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.
+#
+
+from wpilib import XboxController
+import commands2
+import constants
+from subsystems.drivesubsystems import DriveSubsystem
+from wpimath.trajectory import Trajectory, TrajectoryConfig, TrajectoryGenerator
+from wpimath.geometry import Translation2d, Pose2d, Rotation2d
+from wpimath.controller import ProfiledPIDController, PIDController
+import math
+
+
+class RobotContainer:
+    def __init__(self):
+        """
+        The robot's subsystems
+        """
+        self.robotDrive = DriveSubsystem()
+
+        # Driver Controller
+        self.driverController = XboxController(
+            constants.OIConstants.kDriverControllerPort
+        )
+
+        """
+        Configure default commands
+        """
+        self.robotDrive.setDefaultCommand(
+            # The left stick controls translation of the robot.
+            # turning is controlled by the X axis of the right stick.
+            commands2.RunCommand(
+                lambda: self.robotDrive.drive(
+                    # Multiply by max speed to map the joystick unitless inputs to actual units.
+                    # This will map the [-1, 1] to [max speed backwards, max speed forwards],
+                    # converting them to actual units.
+                    self.driverController.getYChannel()
+                    * constants.DriveConstants.kMaxSpeedMetersPerSecond,
+                    self.driverController.getXChannel()
+                    * constants.DriveConstants.kMaxSpeedMetersPerSecond,
+                    self.driverController.getX(constants.Hand.kRight)
+                    * constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
+                    False,
+                ),
+                self.robotDrive,
+            )
+        )
+
+    def configureButtonBindings(self):
+        pass
+
+    def getAutonomousCommand(self):
+        """
+        Create config for trajectory
+        """
+        config = TrajectoryConfig(
+            constants.AutoConstants.kMaxSpeedMetersPerSecond,
+            constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared,
+        ).setKinematics(constants.DriveConstants.kDriveKinematics)
+
+        # An example trajectory to follow. All units in meters.
+
+        example_trajectory = TrajectoryGenerator.generateTrajectory(
+            # Start at the origin facing the +X direction
+            Pose2d(0, 0, Rotation2d(0)),
+            # Pass through these two interior waypoints, making an 's' curve path
+            [Translation2d(1, 1), Translation2d(2, -1)],
+            # End 3 meters straight ahead of where we started, facing forward
+            Pose2d(3, 0, Rotation2d(0)),
+            config,
+        )
+
+        theta_controller = ProfiledPIDController(
+            constants.AutoConstants.kPThetaController,
+            0,
+            0,
+            constants.AutoConstants.kThetaControllerConstraints,
+        )
+        theta_controller.enableContinuousInput(-math.pi, math.pi)
+
+        swerve_controller_command = commands2.Swerve4ControllerCommand(
+            example_trajectory,
+            self.robotDrive.getPose,
+            # Functional interface to feed supplier
+            constants.DriveConstants.kDriveKinematics,
+            # Position controllers
+            PIDController(constants.AutoConstants.kPXController, 0, 0),
+            PIDController(constants.AutoConstants.kPYController, 0, 0),
+            theta_controller,
+            self.robotDrive.setModuleStates,
+            self.robotDrive,
+        )
+
+        """
+        Reset odometry to the initial pose of the trajectory, run path following
+        command, then stop at the end.
+        """
+        return commands2.Commands.sequence(
+            commands2.InstantCommand(
+                lambda: self.robotDrive.resetOdometry(
+                    example_trajectory.getInitialPose()
+                )
+            ),
+            swerve_controller_command,
+            commands2.InstantCommand(lambda: self.robotDrive.drive(0, 0, 0, False)),
+        )