diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 17b65c47..3def1541 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,19 +10,23 @@ /** Add your docs here. */ public class Constants { public class drivetrainConstants { - + } public class RobotMap { public class CAN { - + public static final int COLLECTOR_MOTOR = 9; } + + public static final int COLLECTOR_ENTRY_BEAMBREAK = 1; } - public class VisionConstants { - //This is a magic number from gridlock, may need to be changed or removed entirely - public static final double PROCESS_LATENCY = 0.0472; // TODO test - public static final Translation2d FIELD_LIMIT = new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0)); - public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5)); + public class VisionConstants { + // This is a magic number from gridlock, may need to be changed or removed entirely + public static final double PROCESS_LATENCY = 0.0472; // TODO test + public static final Translation2d FIELD_LIMIT = + new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0)); + public static final Translation2d VISION_LIMIT = + new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5)); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a90b3da..360dc450 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,17 +1,43 @@ package frc.robot; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.command.*; +import frc.robot.subsystem.*; import frc.thunder.LightningContainer; public class RobotContainer extends LightningContainer { + + /* Setting up bindings for necessary control of the swerve drive platform */ + XboxController driver = new XboxController(0); // My joystick + + // Create subsystems + Collector collector = new Collector(); + @Override - protected void configureButtonBindings() {} + protected void configureButtonBindings() { + new Trigger(driver::getRightBumper).whileTrue(new Collect(collector)); + } @Override - protected void configureDefaultCommands() {} + protected void configureDefaultCommands() { + // Get collector entry beam break state, then run collector if object is present + collector.setDefaultCommand(new RunCommand(() -> { + if (!collector.getEntryBeamBreakState()) { + collector.setPower(1d); + } else { + collector.stop(); + } + }, collector)); + } @Override protected void configureAutonomousCommands() {} - + @Override protected void releaseDefaultCommands() {} diff --git a/src/main/java/frc/robot/command/Collect.java b/src/main/java/frc/robot/command/Collect.java new file mode 100644 index 00000000..e7bbfa37 --- /dev/null +++ b/src/main/java/frc/robot/command/Collect.java @@ -0,0 +1,38 @@ +// 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.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.Collector; + +public class Collect extends Command { + + // Declares collector + private Collector collector; + + public Collect(Collector collector) { + // Sets collector from parameter + this.collector = collector; + addRequirements(collector); + } + + @Override + public void initialize() { + collector.setPower(1d); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + collector.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystem/Collector.java b/src/main/java/frc/robot/subsystem/Collector.java new file mode 100644 index 00000000..045b695d --- /dev/null +++ b/src/main/java/frc/robot/subsystem/Collector.java @@ -0,0 +1,51 @@ +// 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.subsystem; + +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Collector extends SubsystemBase { + + // Declare collector hardware + private TalonFX collectorMotor; + private DigitalInput collectorEntryBeamBreak; + + public Collector() { + // Initialize collector hardware + collectorMotor = new TalonFX(Constants.RobotMap.CAN.COLLECTOR_MOTOR); + collectorEntryBeamBreak = new DigitalInput(Constants.RobotMap.COLLECTOR_ENTRY_BEAMBREAK); + } + + @Override + public void periodic() { + + } + + /** + * @return When an object is present, returns false, otherwise returns true + */ + public boolean getEntryBeamBreakState() { + return collectorEntryBeamBreak.get(); + } + + /** + * Sets the power of the collector motor + * @param power Double value from -1.0 to 1.0 (positive collects inwards) + */ + public void setPower(double power) { + collectorMotor.set(power); + } + + /** + * Stops the collector + */ + public void stop() { + setPower(0d); + } + +}