diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index f1d1b2d..3682af5 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -1,27 +1,48 @@ package frc.robot; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; 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(), + shooter.runPercent(0.8), fastMode); } @@ -51,6 +72,21 @@ public Command shootCycle(BooleanSupplier fastMode) { .repeatedly()); } + public Command shootOnceSetpoint(DoubleSupplier setpoint) { + return Commands.parallel( + shooter.toSetpoint(setpoint) + .withTimeout(0.001) + .andThen(shooter.toSetpoint(setpoint).until(shooter::atSetpoint)), + Commands.sequence( + intakeIfNeeded(), + Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS), + indexer.indexIntoShooter())); + } + + public Command shootCycleSetpoint(DoubleSupplier setpoint) { + return shootOnceSetpoint(setpoint).repeatedly(); + } + public Command reverseAll() { return Commands.parallel(shooter.reverse(), indexer.reverse()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5274408..e7292c2 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,36 @@ 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 + .b() + .whileTrue(orchestrator.shootCycleSetpoint(() -> 0.5)) + .onFalse(orchestrator.stopAll()); + + driverController + .b() + .onTrue(orchestrator.shootOnceSetpoint(() -> 0.5)) + .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..d6d92b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -15,11 +15,13 @@ public class Shooter extends SubsystemBase { public Shooter(ShooterIO io) { this.io = io; this.inputs = new ShooterIOInputsAutoLogged(); + this.io.setSpeed(0.5); // TODO: Change the random value } @Override public void periodic() { io.updateInputs(inputs); + io.goToSetpoint(); Logger.processInputs("Shooter", inputs); } @@ -36,24 +38,39 @@ public Command reverse() { ); } - - public Command speedUp() { + public Command fullSpeed() { return Commands.run( () -> { - io.setPercent(ShooterConstants.SPEED); + io.setPercent(1); }, this ); } - public Command fullSpeed() { + public Command runPercent(double percent){ + return Commands.run(() -> { + io.setPercent(percent); + }, + this); + } + + public boolean atSetpoint() { + return inputs.atSetpoint; + } + + public Command toSetpoint(double setpointPercent) { return Commands.run( () -> { - io.setPercent(1); + io.setSpeed(setpointPercent); }, - this - ); + this); } - + public Command toSetpoint(DoubleSupplier setpointPercent) { + return Commands.run( + () -> { + io.setSpeed(setpointPercent.getAsDouble()); + }, + 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..25e0727 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,8 +2,6 @@ public class ShooterConstants { - public static final double SPEED = 0.8; - public static final double REVERSE_SHOOTER_PERCENT = -0.5; public static final int SHOOTER_MOTOR_ID = 8; @@ -12,5 +10,11 @@ public class ShooterConstants { public static final double ENCODER_VELOCITY_CONVERSION = 1.0; - public static final double SPINUP_SECONDS = 0.1; + public static final double SPINUP_SECONDS = 0.6; + + public static final double TOLERANCE = 0.1; + + public static final double PID_P = 15.0; + + public static final double PID_D = 7.0; } \ 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..e35f6be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -16,9 +16,15 @@ class ShooterIOInputs { public double temperature; + public double angularVelocity; + public boolean readyToShoot = false; + + public boolean atSetpoint = false; } + default void setSpeed(double setpoint) {} + default void updateInputs(ShooterIOInputs inputs) {} default void setPercent(double percent) {} @@ -26,4 +32,7 @@ default void setPercent(double percent) {} default void setVoltage(double voltage) {} default void stop() {} + + default void goToSetpoint() {} + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 70c2267..4c62494 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -3,11 +3,13 @@ import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; @@ -17,14 +19,24 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkMax motor; private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + double setpoint = 0.0; + public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); + controller = motor.getClosedLoopController(); + var config = new SparkMaxConfig(); config.inverted(false) .idleMode(IdleMode.kBrake) .voltageCompensation(12) .smartCurrentLimit(30); + config + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) + .positionWrappingEnabled(false) + .pidf(PID_P, 0.0, PID_D, 0.0); EncoderConfig enc = new EncoderConfig(); enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); @@ -43,6 +55,13 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); inputs.velocity = encoder.getVelocity(); + inputs.angularVelocity = inputs.velocity * (2 * Math.PI / 60); + inputs.atSetpoint = Math.abs(setpoint - inputs.velocity) < TOLERANCE; + } + + @Override + public void setSpeed(double setpoint) { + this.setpoint = setpoint; } public void setPercent(double percent) { @@ -56,4 +75,9 @@ public void setVoltage(double volts) { public void stop() { motor.set(0); } + + @Override + public void goToSetpoint() { + controller.setReference(this.setpoint, SparkBase.ControlType.kVelocity); + } }