From 332d1bea33352965bd70891caca64c6b5b8e6887 Mon Sep 17 00:00:00 2001 From: NutHouse coco-prog-3 Date: Fri, 31 Jan 2025 17:33:54 -0700 Subject: [PATCH] coralScorer works with ls. ls added to coralscorerspark --- src/main/java/frc/robot/RobotContainer.java | 18 ++++++++ .../robot/commands/CoralScorerCommand.java | 30 ++++++++++++ .../subsystems/CoralScorer/CoralScorer.java | 28 ++++++++++- .../subsystems/CoralScorer/CoralScorerIO.java | 28 +++-------- .../CoralScorer/CoralScorerIOSpark.java | 46 +++++++++++-------- .../CoralScorer/CoralSocrerIOTalonFX.java | 4 +- .../frc/robot/subsystems/CoralScorerIO.java | 5 -- 7 files changed, 110 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/commands/CoralScorerCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/CoralScorerIO.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cd9608..c4ed7f7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,7 +39,10 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.CoralScorerCommand; import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.CoralScorer.CoralScorer; +import frc.robot.subsystems.CoralScorer.CoralScorerIOSpark; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.flywheel_example.Flywheel; @@ -74,6 +77,8 @@ public class RobotContainer { private final Drive m_drivebase; private final Flywheel m_flywheel; + + private final CoralScorer m_CoralScorer = new CoralScorer(new CoralScorerIOSpark()); // These are "Virtual Subsystems" that report information but have no motors private final Accelerometer m_accel; private final Vision m_vision; @@ -243,6 +248,19 @@ private void configureBindings() { () -> turnStickX.value()), m_drivebase)); + m_CoralScorer.setDefaultCommand( + Commands.run( + () -> + m_CoralScorer.runVolts( + driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()), + m_CoralScorer)); + driverController + .x() + .whileTrue( + new CoralScorerCommand( + m_CoralScorer, + driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis())); + // Press A button -> BRAKE driverController .a() diff --git a/src/main/java/frc/robot/commands/CoralScorerCommand.java b/src/main/java/frc/robot/commands/CoralScorerCommand.java new file mode 100644 index 0000000..66dcd40 --- /dev/null +++ b/src/main/java/frc/robot/commands/CoralScorerCommand.java @@ -0,0 +1,30 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CoralScorer.CoralScorer; + +public class CoralScorerCommand extends Command { + + private final CoralScorer haberdashery; + + private final double volts; + + public CoralScorerCommand(CoralScorer haberdashery, double volts) { + this.haberdashery = haberdashery; + this.volts = volts; + addRequirements(haberdashery); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + haberdashery.runVoltsWithLS(volts); + } + + @Override + public void end(boolean interupted) { + haberdashery.stop(); + } +} diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java index 6d847a8..c88aa80 100644 --- a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java @@ -1,5 +1,29 @@ package frc.robot.subsystems.CoralScorer; -public class CoralScorer { - +import frc.robot.util.RBSISubsystem; + +public class CoralScorer extends RBSISubsystem { + + private final CoralScorerIO io; + + public CoralScorer(CoralScorerIO io) { + this.io = io; + } + + /** Stop in open loop. */ + public void stop() { + io.stop(); + } + + public void runVolts(double volts) { + io.setVolts(3 * volts); + } + + public void runVoltsWithLS(double volts) { + if (io.getLightStop() == false) { + io.setVolts(3 * volts); + } else { + io.setVolts(-1.5 + (3 * volts)); + } + } } diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java index 84a2bb7..25d19b0 100644 --- a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java @@ -14,35 +14,21 @@ // GNU General Public License for more details. package frc.robot.subsystems.CoralScorer; +import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLog; public interface CoralScorerIO { - // IMPORTANT: Include here all devices that are part of this mechanism! - public final int[] powerPorts = {}; - @AutoLog - public static class CoralScorerIOInputs { - public double positionRad = 0.0; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; - } + public static class IntakeIOInputs {} - /** Updates the set of loggable inputs. */ - public default void updateInputs(CoralScorerIOInputs inputs) {} + public default void lightStop(BooleanSupplier lightStop) {} - /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} + public default void setVolts(double volts) {} - /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) {} + public default boolean getLightStop() { + return false; + } - /** Stop in open loop. */ public default void stop() {} - - public default void moveXInches(double inches) {} - - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) {} } diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIOSpark.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIOSpark.java index 871cd58..f24c552 100644 --- a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIOSpark.java @@ -1,25 +1,35 @@ package frc.robot.subsystems.CoralScorer; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import java.util.function.BooleanSupplier; public class CoralScorerIOSpark implements CoralScorerIO { -private SparkMax motor = new SparkMax(0, MotorType.kBrushless); -private SparkMaxConfig config = new SparkMaxConfig(); -private SparkClosedLoopController m_pidController; -public CoralScorerIOSpark() { - config.encoder.positionConversionFactor(1000); - config. -} -@Override -public void moveXInches(double inches) { -motor.configure(config, null, null); -motor.getEncoder().getPosition(); -m_pidController.setReference(inches, SparkBase.ControlType.kSmartMotion); -} - + public CoralScorerIOSpark() {} + + private final SparkMax coralMotor = new SparkMax(16, MotorType.kBrushless); + + private final DigitalInput lightStop = new DigitalInput(1); + + @Override + public void setVolts(double volts) { + coralMotor.setVoltage(volts); + } + + @Override + public void lightStop(BooleanSupplier lightStop) { + if (lightStop.getAsBoolean()) {} + } + + @Override + public boolean getLightStop() { + return lightStop.get(); + } + + @Override + public void stop() { + coralMotor.stopMotor(); + } } diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralSocrerIOTalonFX.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralSocrerIOTalonFX.java index b221079..61c12e7 100644 --- a/src/main/java/frc/robot/subsystems/CoralScorer/CoralSocrerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralSocrerIOTalonFX.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.CoralScorer; -public class CoralSocrerIOTalonFX { - -} +public class CoralSocrerIOTalonFX {} diff --git a/src/main/java/frc/robot/subsystems/CoralScorerIO.java b/src/main/java/frc/robot/subsystems/CoralScorerIO.java deleted file mode 100644 index b9fee14..0000000 --- a/src/main/java/frc/robot/subsystems/CoralScorerIO.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems; - -public class CoralScorerIO { - -}