diff --git a/.gitignore b/.gitignore index 9a9ca7b..1055d0a 100644 --- a/.gitignore +++ b/.gitignore @@ -28,7 +28,7 @@ *.lai *.la *.a -*.lib +*.frc.robot.lib # Executables *.exe diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index f1d1b2d..96b0550 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -7,22 +7,42 @@ import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Orchestrator { private final Indexer indexer; private final Shooter shooter; + @AutoLogOutput + private double Distance = 300.0; + @AutoLogOutput private double shootingPower = 0.0; public Orchestrator(Indexer indexer, Shooter shooter) { this.indexer = indexer; this.shooter = shooter; } + public Command spinUpDistance(Double distance) { + Distance = distance; + double percent = (8.331 * Math.pow(10, -5) * Math.pow(distance, 2) + 0.0734 * distance + 15.709) / 100; + Logger.recordOutput("Shooter/Percent", percent); + return shooter.runPercent(percent); + } + public Command shootCycleDistance() { + return Commands.parallel( + spinUpDistance(420.0), + Commands.sequence( + intakeIfNeeded(), + Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS), + indexer.indexIntoShooter()) + .repeatedly()); + } public Command spinUp(BooleanSupplier fastMode) { return Commands.either( shooter.fullSpeed(), - shooter.speedUp(), - fastMode); + shooter.runPercent(0.8), + fastMode); } public Command intakeIfNeeded() { @@ -58,4 +78,4 @@ public Command reverseAll() { public Command stopAll() { return Commands.parallel(shooter.stop(), indexer.stop()); } -} +} \ 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 5274408..720d12d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIOSparkMax; import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; @@ -31,7 +30,6 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Drive drive; // The robot's subsystems and commands are defined here... - private final Vision vision; private final Indexer indexer = new Indexer(new IndexerIOSparkMax()); private final Shooter shooter = new Shooter(new ShooterIOSparkMax()); private final Orchestrator orchestrator = new Orchestrator(indexer, shooter); @@ -44,7 +42,6 @@ public class RobotContainer { public RobotContainer() { // Configure the trigger bindings drive = new Drive(new DriveIOSparkMax()); - vision = new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){}); configureBindings(); } @@ -75,16 +72,26 @@ private void configureBindings() { .rightTrigger() .whileTrue(orchestrator.shootCycle(() -> fastMode)) .onFalse(orchestrator.stopAll()); - driverController.y().whileTrue(shooter.speedUp()).onFalse(shooter.stop()); - driverController - .leftTrigger() - .whileTrue(orchestrator.reverseAll()) - .onFalse(orchestrator.stopAll()); - + driverController .rightBumper() .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); + + driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); + + driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); + + driverController.y().onTrue(indexer.indexIntoShooter()); + + + driverController + .leftBumper() + .whileTrue(orchestrator.reverseAll()) + .onFalse(orchestrator.stopAll()); + + + } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 52e56b4..75d752c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -37,15 +37,12 @@ public Command reverse() { } - public Command speedUp() { + public Command toVelocity(double velocityRPM) { return Commands.run( - () -> { - io.setPercent(ShooterConstants.SPEED); - }, + () -> io.setVelocity(velocityRPM), this ); } - public Command fullSpeed() { return Commands.run( () -> { @@ -54,6 +51,11 @@ public Command fullSpeed() { this ); } - + public Command runPercent(double percent){ + return Commands.run(() -> { + io.setPercent(percent); + }, + this); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index de81527..a44339a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,15 +2,11 @@ public class ShooterConstants { - public static final double SPEED = 0.8; - + public static final double TOLERANCE = 0.0; + public static final double ENCODER_POSITION_CONVERSION_FACTOR = 1.0; + public static final double ENCODER_VELOCITY_CONVERSION_FACTOR = 1.0; public static final double REVERSE_SHOOTER_PERCENT = -0.5; - public static final int SHOOTER_MOTOR_ID = 8; + public static final double SPINUP_SECONDS = 0.6; - public static final double ENCODER_POSITION_CONVERSION = 1.0; - - public static final double ENCODER_VELOCITY_CONVERSION = 1.0; - - public static final double SPINUP_SECONDS = 0.1; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2a6a301..185f43b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,21 +1,17 @@ package frc.robot.subsystems.shooter; -import org.ejml.dense.row.factory.DecompositionFactory_CDRM; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @AutoLog class ShooterIOInputs { - - public double velocity; - + public double velocityRPM; public double appliedVolts; - public double currentAmps; - public double temperature; - + public double targetVelocityRPM = 0.0; + public boolean atTargetVelocity; public boolean readyToShoot = false; } @@ -23,6 +19,8 @@ default void updateInputs(ShooterIOInputs inputs) {} default void setPercent(double percent) {} + default void setVelocity(double velocityRPM) {} + default void setVoltage(double voltage) {} default void stop() {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 70c2267..22825de 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -3,12 +3,12 @@ import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; @@ -16,25 +16,40 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkMax motor; private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + + private double targetVelocityRPM = 0.0; public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); + encoder = motor.getEncoder(); + controller = motor.getClosedLoopController(); var config = new SparkMaxConfig(); config.inverted(false) - .idleMode(IdleMode.kBrake) + .idleMode(IdleMode.kCoast) .voltageCompensation(12) .smartCurrentLimit(30); - EncoderConfig enc = new EncoderConfig(); - enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); - enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); - config.apply(enc); + config + .encoder + .positionConversionFactor(ENCODER_POSITION_CONVERSION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_CONVERSION_FACTOR); - motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + config + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) + .pidf(0.0001, 0.0, 0.0, 0.0); - encoder = motor.getEncoder(); + config + .signals + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override @@ -42,18 +57,33 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.temperature = motor.getMotorTemperature(); inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); - inputs.velocity = encoder.getVelocity(); + inputs.velocityRPM = encoder.getVelocity(); + inputs.targetVelocityRPM = targetVelocityRPM; + + inputs.atTargetVelocity = + targetVelocityRPM > 0 && + Math.abs(targetVelocityRPM - inputs.velocityRPM) < TOLERANCE; } + @Override public void setPercent(double percent) { motor.set(percent); } + @Override + public void setVelocity(double velocityRPM) { + this.targetVelocityRPM = velocityRPM; + controller.setReference(velocityRPM, ControlType.kVelocity); + } + + @Override public void setVoltage(double volts) { motor.setVoltage(volts); } + @Override public void stop() { + targetVelocityRPM = 0.0; motor.set(0); } -} +} \ No newline at end of file