diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..e4dc6bf --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,9 @@ +package frc.robot; + +import com.ctre.phoenix6.CANBus; + +public class Constants { + public static final CANBus canBus = new CANBus("GTX7130"); + public static final double goalPoseX = 4.625; + public static final double goalPoseY = 4.034; +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 72147f1..f0b339b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,101 +21,110 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.commands.Shoot; import frc.robot.commands.SwerveWithAim; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.indexer.IndexerConstants; +import frc.robot.subsystems.indexer.IndexerSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; public class RobotContainer { - private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.95).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - - /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - private final CommandXboxController joystick = new CommandXboxController(0); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - // private final Intake intake = new Intake(); - - /* Path follower */ - private final SendableChooser autoChooser; - - public RobotContainer() { - autoChooser = AutoBuilder.buildAutoChooser("Tests"); - SmartDashboard.putData("Auto Mode", autoChooser); - - configureBindings(); - - // Warmup PathPlanner to avoid Java pauses - FollowPathCommand.warmupCommand().schedule(); - } - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - new SwerveWithAim( - drivetrain, - () -> -joystick.getLeftY() * MaxSpeed, // Drive forward with negative Y (forward) - () -> -joystick.getLeftX() * MaxSpeed, // Drive left with negative X (left) - () -> -joystick.getRightX() * MaxAngularRate, // Drive counterclockwise with negative X (left) - joystick.getHID()::getRightBumperButton - ) - ); - - // joystick.rightTrigger(0.2).whileTrue( - // Commands.runEnd(() -> intake.setVoltage(-12), () -> intake.setVoltage(0), intake) - // ); - - - // Idle while the robot is disabled. This ensures the configured - // neutral mode is applied to the drive motors while disabled. - final var idle = new SwerveRequest.Idle(); - RobotModeTriggers.disabled().whileTrue( - drivetrain.applyRequest(() -> idle).ignoringDisable(true) - ); - - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) - )); - - joystick.povUp().whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(0.5).withVelocityY(0)) - ); - joystick.povDown().whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(-0.5).withVelocityY(0)) - ); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // Reset the field-centric heading on left bumper press. - joystick.leftBumper().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); - - drivetrain.registerTelemetry(logger::telemeterize); - } - - public Command getAutonomousCommand() { - /* Run the path selected from the auto chooser */ - return autoChooser.getSelected(); - } - - public Command rumble(double strength, double timeSeconds) { - return Commands.runEnd( - () -> joystick.setRumble(RumbleType.kBothRumble, strength), - () -> joystick.setRumble(RumbleType.kBothRumble, 0) - ).withTimeout(timeSeconds); - } + private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired + // top + // speed + private double MaxAngularRate = RotationsPerSecond.of(0.95).in(RadiansPerSecond); // 3/4 of a rotation per + // second + // max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + private final Telemetry logger = new Telemetry(MaxSpeed); + + private final CommandXboxController joystick = new CommandXboxController(0); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + // private final Intake intake = new Intake(); + + /* Path follower */ + private final SendableChooser autoChooser; + + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final IndexerSubsystem indexerSubsystem = new IndexerSubsystem(); + + public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser("Tests"); + SmartDashboard.putData("Auto Mode", autoChooser); + + configureBindings(); + + // Warmup PathPlanner to avoid Java pauses + FollowPathCommand.warmupCommand().schedule(); + indexerSubsystem.setDefaultCommand(indexerSubsystem.run(0, 0)); + } + + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + new SwerveWithAim( + drivetrain, + () -> -joystick.getLeftY() * MaxSpeed, // Drive forward with negative Y + // (forward) + () -> -joystick.getLeftX() * MaxSpeed, // Drive left with negative X + // (left) + () -> -joystick.getRightX() * MaxAngularRate, // Drive counterclockwise + // with negative X (left) + joystick.getHID()::getRightBumperButton)); + + // joystick.rightTrigger(0.2).whileTrue( + // Commands.runEnd(() -> intake.setVoltage(-12), () -> intake.setVoltage(0), + // intake) + // ); + + // Idle while the robot is disabled. This ensures the configured + // neutral mode is applied to the drive motors while disabled. + final var idle = new SwerveRequest.Idle(); + RobotModeTriggers.disabled().whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); + + joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + joystick.b().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())))); + + joystick.povUp().whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0))); + joystick.povDown().whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0))); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // Reset the field-centric heading on left bumper press. + joystick.leftBumper().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + + drivetrain.registerTelemetry(logger::telemeterize); + + joystick.rightTrigger().whileTrue(new Shoot(shooterSubsystem, () -> drivetrain.getState().Pose)); + joystick.rightBumper().whileTrue(indexerSubsystem.run(IndexerConstants.indexerUpSpeed,IndexerConstants.indexerDownSpeed)); + + shooterSubsystem.registerTelemetry(logger::telemeterizeShooter); + } + + public Command getAutonomousCommand() { + /* Run the path selected from the auto chooser */ + return autoChooser.getSelected(); + } + + public Command rumble(double strength, double timeSeconds) { + return Commands.runEnd( + () -> joystick.setRumble(RumbleType.kBothRumble, strength), + () -> joystick.setRumble(RumbleType.kBothRumble, 0)).withTimeout(timeSeconds); + } } diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java index ff0ca74..e9c3db2 100644 --- a/src/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/Telemetry.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; @@ -57,6 +58,9 @@ public Telemetry(double maxSpeed) { private final StructPublisher shooterPose = componentStateTable.getStructTopic("ShooterPose", Pose3d.struct).publish(); private final StructPublisher climberPose = componentStateTable.getStructTopic("ClimberPose", Pose3d.struct).publish(); + private final NetworkTable shooterStateTable = inst.getTable("ShooterState"); + private final DoublePublisher shooterAngle = shooterStateTable.getDoubleTopic("ShooterAngle").publish(); + /* Robot pose for field positioning */ private final NetworkTable table = inst.getTable("Pose"); private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); @@ -126,9 +130,10 @@ public void telemeterize(SwerveDriveState state) { } /** Accept the shooter angle and telemeterize it to SmartDashboard */ - public void telemeterizeShooter(double pitchRadius) { + public void telemeterizeShooter(double pitchRotations) { // non-zero values are model asset offset for ascope - shooterPose.set(new Pose3d(-0.085, 0.0, 0.507, new Rotation3d(0.0, pitchRadius - 0.62, 0.0))); + shooterPose.set(new Pose3d(-0.085, 0.0, 0.507, new Rotation3d(0.0, Units.rotationsToRadians(pitchRotations) - 0.62, 0.0))); + shooterAngle.set(pitchRotations); } /** Accept the shooter angle and telemeterize it to SmartDashboard*/ diff --git a/src/main/java/frc/robot/commands/Shoot.java b/src/main/java/frc/robot/commands/Shoot.java new file mode 100644 index 0000000..a1437ce --- /dev/null +++ b/src/main/java/frc/robot/commands/Shoot.java @@ -0,0 +1,63 @@ +package frc.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj2.command.Command; +import frc.FSLib.util.AllianceFlipUtil; +import frc.robot.Constants; +import frc.robot.subsystems.shooter.ShooterSubsystem; + +public class Shoot extends Command { + private final ShooterSubsystem shooterSubsystem; + private final Supplier robotPoseSupplier; + private final InterpolatingDoubleTreeMap speedUpMap = new InterpolatingDoubleTreeMap(); + private final InterpolatingDoubleTreeMap speedDownMap = new InterpolatingDoubleTreeMap(); + private final InterpolatingDoubleTreeMap angleMap = new InterpolatingDoubleTreeMap(); + + public Shoot( + ShooterSubsystem shooterSubsystem, + Supplier robotPoseSupplier) { + this.shooterSubsystem = shooterSubsystem; + this.robotPoseSupplier = robotPoseSupplier; + addRequirements(shooterSubsystem); + + speedUpMap.put(0.0, 0.0);// rps + speedUpMap.put(1.0, -4.0);// rps + speedDownMap.put(0.0, 0.0);// rps + speedDownMap.put(0.0, 0.0);// rps + angleMap.put(0.0, 0.0); + angleMap.put(0.0, 0.0); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + Pose2d currentPose = robotPoseSupplier.get(); + Translation2d goalPose = new Translation2d(Constants.goalPoseX, Constants.goalPoseY); + Translation2d trueTarget = AllianceFlipUtil.flip(goalPose); + double distance = currentPose.getTranslation().getDistance(trueTarget); + // some calculations here + double shooterUpSpeed = speedUpMap.get(distance); // replace with actual calculation + shooterSubsystem.setShooterUpSpeed(shooterUpSpeed); + double ShooterDownSpeed = speedDownMap.get(distance); + shooterSubsystem.setShooterDownSpeed(ShooterDownSpeed); + double shooterAngleTarget = angleMap.get(distance); + shooterSubsystem.setShooterAngle(shooterAngleTarget); + } + + @Override + public void end(boolean interrupted) { + shooterSubsystem.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java new file mode 100644 index 0000000..3a21682 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.indexer; + +public class IndexerConstants { + public static final int indexerUpID = 44; + public static final int indexerDownID = 43; + public static final double indexerUpSpeed = 1.0; + public static final double indexerDownSpeed = -0.1; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java new file mode 100644 index 0000000..aa50ba3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.indexer; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class IndexerSubsystem extends SubsystemBase { + TalonFX indexerUp = new TalonFX(IndexerConstants.indexerUpID, Constants.canBus); + TalonFX indexerDown = new TalonFX(IndexerConstants.indexerDownID, Constants.canBus); + + public IndexerSubsystem() { + TalonFXConfiguration indexerUpConfig = new TalonFXConfiguration(); + indexerUpConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + indexerUpConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + indexerUpConfig.CurrentLimits.SupplyCurrentLimit = 70; + indexerUp.getConfigurator().apply(indexerUpConfig); + + TalonFXConfiguration indexerDownConfig = new TalonFXConfiguration(); + indexerDownConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + indexerDownConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + indexerDownConfig.CurrentLimits.SupplyCurrentLimit = 70; + indexerDown.getConfigurator().apply(indexerDownConfig); + } + + public void setIndexerUpSpeed(double speedUp) { + indexerUp.set(speedUp); + } + + public void setIndexerDownSpeed(double speedDown) { + indexerDown.set(speedDown); + } + + public void stop() { + indexerUp.set(0); + indexerDown.set(0); + } + + public Command run(double speedUp, double speedDown) { + return this.runEnd( + () -> { + setIndexerUpSpeed(speedUp); + setIndexerDownSpeed(speedDown); + }, + () -> stop() + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..5f8283a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.shooter; + +public class ShooterConstants { + public static final int shooterUpID = 54; + public static final int shooterDownID = 55; + public static final int shooterAngleID = 56; + public static final double StatorCurrentLimit = 120; + public static final double RotorToSensorRatio = 1; + public static final double SensorToMechanismRatio =3; + public static final double Slot0kS = 0.2; + public static final double Slot0kV = 0.12; + public static final double Slot0kP = 0.1; + public static final double Slot0kI = 0; + public static final double Slot0kD = 0; + public static final double MotionMagicAccelerationUp = 250; + public static final double MotionMagicAccelerationDown = 250; + public static final double Slot2kS = 0; + public static final double Slot2kV = 0; + public static final double Slot2kP = 0; + public static final double Slot2kI = 0; + public static final double Slot2kD = 0; + public static final double StatorCurrentLimitAngle = 120; + public static final double RotorToSensorRatioAngle = 1; + public static final double SensorToMechanismRatioAngle =3; + public static final double ForwardSoftLimitThreshold = 0; + public static final double ReverseSoftLimitThreshold = 0; + public static final double MotionMagicCruiseVelocityAngle = 0.05; + public static final double MotionMagicAccelerationAngle = 0.3; + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java new file mode 100644 index 0000000..096894e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -0,0 +1,109 @@ +package frc.robot.subsystems.shooter; + +import java.util.function.DoubleConsumer; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class ShooterSubsystem extends SubsystemBase { + TalonFX shooterUp = new TalonFX(ShooterConstants.shooterUpID, Constants.canBus); + TalonFX shooterDown = new TalonFX(ShooterConstants.shooterDownID, Constants.canBus); + TalonFX shooterAngle = new TalonFX(ShooterConstants.shooterAngleID, Constants.canBus); + double shooterAngleTarget = 0.0; + + private final MotionMagicVelocityVoltage reqUp = new MotionMagicVelocityVoltage(0).withEnableFOC(true).withUseTimesync(true); + private final MotionMagicVelocityVoltage reqDown = new MotionMagicVelocityVoltage(0).withEnableFOC(true).withUseTimesync(true); + private DoubleConsumer telemetry; + + public ShooterSubsystem() { + TalonFXConfiguration shooterUpConfig = new TalonFXConfiguration(); + shooterUpConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + shooterUpConfig.CurrentLimits.StatorCurrentLimitEnable = true; + shooterUpConfig.CurrentLimits.StatorCurrentLimit = ShooterConstants.StatorCurrentLimit; + shooterUpConfig.Feedback.RotorToSensorRatio = ShooterConstants.RotorToSensorRatio; + shooterUpConfig.Feedback.SensorToMechanismRatio = ShooterConstants.SensorToMechanismRatio; + var slot0Config = shooterUpConfig.Slot0; + slot0Config.kS = ShooterConstants.Slot0kS; + slot0Config.kV = ShooterConstants.Slot0kV; + slot0Config.kP = ShooterConstants.Slot0kP; + slot0Config.kI = ShooterConstants.Slot0kI; + slot0Config.kD = ShooterConstants.Slot0kD; + var motationConfigUp = shooterUpConfig.MotionMagic; + motationConfigUp.MotionMagicAcceleration = ShooterConstants.MotionMagicAccelerationUp; + shooterUp.getConfigurator().apply(shooterUpConfig); + + TalonFXConfiguration shooterDownConfig = new TalonFXConfiguration(); + shooterDownConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + shooterDownConfig.CurrentLimits.StatorCurrentLimitEnable = true; + shooterDownConfig.CurrentLimits.StatorCurrentLimit = ShooterConstants.StatorCurrentLimit; + shooterDownConfig.Feedback.RotorToSensorRatio = ShooterConstants.RotorToSensorRatio; + shooterDownConfig.Feedback.SensorToMechanismRatio = ShooterConstants.SensorToMechanismRatio; + var slot0ConfigDown = shooterDownConfig.Slot0; + slot0ConfigDown.kS = ShooterConstants.Slot0kS; + slot0ConfigDown.kV = ShooterConstants.Slot0kV; + slot0ConfigDown.kP = ShooterConstants.Slot0kP; + slot0ConfigDown.kI = ShooterConstants.Slot0kI; + slot0ConfigDown.kD = ShooterConstants.Slot0kD; + var motationConfigDown = shooterDownConfig.MotionMagic; + motationConfigDown.MotionMagicAcceleration = ShooterConstants.MotionMagicAccelerationDown; + shooterDown.getConfigurator().apply(shooterDownConfig); + + TalonFXConfiguration shooterAngleConfig = new TalonFXConfiguration(); + shooterAngleConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + shooterAngleConfig.CurrentLimits.StatorCurrentLimitEnable = true; + shooterAngleConfig.CurrentLimits.StatorCurrentLimit = ShooterConstants.StatorCurrentLimitAngle; + shooterAngleConfig.Feedback.RotorToSensorRatio = ShooterConstants.RotorToSensorRatioAngle; + shooterAngleConfig.Feedback.SensorToMechanismRatio = ShooterConstants.SensorToMechanismRatioAngle; + shooterAngleConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + shooterAngleConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + shooterAngleConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ShooterConstants.ForwardSoftLimitThreshold; + shooterAngleConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ShooterConstants.ReverseSoftLimitThreshold; + var slot2Config = shooterAngleConfig.Slot2; + slot2Config.kS = ShooterConstants.Slot2kS; + slot2Config.kV = ShooterConstants.Slot2kV; + slot2Config.kP = ShooterConstants.Slot2kP; + slot2Config.kI = ShooterConstants.Slot2kI; + slot2Config.kD = ShooterConstants.Slot2kD; + var motationConfigAngle = shooterAngleConfig.MotionMagic; + motationConfigAngle.MotionMagicCruiseVelocity = ShooterConstants.MotionMagicCruiseVelocityAngle; + motationConfigAngle.MotionMagicAcceleration = ShooterConstants.MotionMagicAccelerationAngle; + shooterAngle.getConfigurator().apply(shooterAngleConfig); + } + + public double getShooterAnglePos() { + return (shooterAngle.getPosition().getValueAsDouble()); + } + + public void setShooterAngle(double shooterAngleTarget) { + shooterAngle.set(shooterAngleTarget); + } + + public void setShooterUpSpeed(double upSpeedRPS) { + // shooterUp.set(speed); + shooterUp.setControl(reqUp.withVelocity(upSpeedRPS)); + } + + public void setShooterDownSpeed(double downSpeedRPS) { + shooterDown.setControl(reqDown.withVelocity(downSpeedRPS)); + } + + public void stop() { + shooterUp.set(0); + shooterDown.set(0); + shooterAngle.set(0); + } + + public void registerTelemetry(DoubleConsumer telemetry) { + this.telemetry = telemetry; + } + + @Override + public void periodic() { + telemetry.accept(getShooterAnglePos()); + } +} diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-frc2026-latest.json rename to vendordeps/Phoenix6-26.1.1.json index 8f6e30f..7a0eca0 100644 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2026-latest.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true,