diff --git a/build.gradle b/build.gradle index 9a4bb94..5f5fced 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "${wpiVersion}" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/lib/RoboLib b/lib/RoboLib index eeccb75..eea7693 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee +Subproject commit eea76939a3cbb76aba47874adf15eaf49a8333de diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eeab7a5..d2535bc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Unit; import edu.wpi.first.units.measure.Angle; import java.security.PublicKey; +import java.util.Queue; import com.ctre.phoenix6.signals.InvertedValue; @@ -68,10 +69,10 @@ public final class Constants { } public static class IntakeConstants { - public static final int DEPLOY_MOTOR_ID = 12; - public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this + public static final int DEPLOY_MOTOR_ID = 17; + public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this - public static final int INTAKE_MOTOR_ID = 1; + public static final int INTAKE_MOTOR_ID = 0; public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this } @@ -79,31 +80,33 @@ public final class Constants { public static final int LAUNCHER_MOTOR_ID = 2; public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this - public static final int TURRET_MOTOR_ID = 1; + public static final int TURRET_MOTOR_ID = 0; public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final int TURRET_LIMIT_CHANNEL = 0; public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); public static final double HOMING_SPEED = .35; - public static final double TURRET_GEAR_RATIO = 1; + public static final double TURRET_GEAR_RATIO = 0; - public static final int HOOD_SERVO_CHANNEL = 3; + public static final int HOOD_SERVO_CHANNEL = 9; public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this - public static final int HOOD_ENCODER_ID = 4; - public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); + public static final int HOOD_ENCODER_ID = 0; + public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5); public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0); - public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(20); - public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(40); - public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(60); + public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(100); + public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(200); + public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(300); + public static final double DEFLECTOR_SERVO_RATIO = 1.0; } public static class HopperConstants { - public static final int KICKER_MOTOR_ID = 0; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final int KICKER_MOTOR_ID = 9; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; - public static final int SPINDEXER_MOTOR_ID = 1; - public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + public static final int SPINDEXER_MOTOR_ID = 8; + public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final double SPINDEXER_MOTOR_SPEED = .6; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7eeabe5..8cc022a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; import frc.robot.commands.ManualHopperCommand; +import frc.robot.commands.ManualIntake; import frc.robot.commands.ManualShootCommand; import frc.robot.commands.SetDeflectorCommand; import frc.robot.subsystems.DriveSubsystem; @@ -81,9 +82,15 @@ public class RobotContainer { m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - m_driverController.y().onTrue(intakeSubsystem.deployCommand()); + /*m_driverController.y().onTrue(intakeSubsystem.deployCommand()); m_driverController.a().onTrue(intakeSubsystem.retractCommand()); - m_driverController.b().onTrue(intakeSubsystem.reverseCommand()); + m_driverController.b().onTrue(intakeSubsystem.reverseCommand());*/ + + m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); + m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); + //m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand()); + + intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); } /** diff --git a/src/main/java/frc/robot/commands/ManualHopperCommand.java b/src/main/java/frc/robot/commands/ManualHopperCommand.java index fe7a2c5..ac7c03b 100644 --- a/src/main/java/frc/robot/commands/ManualHopperCommand.java +++ b/src/main/java/frc/robot/commands/ManualHopperCommand.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import frc.robot.Constants; import frc.robot.subsystems.HopperSubsystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -22,13 +23,13 @@ public class ManualHopperCommand extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - m_subsystem.spin(.5); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.5) + if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2) { m_subsystem.feed(); } @@ -36,6 +37,14 @@ public class ManualHopperCommand extends Command { { m_subsystem.stop(); } + if(m_xboxController.getRightBumper()) + { + m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED); + } + if(m_xboxController.getLeftBumper()) + { + m_subsystem.spin(0); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ManualIntake.java b/src/main/java/frc/robot/commands/ManualIntake.java new file mode 100644 index 0000000..1359be0 --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualIntake.java @@ -0,0 +1,62 @@ +// 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; + +import frc.robot.subsystems.IntakeSubsystem; + +import javax.lang.model.util.ElementScanner14; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ManualIntake extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final IntakeSubsystem m_subsystem; + private final XboxController m_XboxController; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) { + m_subsystem = subsystem; + m_XboxController = xboxController; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(m_XboxController.getLeftBumper()==true) + { + m_subsystem.extend(); + } + else if(m_XboxController.getRightBumper()==true) + { + m_subsystem.retract(); + } + else + { + m_subsystem.neutral(); + } + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index 3827b32..1a857de 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -6,6 +6,7 @@ package frc.robot.commands; import frc.robot.subsystems.TurretSubsystem; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. */ @@ -23,13 +24,25 @@ public class ManualShootCommand extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(m_controller.getLeftTriggerAxis()>.5 || m_controller.getRightTriggerAxis()>.5) + if(m_controller.getRightTriggerAxis()>.8) { m_subsystem.setLauncher(1); + m_controller.setRumble(RumbleType.kBothRumble, 1); + } + else if(m_controller.getRightTriggerAxis()>.5) + { + m_subsystem.setLauncher(.7); + m_controller.setRumble(RumbleType.kBothRumble, .67); + } + else if(m_controller.getRightTriggerAxis()>.2) + { + m_subsystem.setLauncher(.5); + m_controller.setRumble(RumbleType.kBothRumble, .33); } else { m_subsystem.setLauncher(0); + m_controller.setRumble(RumbleType.kBothRumble, 0); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 20846d8..27d9e5d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -128,7 +128,7 @@ public class DriveSubsystem extends TestableSubsystem { @Override public void run() { - driveSystem.setChassisSpeedsFromJoystickRobotRelative( + driveSystem.setChassisSpeedsFromJoystickFieldRelative( -(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())), -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())), -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4)))); @@ -197,7 +197,6 @@ public class DriveSubsystem extends TestableSubsystem { } assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue(); - } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index d0e5eb0..875f303 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -69,12 +69,12 @@ public class IntakeSubsystem extends TestableSubsystem { public Command deployCommand() { - return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(50), () -> setSpeed(0), this)); + return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(.5), () -> setSpeed(0), this)); } public Command reverseCommand() { - return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-50), () -> setSpeed(0), this)); + return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-.5), () -> setSpeed(0), this)); } public Command retractCommand() @@ -82,13 +82,18 @@ public class IntakeSubsystem extends TestableSubsystem { return this.runOnce(() -> setSpeed(0)).andThen(new FunctionalCommand(this::retract, this::retract, interrupted -> neutral(), this::isRetracted, this)); } - @PostTest(name = "A friendly name", enabled = true) - public void myFailingPostTest(){ - assertThat(true).isFalse(); + public Command startSpeedCommand() + { + return this.runOnce(() -> setSpeed(-.5)); } - @PostTest(enabled = true) - public void myPassingPostTest(){ - assertThat("Hello World").endWith("World"); + public Command killSpeedCommand() + { + return this.runOnce(() -> setSpeed(0)); + } + + public Command revSpeedCommand() + { + return this.runOnce(() -> setSpeed(.5)); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 3806997..7ee12fa 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -33,11 +33,9 @@ public class TurretSubsystem extends TestableSubsystem { //private TalonFX turretMotor; private SparkMax launcherMotor; private Servo hood; - private CANcoder hoodEncoder; private Angle limitPos; private Angle limitNeg; private boolean turretGoingPos; - private Angle targElev; public TurretSubsystem() { turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL); @@ -50,42 +48,42 @@ public class TurretSubsystem extends TestableSubsystem { config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL); - hoodEncoder = new CANcoder(Constants.ShooterConstants.HOOD_ENCODER_ID); currentState = TurretState.INIT; limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS; turretGoingPos = false; - targElev = edu.wpi.first.units.Units.Degrees.of(45); } public void setLauncher(double speed) {launcherMotor.set(speed);} public void setTargetElevation(Angle elev) { - if(elev.gt(Constants.ShooterConstants.LIMIT_ELEVATION_NEG) && elev.lt(Constants.ShooterConstants.LIMIT_ELEVATION_POS)) - { - targElev=elev; + if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){ + elev=Units.Degrees.of(300).minus(elev); } + elev=elev.div(1.5); + elev=elev.div(Constants.ShooterConstants.DEFLECTOR_SERVO_RATIO); + hood.setAngle(elev.in(Units.Degrees)); } public void startHoming() { //turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); - currentState = TurretState.HOMING; + //currentState = TurretState.HOMING; } public void startRotate(double speed) { - turretGoingPos=speed>0; + /*turretGoingPos=speed>0; if(!passedLimit()) { - //turretMotor.set(speed); + turretMotor.set(speed); } else { - //turretMotor.set(0); - } + turretMotor.set(0); + }*/ } public boolean passedLimit(){ @@ -126,31 +124,11 @@ public class TurretSubsystem extends TestableSubsystem { private void periodicManual() { - if(passedLimit()) + //If the turret move, it will be a uh-oh, so DONT MOVE IT + + /*if(passedLimit()) { //turretMotor.set(0); - } - - int k = 1; - if(Constants.ShooterConstants.HOOD_SERVO_INVERTED) { - k=-1; - } - - Angle curElev = hoodEncoder.getAbsolutePosition().getValue(); - if(targElev.minus(curElev).abs(Units.Degrees)>Constants.ShooterConstants.TARGET_ELEVATION_DIF.in(Units.Degrees)) - { - if(targElev.gt(curElev)) - { - hood.set(k * 0.5); - } - else - { - hood.set(k * -0.5); - } - } - else - { - hood.set(0); - } + }*/ } }