Merge branch 'main' into kraken_swerve

src/main/java/frc/robot/RobotContainer.java

@@ -14,12 +14,16 @@ import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
@@ -40,6 +44,8 @@ public class RobotContainer {
 	private CommandXboxController driver;
 	private CommandXboxController operator;
 
+	private SendableChooser<Command> autoChooser;
+
 	public RobotContainer() {
 		arm = new Arm();
 
@@ -58,8 +64,12 @@ public class RobotContainer {
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
+		autoChooser = AutoBuilder.buildAutoChooser();
+
 		configureButtonBindings();
 
+		configureNamedCommands();
+
 		configureShuffleboard();
 	}
 
@@ -86,7 +96,7 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.runElevator(operator::getLeftY)
+			elevator.runAssistedElevator(operator::getLeftY)
 		);
 
 		indexer.setDefaultCommand(
@@ -148,10 +158,22 @@ public class RobotContainer {
 		);
 	}
 
+	private void configureNamedCommands() {
+		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
+	}
+
 	//creates tabs and transforms them on the shuffleboard
     private void configureShuffleboard() {
+		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
 
+		Shuffleboard.selectTab(OIConstants.kAutoTab);
+
+		autoTab.add("Auto Selection", autoChooser)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kComboBoxChooser);
+
 		sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
@@ -164,7 +186,7 @@ public class RobotContainer {
   }
 
 	public Command getAutonomousCommand() {
-		return new PrintCommand("NO AUTO DEFINED");
+		return autoChooser.getSelected();
 	}
 
 	//teleop routines
@@ -222,4 +244,14 @@ public class RobotContainer {
 			() -> elevatorFirst
 		);
 	}
+
+	/*
+	 * A moveManipulator method that will guarantee a safe movement. 
+	 * Here in case we need want to skip moveManipulator debugging
+	 */
+	@SuppressWarnings("unused")
+	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
+		return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+			.andThen(arm.goToSetpoint(armPosition, 2));
+	}
 }

src/main/java/frc/robot/constants/ArmConstants.java

@@ -1,15 +1,37 @@
 package frc.robot.constants;
 
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ArmConstants {
     public static final int kArmMotorID = 0;
     public static final int kCANcoderID = 0;
 
-    public static final double kEncoderConversionFactor = 0;
+    public static final int kMotorAmpsMax = 0;
 
     public static final double kArmMaxVelocity = 0;
 
+    public static final double kPositionalP = 0;
+    public static final double kPositionalI = 0;
+    public static final double kPositionalD = 0;
+    public static final double kPositionalTolerance = Units.degreesToRadians(1);
+
+    public static final double kVelocityP = 0;
+    public static final double kVelocityI = 0;
+    public static final double kVelocityD = 0;
+    // TODO Is this reasonable?
+    public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
+
     public static final double kArmCoralIntakePosition = 0;
     public static final double kArmL1Position = 0;
     public static final double kArmL2Position = 0;
@@ -19,7 +41,39 @@ public class ArmConstants {
     public static final double kArmL3AlgaePosition = 0;
     public static final double kArmSafeStowPosition = 0;
 
+    public static final double kMagnetOffset = 0.0;
+    public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;
+
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
+
+    public static final SysIdRoutine.Config kSysIDConfig = new Config(
+        Volts.of(kSysIDRampRate).per(Second),
+        Volts.of(kSysIDStepVolts),
+        Seconds.of(kSysIDTimeout)
+    );
+
     public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
 
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+        canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
+        // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
+        //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
+
+        motorConfig
+            .smartCurrentLimit(kMotorAmpsMax)
+            .idleMode(kIdleMode);
+    }
+
 
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -1,5 +1,13 @@
 package frc.robot.constants;
 
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
@@ -15,4 +23,21 @@ public class AutoConstants {
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXController, 0, 0),
+        new PIDConstants(kPYController, 0, 0)
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+        }
+    }
 }

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -43,6 +43,7 @@ public class ElevatorConstants {
     public static final double kElevatorL2AlgaePosition = 0;
     public static final double kElevatorL3AlgaePosition = 0;
     public static final double kElevatorBracePosition = 0;
+    public static final double kElevatorMaxHeight = 0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -2,6 +2,12 @@ package frc.robot.constants;
 
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ModuleConstants {
@@ -20,52 +26,73 @@ public class ModuleConstants {
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 
-    public static final int kDriveMotorCurrentLimit = 40;
+    public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final double kTurningFactor = 2 * Math.PI;
+    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+    
+    public static final double kDriveP = .04;
+    public static final double kDriveI = 0;
+    public static final double kDriveD = 0;
+    public static final double kDriveS = 0;
+    public static final double kDriveV = kDrivingVelocityFeedForward;
+    public static final double kDriveA = 0;
+    
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final int kDriveMotorStatorCurrentLimit = 120;
     public static final int kTurnMotorCurrentLimit = 20;
 
+    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
+
+    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM 
 
-    public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
     public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
 
-    static {
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
-        // Use module constants to calculate conversion factors and feed forward gain.
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
-        double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
-        double turningFactor = 2 * Math.PI;
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
-        double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
 
-        drivingConfig
+    static {
-            .idleMode(IdleMode.kBrake)
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
-            .smartCurrentLimit(kDriveMotorCurrentLimit);
+
-        drivingConfig.encoder
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
-            .positionConversionFactor(drivingFactor) // meters
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
-            .velocityConversionFactor(drivingFactor / 60.0); // meters per second
+
-        drivingConfig.closedLoop
+        kDriveMotorConfig.Inverted = kDriveInversionState;
-            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
-            // These are example gains you may need to them for your own robot!
+
-            .pid(0.04, 0, 0)
+        kDriveSlot0Config.kP = kDriveP;
-            .velocityFF(drivingVelocityFeedForward)
+        kDriveSlot0Config.kI = kDriveI;
-            .outputRange(-1, 1);
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;
 
         turningConfig
-            .idleMode(IdleMode.kBrake)
+            .idleMode(kTurnIdleMode)
-            .smartCurrentLimit(20);
+            .smartCurrentLimit(kTurnMotorCurrentLimit);
         turningConfig.absoluteEncoder
             // Invert the turning encoder, since the output shaft rotates in the opposite
             // direction of the steering motor in the MAXSwerve Module.
             .inverted(true)
-            .positionConversionFactor(turningFactor) // radians
+            .positionConversionFactor(kTurningFactor) // radians
-            .velocityConversionFactor(turningFactor / 60.0); // radians per second
+            .velocityConversionFactor(kTurningFactor / 60.0); // radians per second
         turningConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             // These are example gains you may need to them for your own robot!
-            .pid(1, 0, 0)
+            .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
             // 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.
             .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, turningFactor);
+            .positionWrappingInputRange(0, kTurningFactor);
     }
 }

src/main/java/frc/robot/constants/OIConstants.java

@@ -6,5 +6,6 @@ public class OIConstants {
     
     public static final double kDriveDeadband = 0.05;
 
+    public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -4,6 +4,8 @@ import java.util.function.DoubleSupplier;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -14,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ArmConstants;
 
 public class Arm extends SubsystemBase {
-    private SparkMax armMotor;
+    protected SparkMax armMotor;
 
     private CANcoder canCoder;
 
@@ -29,7 +31,28 @@ public class Arm extends SubsystemBase {
             MotorType.kBrushless
         );
 
+        armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        positionController = new PIDController(
+            ArmConstants.kPositionalP, 
+            ArmConstants.kPositionalI, 
+            ArmConstants.kPositionalD
+        );
+        
+        // TODO: Generate constants for continuous input range based on CANcoder configuration?
+        positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
+        positionController.setTolerance(ArmConstants.kPositionalTolerance);
+
+        velocityController = new PIDController(
+            ArmConstants.kVelocityP, 
+            ArmConstants.kVelocityI, 
+            ArmConstants.kVelocityD
+        );
+
+        velocityController.setTolerance(ArmConstants.kVelocityTolerance);
+
         canCoder = new CANcoder(ArmConstants.kCANcoderID);
+        canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
     }
 
     /**
@@ -85,7 +108,7 @@ public class Arm extends SubsystemBase {
     }
 
     public double getEncoderPosition() {
-        return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble());
+        return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble());
     }
 
     public double getEncoderVelocity() {

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -4,9 +4,14 @@
 
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -15,10 +20,10 @@ 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.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
 
@@ -30,7 +35,7 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private ADIS16470_IMU m_gyro;
+    private AHRS ahrs;
 
     // Odometry class for tracking robot pose
     private SwerveDriveOdometry m_odometry;
@@ -61,11 +66,41 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        m_gyro = new ADIS16470_IMU();
+        ahrs = new AHRS(NavXComType.kMXP_SPI);
 
         m_odometry = new SwerveDriveOdometry(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(ahrs.getAngle()),
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+        });
+
+        AutoBuilder.configure(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPPDriveController, 
+            AutoConstants.kRobotConfig,
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
+    }
+
+    @Override
+    public void periodic() {
+        // Update the odometry in the periodic block
+        m_odometry.update(
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -74,17 +109,21 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
-    @Override
+    public ChassisSpeeds getCurrentChassisSpeeds() {
-    public void periodic() {
+        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
-        // Update the odometry in the periodic block
+            m_frontLeft.getState(),
-        m_odometry.update(
+            m_frontRight.getState(),
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            m_rearLeft.getState(),
-            new SwerveModulePosition[] {
+            m_rearRight.getState()
-                m_frontLeft.getPosition(),
+        );
-                m_frontRight.getPosition(),
+    }
-                m_rearLeft.getPosition(),
+
-                m_rearRight.getPosition()
+    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
-        });
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+
+        setModuleStates(newStates);
     }
 
     /**
@@ -103,7 +142,7 @@ public class Drivetrain extends SubsystemBase {
      */
     public void resetOdometry(Pose2d pose) {
         m_odometry.resetPosition(
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -144,7 +183,7 @@ public class Drivetrain extends SubsystemBase {
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
+                        Rotation2d.fromDegrees(getGyroValue()))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -194,7 +233,11 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        m_gyro.reset();
+        ahrs.reset();;
+    }
+
+    public double getGyroValue() {
+        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -203,7 +246,7 @@ public class Drivetrain extends SubsystemBase {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public double getHeading() {
-        return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
+        return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
     }
 
     /**
@@ -212,6 +255,6 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 }

src/main/java/frc/robot/subsystems/Elevator.java

@@ -97,8 +97,13 @@ public class Elevator extends SubsystemBase {
         return motionTarget > ElevatorConstants.kElevatorBracePosition;
     }
 
-    //manual command that keeps ouput speed consistent no matter the direction
+    /**
-    public Command runElevator(DoubleSupplier speed) {
+     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed How fast the elevator moves
+     * @return Sets motor voltage to move the elevator relative to the speed parameter
+     */
+    public Command runAssistedElevator(DoubleSupplier speed) {
         return run(() -> {
             double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
 
@@ -108,11 +113,29 @@ public class Elevator extends SubsystemBase {
             ) + feedForward.calculate(realSpeedTarget);
 
             elevatorMotor1.setVoltage(voltsOut);
-        }).until(bottomLimitSwitch::get);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
     }
 
+    /**
+     * A manual translation command that uses feed forward calculation to maintain position
+     * 
+     * @param speed The speed at which the elevator translates
+     * @return Sets motor voltage to translate the elevator and maintain position
+     */
+    public Command runManualElevator(double speed) {
+        return run(() -> {
+            elevatorMotor1.set(speed);
+        });
+    }
     
-    //go to setpoint command
+    /**
+     * Moves the elevator to a target destination (setpoint)
+     * 
+     * @param setpoint Target destination of the subsystem
+     * @param timeout Time to achieve the setpoint before quitting
+     * @return Sets motor voltage to achieve the target destination
+     */
     public Command goToSetpoint(double setpoint, double timeout) {
         return run(() -> {
             double voltsOut = positionController.calculate(
@@ -126,7 +149,21 @@ public class Elevator extends SubsystemBase {
         ).withTimeout(timeout);
     }
 
+    /**
+     * Returns the current encoder position
+     * 
+     * @return Current encoder position
+     */
     public double getEncoderPosition() {
         return encoder.getPosition();
     }
+
+    /**
+     * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
+     * 
+     * @return The value of bottomLimitSwitch
+     */
+    public boolean getBottomLimitSwitch() {
+        return bottomLimitSwitch.get();
+    }
 }

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -15,21 +15,22 @@ import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.RelativeEncoder;
 
 import frc.robot.constants.ModuleConstants;
 
 public class MAXSwerveModule {
-  private final SparkMax m_drivingSpark;
+  private final TalonFX m_drive;
   private final SparkMax m_turningSpark;
 
-  private final RelativeEncoder m_drivingEncoder;
   private final AbsoluteEncoder m_turningEncoder;
 
-  private final SparkClosedLoopController m_drivingClosedLoopController;
   private final SparkClosedLoopController m_turningClosedLoopController;
 
+  private final VelocityVoltage driveVelocityRequest;
+
   private double m_chassisAngularOffset = 0;
   private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
 
@@ -40,26 +41,29 @@ public class MAXSwerveModule {
    * Encoder.
    */
   public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
+    m_drive = new TalonFX(drivingCANId);
     m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
 
-    m_drivingEncoder = m_drivingSpark.getEncoder();
     m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
 
-    m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
     m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
 
+    driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
+
     // Apply the respective configurations to the SPARKS. Reset parameters before
     // applying the configuration to bring the SPARK to a known good state. Persist
     // the settings to the SPARK to avoid losing them on a power cycle.
-    m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
-        PersistMode.kPersistParameters);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+
     m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
         PersistMode.kPersistParameters);
 
     m_chassisAngularOffset = chassisAngularOffset;
     m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
   }
 
   /**
@@ -70,7 +74,7 @@ public class MAXSwerveModule {
   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(),
+    return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
         new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
   }
 
@@ -82,8 +86,7 @@ public class MAXSwerveModule {
   public SwerveModulePosition getPosition() {
     // Apply chassis angular offset to the encoder position to get the position
     // relative to the chassis.
-    return new SwerveModulePosition(
+    return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
-        m_drivingEncoder.getPosition(),
         new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
   }
 
@@ -102,14 +105,21 @@ public class MAXSwerveModule {
     correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
 
     // Command driving and turning SPARKS towards their respective setpoints.
-    m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
+    m_drive.setControl(
+      driveVelocityRequest.withVelocity(
+        correctedDesiredState.speedMetersPerSecond
+      ).withFeedForward(
+        correctedDesiredState.speedMetersPerSecond
+      )
+    );
+
     m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
 
     m_desiredState = desiredState;
   }
 
   public void setVoltageDrive(double voltage){
-    m_drivingSpark.setVoltage(voltage);
+    m_drive.setVoltage(voltage);
   }
 
   public void setVoltageTurn(double voltage) {
@@ -117,7 +127,7 @@ public class MAXSwerveModule {
   }
 
   public double getVoltageDrive() {
-    return m_drivingSpark.get() * RobotController.getBatteryVoltage();
+    return m_drive.get() * RobotController.getBatteryVoltage();
   }
 
   public double getVoltageTurn() {
@@ -126,6 +136,6 @@ public class MAXSwerveModule {
 
   /** Zeroes all the SwerveModule encoders. */
   public void resetEncoders() {
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
   }
 }

src/main/java/frc/robot/subsystems/sysid/ArmSysID.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.constants.ArmConstants;
+import frc.robot.subsystems.Arm;
+
+public class ArmSysID extends Arm {
+    private MutVoltage appliedVoltage;
+
+    private MutAngle armPosition;
+
+    private MutAngularVelocity armVelocity;
+
+    private SysIdRoutine routine;
+
+    public ArmSysID() {
+        super();
+
+        appliedVoltage = Volts.mutable(0);
+
+        armPosition = Radians.mutable(0);
+
+        armVelocity = RadiansPerSecond.mutable(0);
+
+        routine = new SysIdRoutine(
+            ArmConstants.kSysIDConfig,
+            new SysIdRoutine.Mechanism(
+                armMotor::setVoltage, 
+                (log) -> {
+                    log.motor("armMotor")
+                        .voltage(appliedVoltage.mut_replace(
+                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                        ))
+                        .angularPosition(armPosition.mut_replace(
+                            getEncoderPosition(), Radians
+                        ))
+                        .angularVelocity(armVelocity.mut_replace(
+                            getEncoderVelocity(), RadiansPerSecond
+                        ));
+                }, 
+                this
+            )
+        );
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+}