From 3d054b043e93a8e9d9a526625423859d23e49a34 Mon Sep 17 00:00:00 2001 From: Jordan <117786700+jordanjcoderman@users.noreply.github.com> Date: Tue, 28 Nov 2023 17:34:41 -0800 Subject: [PATCH 1/3] Start Phoenix 6 swerve implementation --- .../java/frc/robot/swerve/SwerveCorner.java | 12 - .../java/frc/robot/swerve/SwerveModule.java | 225 --------- .../robot/swerve/SwerveModuleConstants.java | 22 - .../frc/robot/swerve/SwerveSubsystem.java | 356 -------------- vendordeps/Phoenix6.json | 339 +++++++++++++ vendordeps/PhoenixProAnd5.json | 456 ------------------ 6 files changed, 339 insertions(+), 1071 deletions(-) delete mode 100644 src/main/java/frc/robot/swerve/SwerveCorner.java delete mode 100644 src/main/java/frc/robot/swerve/SwerveModule.java delete mode 100644 src/main/java/frc/robot/swerve/SwerveModuleConstants.java delete mode 100644 src/main/java/frc/robot/swerve/SwerveSubsystem.java create mode 100644 vendordeps/Phoenix6.json delete mode 100644 vendordeps/PhoenixProAnd5.json diff --git a/src/main/java/frc/robot/swerve/SwerveCorner.java b/src/main/java/frc/robot/swerve/SwerveCorner.java deleted file mode 100644 index 39d3450..0000000 --- a/src/main/java/frc/robot/swerve/SwerveCorner.java +++ /dev/null @@ -1,12 +0,0 @@ -// 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.swerve; - -public enum SwerveCorner { - FRONT_LEFT, - FRONT_RIGHT, - BACK_LEFT, - BACK_RIGHT; -} diff --git a/src/main/java/frc/robot/swerve/SwerveModule.java b/src/main/java/frc/robot/swerve/SwerveModule.java deleted file mode 100644 index 560fdd4..0000000 --- a/src/main/java/frc/robot/swerve/SwerveModule.java +++ /dev/null @@ -1,225 +0,0 @@ -// 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.swerve; - -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenixpro.StatusCode; -import com.ctre.phoenixpro.StatusSignalValue; -import com.ctre.phoenixpro.configs.ClosedLoopGeneralConfigs; -import com.ctre.phoenixpro.configs.TalonFXConfiguration; -import com.ctre.phoenixpro.controls.DutyCycleOut; -import com.ctre.phoenixpro.controls.PositionVoltage; -import com.ctre.phoenixpro.controls.VelocityVoltage; -import com.ctre.phoenixpro.hardware.TalonFX; -import com.ctre.phoenixpro.signals.InvertedValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import frc.robot.config.Config; -import frc.robot.util.CircleConverter; -import frc.robot.util.CtreModuleState; -import frc.robot.util.GearingConverter; -import org.littletonrobotics.junction.Logger; - -public class SwerveModule { - private static final GearingConverter STEER_MOTOR_GEARING_CONVERTER = - GearingConverter.fromReduction(Config.SWERVE_STEER_GEARING_REDUCTION); - - private static final GearingConverter DRIVE_MOTOR_GEARING_CONVERTER = - GearingConverter.fromReduction(Config.SWERVE_DRIVE_GEARING_REDUCTION); - private static final CircleConverter DRIVE_MOTOR_WHEEL_CONVERTER = - CircleConverter.fromDiameter(Config.WHEEL_DIAMETER); - - private final SwerveModuleConstants constants; - private final TalonFX driveMotor; - private final TalonFX steerMotor; - private final CANCoder encoder; - private final DutyCycleOut driveVoltageOpenLoopRequest = - new DutyCycleOut(0, Config.SWERVE_USE_FOC, true); - private final PositionVoltage steerMotorControl = - new PositionVoltage(0, Config.SWERVE_USE_FOC, 0, 0, false); - private final VelocityVoltage driveVoltageClosedLoopRequest = - new VelocityVoltage(0, Config.SWERVE_USE_FOC, 0, 0, false); - private Rotation2d previousAngle = new Rotation2d(); - private double commandedDriveVelocity = 0; - private boolean rotorPositionSet = false; - - private StatusSignalValue driveMotorStatorCurrent; - - public SwerveModule( - SwerveModuleConstants constants, TalonFX driveMotor, TalonFX steerMotor, CANCoder encoder) { - this.constants = constants; - this.driveMotor = driveMotor; - this.steerMotor = steerMotor; - this.encoder = encoder; - - com.ctre.phoenixpro.configs.TalonFXConfiguration driveMotorConfigs = new TalonFXConfiguration(); - - driveMotorConfigs.Slot0.kP = Config.SWERVE_DRIVE_KP; - driveMotorConfigs.Slot0.kI = Config.SWERVE_DRIVE_KI; - driveMotorConfigs.Slot0.kD = Config.SWERVE_DRIVE_KD; - driveMotorConfigs.Slot0.kV = Config.SWERVE_DRIVE_KV; - driveMotorConfigs.Slot0.kS = Config.SWERVE_DRIVE_KS; - - driveMotorConfigs.Voltage.PeakForwardVoltage = 12; - driveMotorConfigs.Voltage.PeakReverseVoltage = -12; - - driveMotorConfigs.CurrentLimits.SupplyCurrentLimit = 15; - driveMotorConfigs.CurrentLimits.SupplyCurrentLimitEnable = true; - - if (constants.driveInversion) { - driveMotorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - } else { - driveMotorConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - } - - StatusCode driveStatus = StatusCode.StatusCodeNotInitialized; - for (int i = 0; i < 5; ++i) { - driveStatus = driveMotor.getConfigurator().apply(driveMotorConfigs); - if (driveStatus.isOK()) break; - } - if (!driveStatus.isOK()) { - System.out.println("Could not apply configs, error code: " + driveStatus.toString()); - } - - com.ctre.phoenixpro.configs.TalonFXConfiguration steerMotorConfigs = new TalonFXConfiguration(); - ClosedLoopGeneralConfigs steerMotorClosedLoopGeneralConfigs = new ClosedLoopGeneralConfigs(); - steerMotorClosedLoopGeneralConfigs.ContinuousWrap = false; - - steerMotorConfigs.Slot0.kV = Config.SWERVE_STEER_KV; - steerMotorConfigs.Slot0.kP = Config.SWERVE_STEER_KP; - steerMotorConfigs.Slot0.kI = Config.SWERVE_STEER_KI; - steerMotorConfigs.Slot0.kD = Config.SWERVE_STEER_KD; - steerMotorConfigs.Slot0.kS = Config.SWERVE_STEER_KS; - - steerMotorConfigs.CurrentLimits.SupplyCurrentLimit = 35; - steerMotorConfigs.CurrentLimits.SupplyCurrentLimitEnable = true; - steerMotorConfigs.MotorOutput.DutyCycleNeutralDeadband = 0; - - if (constants.angleInversion) { - steerMotorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - } else { - steerMotorConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - } - - StatusCode steerStatus = StatusCode.StatusCodeNotInitialized; - for (int i = 0; i < 5; ++i) { - steerStatus = steerMotor.getConfigurator().apply(steerMotorConfigs); - if (steerStatus.isOK()) break; - } - if (!steerStatus.isOK()) { - System.out.println("Could not apply configs, error code: " + steerStatus.toString()); - } - - driveMotorStatorCurrent = driveMotor.getStatorCurrent(); - } - - public void log() { - Logger.getInstance() - .recordOutput( - "Swerve/" + constants.corner.toString() + "/DriveMotorStatorCurrent", - driveMotorStatorCurrent.refresh().getValue()); - Logger.getInstance() - .recordOutput( - "Swerve/" + constants.corner.toString() + "/CancoderPositionWithOffset", - getCancoderPosition().getDegrees()); - Logger.getInstance() - .recordOutput( - "Swerve/" + constants.corner.toString() + "/CancoderPositionNoOffset", - getRawCancoderPosition().getDegrees()); - Logger.getInstance() - .recordOutput( - "Swerve/" + constants.corner.toString() + "/SteerMotorPosition", - getSteerMotorPosition().getDegrees()); - } - - public void setDesiredState( - SwerveModuleState state, boolean openLoop, boolean skipJitterOptimization) { - final var steerMotorPosition = getSteerMotorPosition(); - state = CtreModuleState.optimize(state, steerMotorPosition); - - double commandedSteerPosition = - STEER_MOTOR_GEARING_CONVERTER.gearingToMotor(state.angle.getRotations()); - steerMotor.setControl(steerMotorControl.withPosition(commandedSteerPosition)); - - boolean isStopped = Math.abs(state.speedMetersPerSecond) <= SwerveSubsystem.MAX_VELOCITY * 0.01; - Rotation2d angle = isStopped && !skipJitterOptimization ? this.previousAngle : state.angle; - this.previousAngle = angle; - - var wheelRotationsPerSecond = - DRIVE_MOTOR_WHEEL_CONVERTER.distanceToRotations(state.speedMetersPerSecond); - var motorRotationsPerSecond = - DRIVE_MOTOR_GEARING_CONVERTER.gearingToMotor(wheelRotationsPerSecond); - - this.commandedDriveVelocity = Units.metersToInches(state.speedMetersPerSecond); - - if (openLoop) { - driveMotor.setControl( - driveVoltageOpenLoopRequest.withOutput( - state.speedMetersPerSecond / SwerveSubsystem.MAX_VELOCITY)); - } else { - driveMotor.setControl(driveVoltageClosedLoopRequest.withVelocity(motorRotationsPerSecond)); - } - } - - public SwerveModuleState getState() { - final var steerMotorPosition = getSteerMotorPosition(); - final var driveMotorVelocity = Units.inchesToMeters(getDriveMotorVelocity()); - - return new SwerveModuleState(driveMotorVelocity, steerMotorPosition); - } - - public SwerveModulePosition getPosition() { - final var steerMotorPosition = getSteerMotorPosition(); - final var driveMotorPosition = getDriveMotorPosition(); - - return new SwerveModulePosition(driveMotorPosition, steerMotorPosition); - } - - private Rotation2d getSteerMotorPosition() { - double rotationsBeforeGearing = steerMotor.getPosition().getValue(); - double rotations = STEER_MOTOR_GEARING_CONVERTER.beforeToAfterGearing(rotationsBeforeGearing); - return Rotation2d.fromRotations(rotations); - } - - private double getDriveMotorPosition() { - final var rotationsBeforeGearing = driveMotor.getPosition().getValue(); - final var rotations = - DRIVE_MOTOR_GEARING_CONVERTER.beforeToAfterGearing(rotationsBeforeGearing); - - final var meters = DRIVE_MOTOR_WHEEL_CONVERTER.rotationsToDistance(rotations); - return meters; - } - - private double getDriveMotorVelocity() { - final var rotationsPerSecondBeforeGearing = driveMotor.getVelocity().getValue(); - final var rotationsPerSecond = - DRIVE_MOTOR_GEARING_CONVERTER.beforeToAfterGearing(rotationsPerSecondBeforeGearing); - final var metersPerSecond = DRIVE_MOTOR_WHEEL_CONVERTER.rotationsToDistance(rotationsPerSecond); - final var inchesPerSecond = metersPerSecond * 39.37; - return inchesPerSecond; - } - - public void resetWheelAngle() { - if (!rotorPositionSet) { - final var absolutePosition = getCancoderPosition(); - double rotations = absolutePosition.getRotations(); - double rotationsBeforeGearing = STEER_MOTOR_GEARING_CONVERTER.gearingToMotor(rotations); - StatusCode status = steerMotor.setRotorPosition(rotationsBeforeGearing); - if (!status.isError()) { - rotorPositionSet = true; - } - } - } - - private final Rotation2d getCancoderPosition() { - return getRawCancoderPosition().minus(constants.angleOffset); - } - - private Rotation2d getRawCancoderPosition() { - return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - } -} diff --git a/src/main/java/frc/robot/swerve/SwerveModuleConstants.java b/src/main/java/frc/robot/swerve/SwerveModuleConstants.java deleted file mode 100644 index 6c01ae3..0000000 --- a/src/main/java/frc/robot/swerve/SwerveModuleConstants.java +++ /dev/null @@ -1,22 +0,0 @@ -// 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.swerve; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SwerveModuleConstants { - public final Rotation2d angleOffset; - public final SwerveCorner corner; - public final boolean angleInversion; - public final boolean driveInversion; - - public SwerveModuleConstants( - Rotation2d angleOffset, SwerveCorner corner, boolean angleInversion, boolean driveInversion) { - this.angleOffset = angleOffset; - this.corner = corner; - this.angleInversion = angleInversion; - this.driveInversion = driveInversion; - } -} diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java deleted file mode 100644 index 22c5432..0000000 --- a/src/main/java/frc/robot/swerve/SwerveSubsystem.java +++ /dev/null @@ -1,356 +0,0 @@ -// 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.swerve; - -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; -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.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.config.Config; -import frc.robot.controller.DriveController; -import frc.robot.fms.FmsSubsystem; -import frc.robot.imu.ImuSubsystem; -import frc.robot.localization.LocalizationSubsystem; -import frc.robot.util.scheduling.LifecycleSubsystem; -import frc.robot.util.scheduling.SubsystemPriority; -import org.littletonrobotics.junction.Logger; - -public class SwerveSubsystem extends LifecycleSubsystem { - private static final Translation2d FRONT_LEFT_LOCATION = Config.SWERVE_FRONT_LEFT_LOCATION; - private static final Translation2d FRONT_RIGHT_LOCATION = Config.SWERVE_FRONT_RIGHT_LOCATION; - private static final Translation2d BACK_LEFT_LOCATION = Config.SWERVE_BACK_LEFT_LOCATION; - private static final Translation2d BACK_RIGHT_LOCATION = Config.SWERVE_BACK_RIGHT_LOCATION; - public static final SwerveDriveKinematics KINEMATICS = - new SwerveDriveKinematics( - FRONT_LEFT_LOCATION, FRONT_RIGHT_LOCATION, BACK_LEFT_LOCATION, BACK_RIGHT_LOCATION); - public static final double MAX_VELOCITY = - ((6080.0 / 60.0) / Config.SWERVE_DRIVE_GEARING_REDUCTION) * (Config.WHEEL_DIAMETER * Math.PI); - public static final double MAX_ANGULAR_VELOCITY = 20; - - private final ImuSubsystem imu; - private final SwerveModule frontRight; - private final SwerveModule frontLeft; - private final SwerveModule backRight; - private final SwerveModule backLeft; - private boolean doneResetting = false; - - private boolean snapToAngle = false; - private boolean xSwerveEnabled = false; - - private final PIDController xController = - new PIDController( - Config.SWERVE_TRANSLATION_PID.kP, - Config.SWERVE_TRANSLATION_PID.kI, - Config.SWERVE_TRANSLATION_PID.kD); - private final PIDController yController = - new PIDController( - Config.SWERVE_TRANSLATION_PID.kP, - Config.SWERVE_TRANSLATION_PID.kI, - Config.SWERVE_TRANSLATION_PID.kD); - private final PIDController thetaController = - new PIDController( - Config.SWERVE_ROTATION_PID.kP, - Config.SWERVE_ROTATION_PID.kI, - Config.SWERVE_ROTATION_PID.kD); - private final PIDController snapThetaController = - new PIDController( - Config.SWERVE_ROTATION_SNAP_PID.kP, - Config.SWERVE_ROTATION_SNAP_PID.kI, - Config.SWERVE_ROTATION_SNAP_PID.kD); - - private final ProfiledPIDController xProfiledController = - new ProfiledPIDController( - Config.SWERVE_TRANSLATION_PID.kP, - Config.SWERVE_TRANSLATION_PID.kI, - Config.SWERVE_TRANSLATION_PID.kD, - new TrapezoidProfile.Constraints(2.0, 1.5)); - private final ProfiledPIDController yProfiledController = - new ProfiledPIDController( - Config.SWERVE_TRANSLATION_PID.kP, - Config.SWERVE_TRANSLATION_PID.kI, - Config.SWERVE_TRANSLATION_PID.kD, - new TrapezoidProfile.Constraints(2.0, 1.5)); - private final ProfiledPIDController thetaProfiledController = - new ProfiledPIDController( - Config.SWERVE_ROTATION_PID.kP, - Config.SWERVE_ROTATION_PID.kI, - Config.SWERVE_ROTATION_PID.kD, - new TrapezoidProfile.Constraints(Math.PI * 2.0, Math.PI * 0.75)); - private Rotation2d goalAngle = new Rotation2d(); - - public SwerveSubsystem( - ImuSubsystem imu, - SwerveModule frontRight, - SwerveModule frontLeft, - SwerveModule backRight, - SwerveModule backLeft) { - super(SubsystemPriority.SWERVE); - - this.imu = imu; - this.frontRight = frontRight; - this.frontLeft = frontLeft; - this.backRight = backRight; - this.backLeft = backLeft; - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - snapThetaController.enableContinuousInput(-Math.PI, Math.PI); - thetaProfiledController.enableContinuousInput(-Math.PI, Math.PI); - } - - @Override - public void disabledPeriodic() { - frontRight.resetWheelAngle(); - frontLeft.resetWheelAngle(); - backRight.resetWheelAngle(); - backLeft.resetWheelAngle(); - } - - @Override - public void robotPeriodic() { - frontLeft.log(); - frontRight.log(); - backLeft.log(); - backRight.log(); - - Logger.getInstance().recordOutput("Swerve/ModuleStates", getModuleStates()); - - ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/X", chassisSpeeds.vxMetersPerSecond); - Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/Y", chassisSpeeds.vyMetersPerSecond); - Logger.getInstance() - .recordOutput("Swerve/ChassisSpeeds/Omega", chassisSpeeds.omegaRadiansPerSecond); - - Translation2d t = - new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); - double velocity = t.getDistance(new Translation2d()); - Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/theta", t.getAngle().getRadians()); - Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/velocity", velocity); - Logger.getInstance() - .recordOutput( - "Swerve/ChassisSpeeds/spinRatio", chassisSpeeds.omegaRadiansPerSecond / velocity); - - Logger.getInstance().recordOutput("Swerve/SnapToAngle/Goal", goalAngle.getDegrees()); - Logger.getInstance().recordOutput("Swerve/SnapToAngle/Enabled", snapToAngle); - } - - @Override - public void enabledPeriodic() { - if (xSwerveEnabled) { - xSwerve(); - } - } - - public ChassisSpeeds getChassisSpeeds() { - final var frontLeftState = frontLeft.getState(); - final var frontRightState = frontRight.getState(); - final var backLeftState = backLeft.getState(); - final var backRightState = backRight.getState(); - - return KINEMATICS.toChassisSpeeds( - frontLeftState, frontRightState, backLeftState, backRightState); - } - - public SwerveModuleState[] getModuleStates() { - return new SwerveModuleState[] { - frontLeft.getState(), frontRight.getState(), backLeft.getState(), backRight.getState() - }; - } - - public SwerveModulePosition[] getModulePositions() { - return new SwerveModulePosition[] { - frontLeft.getPosition(), - frontRight.getPosition(), - backLeft.getPosition(), - backRight.getPosition() - }; - } - - public void setSnapToAngle(Rotation2d angle) { - goalAngle = angle; - snapToAngle = true; - } - - public void disableSnapToAngle() { - snapToAngle = false; - } - - public void setXSwerve(boolean xSwerve) { - this.xSwerveEnabled = xSwerve; - } - - public void setChassisSpeeds(ChassisSpeeds speeds, boolean openLoop) { - if (xSwerveEnabled) { - return; - } - - if (snapToAngle) { - speeds.omegaRadiansPerSecond = - snapThetaController.calculate(imu.getRobotHeading().getRadians(), goalAngle.getRadians()); - } - Logger.getInstance().recordOutput("Swerve/CommandedSpeeds/X", speeds.vxMetersPerSecond); - Logger.getInstance().recordOutput("Swerve/CommandedSpeeds/Y", speeds.vyMetersPerSecond); - Logger.getInstance().recordOutput("Swerve/CommandedSpeeds/Omega", speeds.omegaRadiansPerSecond); - - // Twist computation. - double lookAheadSeconds = 0.02; - Pose2d target_pose = - new Pose2d( - lookAheadSeconds * speeds.vxMetersPerSecond, - lookAheadSeconds * speeds.vyMetersPerSecond, - Rotation2d.fromRadians(lookAheadSeconds * speeds.omegaRadiansPerSecond)); - Twist2d twist = (new Pose2d()).log(target_pose); - speeds.vxMetersPerSecond = twist.dx / lookAheadSeconds; - speeds.vyMetersPerSecond = twist.dy / lookAheadSeconds; - speeds.omegaRadiansPerSecond = twist.dtheta / lookAheadSeconds; // omega should stay the same. - // Kinematics to convert target chassis speeds to module states. - final var moduleStates = KINEMATICS.toSwerveModuleStates(speeds); - setModuleStates(moduleStates, openLoop, false); - } - - public void setModuleStates( - SwerveModuleState[] moduleStates, boolean openLoop, boolean skipJitterOptimization) { - SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, MAX_VELOCITY); - Logger.getInstance().recordOutput("Swerve/GoalModuleStates", moduleStates); - frontLeft.setDesiredState(moduleStates[0], openLoop, skipJitterOptimization); - frontRight.setDesiredState(moduleStates[1], openLoop, skipJitterOptimization); - backLeft.setDesiredState(moduleStates[2], openLoop, skipJitterOptimization); - backRight.setDesiredState(moduleStates[3], openLoop, skipJitterOptimization); - } - - public void xSwerve() { - setModuleStates( - new SwerveModuleState[] { - new SwerveModuleState(0.0, Rotation2d.fromDegrees(45)), - new SwerveModuleState(0.0, Rotation2d.fromDegrees(135)), - new SwerveModuleState(0.0, Rotation2d.fromDegrees(135)), - new SwerveModuleState(0.0, Rotation2d.fromDegrees(45)) - }, - true, - true); - } - - public Command getXSwerveCommand() { - return run(() -> setXSwerve(true)); - } - - public Command disableXSwerveCommand() { - return runOnce(() -> setXSwerve(false)); - } - - public Command stopSwerveCommand() { - return runOnce(() -> setChassisSpeeds(new ChassisSpeeds(), true)); - } - - public void driveTeleop( - double sidewaysPercentage, - double forwardPercentage, - double thetaPercentage, - boolean fieldRelative, - boolean openLoop) { - Translation2d robotTranslation = - new Translation2d(forwardPercentage, sidewaysPercentage).times(MAX_VELOCITY); - Rotation2d fieldRelativeHeading = imu.getRobotHeading(); - - Logger.getInstance().recordOutput("Swerve/sidewaysPercentage", sidewaysPercentage); - Logger.getInstance().recordOutput("Swerve/forwardPercentage", forwardPercentage); - Logger.getInstance().recordOutput("Swerve/thetaPercentage", thetaPercentage); - - if (FmsSubsystem.isRedAlliance()) { - fieldRelativeHeading = fieldRelativeHeading.plus(Rotation2d.fromDegrees(180)); - } - - ChassisSpeeds chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - robotTranslation.getX(), - robotTranslation.getY(), - thetaPercentage * MAX_ANGULAR_VELOCITY, - fieldRelative ? fieldRelativeHeading : new Rotation2d()); - SwerveModuleState[] moduleStates = KINEMATICS.toSwerveModuleStates(chassisSpeeds); - setChassisSpeeds(KINEMATICS.toChassisSpeeds(moduleStates), openLoop); - } - - public Command getDriveTeleopCommand(DriveController controller) { - return Commands.run( - () -> { - if (!DriverStation.isTeleopEnabled()) { - return; - } - - boolean openLoop = true; - - driveTeleop( - -controller.getSidewaysPercentage(), - controller.getForwardPercentage(), - controller.getThetaPercentage(), - true, - openLoop); - }, - this) - .withName("SwerveDriveTeleop"); - } - - public Command getFollowTrajectoryCommand( - PathPlannerTrajectory traj, LocalizationSubsystem localization) { - return new PPSwerveControllerCommand( - traj, - localization::getPose, - SwerveSubsystem.KINEMATICS, - // x controller - xController, - // y controller - yController, - // theta controller - thetaController, - states -> setModuleStates(states, false, false), - false, - this) - .withName("SwerveFollowTrajectory"); - } - - // Create a command that accepts a Pose2d and drives to it using a PPHolonomicDriveController - // The command should exit once it's at the pose - public Command goToPoseCommand(Pose2d goal, LocalizationSubsystem localization) { - return run(() -> { - Logger.getInstance().recordOutput("AutoAlign/TargetPose", goal); - Pose2d pose = localization.getPose(); - double xVelocity = xProfiledController.calculate(pose.getX(), goal.getX()); - double yVelocity = yProfiledController.calculate(pose.getY(), goal.getY()); - double thetaVelocity = - thetaProfiledController.calculate( - pose.getRotation().getRadians(), goal.getRotation().getRadians()); - - ChassisSpeeds chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - xVelocity, yVelocity, thetaVelocity, pose.getRotation()); - - setChassisSpeeds(chassisSpeeds, false); - }) - .until( - () -> { - // 3 degree rotation and 0.1 meter distance - Pose2d pose = localization.getPose(); - double distanceRelative = goal.getTranslation().getDistance(pose.getTranslation()); - Rotation2d rotationDifference = goal.getRotation().minus(pose.getRotation()); - if (distanceRelative < 0.1 && Math.abs(rotationDifference.getDegrees()) < 3) { - return true; - } else { - return false; - } - }) - .withName("SwerveGoToPose"); - } -} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..c1d8140 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-2", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.0.0-beta-2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.0.0-beta-2", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.0.0-beta-2", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/PhoenixProAnd5.json b/vendordeps/PhoenixProAnd5.json deleted file mode 100644 index 1022cce..0000000 --- a/vendordeps/PhoenixProAnd5.json +++ /dev/null @@ -1,456 +0,0 @@ -{ - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "wpiapi-java", - "version": "23.0.11" - } - ], - "fileName": "PhoenixProAnd5.json", - "frcYear": 2023, - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", - "name": "CTRE-Phoenix (Pro And v5)", - "jniDependencies": [ - { - "simMode": "hwsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "5.30.4" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "5.30.4" - }, - { - "simMode": "hwsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - }, - { - "simMode": "swsim", - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "skipInvalidPlatforms": true, - "isJar": false, - "version": "23.0.11" - } - ], - "mavenUrls": ["https://maven.ctr-electronics.com/release/"], - "cppDependencies": [ - { - "simMode": "hwsim", - "groupId": "com.ctre.phoenix", - "libName": "CTRE_Phoenix_WPI", - "artifactId": "wpiapi-cpp", - "skipInvalidPlatforms": true, - "version": "5.30.4", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "sharedLibrary": true - }, - { - "simMode": "hwsim", - "groupId": "com.ctre.phoenix", - "libName": "CTRE_Phoenix", - "artifactId": "api-cpp", - "skipInvalidPlatforms": true, - "version": "5.30.4", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "sharedLibrary": true - }, - { - "simMode": "hwsim", - "groupId": "com.ctre.phoenix", - "libName": "CTRE_PhoenixCCI", - "artifactId": "cci", - "skipInvalidPlatforms": true, - "version": "5.30.4", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "sharedLibrary": true - }, - { - "simMode": "hwsim", - "groupId": "com.ctre.phoenixpro", - "libName": "CTRE_PhoenixTools", - "artifactId": "tools", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenix.sim", - "libName": "CTRE_Phoenix_WPISim", - "artifactId": "wpiapi-cpp-sim", - "skipInvalidPlatforms": true, - "version": "5.30.4", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenix.sim", - "libName": "CTRE_PhoenixSim", - "artifactId": "api-cpp-sim", - "skipInvalidPlatforms": true, - "version": "5.30.4", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenix.sim", - "libName": "CTRE_PhoenixCCISim", - "artifactId": "cci-sim", - "skipInvalidPlatforms": true, - "version": "5.30.4", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_PhoenixTools_Sim", - "artifactId": "tools-sim", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimTalonSRX", - "artifactId": "simTalonSRX", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimTalonFX", - "artifactId": "simTalonFX", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimVictorSPX", - "artifactId": "simVictorSPX", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimPigeonIMU", - "artifactId": "simPigeonIMU", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimCANCoder", - "artifactId": "simCANCoder", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimProTalonFX", - "artifactId": "simProTalonFX", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimProCANcoder", - "artifactId": "simProCANcoder", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_SimProPigeon2", - "artifactId": "simProPigeon2", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - }, - { - "simMode": "hwsim", - "groupId": "com.ctre.phoenixpro", - "libName": "CTRE_PhoenixPro_WPI", - "artifactId": "wpiapi-cpp", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "sharedLibrary": true - }, - { - "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_PhoenixPro_WPISim", - "artifactId": "wpiapi-cpp-sim", - "skipInvalidPlatforms": true, - "version": "23.0.11", - "headerClassifier": "headers", - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "sharedLibrary": true - } - ], - "version": "23.0.11", - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a" -} From 310dee050b0f05ddae5ff7766a542bc0ca4ff1b3 Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Tue, 28 Nov 2023 18:51:29 -0800 Subject: [PATCH 2/3] worked on adding phoenix6 swerve base --- src/main/java/frc/robot/autos/Autos.java | 7 +- src/main/java/frc/swerve/SwerveSubsystem.java | 215 ++++++++++++++++++ 2 files changed, 219 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/swerve/SwerveSubsystem.java diff --git a/src/main/java/frc/robot/autos/Autos.java b/src/main/java/frc/robot/autos/Autos.java index e24c97a..8c4ff8d 100644 --- a/src/main/java/frc/robot/autos/Autos.java +++ b/src/main/java/frc/robot/autos/Autos.java @@ -21,9 +21,10 @@ import frc.robot.intake.IntakeState; import frc.robot.intake.IntakeSubsystem; import frc.robot.localization.LocalizationSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.wrist.Positions; import frc.robot.wrist.WristSubsystem; +import frc.swerve.SwerveSubsystem; + import java.lang.ref.WeakReference; import java.util.EnumMap; import java.util.EnumSet; @@ -125,7 +126,7 @@ public Autos( SwerveSubsystem.KINEMATICS, Config.SWERVE_TRANSLATION_PID, Config.SWERVE_ROTATION_PID, - (states) -> swerve.setModuleStates(states, false, false), + (states) -> swerve.getModuleStates(states, false, false), eventMap, false, swerve); @@ -190,7 +191,7 @@ private Command buildAutoCommand(AutoKind auto) { } String autoName = "Auto" + auto.toString(); - Command autoCommand = Commands.runOnce(() -> swerve.driveTeleop(0, 0, 0, true, true), swerve); + Command autoCommand = Commands.runOnce(() -> swerve.driveTeleop(0.0, 0.0, 0.0), swerve); if (auto == AutoKind.DO_NOTHING) { return autoCommand diff --git a/src/main/java/frc/swerve/SwerveSubsystem.java b/src/main/java/frc/swerve/SwerveSubsystem.java new file mode 100644 index 0000000..fe93001 --- /dev/null +++ b/src/main/java/frc/swerve/SwerveSubsystem.java @@ -0,0 +1,215 @@ +// 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.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SwerveModuleSteerFeedbackType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +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.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.config.Config; +import frc.robot.controller.DriveController; +import frc.robot.imu.ImuSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import org.littletonrobotics.junction.Logger; + +public class SwerveSubsystem extends LifecycleSubsystem { + public static final SwerveDriveKinematics KINEMATICS = + new SwerveDriveKinematics( + Config.SWERVE_FRONT_LEFT_LOCATION, + Config.SWERVE_FRONT_RIGHT_LOCATION, + Config.SWERVE_BACK_LEFT_LOCATION, + Config.SWERVE_BACK_RIGHT_LOCATION); + + private static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withPigeon2Id(1) + .withSupportsPro(true) + .withCANbusName("581CANivore"); + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(Config.SWERVE_DRIVE_GEARING_REDUCTION) + .withSteerMotorGearRatio(Config.SWERVE_STEER_GEARING_REDUCTION) + .withWheelRadius(Config.WHEEL_DIAMETER / 2.0) + // TODO: Ask Saikiran about this + .withSlipCurrent(800) + .withSteerMotorGains( + new CustomSlotGains( + Config.SWERVE_STEER_KP, + Config.SWERVE_STEER_KI, + Config.SWERVE_STEER_KD, + Config.SWERVE_STEER_KV, + Config.SWERVE_STEER_KS)) + .withDriveMotorGains( + new CustomSlotGains( + Config.SWERVE_DRIVE_KP, + Config.SWERVE_DRIVE_KI, + Config.SWERVE_DRIVE_KD, + Config.SWERVE_DRIVE_KV, + Config.SWERVE_DRIVE_KS)) + .withSpeedAt12VoltsMps( + 6) // Theoretical free speed is 10 meters per second at 12v applied output + .withSteerInertia(0.0001) + .withDriveInertia(0.001) + .withFeedbackSource(SwerveModuleSteerFeedbackType.FusedCANcoder) + // TODO: Ask Saikran about this + .withCouplingGearRatio( + 0.0) // Every 1 rotation of the azimuth results in couple ratio drive turns + .withSteerMotorInverted(true); + private static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + Config.SWERVE_FL_STEER_MOTOR_ID, + Config.SWERVE_FL_DRIVE_MOTOR_ID, + Config.SWERVE_FL_CANCODER_ID, + Rotation2d.fromDegrees(182.021484375).getRotations(), + Config.SWERVE_FRONT_LEFT_LOCATION.getX(), + Config.SWERVE_FRONT_LEFT_LOCATION.getY(), + true); + private static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + Config.SWERVE_FR_STEER_MOTOR_ID, + Config.SWERVE_FR_DRIVE_MOTOR_ID, + Config.SWERVE_FR_CANCODER_ID, + Rotation2d.fromDegrees(159.521484375).getRotations(), + Config.SWERVE_FRONT_RIGHT_LOCATION.getX(), + Config.SWERVE_FRONT_RIGHT_LOCATION.getY(), + true); + private static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + Config.SWERVE_BL_STEER_MOTOR_ID, + Config.SWERVE_BR_DRIVE_MOTOR_ID, + Config.SWERVE_BL_CANCODER_ID, + Rotation2d.fromDegrees(2.548828125).getRotations(), + Config.SWERVE_BACK_LEFT_LOCATION.getX(), + Config.SWERVE_BACK_LEFT_LOCATION.getY(), + true); + private static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + Config.SWERVE_BR_STEER_MOTOR_ID, + Config.SWERVE_BR_DRIVE_MOTOR_ID, + Config.SWERVE_BR_CANCODER_ID, + Rotation2d.fromDegrees(110.390625).getRotations(), + Config.SWERVE_BACK_RIGHT_LOCATION.getX(), + Config.SWERVE_BACK_RIGHT_LOCATION.getY(), + true); + + private static final SwerveDrivetrain drivetrain = + new SwerveDrivetrain(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + + private static final double MAX_VELOCITY = + ((6080.0 / 60.0) / Config.SWERVE_DRIVE_GEARING_REDUCTION) * (Config.WHEEL_DIAMETER * Math.PI); + private static final double MAX_ANGULAR_VELOCITY = 20; + + private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric(); + private final SwerveRequest.FieldCentricFacingAngle driveRequestSnaps = + new SwerveRequest.FieldCentricFacingAngle(); + private final SwerveRequest.SwerveDriveBrake driveRequestX = new SwerveRequest.SwerveDriveBrake(); + + private final ImuSubsystem imu; + private boolean snapToAngle = false; + private Rotation2d goalAngle = new Rotation2d(); + private boolean xSwerveEnabled; + + public SwerveSubsystem(ImuSubsystem imu) { + super(SubsystemPriority.SWERVE); + this.imu = imu; + drivetrain.registerTelemetry( + (SwerveDrivetrain.SwerveDriveState state) -> { + Logger.getInstance().recordOutput("Swerve/FailedDaqs", state.FailedDaqs); + Logger.getInstance().recordOutput("Swerve/SuccessfulDaqs", state.SuccessfulDaqs); + Logger.getInstance().recordOutput("Swerve/Pose", state.Pose); + Logger.getInstance().recordOutput("Swerve/ModuleStates", state.ModuleStates); + Logger.getInstance().recordOutput("Swerve/OdometryPeriod", state.OdometryPeriod); + }); + } + + public Command getXSwerveCommand() { + return runOnce(() -> setXSwerve(false)); + } + + public Command getDriveTeleopCommand(DriveController controller) { + return Commands.run( + () -> { + if (!DriverStation.isTeleopEnabled()) { + return; + } + + boolean openLoop = true; + + driveTeleop( + -controller.getSidewaysPercentage(), + controller.getForwardPercentage(), + controller.getThetaPercentage()); + }, + this) + .withName("SwerveDriveTeleop"); + } + + public void driveTeleop( + double SidewaysPercentage, double forwardPercentage, double thetaPercentage) { + setChassisSpeeds(KINEMATICS.toChassisSpeeds(getModuleStates()), false); + } + + public void setSnapToAngle(Rotation2d angle) { + goalAngle = angle; + snapToAngle = true; + } + + public void disableSnapToAngle() { + snapToAngle = false; + } + + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean closedLoop) { + if (xSwerveEnabled) { + drivetrain.setControl(driveRequestX); + } else if (snapToAngle) { + drivetrain.setControl( + driveRequestSnaps + .withIsOpenLoop(!closedLoop) + .withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withTargetDirection(goalAngle)); + } else { + drivetrain.setControl( + driveRequest + .withIsOpenLoop(!closedLoop) + .withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withRotationalRate(chassisSpeeds.omegaRadiansPerSecond)); + } + } + + public void setXSwerve(boolean enabled) { + this.xSwerveEnabled = enabled; + } + + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + drivetrain.getModule(0).getPosition(true), + drivetrain.getModule(1).getPosition(true), + drivetrain.getModule(2).getPosition(true), + drivetrain.getModule(3).getPosition(true), + }; + } + + private SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + drivetrain.getModule(0).getCurrentState(), + drivetrain.getModule(1).getCurrentState(), + drivetrain.getModule(2).getCurrentState(), + drivetrain.getModule(3).getCurrentState(), + }; + } +} From ad129571f0f83a68daf6694e4096809d80babb9a Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Tue, 28 Nov 2023 18:53:43 -0800 Subject: [PATCH 3/3] worked on adding phoenix 6 swerve --- src/main/java/frc/robot/Robot.java | 39 +++---------------- .../java/frc/robot/autos/Autobalance.java | 2 +- src/main/java/frc/robot/config/Config.java | 18 ++------- src/main/java/frc/robot/imu/ImuSubsystem.java | 3 +- .../localization/LocalizationSubsystem.java | 3 +- .../java/frc/robot/managers/AutoRotate.java | 3 +- src/main/java/frc/swerve/CustomSlotGains.java | 17 ++++++++ 7 files changed, 33 insertions(+), 52 deletions(-) create mode 100644 src/main/java/frc/swerve/CustomSlotGains.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a572cc8..d9cb192 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,7 @@ package frc.robot; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.Pigeon2; -import com.ctre.phoenixpro.hardware.TalonFX; +import com.ctre.phoenix6.hardware.Pigeon2; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.PowerDistribution; @@ -29,11 +27,11 @@ import frc.robot.intake.IntakeSubsystem; import frc.robot.localization.LocalizationSubsystem; import frc.robot.managers.AutoRotate; -import frc.robot.swerve.SwerveModule; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystemManager; import frc.robot.wrist.Positions; import frc.robot.wrist.WristSubsystem; +import frc.swerve.SwerveSubsystem; + import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; @@ -55,35 +53,11 @@ public class Robot extends LoggedRobot { private final RumbleControllerSubsystem rumbleController = new RumbleControllerSubsystem(new XboxController(1)); - private final SwerveModule frontLeft = - new SwerveModule( - Config.SWERVE_FL_CONSTANTS, - new TalonFX(Config.SWERVE_FL_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_FL_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_FL_CANCODER_ID)); - private final SwerveModule frontRight = - new SwerveModule( - Config.SWERVE_FR_CONSTANTS, - new TalonFX(Config.SWERVE_FR_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_FR_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_FR_CANCODER_ID)); - private final SwerveModule backLeft = - new SwerveModule( - Config.SWERVE_BL_CONSTANTS, - new TalonFX(Config.SWERVE_BL_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_BL_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_BL_CANCODER_ID)); - private final SwerveModule backRight = - new SwerveModule( - Config.SWERVE_BR_CONSTANTS, - new TalonFX(Config.SWERVE_BR_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_BR_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_BR_CANCODER_ID)); + private final FmsSubsystem fmsSubsystem = new FmsSubsystem(); private final ImuSubsystem imu = new ImuSubsystem(new Pigeon2(Config.PIGEON_ID)); - private final SwerveSubsystem swerve = - new SwerveSubsystem(imu, frontRight, frontLeft, backRight, backLeft); + private final SwerveSubsystem swerve = new SwerveSubsystem(imu); private final IntakeSubsystem intake = new IntakeSubsystem(new CANSparkMax(Config.INTAKE_MOTOR_ID, MotorType.kBrushless)); private final WristSubsystem wrist = @@ -194,8 +168,7 @@ private void configureButtonBindings() { () -> driveController.getSidewaysPercentage() == 0 && driveController.getForwardPercentage() == 0 - && driveController.getThetaPercentage() == 0) - .onFalse(swerve.disableXSwerveCommand()); + && driveController.getThetaPercentage() == 0); // operator home wrist operatorController.back().onTrue(wrist.getHomeCommand()); diff --git a/src/main/java/frc/robot/autos/Autobalance.java b/src/main/java/frc/robot/autos/Autobalance.java index b253533..e2dfa6e 100644 --- a/src/main/java/frc/robot/autos/Autobalance.java +++ b/src/main/java/frc/robot/autos/Autobalance.java @@ -13,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.fms.FmsSubsystem; import frc.robot.imu.ImuSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; +import frc.swerve.SwerveSubsystem; public class Autobalance extends LifecycleSubsystem { private final SwerveSubsystem swerve; diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 91c5db0..6191800 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -5,12 +5,9 @@ package frc.robot.config; import com.pathplanner.lib.auto.PIDConstants; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import frc.robot.swerve.SwerveCorner; -import frc.robot.swerve.SwerveModuleConstants; public class Config { public static final String SERIAL_NUMBER = System.getenv("serialnum"); @@ -38,27 +35,18 @@ public class Config { public static final int SWERVE_FL_DRIVE_MOTOR_ID = 2; public static final int SWERVE_FL_STEER_MOTOR_ID = 3; public static final int SWERVE_FL_CANCODER_ID = 10; - public static final SwerveModuleConstants SWERVE_FL_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(284.68), SwerveCorner.FRONT_LEFT, false, false); + public static final int SWERVE_FR_DRIVE_MOTOR_ID = 4; public static final int SWERVE_FR_STEER_MOTOR_ID = 5; public static final int SWERVE_FR_CANCODER_ID = 11; - public static final SwerveModuleConstants SWERVE_FR_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(259.89), SwerveCorner.FRONT_RIGHT, false, false); + public static final int SWERVE_BL_DRIVE_MOTOR_ID = 6; public static final int SWERVE_BL_STEER_MOTOR_ID = 7; public static final int SWERVE_BL_CANCODER_ID = 12; - public static final SwerveModuleConstants SWERVE_BL_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(35.60), SwerveCorner.BACK_LEFT, false, false); + public static final int SWERVE_BR_DRIVE_MOTOR_ID = 8; public static final int SWERVE_BR_STEER_MOTOR_ID = 9; public static final int SWERVE_BR_CANCODER_ID = 13; - public static final SwerveModuleConstants SWERVE_BR_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(116.37), SwerveCorner.BACK_RIGHT, false, false); public static final int WRIST_MOTOR_ID = 16; public static final double WRIST_GEARING = 1.0 / 96.0; diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java index 17e6155..6baf610 100644 --- a/src/main/java/frc/robot/imu/ImuSubsystem.java +++ b/src/main/java/frc/robot/imu/ImuSubsystem.java @@ -4,7 +4,6 @@ package frc.robot.imu; -import com.ctre.phoenix.sensors.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -12,6 +11,8 @@ import frc.robot.util.scheduling.SubsystemPriority; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.hardware.Pigeon2; + public class ImuSubsystem extends LifecycleSubsystem { private final Pigeon2 imu; diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index 35a5a05..fa98b60 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -11,9 +11,10 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.fms.FmsSubsystem; import frc.robot.imu.ImuSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; +import frc.swerve.SwerveSubsystem; + import org.littletonrobotics.junction.Logger; public class LocalizationSubsystem extends LifecycleSubsystem { diff --git a/src/main/java/frc/robot/managers/AutoRotate.java b/src/main/java/frc/robot/managers/AutoRotate.java index b480fcf..f403262 100644 --- a/src/main/java/frc/robot/managers/AutoRotate.java +++ b/src/main/java/frc/robot/managers/AutoRotate.java @@ -7,9 +7,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.fms.FmsSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; +import frc.swerve.SwerveSubsystem; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/swerve/CustomSlotGains.java b/src/main/java/frc/swerve/CustomSlotGains.java new file mode 100644 index 0000000..f7a3934 --- /dev/null +++ b/src/main/java/frc/swerve/CustomSlotGains.java @@ -0,0 +1,17 @@ +// 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.swerve; + +import com.ctre.phoenix6.configs.Slot0Configs; + +public class CustomSlotGains extends Slot0Configs { + public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kV = kV; + this.kS = kS; + } +}