diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 37898345..f8945793 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -65,9 +65,12 @@ public static class Drivetrain { } public static class Indexer { - // TODO: fix these constants + + public static final int BEAM_BREAK_ID = 0; public static final int INTAKE_ID = 11; public static final int FEEDER_ID = 12; + + // TODO: fix these constants public static final double FEEDER_KP = 0.5; public static final double FEEDER_KI = 0.1; public static final double FEEDER_KD = 0.001; @@ -77,7 +80,7 @@ public static class Indexer { public static final double INTAKE_GEAR_RATIO = 1.0; public static final double INTAKE_MOI = 0.025; public static final double FEEDER_MOI = 0.025; - public static final int BEAM_BREAK_ID = 0; + } @@ -217,10 +220,11 @@ public enum ElevatorState { } public static class Tramp { + public static final int TRAMP_BEAM_BREAK_CHANNEL = 1; public static final double GEAR_RATIO = 3.0; public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D - public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later - public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later + public static final int MOTOR_ID = -1; //TODO: Configure this later + } } diff --git a/src/main/java/org/team1540/robot2024/commands/TrampShoot.java b/src/main/java/org/team1540/robot2024/commands/TrampShoot.java new file mode 100644 index 00000000..b2f42099 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/TrampShoot.java @@ -0,0 +1,22 @@ +package org.team1540.robot2024.commands; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import org.team1540.robot2024.subsystems.tramp.Tramp; + +public class TrampShoot extends ParallelDeadlineGroup { + public TrampShoot(Tramp tramp) { + super( + Commands.sequence( + Commands.waitUntil(() -> !tramp.isNoteStaged()), + Commands.waitSeconds(2) //TODO: tune this + ), + Commands.startEnd( + () -> tramp.setPercent(0.5), //TODO: tune this + () -> tramp.stop(), + tramp + ) + ); + } +} + diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java index 0112797d..b9d8720a 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java @@ -17,7 +17,7 @@ public StageTrampCommand(Tramp tramp, Indexer indexer) { @Override public void initialize() { - tramp.setPercent(0.5); + tramp.setPercent(0.5); //TODO: Tune this indexer.setFeederVelocity(-600); indexer.setIntakePercent(0.5); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java index 7b91cddf..0ee859ca 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java @@ -7,9 +7,9 @@ import org.team1540.robot2024.Constants.Tramp; public class TrampIOSparkMax implements TrampIO { - private final DigitalInput beamBreak = new DigitalInput(0); + private final DigitalInput beamBreak = new DigitalInput(Tramp.TRAMP_BEAM_BREAK_CHANNEL); //TODO: Potentially change name :D - private final CANSparkMax motor = new CANSparkMax(Tramp.TRAMP_MOTOR_ID, MotorType.kBrushless); + private final CANSparkMax motor = new CANSparkMax(Tramp.MOTOR_ID, MotorType.kBrushless); private final RelativeEncoder motorEncoder = motor.getEncoder(); public TrampIOSparkMax() {