diff --git a/build.gradle b/build.gradle index 9b32861..2f26112 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/settings.gradle b/settings.gradle index 969c7b0..486702a 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/deploy/pathplanner/paths/B Coarl Station R.path b/src/main/deploy/pathplanner/paths/B Coarl Station R.path new file mode 100644 index 0000000..1a19bf7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B Coarl Station R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.7684311224489797, + "y": 2.13345025510204 + }, + "prevControl": null, + "nextControl": { + "x": 1.6863520408163264, + "y": 1.8424426020408156 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4326530612244897, + "y": 1.3201211734693867 + }, + "prevControl": { + "x": 1.5445790816326532, + "y": 1.6932079081632638 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 55.451632943988734 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 65.397205841715 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65e1d18..1274010 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -208,7 +208,7 @@ public static final class AutoConstants { //TODO: The below constants are used i public static final int ELEVATOR_MOTOR_LEFT_ID = 10; public static final int ELEVATOR_MOTOR_RIGHT_ID = 11; public static final double ELEVATOR_LOWER_LIMIT = 0.335449; //.403809; //TODO: Placeholder - //public static final double ELEVATOR_UPPER_LIMIT = 43; //TODO: Placeholder + public static final double ELEVATOR_UPPER_LIMIT = 43; //TODO: Placeholder public static final double ELEVATOR_MAX_ROTATIONS_PER_SEC = 80; public static final double ELEVATOR_CAN_TO_MOTOR_RATIO = 5.24954/0.916748; public static final int ELEVATOR_CAN_CODER_ID = 4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f76971..63e79d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,9 +3,12 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; 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.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -180,7 +183,19 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return autoChooser.getSelected(); + return autoChooser.getSelected(); + // Load the path we want to pathfind to and follow +PathPlannerPath path = PathPlannerPath.fromPathFile("B Coarl Station R"); + +// Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path. +PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720)); + +// Since AutoBuilder is configured, we can use it to build pathfinding commands +Command pathfindingCommand = AutoBuilder.pathfindThenFollowPath( + path, + constraints); //return new PathPlannerAuto("New Auto"); // Command[] autoCommands = new Command[numOfAutoActions.getSelected()*2]; @@ -224,4 +239,4 @@ public void setupAutoChoosers() { hasSetupAutoChoosers = true; } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/CmdElevatorSetPosition.java b/src/main/java/frc/robot/commands/RobotTurn.java similarity index 60% rename from src/main/java/frc/robot/commands/CmdElevatorSetPosition.java rename to src/main/java/frc/robot/commands/RobotTurn.java index 825cb29..641d83c 100644 --- a/src/main/java/frc/robot/commands/CmdElevatorSetPosition.java +++ b/src/main/java/frc/robot/commands/RobotTurn.java @@ -4,14 +4,23 @@ package frc.robot.commands; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveSubsystem; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class CmdElevatorSetPosition extends Command { - /** Creates a new CmdElevatorSetPosition. */ - public CmdElevatorSetPosition() { - // Use addRequirements() here to declare subsystem dependencies. - } +public class RobotTurn extends Command { + + + private SwerveSubsystem swerveSubsystem; + + /** Creates a new RobotTurn. */ + public RobotTurn(SwerveSubsystem swerveSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(swerveSubsystem ); + this.swerveSubsystem = swerveSubsystem; + } + // Called when the command is initially scheduled. @Override @@ -19,9 +28,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} -double speed = 0.0; - + public void execute() { + swerveSubsystem.drive(new Translation2d(),1d,false,false ); + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) {} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java deleted file mode 100644 index 6eca19a..0000000 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ /dev/null @@ -1,215 +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.subsystems; - -import java.util.ResourceBundle.Control; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import javax.swing.text.Position; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.ControlModeValue; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.DutyCycle; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants; - -public class ElevatorSubsystem extends SubsystemBase { - - private final TalonFX motor1 = new TalonFX(Constants.ELEVATOR_MOTOR_LEFT_ID); - private final TalonFX motor2 = new TalonFX(Constants.ELEVATOR_MOTOR_RIGHT_ID); - private boolean enableSetPosition = true; - private double elevatorSetPosition; - private PositionVoltage elevatorPositionVoltage = new PositionVoltage(0); - private VelocityVoltage elevatorVelocityVoltage = new VelocityVoltage(0); - private TalonFXConfiguration climberConfig = new TalonFXConfiguration(); - private CommandXboxController driver; - private Servo elevatorStopper; - private double elevatorStopperPosition; - private final CANcoder elevatorCANcoder; - - /** Creates a new Elevator. */ - public ElevatorSubsystem() { - motor1.clearStickyFaults(); - motor2.clearStickyFaults(); - - climberConfig.CurrentLimits.SupplyCurrentLimit = 40; - climberConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - - // climberConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Constants.ELEVATOR_UPPER_LIMIT; - climberConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Constants.ELEVATOR_LOWER_LIMIT; - climberConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - climberConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - // climberConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - // climberConfig.DifferentialSensors.DifferentialRemoteSensorID = 4; - - climberConfig.Slot0.kP = 0.435; - climberConfig.Slot0.kI = 0.02; - climberConfig.Slot0.kD = 0.0; - climberConfig.Slot0.kG = 0.65; - - climberConfig.Slot1.kP = 0.17; - climberConfig.Slot1.kI = 0.006;//0.005; - climberConfig.Slot1.kD = 0.01; - climberConfig.Slot1.kG = 0.65; - - climberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - climberConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - elevatorCANcoder = new CANcoder(Constants.ELEVATOR_CAN_CODER_ID); - - motor1.getConfigurator().apply(climberConfig); - motor2.getConfigurator().apply(climberConfig); - motor2.setControl(new Follower(Constants.ELEVATOR_MOTOR_LEFT_ID, false)); - resetMotorEncoderToAbsolute(); - elevatorStopper = new Servo(0); - } - - public Command spinMotor(double speed) { - return this.runEnd(()->motor1.set(speed), ()->stopMotor()); - } - - public Command stopMotor() { - return this.runOnce(()-> {motor1.stopMotor(); - enableSetPosition = false;}); - } - - public void setHoldPosition(double setPosition) { - motor1.setPosition(setPosition, .1); - } - - public double getPosition(){ - // return elevatorCANcoder.getPosition().getValueAsDouble(); - return motor1.getPosition().getValueAsDouble(); - } - - public Command stopElevator() { - enableSetPosition = true; - return this.runOnce(()->stopMotor()); - } - - public Command resetPosition() { - return this.run(()->motor1.setPosition(0)); - } - - public Command elevatorToIntakeAuto(){ - return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.CORALSTATION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.CORALSTATION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); - } - - public Command elevatorToL2Auto(){ - return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.L2REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.L2REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); - } - - public Command elevatorToL4Auto(){ - return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.L4REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.L4REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); - } - - //lets the driver control the position, when no input is given the elevator will hold its position - public void driveByJoystick(DoubleSupplier amount) { - if(amount.getAsDouble()>.1 || amount.getAsDouble()<-.1){ - double amountSpin = MathUtil.applyDeadband(amount.getAsDouble(), .1); - elevatorVelocityVoltage.Velocity = -amountSpin*Constants.ELEVATOR_MAX_ROTATIONS_PER_SEC; - motor1.setControl(elevatorVelocityVoltage); - resetMotorEncoderToAbsolute(); - elevatorSetPosition = getPosition(); - } - else { - resetMotorEncoderToAbsolute(); - if (getPosition() <= elevatorSetPosition) { - motor1.setControl(elevatorPositionVoltage.withPosition(elevatorSetPosition).withSlot(0)); - } - else { - motor1.setControl(elevatorPositionVoltage.withPosition(elevatorSetPosition).withSlot(1)); - } - } - } - - //gives new set positions for the elevator to go to - public Command L1Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L1REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command L2Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L2REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command L3Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L3REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command L4Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L4REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command intakeCoral(){ - return this.runOnce(() -> elevatorSetPosition = Constants.CORALSTATION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command returnHome(){ - return this.runOnce(() -> elevatorSetPosition = Constants.ELEVATOR_LOWER_LIMIT * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command linearActuatorOut(){ - return this.run( - () -> elevatorStopper.set(1) - ); - } - - public Command linearActuatorIn() { - return this.run( - () -> elevatorStopper.set(200d / 370d) - ); - } - - - public Angle getElevatorPosition() { - return elevatorCANcoder.getAbsolutePosition().getValue(); -} -public void resetMotorEncoderToAbsolute() { - // Angle newPosition = getElevatorPosition() ; - - motor1.setPosition(elevatorCANcoder.getPosition().getValueAsDouble()*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - //makes sure when the robot is disabled the position is being updated so when reenabled the elevator will not - //go back to the position it was at when it was disabled (hazard) - - if (RobotState.isDisabled()) { - resetMotorEncoderToAbsolute(); - elevatorSetPosition = getPosition(); - } - - elevatorStopperPosition = elevatorStopper.getAngle(); - SmartDashboard.putNumber("Servo Position", elevatorStopperPosition); - SmartDashboard.putNumber("Elevator Position", getPosition()); - SmartDashboard.putNumber("Set Position", elevatorSetPosition); - SmartDashboard.putNumber("Elevator Motor Temperature", motor1.getDeviceTemp().getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 218adaf..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,64 +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.subsystems; - -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class IntakeSubsystem extends SubsystemBase { - private final TalonFX intakeMotor = new TalonFX(Constants.INTAKE_MOTOR_ID); - private final TalonFXConfiguration intakeConfig= new TalonFXConfiguration(); - /** Creates a new IntakeModule. */ - public IntakeSubsystem() { - intakeConfig.CurrentLimits.SupplyCurrentLimit = 60; - intakeConfig.CurrentLimits.SupplyCurrentLowerLimit = 35; - intakeConfig.CurrentLimits.SupplyCurrentLowerTime = .1; - intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - intakeMotor.clearStickyFaults(); - } - - public boolean coralInIntake() { - if (intakeMotor.getSupplyVoltage().getValueAsDouble() > 20) { - return true; - } - else return false; - } - - public void spinMotor(double speed) { - intakeMotor.set(speed); - } - - public void stopMotor() { - intakeMotor.stopMotor(); - } - - public Command intakeAlgae() { - return this.run( - () -> spinMotor(.3) - ); - } - - public Command stopIntake() { - return new InstantCommand(() -> stopMotor(), this); - } - - public Command intakeCoral() { - return new InstantCommand(()->spinMotor(-.3), this); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putNumber("Intake Motor Temperature", intakeMotor.getDeviceTemp().getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java b/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java index dfa7777..21fd7ee 100644 --- a/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java @@ -47,8 +47,8 @@ public final class VisionPoseEstimator { - private static final Matrix multiTagStdDevs = Constants.PhotonVision.multiTagStdDevs; - private static final Object singleTagStdDevs = Constants.PhotonVision.singleTagStdDevs; + private static final Matrix multiTagStdDevs = null; + private static final Object singleTagStdDevs = null; private final PhotonCamera camera1; private final PhotonCamera camera2; private PhotonCamera currentCamera; @@ -164,7 +164,7 @@ public PhotonPipelineResult getLatestResult() { */ public Optional getEstimatedVisionGlobalPose() { useBestCamera(); - var visionEst = currentPhotonEstimator.update(getLatestResult()); + var visionEst = currentPhotonEstimator.update(null); double latestTimestamp = currentCamera.getLatestResult().getTimestampSeconds(); // double latestTimestamp = currentCamera.getAllUnreadResults().get(currentCamera.getAllUnreadResults().size()-1).getTimestampSeconds(); boolean newResult = Math.abs(latestTimestamp - lastEstTimestamp) > 1e-5; @@ -200,34 +200,34 @@ public double getTargetRotationValue(Rotation2d CurrentYaw){ * * @param estimatedPose The estimated pose to guess standard deviations for. */ - public Matrix getVisionEstimationStdDevs(Pose2d estimatedPose) { + public Matrix getVisionEstimationStdDevs(Pose2d estimatedPose) { if (DriverStation.isDisabled()) { return VecBuilder.fill(0.001d, 0.001d, 0.001d); } - Matrix estStdDevs = (Matrix) singleTagStdDevs; - var targets = getLatestResult().getTargets(); - int numTags = 0; - double avgDist = 0; - for (var tgt : targets) { - var tagPose = currentPhotonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) continue; - numTags++; - avgDist += - tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); - } - if (numTags == 0) return (Matrix) estStdDevs; - avgDist /= numTags; - - // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = multiTagStdDevs; - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - - return (Matrix) estStdDevs; - } + var estStdDevs = singleTagStdDevs; + var targets = getLatestResult().getTargets(); + int numTags = 0; + double avgDist = 0; + for (var tgt : targets) { + var tagPose = currentPhotonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); + } + if (numTags == 0) return (Matrix) estStdDevs; + avgDist /= numTags; + + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = multiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.equals(1 + (avgDist * avgDist / 30)); + + return (Matrix) estStdDevs; + } /** * @return The {@link Pose2d} of the robot according to the {@link SwerveDrivePoseEstimator} diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java deleted file mode 100644 index 4b4b850..0000000 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ /dev/null @@ -1,142 +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.subsystems; - -import java.util.function.DoubleSupplier; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class WristSubsystem extends SubsystemBase { - private TalonFX wristMotor = new TalonFX(Constants.WRIST_MOTOR_ID); - private TalonFXConfiguration wristConfig = new TalonFXConfiguration(); - private VelocityVoltage wristVelocityVoltage = new VelocityVoltage(0); - private PositionVoltage wristPositionVoltage = new PositionVoltage(0); - private double wristSetPosition; - - /** Creates a new WristModule. */ - public WristSubsystem() { - wristConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Constants.WRIST_UPPER_LIMIT; - wristConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Constants.WRIST_LOWER_LIMIT; - wristConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - wristConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - - wristConfig.CurrentLimits.SupplyCurrentLimit = 60; - wristConfig.CurrentLimits.SupplyCurrentLowerLimit = 35; - wristConfig.CurrentLimits.SupplyCurrentLowerTime = .01; - wristConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - wristConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - wristConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - wristConfig.Slot0.kP = .2;//0.3; - wristConfig.Slot0.kI = 0.001;//.005; - wristConfig.Slot0.kD = 0.0; - wristConfig.Slot0.kG = -.16; - - wristConfig.Slot1.kP =0.0; - wristConfig.Slot1.kI = 0.0; - wristConfig.Slot1.kD = 0.0; - wristConfig.Slot1.kG = .3; - - wristMotor.getConfigurator().apply(wristConfig); - wristMotor.clearStickyFaults(); - } - - public void spinWristMotor(double speed){ - wristMotor.set(speed); - } - - public void stopWristMotor(){ - wristMotor.stopMotor(); - } - - public double getWristPosition() { - return wristMotor.getPosition().getValueAsDouble(); - } - - public void updatePosition(){ - wristPositionVoltage.Position = getWristPosition(); - } - - public void spinByJoystick(DoubleSupplier amount) { - double spinAmount = MathUtil.applyDeadband(amount.getAsDouble(), .1); - if(spinAmount>.1||spinAmount<-.01){ - wristVelocityVoltage.Velocity = spinAmount*Constants.WRIST_MAX_ROTATIONS_PER_SEC; - wristMotor.setControl(wristVelocityVoltage); - wristSetPosition = getWristPosition(); - } - else { - wristMotor.setControl(wristPositionVoltage.withPosition(wristSetPosition).withSlot(0)); - } - } - - public Command wristToIntake(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_INTAKE_POSITION); - } - - public Command wristToL1(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_L1_POSITION); - } - - public Command wristToL1Auto(){ - return this.runOnce(() -> wristMotor.setControl(wristPositionVoltage.withPosition(Constants.WRIST_L1_POSITION).withSlot(0))); - } - - public Command wristToLMIDAuto(){ - return this.runOnce(() -> wristMotor.setControl(wristPositionVoltage.withPosition(Constants.WRIST_LMID_POSITION).withSlot(0))); - } - - public Command wristToL4Auto(){ - return this.runOnce(() -> wristMotor.setControl(wristPositionVoltage.withPosition(Constants.WRIST_L4_POSITION).withSlot(0))); - } - - public Command wristToLMID(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_LMID_POSITION); - } - - public Command wristToL4(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_L4_POSITION); - } - - public Command wristToBarge() { - return this.runOnce(() -> wristSetPosition = Constants.WIRST_BARGE_POSITION); - } - - public Command spinWrist(double speed) { - return new InstantCommand(()->spinWrist(speed), this); - } - - public Command stopWrist() { - return new InstantCommand(()-> stopWristMotor(), this); - } - - public Command resetPosition() { - return this.runOnce(() -> wristMotor.setPosition(0)); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - //makes sure when the robot is disabled the position is being updated so when reenabled the wrist will not - //go back to the position it was at when it was disabled (hazard) - if(RobotState.isDisabled()) { - wristSetPosition = getWristPosition(); - } - SmartDashboard.putNumber(" Wrist Set Position", wristSetPosition); - SmartDashboard.putNumber("Real Position", wristMotor.getPosition().getValueAsDouble()); - } -} diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index a93cd12..acc78db 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,21 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -428,22 +414,6 @@ "osxuniversal" ], "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.3.1", - "libName": "CTRE_SimProCANdi", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 3718e0a..c54ae11 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 1219919..990ce2f 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.2.1", + "version": "v2025.2.1-rc1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.2.1-rc1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.2.1", + "version": "v2025.2.1-rc1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.2.1-rc1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.2.1" + "version": "v2025.2.1-rc1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.2.1" + "version": "v2025.2.1-rc1" } ] } \ No newline at end of file