Added example SwerveControllerCommand

SwerveControllerCommand/Subsystems/DriveSubsystem.py

@@ -0,0 +1,166 @@
+"""
+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 ADXRS450_Gyro
+import Subsystems.SwerveModule
+from wpimath.kinematics import ChassisSpeeds
+from wpimath.kinematics import SwerveDrive3Kinematics, SwerveDrive3Odometry
+from wpimath.geometry import Pose2d, Rotation2d
+import constants as Constants
+
+class DriveSubsystem:
+    def __init__(self):
+        """
+        Robot swerve modules
+        """
+        self.m_frontLeft = SwerveModule(
+            Constants.DriveConstants.kFrontLeftDriveMotorPort,
+            Constants.DriveConstants.kFrontLeftTurningMotorPort,
+            Constants.DriveConstants.kFrontLeftDriveEncoderPorts,
+            Constants.DriveConstants.kFrontLeftTurningEncoderPorts,
+            Constants.DriveConstants.kFrontLeftDriveEncoderReversed,
+            Constants.DriveConstants.kFrontLeftTurningEncoderReversed
+        )
+
+        self.m_rearLeft = SwerveModule(
+            Constants.DriveConstants.kRearLeftDriveMotorPort,
+            Constants.DriveConstants.kRearLeftTurningMotorPort,
+            Constants.DriveConstants.kRearLeftDriveEncoderPorts,
+            Constants.DriveConstants.kRearLeftTurningEncoderPorts,
+            Constants.DriveConstants.kRearLeftDriveEncoderReversed,
+            Constants.DriveConstants.kRearLeftTurningEncoderReversed
+        )
+
+        self.m_frontRight = SwerveModule(
+            Constants.DriveConstants.kFrontRightDriveMotorPort,
+            Constants.DriveConstants.kFrontRightTurningMotorPort,
+            Constants.DriveConstants.kFrontRightDriveEncoderPorts,
+            Constants.DriveConstants.kFrontRightTurningEncoderPorts,
+            Constants.DriveConstants.kFrontRightDriveEncoderReversed,
+            Constants.DriveConstants.kFrontRightTurningEncoderReversed
+        )
+
+        self.m_rearRight = SwerveModule(
+            Constants.DriveConstants.kRearRightDriveMotorPort,
+            Constants.DriveConstants.kRearRightTurningMotorPort,
+            Constants.DriveConstants.kRearRightDriveEncoderPorts,
+            Constants.DriveConstants.kRearRightTurningEncoderPorts,
+            Constants.DriveConstants.kRearRightDriveEncoderReversed,
+            Constants.DriveConstants.kRearRightTurningEncoderReversed
+        )
+
+        """
+        The gyro sensor
+        """
+        self.m_gyro = ADXRS450_Gyro()
+
+        """
+        Odometry class for tracking robot pose
+        """
+        self.m_odometry = SwerveDrive3Odometry(
+            Constants.DriveConstants.kDriveKinematics,
+            self.m_gyro.getRotation2d(),
+            [
+                self.m_frontLeft.getPosition(),
+                self.m_frontRight.getPosition(),
+                self.m_rearLeft.getPosition(),
+                self.m_rearRight.getPosition()
+            ]
+        )
+
+    def periodic(self):
+        """
+        Update the odometry in the periodic block
+        """
+        self.m_odometry.update(
+            self.m_gyro.getRotation2d(),
+            [
+                self.m_frontLeft.getPosition(),
+                self.m_frontRight.getPosition(),
+                self.m_rearLeft.getPosition(),
+                self.m_rearRight.getPosition()
+            ]
+        )
+
+    def getPose(self):
+        """
+        Returns the currently-estimated pose of the robot.
+        """
+        return self.m_odometry.getPoseMeters()
+
+    def resetOdometry(self, pose):
+        """
+        Resets the odometry to the specified pose.
+        """
+        self.m_odometry.resetPosition(
+            self.m_gyro.getRotation2d(),
+            [
+                self.m_frontLeft.getPosition(),
+                self.m_frontRight.getPosition(),
+                self.m_rearLeft.getPosition(),
+                self.m_rearRight.getPosition()
+            ],
+            pose
+        )
+
+    def drive(self, xSpeed, ySpeed, rot, fieldRelative):
+        """
+        Method to drive the robot using joystick info.
+        """
+        swerveModuleStates = Constants.DriveConstants.kDriveKinematics.toSwerveModuleStates(
+            ChassisSpeeds.discretize(
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                    xSpeed, ySpeed, rot, self.m_gyro.getRotation2d()
+                ) if fieldRelative else ChassisSpeeds(xSpeed, ySpeed, rot),
+                Constants.DriveConstants.kDrivePeriod
+            )
+        )
+        SwerveDrive3Kinematics.desaturateWheelSpeeds(
+            swerveModuleStates, Constants.DriveConstants.kMaxSpeedMetersPerSecond
+        )
+        self.m_frontLeft.setDesiredState(swerveModuleStates[0])
+        self.m_frontRight.setDesiredState(swerveModuleStates[1])
+        self.m_rearLeft.setDesiredState(swerveModuleStates[2])
+        self.m_rearRight.setDesiredState(swerveModuleStates[3])
+
+    def setModuleStates(self, desiredStates):
+        """
+        Sets the swerve ModuleStates.
+        """
+        SwerveDrive3Kinematics.desaturateWheelSpeeds(
+        desiredStates, Constants.DriveConstants.kMaxSpeedMetersPerSecond
+        )
+        self.m_frontLeft.setDesiredState(desiredStates[0])
+        self.m_frontRight.setDesiredState(desiredStates[1])
+        self.m_rearLeft.setDesiredState(desiredStates[2])
+        self.m_rearRight.setDesiredState(desiredStates[3])
+def resetEncoders(self):
+    """
+    Resets the drive encoders to currently read a position of 0.
+    """
+    self.m_frontLeft.resetEncoders()
+    self.m_rearLeft.resetEncoders()
+    self.m_frontRight.resetEncoders()
+    self.m_rearRight.resetEncoders()
+
+def zeroHeading(self):
+    """
+    Zeroes the heading of the robot.
+    """
+    self.m_gyro.reset()
+
+def getHeading(self):
+    """
+    Returns the heading of the robot.
+    """
+    return self.m_gyro.getRotation2d().getDegrees()
+
+def getTurnRate(self):
+    """
+    Returns the turn rate of the robot.
+    """
+    return self.m_gyro.getRate() * (-1.0 if Constants.DriveConstants.kGyroReversed else 1.0)
+

SwerveControllerCommand/Subsystems/SwerveModule.py

@@ -0,0 +1,107 @@
+from wpilib import Encoder, Spark
+from wpimath.geometry import Rotation2d
+from wpimath.kinematics import SwerveModulePosition, SwerveModuleState
+from wpimath.trajectory import TrapezoidProfile
+from wpimath.controller import PIDController, ProfiledPIDController
+import constants
+
+class SwerveModule:
+    def __init__(
+        self,
+        driveMotorChannel,
+        turningMotorChannel,
+        driveEncoderChannels,
+        turningEncoderChannels,
+        driveEncoderReversed,
+        turningEncoderReversed,
+    ):
+        self.m_driveMotor = Spark(driveMotorChannel)
+        self.m_turningMotor = Spark(turningMotorChannel)
+
+        self.m_driveEncoder = Encoder(*driveEncoderChannels)
+        self.m_turningEncoder = Encoder(*turningEncoderChannels)
+
+        self.m_drivePIDController = PIDController(
+            constants.ModuleConstants.kPModuleDriveController, 0, 0
+        )
+
+        self.m_turningPIDController = ProfiledPIDController(
+            constants.ModuleConstants.kPModuleTurningController,
+            0,
+            0,
+            TrapezoidProfile.Constraints(
+                constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
+                constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared,
+            ),
+        )
+
+        """
+        Set the distance per pulse for the drive encoder.
+        """
+        self.m_driveEncoder.setDistancePerPulse(
+            constants.ModuleConstants.kDriveEncoderDistancePerPulse
+        )
+        self.m_driveEncoder.setReverseDirection(driveEncoderReversed)
+
+        """
+        Set the distance in radians per pulse for the turning encoder.
+        """
+        self.m_turningEncoder.setDistancePerPulse(
+            constants.ModuleConstants.kTurningEncoderDistancePerPulse
+        )
+        self.m_turningEncoder.setReverseDirection(turningEncoderReversed)
+
+        """
+        Limit the PID Controller's input range between -pi and pi and set the input
+        to be continuous.
+        """
+        self.m_turningPIDController.enableContinuousInput(-3.14159, 3.14159)
+
+    def getState(self):
+        return SwerveModuleState(
+            self.m_driveEncoder.getRate(),
+            Rotation2d(self.m_turningEncoder.getDistance()),
+        )
+
+    def getPosition(self):
+        return SwerveModulePosition(
+            self.m_driveEncoder.getDistance(),
+            Rotation2d(self.m_turningEncoder.getDistance()),
+        )
+
+    def setDesiredState(self, desiredState):
+        encoderRotation = Rotation2d(self.m_turningEncoder.getDistance())
+
+        """
+        Optimize the reference state to avoid spinning further than 90 degrees
+        """
+        state = SwerveModuleState.optimize(desiredState, encoderRotation)
+
+        """
+        Scale speed by cosine of angle error.
+        """
+        state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos()
+
+        """
+        Calculate the drive output from the drive PID controller.
+        """
+        driveOutput = self.m_drivePIDController.calculate(
+            self.m_driveEncoder.getRate(), state.speedMetersPerSecond
+        )
+
+        """
+        Calculate the turning motor output from the turning PID controller.
+        """
+        turnOutput = self.m_turningPIDController.calculate(
+            self.m_turningEncoder.getDistance(), state.angle.getRadians()
+        )
+
+        """
+        Set motor outputs
+        """
+        self.m_driveMotor.set(driveOutput)
+        self.m_turningMotor.set(turnOutput)
+
+    def resetEncoders(self):
+        self.m_driveEncoder.reset()
+        self.m_turningEncoder.reset()

SwerveControllerCommand/constants.py

@@ -0,0 +1,92 @@
+"""
+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
+        )