Added example SwerveControllerCommand

SwerveControllerCommand/constants.py

@@ -0,0 +1,93 @@
+#
+# 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 Constants:
+    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 as 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)),
+        )