diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..9789407 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,12 @@ package frc.robot; +import com.revrobotics.CANSparkBase.IdleMode; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -16,4 +22,120 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + + public static final class DriveConstants { + // Driving Parameters - Note that these are not the maximum capable speeds of + // the robot, rather the allowed maximum speeds + public static final double kMaxSpeedMetersPerSecond = 4.8; + public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second + + public static final double kDirectionSlewRate = 1.2; // radians per second + public static final double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%) + public static final double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) + + // Chassis configuration + public static final double kTrackWidth = Units.inchesToMeters(26.5); + // Distance between centers of right and left wheels on robot + public static final double kWheelBase = Units.inchesToMeters(26.5); + // Distance between front and back wheels on robot + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + // Angular offsets of the modules relative to the chassis in radians + public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI / 2; + + // SPARK MAX CAN IDs + public static final int kFrontLeftDrivingCanId = 11; + public static final int kRearLeftDrivingCanId = 13; + public static final int kFrontRightDrivingCanId = 15; + public static final int kRearRightDrivingCanId = 17; + + public static final int kFrontLeftTurningCanId = 10; + public static final int kRearLeftTurningCanId = 12; + public static final int kFrontRightTurningCanId = 14; + public static final int kRearRightTurningCanId = 16; + + public static final boolean kGyroReversed = false; + } + + public static final class ModuleConstants { + // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. + // This changes the drive speed of the module (a pinion gear with more teeth will result in a + // robot that drives faster). + public static final int kDrivingMotorPinionTeeth = 14; + + // Invert the turning encoder, since the output shaft rotates in the opposite direction of + // the steering motor in the MAXSwerve Module. + public static final boolean kTurningEncoderInverted = true; + + // Calculations required for driving motor conversion factors and feed forward + public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; + public static final double kWheelDiameterMeters = 0.0762; + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion + public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) + / kDrivingMotorReduction; + + public static final double kDrivingEncoderPositionFactor = (kWheelDiameterMeters * Math.PI) + / kDrivingMotorReduction; // meters + public static final double kDrivingEncoderVelocityFactor = ((kWheelDiameterMeters * Math.PI) + / kDrivingMotorReduction) / 60.0; // meters per second + + public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians + public static final double kTurningEncoderVelocityFactor = (2 * Math.PI) / 60.0; // radians per second + + public static final double kTurningEncoderPositionPIDMinInput = 0; // radians + public static final double kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor; // radians + + public static final double kDrivingP = 0.04; + public static final double kDrivingI = 0; + public static final double kDrivingD = 0; + public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; + public static final double kDrivingMinOutput = -1; + public static final double kDrivingMaxOutput = 1; + + public static final double kTurningP = 1; + public static final double kTurningI = 0; + public static final double kTurningD = 0; + public static final double kTurningFF = 0; + public static final double kTurningMinOutput = -1; + public static final double kTurningMaxOutput = 1; + + public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; + public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; + + public static final int kDrivingMotorCurrentLimit = 50; // amps + public static final int kTurningMotorCurrentLimit = 20; // amps + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + public static final double kDriveDeadband = 0.05; + } + + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; + + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + } + + public static final class NeoMotorConstants { + public static final double kFreeSpeedRpm = 5676; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..75a38b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,28 @@ package frc.robot; +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.OIConstants; import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.SwerveSubsystem.DriveSubsystem; +import java.util.List; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.math.trajectory.Trajectory; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -20,15 +35,27 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + public static DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + // private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = + public static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings + m_driveSubsystem.setDefaultCommand( + // The left stick controls translation of the robot. + // Turning is controlled by the X axis of the right stick. + new RunCommand( + () -> m_driveSubsystem.drive( + -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), + true, true), + m_driveSubsystem)); + configureBindings(); } @@ -43,12 +70,13 @@ public RobotContainer() { */ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); + // new Trigger(m_exampleSubsystem::exampleCondition) + // .onTrue(new ExampleCommand(m_exampleSubsystem)); // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } /** @@ -58,6 +86,43 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + // Create config for trajectory + TrajectoryConfig config = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(DriveConstants.kDriveKinematics); + + // An example trajectory to follow. All units in meters. + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of(new Translation2d(1, 1), new Translation2d(2, -1)), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + config); + + var thetaController = new ProfiledPIDController( + AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + exampleTrajectory, + m_driveSubsystem::getPose, // Functional interface to feed supplier + DriveConstants.kDriveKinematics, + + // Position controllers + new PIDController(AutoConstants.kPXController, 0, 0), + new PIDController(AutoConstants.kPYController, 0, 0), + thetaController, + m_driveSubsystem::setModuleStates, + m_driveSubsystem); + + // Reset odometry to the starting pose of the trajectory. + m_driveSubsystem.resetOdometry(exampleTrajectory.getInitialPose()); + + // Run path following command, then stop at the end. + return swerveControllerCommand.andThen(() -> m_driveSubsystem.drive(0, 0, 0, false, false)); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem/DriveSubsystem.java new file mode 100644 index 0000000..0eb12ad --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem/DriveSubsystem.java @@ -0,0 +1,247 @@ +// 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. + +package frc.robot.subsystems.SwerveSubsystem; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.I2C; +import frc.robot.Constants.DriveConstants; +import frc.utils.SwerveUtils; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriveSubsystem extends SubsystemBase { + // Create SwerveModules + private final SwerveModule m_frontLeft = new SwerveModule( + DriveConstants.kFrontLeftDrivingCanId, + DriveConstants.kFrontLeftTurningCanId, + DriveConstants.kFrontLeftChassisAngularOffset); + + private final SwerveModule m_frontRight = new SwerveModule( + DriveConstants.kFrontRightDrivingCanId, + DriveConstants.kFrontRightTurningCanId, + DriveConstants.kFrontRightChassisAngularOffset); + + private final SwerveModule m_rearLeft = new SwerveModule( + DriveConstants.kRearLeftDrivingCanId, + DriveConstants.kRearLeftTurningCanId, + DriveConstants.kBackLeftChassisAngularOffset); + + private final SwerveModule m_rearRight = new SwerveModule( + DriveConstants.kRearRightDrivingCanId, + DriveConstants.kRearRightTurningCanId, + DriveConstants.kBackRightChassisAngularOffset); + + // The gyro sensor + private AHRS m_gyro = new AHRS(I2C.Port.kOnboard); + // Slew rate filter variables for controlling lateral acceleration + private double m_currentRotation = 0.0; + private double m_currentTranslationDir = 0.0; + private double m_currentTranslationMag = 0.0; + + private SlewRateLimiter m_magLimiter = new SlewRateLimiter(DriveConstants.kMagnitudeSlewRate); + private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DriveConstants.kRotationalSlewRate); + private double m_prevTime = WPIUtilJNI.now() * 1e-6; + + // Odometry class for tracking robot pose + SwerveDriveOdometry m_odometry = new SwerveDriveOdometry( + DriveConstants.kDriveKinematics, + Rotation2d.fromDegrees(m_gyro.getAngle()), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }); + + /** Creates a new DriveSubsystem. */ + public DriveSubsystem() { + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + m_odometry.update( + Rotation2d.fromDegrees(m_gyro.getAngle()), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition( + Rotation2d.fromDegrees(m_gyro.getAngle()), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }, + pose); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the + * field. + * @param rateLimit Whether to enable rate limiting for smoother control. + */ + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { + + double xSpeedCommanded; + double ySpeedCommanded; + + if (rateLimit) { + // Convert XY to polar for rate limiting + double inputTranslationDir = Math.atan2(ySpeed, xSpeed); + double inputTranslationMag = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); + + // Calculate the direction slew rate based on an estimate of the lateral acceleration + double directionSlewRate; + if (m_currentTranslationMag != 0.0) { + directionSlewRate = Math.abs(DriveConstants.kDirectionSlewRate / m_currentTranslationMag); + } else { + directionSlewRate = 500.0; //some high number that means the slew rate is effectively instantaneous + } + + + double currentTime = WPIUtilJNI.now() * 1e-6; + double elapsedTime = currentTime - m_prevTime; + double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir); + if (angleDif < 0.45*Math.PI) { + m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + } + else if (angleDif > 0.85*Math.PI) { + if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking + // keep currentTranslationDir unchanged + m_currentTranslationMag = m_magLimiter.calculate(0.0); + } + else { + m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + } + } + else { + m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.calculate(0.0); + } + m_prevTime = currentTime; + + xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir); + ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir); + m_currentRotation = m_rotLimiter.calculate(rot); + + + } else { + xSpeedCommanded = xSpeed; + ySpeedCommanded = ySpeed; + m_currentRotation = rot; + } + + // Convert the commanded speeds into the correct units for the drivetrain + double xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond; + double ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond; + double rotDelivered = m_currentRotation * DriveConstants.kMaxAngularSpeed; + + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle())) + : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + m_frontRight.setDesiredState(swerveModuleStates[1]); + m_rearLeft.setDesiredState(swerveModuleStates[2]); + m_rearRight.setDesiredState(swerveModuleStates[3]); + } + + /** + * Sets the wheels into an X formation to prevent movement. + */ + public void setX() { + m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); + m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); + m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); + m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); + } + + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired SwerveModule states. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(desiredStates[0]); + m_frontRight.setDesiredState(desiredStates[1]); + m_rearLeft.setDesiredState(desiredStates[2]); + m_rearRight.setDesiredState(desiredStates[3]); + } + + /** Resets the drive encoders to currently read a position of 0. */ + public void resetEncoders() { + m_frontLeft.resetEncoders(); + m_rearLeft.resetEncoders(); + m_frontRight.resetEncoders(); + m_rearRight.resetEncoders(); + } + + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from -180 to 180 + */ + public double getHeading() { + return Rotation2d.fromDegrees(m_gyro.getAngle()).getDegrees(); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem/SwerveModule.java new file mode 100644 index 0000000..edd98ed --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem/SwerveModule.java @@ -0,0 +1,164 @@ +// 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. + +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; + +import frc.robot.Constants.ModuleConstants; + +public class SwerveModule { + private final CANSparkMax m_drivingSparkMax; + private final CANSparkMax m_turningSparkMax; + + private final RelativeEncoder m_drivingEncoder; + private final AbsoluteEncoder m_turningEncoder; + + private final SparkPIDController m_drivingPIDController; + private final SparkPIDController m_turningPIDController; + + private double m_chassisAngularOffset = 0; + private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); + + /** + * Constructs a SwerveModule and configures the driving and turning motor, + * encoder, and PID controller. This configuration is specific to the REV + * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore + * Encoder. + */ + public SwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { + m_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless); + m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless); + + // Factory reset, so we get the SPARKS MAX to a known state before configuring + // them. This is useful in case a SPARK MAX is swapped out. + m_drivingSparkMax.restoreFactoryDefaults(); + m_turningSparkMax.restoreFactoryDefaults(); + + // Setup encoders and PID controllers for the driving and turning SPARKS MAX. + m_drivingEncoder = m_drivingSparkMax.getEncoder(); + m_turningEncoder = m_turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle); + m_drivingPIDController = m_drivingSparkMax.getPIDController(); + m_turningPIDController = m_turningSparkMax.getPIDController(); + m_drivingPIDController.setFeedbackDevice(m_drivingEncoder); + m_turningPIDController.setFeedbackDevice(m_turningEncoder); + + // Apply position and velocity conversion factors for the driving encoder. The + // native units for position and velocity are rotations and RPM, respectively, + // but we want meters and meters per second to use with WPILib's swerve APIs. + m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor); + m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor); + + // Apply position and velocity conversion factors for the turning encoder. We + // want these in radians and radians per second to use with WPILib's swerve + // APIs. + m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor); + m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor); + + // Invert the turning encoder, since the output shaft rotates in the opposite direction of + // the steering motor in the MAXSwerve Module. + m_turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted); + + // Enable PID wrap around for the turning motor. This will allow the PID + // controller to go through 0 to get to the setpoint i.e. going from 350 degrees + // to 10 degrees will go through 0 rather than the other direction which is a + // longer route. + m_turningPIDController.setPositionPIDWrappingEnabled(true); + m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput); + m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput); + + // Set the PID gains for the driving motor. Note these are example gains, and you + // may need to tune them for your own robot! + m_drivingPIDController.setP(ModuleConstants.kDrivingP); + m_drivingPIDController.setI(ModuleConstants.kDrivingI); + m_drivingPIDController.setD(ModuleConstants.kDrivingD); + m_drivingPIDController.setFF(ModuleConstants.kDrivingFF); + m_drivingPIDController.setOutputRange(ModuleConstants.kDrivingMinOutput, + ModuleConstants.kDrivingMaxOutput); + + // Set the PID gains for the turning motor. Note these are example gains, and you + // may need to tune them for your own robot! + m_turningPIDController.setP(ModuleConstants.kTurningP); + m_turningPIDController.setI(ModuleConstants.kTurningI); + m_turningPIDController.setD(ModuleConstants.kTurningD); + m_turningPIDController.setFF(ModuleConstants.kTurningFF); + m_turningPIDController.setOutputRange(ModuleConstants.kTurningMinOutput, + ModuleConstants.kTurningMaxOutput); + + m_drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode); + m_turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode); + m_drivingSparkMax.setSmartCurrentLimit(ModuleConstants.kDrivingMotorCurrentLimit); + m_turningSparkMax.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit); + + // Save the SPARK MAX configurations. If a SPARK MAX browns out during + // operation, it will maintain the above configurations. + m_drivingSparkMax.burnFlash(); + m_turningSparkMax.burnFlash(); + + m_chassisAngularOffset = chassisAngularOffset; + m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition()); + m_drivingEncoder.setPosition(0); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + // Apply chassis angular offset to the encoder position to get the position + // relative to the chassis. + return new SwerveModuleState(m_drivingEncoder.getVelocity(), + new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); + } + + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + // Apply chassis angular offset to the encoder position to get the position + // relative to the chassis. + return new SwerveModulePosition( + m_drivingEncoder.getPosition(), + new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); + } + + /** + * Sets the desired state for the module. + * + * @param desiredState Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState desiredState) { + // Apply chassis angular offset to the desired state. + SwerveModuleState correctedDesiredState = new SwerveModuleState(); + correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond; + correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset)); + + // Optimize the reference state to avoid spinning further than 90 degrees. + SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctedDesiredState, + new Rotation2d(m_turningEncoder.getPosition())); + + // Command driving and turning SPARKS MAX towards their respective setpoints. + m_drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity); + m_turningPIDController.setReference(optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition); + + m_desiredState = desiredState; + } + + /** Zeroes all the SwerveModule encoders. */ + public void resetEncoders() { + m_drivingEncoder.setPosition(0); + } +} \ No newline at end of file diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java new file mode 100644 index 0000000..867fe0b --- /dev/null +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -0,0 +1,89 @@ +package frc.utils; + +public class SwerveUtils { + + /** + * Steps a value towards a target with a specified step size. + * @param _current The current or starting value. Can be positive or negative. + * @param _target The target value the algorithm will step towards. Can be positive or negative. + * @param _stepsize The maximum step size that can be taken. + * @return The new value for {@code _current} after performing the specified step towards the specified target. + */ + public static double StepTowards(double _current, double _target, double _stepsize) { + if (Math.abs(_current - _target) <= _stepsize) { + return _target; + } + else if (_target < _current) { + return _current - _stepsize; + } + else { + return _current + _stepsize; + } + } + + /** + * Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size. + * @param _current The current or starting angle (in radians). Can lie outside the 0 to 2*PI range. + * @param _target The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range. + * @param _stepsize The maximum step size that can be taken (in radians). + * @return The new angle (in radians) for {@code _current} after performing the specified step towards the specified target. + * This value will always lie in the range 0 to 2*PI (exclusive). + */ + public static double StepTowardsCircular(double _current, double _target, double _stepsize) { + _current = WrapAngle(_current); + _target = WrapAngle(_target); + + double stepDirection = Math.signum(_target - _current); + double difference = Math.abs(_current - _target); + + if (difference <= _stepsize) { + return _target; + } + else if (difference > Math.PI) { //does the system need to wrap over eventually? + //handle the special case where you can reach the target in one step while also wrapping + if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) { + return _target; + } + else { + return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully + } + + } + else { + return _current + stepDirection * _stepsize; + } + } + + /** + * Finds the (unsigned) minimum difference between two angles including calculating across 0. + * @param _angleA An angle (in radians). + * @param _angleB An angle (in radians). + * @return The (unsigned) minimum difference between the two angles (in radians). + */ + public static double AngleDifference(double _angleA, double _angleB) { + double difference = Math.abs(_angleA - _angleB); + return difference > Math.PI? (2 * Math.PI) - difference : difference; + } + + /** + * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). + * @param _angle The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range. + * @return An angle (in radians) from 0 and 2*PI (exclusive). + */ + public static double WrapAngle(double _angle) { + double twoPi = 2*Math.PI; + + if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below + return 0.0; + } + else if (_angle > twoPi) { + return _angle - twoPi*Math.floor(_angle / twoPi); + } + else if (_angle < 0.0) { + return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1); + } + else { + return _angle; + } + } +} \ No newline at end of file