From 429cfd3d0fd10048bfdc4b33cae3493e88f2baff Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Thu, 13 Mar 2025 21:05:55 -0500 Subject: [PATCH] Today's changes: Buttons to specific positions --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 25 ++++-- src/main/java/frc/robot/RobotContainer.java | 31 +++++--- .../ClimberCommands/ExtendWinchCommand.java | 8 +- .../ClimberCommands/GrabCageCommand.java | 8 +- .../ElevatortoPositionCommand.java | 77 +++++++++++++++++++ .../EndEffectorCommands/IntakeCommand.java | 8 +- .../EndEffectorCommands/ShootCommand.java | 10 ++- .../subsystems/climb/ClimberSubsystem.java | 2 +- .../elevator/ElevatorSubsystem.java | 17 +++- 10 files changed, 157 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ElevatorCommands/ElevatortoPositionCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ce151e6..2d051ee 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,7 +55,7 @@ public static class SystemConfig { public static final double SHOOTER_TOLERANCE = 2; // deg public static final double ELEVATOR_TOLERANCE = 0.1; // m - public static final double ELEVATOR_SPEED = 0.5; + public static final double ELEVATOR_SPEED = 0.7; public static final double PIVOT_SPEED = 0.5; public static final double SHOOTER_SPEED = 0.5; public static final double GRABBER_SPEED = 0.5; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b44325..c407f9b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,12 +4,11 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.wpilibj.*; /** * The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as * described in the TimedRobot documentation. If you change the name of this class or the package after creating this @@ -20,11 +19,14 @@ public class Robot extends TimedRobot private static Robot instance; private Command m_autonomousCommand; - + private Debouncer m_debouncer; + PowerDistribution m_powerDist; private RobotContainer m_robotContainer; + private static final double kStallCurrent = 35; //Amps + private boolean isStalled = false; private Timer disabledTimer; - + public Robot() { instance = this; @@ -44,7 +46,10 @@ public void robotInit() // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - + double debounceTime = .25; + DebounceType debounceType = DebounceType.kRising; + m_debouncer = new Debouncer(debounceTime, debounceType); + m_powerDist = new PowerDistribution(); // Create a timer to disable motor brake a few seconds after disable. This will let the robot stop // immediately when disabled, but then also let it be pushed more disabledTimer = new Timer(); @@ -141,6 +146,8 @@ public void teleopInit() @Override public void teleopPeriodic() { + double currentDraw = m_powerDist.getCurrent(8); + isStalled = m_debouncer.calculate(currentDraw > kStallCurrent); } @Override @@ -173,4 +180,8 @@ public void simulationInit() public void simulationPeriodic() { } + public static boolean isStalled(){ + // return isStalled; + return true; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e689f77..43211c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -118,23 +119,31 @@ private void configureBindings() // Operator Controls - operatorController.rightTrigger().whileTrue(new ShootCommand(-.1)); - operatorController.leftTrigger().whileTrue(new ShootCommand(.1)); + operatorController.rightTrigger().whileTrue(new ShootCommand(-1.0)); + // operatorController.leftTrigger().whileTrue(new ShootCommand(.1)); + + operatorController.leftBumper().whileTrue(new IntakeCommand(.3)); + operatorController.rightBumper().whileTrue(new IntakeCommand(-.3)); + // operatorController.leftBumper().whileTrue(new IntakeCommand(.1)); + // operatorController.leftBumper().whileTrue(new ParallelCommandGroup( + // new IntakeCommand(.1), + // new ShootCommand(-.4) + // )); + operatorController.x().onTrue(new ElevatortoPositionCommand(0.5, 1)); + operatorController.y().onTrue(new ElevatortoPositionCommand(0.5, 2)); + operatorController.b().onTrue(new ElevatortoPositionCommand(0.5, 3)); + operatorController.a().onTrue(new ElevatortoPositionCommand(0.5, 4)); + operatorController.povDown().whileTrue(new ElevatorDownCommand(0.5)); - operatorController.rightBumper().whileTrue(new IntakeCommand(-.1)); - operatorController.leftBumper().whileTrue(new IntakeCommand(.1)); - - - // Driver Controls driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); - driverController.rightTrigger().whileTrue(new ExtendWinchCommand(-.1)); - driverController.leftTrigger().whileTrue(new ExtendWinchCommand(.1)); + driverController.leftTrigger().whileTrue(new ExtendWinchCommand(-.1)); + driverController.rightTrigger().whileTrue(new ExtendWinchCommand(.1)); - driverController.rightBumper().whileTrue(new GrabCageCommand(.1)); - driverController.leftBumper().whileTrue(new GrabCageCommand(-1.)); + driverController.rightBumper().whileTrue(new GrabCageCommand(.4)); + driverController.leftBumper().whileTrue(new GrabCageCommand(-.4)); } public void setMotorBrake(boolean brake) diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java index f47beb7..6d246fc 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java @@ -14,7 +14,9 @@ public ExtendWinchCommand(Double _speed) } @Override - public void initialize() {} + public void initialize() { + RobotContainer.climber.Winch(0); + } @Override public void execute() { @@ -22,7 +24,9 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.climber.Winch(0); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java index b10aa1d..9397662 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java @@ -14,7 +14,9 @@ public GrabCageCommand(Double _speed) } @Override - public void initialize() {} + public void initialize() { + RobotContainer.climber.Grab(0); + } @Override public void execute() { @@ -22,7 +24,9 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.climber.Grab(0); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatortoPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatortoPositionCommand.java new file mode 100644 index 0000000..d323f73 --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatortoPositionCommand.java @@ -0,0 +1,77 @@ +package frc.robot.commands.ElevatorCommands; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.wpilibj.Timer; + +public class ElevatortoPositionCommand extends Command +{ + private Debouncer m_debouncer; + private final double speed; + private final int level_1_position = 50; + private final int level_2_position = 150; + private final int level_3_position = 300; + private final int level_4_position = 600; + private final int level; + + public ElevatortoPositionCommand(Double _speed, int _level) + { + addRequirements(RobotContainer.elevator); + speed = _speed; + level = _level; + double debounceTime = .25; + DebounceType debounceType = DebounceType.kRising; + m_debouncer = new Debouncer(debounceTime, debounceType); + } + + @Override + public void initialize() {} + + public void moveToPosition(int position){ + boolean isAtPosition = false; + boolean isStalled = m_debouncer.calculate(isAtPosition); + while(!isStalled){ + if (!isAtPosition && RobotContainer.elevator.getPositionElevator1() >= level_3_position && RobotContainer.elevator.getPositionElevator2() >= level_3_position) { + RobotContainer.elevator.setSpeedEndEffectorTilt(speed); + Timer.delay(1.0); + RobotContainer.elevator.setSpeedEndEffectorTilt(0); + } + RobotContainer.elevator.setSpeedElevator1(speed); + RobotContainer.elevator.setSpeedElevator2(speed); + isAtPosition = (RobotContainer.elevator.getPositionElevator1() >= position && RobotContainer.elevator.getPositionElevator2() >= position); + isStalled = m_debouncer.calculate(isAtPosition); + Timer.delay(.1); + } + RobotContainer.elevator.setSpeedElevator1(0); + RobotContainer.elevator.setSpeedElevator2(0); + } + + @Override + public void execute() + { + switch(level){ + case 1: + moveToPosition(level_1_position); + case 2: + moveToPosition(level_2_position); + case 3: + moveToPosition(level_3_position); + case 4: + moveToPosition(level_4_position); + default: + moveToPosition(level_1_position); + } + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java index 1560887..c630ec1 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; +import frc.robot.Robot; public class IntakeCommand extends Command { @@ -16,17 +17,22 @@ public IntakeCommand(Double _speed) @Override public void initialize() { RobotContainer.endEffector.setSpeedEndEffectorIntake(0); + RobotContainer.endEffector.setSpeedEndEffectorMotor(0); } @Override public void execute() { - RobotContainer.endEffector.setSpeedEndEffectorIntake(speed); + boolean isStalled = Robot.isStalled(); + RobotContainer.endEffector.setSpeedEndEffectorIntake(speed/3); + // Intake moves much faster than Motor! + RobotContainer.endEffector.setSpeedEndEffectorMotor(isStalled?0:speed*1.5); } @Override public void end(boolean interrupted) { RobotContainer.endEffector.setSpeedEndEffectorIntake(0); + RobotContainer.endEffector.setSpeedEndEffectorMotor(0); } @Override diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java index 79d2c13..30a2716 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java @@ -14,16 +14,20 @@ public ShootCommand(Double _speed) } @Override - public void initialize() {} + public void initialize() { + RobotContainer.endEffector.setSpeedEndEffectorMotor(0); + } @Override public void execute() { - RobotContainer.endEffector.setSpeedEndEffectorMotor(speed); + RobotContainer.endEffector.setSpeedEndEffectorMotor(speed*2); } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.endEffector.setSpeedEndEffectorMotor(0); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java index a084acb..2140d3d 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java @@ -16,7 +16,7 @@ // } // @Override -// public void periodic() { +// public void () { // updateEntries(); // } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index fa6a5c3..763dca8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -14,17 +14,17 @@ public class ElevatorSubsystem extends SubsystemBase { private final SparkMax elevator1 = new SparkMax(CANConfig.ELEVATOR_LEFT, MotorType.kBrushless); - private final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless);; - + private final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless); private SparkMaxConfig elevator1Config = new SparkMaxConfig(); private SparkMaxConfig elevator2Config = new SparkMaxConfig(); + private final SparkMax endEffectorTilt = new SparkMax(CANConfig.END_EFFECTOR_TILT, MotorType.kBrushless); public ElevatorSubsystem() { elevator1Config.inverted(true); elevator2Config.inverted(false); elevator1.configure(elevator1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - elevator2.configure(elevator2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + elevator2.configure(elevator2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void setSpeedElevator1(double speed) @@ -37,10 +37,21 @@ public void setSpeedElevator2(double speed) elevator2.set(speed); } + public void setSpeedEndEffectorTilt(double speed) + { + endEffectorTilt.set(speed); + } + public void setSpeed(double speed){ elevator1.set(speed); elevator2.set(speed); } + public double getPositionElevator1(){ + return elevator1.getAbsoluteEncoder().getPosition(); + } + public double getPositionElevator2(){ + return elevator2.getAbsoluteEncoder().getPosition(); + } @Override public void periodic()