diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1743b9ec..7de9aa0a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -148,7 +148,6 @@ public void teleopInit() { CommandScheduler.getInstance() .schedule(new ZeroElevatorHardStop(ElevatorSubsystem.getInstance())); } - // CommandScheduler.getInstance(); // .schedule(zeroArm); // TODO: Fix this to not expose the CommandScheduler } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 404e2792..58226023 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,27 +17,22 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.AutoRoutines.AutoProducer; +// import frc.robot.AutoRoutines.AutoProducer; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.commandGroups.Dealgaenate; import frc.robot.commandGroups.EjectCoralFR; import frc.robot.commandGroups.ElevatorL4; import frc.robot.commandGroups.JamesHardenScore; import frc.robot.commandGroups.PutUpAndShoot; -import frc.robot.commandGroups.RunFunnelUntilDetectionSafeSmooth; import frc.robot.commands.DaleCommands.ArmToAngleCmd; import frc.robot.commands.ElevatorCommands.DefaultElevator; +import frc.robot.commands.ElevatorCommands.SetElevatorHeight; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; -import frc.robot.commands.ElevatorCommands.ZeroElevatorHardStop; import frc.robot.commands.FunnelCommands.RunFunnelAndTootsieInCommand; import frc.robot.commands.FunnelCommands.RunFunnelOutCommand; import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; -import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide; -import frc.robot.commands.TransferPieceBetweenFunnelAndElevator; import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.CoralPosition; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.LedSubsystem; @@ -251,7 +246,7 @@ private void configureBindings() { // Joystick 2: // Elevator - joystick2.x().onTrue(new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L1, false)); + joystick.x().whileTrue(new SetElevatorHeight(elevatorSubsystem, ElevatorPositions.L3)); joystick2.a().onTrue(new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L2, false)); joystick2.y().onTrue(new ElevatorL4(elevatorSubsystem, false)); joystick2.b().onTrue(new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L3, false)); @@ -262,53 +257,56 @@ private void configureBindings() { elevatorSubsystem, ElevatorPositions.safePosition, false)); // reset mode // Shoot Tootsie Slide - joystick2.rightTrigger().whileTrue(new ShootTootsieSlide(tootsieSlideSubsystem)); - - // Intake - joystick2.leftTrigger(); - // .onTrue(new D2Intake(elevatorSubsystem, tootsieSlideSubsystem, funnelSubsystem)); - - // Auto Intake and Eject - Trigger funnelCheckin = - new Trigger( - () -> funnelSubsystem.isCoralCheckedIn() && !CoralPosition.isCoralInTootsieSlide()) - .and(RobotModeTriggers.teleop()); - Trigger ejectTime = - new Trigger( - () -> (funnelSubsystem.isCoralCheckedIn() && CoralPosition.isCoralInTootsieSlide())) - .and(RobotModeTriggers.teleop()); - ejectTime.onTrue(new EjectCoralFR(elevatorSubsystem, tootsieSlideSubsystem)); - funnelCheckin.onTrue(new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.Intake, false)); - funnelCheckin.onTrue( - new RunFunnelUntilDetectionSafeSmooth(elevatorSubsystem, funnelSubsystem, leds)); - Trigger funnelCheckout = - new Trigger( - () -> - CoralPosition.isCoralInFunnel() - && elevatorSubsystem.atIntake() - && elevatorSubsystem.isAtPosition()) - .and(RobotModeTriggers.teleop()); - - funnelCheckout - .and(joystick.rightTrigger().negate()) - .onTrue( - new TransferPieceBetweenFunnelAndElevator( - elevatorSubsystem, funnelSubsystem, tootsieSlideSubsystem)); - Trigger coralInElevator = - new Trigger(() -> CoralPosition.isCoralInTootsieSlide()).and(RobotModeTriggers.teleop()); - coralInElevator.onTrue( - new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.safePosition, false)); + // joystick2.rightTrigger().whileTrue(new ShootTootsieSlide(tootsieSlideSubsystem)); + + // // Intake + // joystick2.leftTrigger(); + // // .onTrue(new D2Intake(elevatorSubsystem, tootsieSlideSubsystem, funnelSubsystem)); + + // //Auto Intake and Eject + // Trigger funnelCheckin = + // new Trigger( + // () -> funnelSubsystem.isCoralCheckedIn() && + // !CoralPosition.isCoralInTootsieSlide()) + // .and(RobotModeTriggers.teleop()); + // Trigger ejectTime = + // new Trigger( + // () -> (funnelSubsystem.isCoralCheckedIn() && + // CoralPosition.isCoralInTootsieSlide())) + // .and(RobotModeTriggers.teleop()); + // ejectTime.onTrue(new EjectCoralFR(elevatorSubsystem, tootsieSlideSubsystem)); + // funnelCheckin.onTrue(new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.Intake, + // false)); + // funnelCheckin.onTrue( + // new RunFunnelUntilDetectionSafeSmooth(elevatorSubsystem, funnelSubsystem, leds)); + // Trigger funnelCheckout = + // new Trigger( + // () -> + // CoralPosition.isCoralInFunnel() + // && elevatorSubsystem.atIntake() + // && elevatorSubsystem.isAtPosition()) + // .and(RobotModeTriggers.teleop()); + + // funnelCheckout + // .and(joystick.rightTrigger().negate()) + // .onTrue( + // new TransferPieceBetweenFunnelAndElevator( + // elevatorSubsystem, funnelSubsystem, tootsieSlideSubsystem)); + // Trigger coralInElevator = + // new Trigger(() -> CoralPosition.isCoralInTootsieSlide()).and(RobotModeTriggers.teleop()); + // coralInElevator.onTrue( + // new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.safePosition, false)); // // Debugging // debugJoystick.leftTrigger().whileTrue(new ShootTootsieSlide(tootsieSlideSubsystem)); - // // debugJoystick - // // .y() - // // .whileTrue( - // // new Dealgaenate( - // // armSubsystem, - // // elevatorSubsystem, - // // Constants.ElevatorConstants.ElevatorPositions.L2DALE)); + // debugJoystick + // .y() + // .whileTrue( + // new Dealgaenate( + // armSubsystem, + // elevatorSubsystem, + // Constants.ElevatorConstants.ElevatorPositions.L2DALE)); // debugJoystick.y().whileTrue(new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L4)); // debugJoystick @@ -382,7 +380,7 @@ private void configureBindings() { // () -> driveTrain.resetPose(new Pose2d(new Translation2d(0, 0), new // Rotation2d())))); - joystick.x().onTrue(new ZeroElevatorHardStop(elevatorSubsystem)); + // joystick.x().onTrue(new ZeroElevatorHardStop(elevatorSubsystem)); // new InstantCommand() @@ -477,154 +475,154 @@ public BooleanSupplier getRedSide() { public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ - int autoValue = autoChooser.getSelected(); - Command autoCommand; - DogLog.log("auto/selected", autoValue); - switch (autoValue) { - case 1: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_PROCESSOR_3, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_PROCESSOR_3, - leds); - break; - case 2: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_PROCESSOR_2, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_PROCESSOR_2, - leds); - break; - case 3: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_PROCESSOR_1, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_PROCESSOR_1, - leds); - break; - case 4: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_CLEAR_3, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_CLEAR_3, - leds); - break; - case 5: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_CLEAR_2, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_CLEAR_2, - leds); - break; - case 6: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_CLEAR_1, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_CLEAR_1, - leds); - break; - case 7: - autoCommand = - redside.getAsBoolean() - ? new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.RED_MID_1, - leds) - : new AutoProducer( - driveTrain, - tootsieSlideSubsystem, - elevatorSubsystem, - funnelSubsystem, - armSubsystem, - Constants.AutoRoutines.BLUE_MID_1, - leds); - break; - default: - autoCommand = null; - break; - } - return autoCommand; + // int autoValue = autoChooser.getSelected(); + // Command autoCommand; + // DogLog.log("auto/selected", autoValue); + // switch (autoValue) { + // case 1: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_PROCESSOR_3, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_PROCESSOR_3, + // leds); + // break; + // case 2: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_PROCESSOR_2, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_PROCESSOR_2, + // leds); + // break; + // case 3: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_PROCESSOR_1, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_PROCESSOR_1, + // leds); + // break; + // case 4: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_CLEAR_3, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_CLEAR_3, + // leds); + // break; + // case 5: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_CLEAR_2, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_CLEAR_2, + // leds); + // break; + // case 6: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_CLEAR_1, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_CLEAR_1, + // leds); + // break; + // case 7: + // autoCommand = + // redside.getAsBoolean() + // ? new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.RED_MID_1, + // leds) + // : new AutoProducer( + // driveTrain, + // tootsieSlideSubsystem, + // elevatorSubsystem, + // funnelSubsystem, + // armSubsystem, + // Constants.AutoRoutines.BLUE_MID_1, + // leds); + // break; + // default: + // autoCommand = null; + // break; + // } + return null; } } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java index f61019cc..57ce45c7 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java @@ -33,4 +33,4 @@ public void end(boolean interrupted) {} public boolean isFinished() { return elevatorSubsystem.isAtPosition(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorHeight.java b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorHeight.java new file mode 100644 index 00000000..eb0c73b9 --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorHeight.java @@ -0,0 +1,49 @@ +// 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.commands.ElevatorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +import frc.robot.subsystems.ElevatorSubsystem; + +/** An example command that uses an example subsystem. */ +public class SetElevatorHeight extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ElevatorSubsystem m_elevator; + + private ElevatorPositions position; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public SetElevatorHeight(ElevatorSubsystem elevator, ElevatorPositions position) { + m_elevator = elevator; + this.position = position; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(elevator); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_elevator.setPosition(position.getHeight()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_elevator.isAtPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java index c19e9668..d5324691 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java @@ -54,4 +54,4 @@ public boolean isFinished() { return timesExceededCurrent >= 10; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java index 4b265808..87c9d815 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java @@ -28,14 +28,14 @@ public void execute() {} @Override public void end(boolean interrupted) { DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); - elevatorSubsystem.resetPositionFiltered(); + // elevatorSubsystem.resetPositionFiltered(); } @Override public boolean isFinished() { DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); boolean inPosition = elevatorSubsystem.isAtPosition() && elevatorSubsystem.atIntake(); - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition); + // DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition); if (inPosition) { ticksAtPosition++; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a00efcb2..e836d1bf 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -14,19 +14,20 @@ import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; + import dev.doglog.DogLog; import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.ElevatorConstants; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.util.LoggedTalonFX; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; - public class ElevatorSubsystem extends SubsystemBase { private static ElevatorSubsystem instance; @@ -42,19 +43,14 @@ public class ElevatorSubsystem extends SubsystemBase { private ElevatorPositions currentLevel; private CANrange distance; // Time of Flight (ToF) sensor + private float tolerance; + private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); private final VelocityVoltage velocityRequest = new VelocityVoltage(0); - private ElevatorSubsystem() { - // Initialize motors - elevatorZeroed = false; - elevatorFilter = LinearFilter.singlePoleIIR(0.1, 0.02); - - distance = - new CANrange( - ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - + /** Creates a new ExampleSubsystem. */ + public ElevatorSubsystem() { motor1 = new LoggedTalonFX( "subsystems/Elevator/motor1", @@ -65,7 +61,6 @@ private ElevatorSubsystem() { "subsystems/Elevator/motor2", ElevatorConstants.MOTOR2_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - currentLevel = ElevatorPositions.Intake; // Set up motor followers and deal with inverted motors @@ -121,9 +116,9 @@ private ElevatorSubsystem() { m1Config.apply(moc); m2Config.apply(moc); - + master = motor1; - currentHeightToF = elevatorFilter.calculate(getToFDistance()); + currentHeightToF = 0f;//elevatorFilter.calculate(getToFDistance()); resetPositionFiltered(); } @@ -177,20 +172,6 @@ public void reduceCurrentLimits() { master.updateCurrentLimits(30, 10); } - public void resetCurrentLimits() { - master.updateCurrentLimits( - Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, - Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); - } - - public void resetElevatorPositionToZero() { - master.setPosition(0); - // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); - // master.setPosition(0); - // master.setControl(controlRequest.withPosition(0).withSlot(0)); - // master.setPosition(0); - } - public boolean checkCurrent() { double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); @@ -203,24 +184,38 @@ public boolean checkCurrent() { return false; } - public double getError() { - return currentLevel.height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION - - master.getPosition().getValueAsDouble(); + /** + * Example command factory method. + * + * @return a command + */ + public Command exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> { + /* one-time action goes here */ + }); } - public ElevatorPositions getLevel() { - return currentLevel; + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; } - public boolean atIntake() { - return currentLevel.equals(ElevatorPositions.Intake); + @Override + public void periodic() { + // This method will be called once per scheduler run } - public void elevateTo(ElevatorPositions level) { - this.currentLevel = level; - this.setPosition(level.height); + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation } public void setPosition(double height) { @@ -231,11 +226,40 @@ public void setPosition(double height) { * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / ElevatorConstants.CARRAIGE_UPDUCTION) .withSlot(0)); - DogLog.log( - "subsystems/Elevator/elevatorSetpoint(rot)", - height + } + + public void elevateTo(ElevatorPositions level) { + this.currentLevel = level; + this.setPosition(level.height); + } + + public ElevatorPositions getLevel() { + return currentLevel; + } + + public boolean atIntake() { + return currentLevel.equals(ElevatorPositions.Intake); + } + + public void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); + } + + public void resetElevatorPositionToZero() { + master.setPosition(0); + // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); + // master.setPosition(0); + // master.setControl(controlRequest.withPosition(0).withSlot(0)); + // master.setPosition(0); + } + + public double getError() { + return currentLevel.height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION); + / Constants.ElevatorConstants.CARRAIGE_UPDUCTION + - master.getPosition().getValueAsDouble(); } public void ElevatorTorqueMode() { @@ -261,8 +285,9 @@ public boolean canFunnelTransferCoralToScoring() { public double getToFDistance() { // 0.11 is the sensor offset DogLog.log( - "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); - return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; + "subsystems/Elevator/ToF/DistanceNoOffset", 0f/*distance.getDistance().getValueAsDouble()*/); + return 0f; + //return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; } public boolean isElevatorZeroed() { @@ -272,58 +297,4 @@ public boolean isElevatorZeroed() { public void elevatorHasBeenZeroed() { elevatorZeroed = true; } - - @Override - public void periodic() { - currentHeightToF = elevatorFilter.calculate(getToFDistance()); - // Time of Flight Sensor - DogLog.log("subsystems/Elevator/getError", getError()); - DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); - DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); - DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); - - DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); - DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); - DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); - DogLog.log( - "subsystems/Elevator/targetHeightRot", - currentLevel.getHeight() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log( - "subsystems/Elevator/currentHeightDist", - master.getPosition().getValueAsDouble() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE - * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); - DogLog.log( - "subsystems/Elevator/command", - this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); - DogLog.log( - "subsystems/Elevator/resetPositionBoolean", - this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log( - "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); - DogLog.log( - "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); - DogLog.log( - "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); - } - - @Override - public void simulationPeriodic() { - // Simulate encoder behavior based on motor speed - double simulatedSpeed = master.getVelocity().getValueAsDouble(); - double currentPosition = master.getPosition().getValueAsDouble(); - - // Update simulated position based on speed (simplified example) - double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop - master.setPosition( - newPosition); // Alarming to have this since running this on the robot will lead to - - // Log simulation data for debugging - DogLog.log("Simulated Position", newPosition); - DogLog.log("Simulated Speed", simulatedSpeed); - } } diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json index b410eb1d..0196095f 100644 --- a/vendordeps/DogLog.json +++ b/vendordeps/DogLog.json @@ -3,7 +3,7 @@ { "groupId": "com.github.jonahsnider", "artifactId": "doglog", - "version": "2025.3.0" + "version": "2025.9.2" } ], "fileName": "DogLog.json", @@ -15,6 +15,6 @@ "https://jitpack.io" ], "cppDependencies": [], - "version": "2025.3.0", + "version": "2025.9.2", "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" } \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index acc78dba..6f40c840 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.2.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +442,38 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "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/photonlib.json b/vendordeps/photonlib.json index 2d7b1d8e..4e7068c7 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.3.1", + "version": "v2025.3.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", + "version": "v2025.3.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.3.1", + "version": "v2025.3.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", + "version": "v2025.3.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.3.1" + "version": "v2025.3.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.3.1" + "version": "v2025.3.2" } ] } \ No newline at end of file