From 41461c9b0707ba66adfc122d3799f0313a621389 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Mon, 27 Apr 2026 23:50:59 -0600 Subject: [PATCH] One controller and elastic layout for demo --- elastic-layout.json | 81 +++++++++ .../java/frc4388/robot/RobotContainer.java | 164 +++--------------- .../robot/constants/BuildConstants.java | 12 +- .../robot/subsystems/intake/Intake.java | 111 ++---------- .../robot/subsystems/shooter/Shooter.java | 16 +- .../robot/subsystems/shooter/ShooterIO.java | 2 +- 6 files changed, 127 insertions(+), 259 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 372dae1..2f0184a 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -2,6 +2,87 @@ "version": 1.0, "grid_size": 128, "tabs": [ + { + "name": "DEMO", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Controller Binds", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 512.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/Controller Binds", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Demo % output", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Demo % output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Indexer FWD % Output", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Indexer FWD % Output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "IndexerTargetOutput", + "x": 896.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/AdvantageKit/Shooter/IndexerTargetOutput", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "IndexerOutput", + "x": 896.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/AdvantageKit/Shooter/IndexerOutput", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + } + ] + } + }, { "name": "Teleoperated", "grid_layout": { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ad95e0c..af0c2a9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -12,37 +12,31 @@ package frc4388.robot; import java.io.File; -import java.util.Optional; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc4388.robot.commands.waitSupplier; import frc4388.robot.commands.Swerve.StayInPosition; -import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; -import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.led.LED; @@ -50,7 +44,6 @@ import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.robot.subsystems.swerve.SimpleSwerveSim; import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.FieldPositions; @@ -107,76 +100,14 @@ public class RobotContainer { // ! /* Autos */ private SendableChooser autoChooser; private Command autoCommand; - - - private Command IntakeExtended = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake) - ); + - private Command LabubuGrowl = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.LabubuGrowl), m_robotIntake) - ); - - // private Command LidarIntake = new SequentialCommandGroup( - // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball - // // RobotIntakeDown, - // // new WaitCommand(1), - // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), - // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) - - // // new RunCommand( - // // () -> m_robotSwerveDrive.driveWithInput( - // // m_lidar.getClosestBall(), - // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), - // // false - // // ), - // // m_robotSwerveDrive - // // ) - // // .withTimeout(5.0) - // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) - // ); - - private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), - IntakeExtended, - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExpelBalls), m_robotIntake) - ); - - private Command WaitIntakeReference = - new WaitUntilCommand(m_robotIntake::intakeAtReference); - - private Command ZeroEncoder = - new InstantCommand(() -> m_robotIntake.io.fixEncoder(), m_robotIntake); - - private Command RobotShootDriving = new SequentialCommandGroup( - new RunCommand(() -> - m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION) - ).withTimeout(20) - ).finallyDo((interrupted) -> - m_robotSwerveDrive.disableRotationOverride() - ); - - private Command IntakeRetracted = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake) - ); - - private Command RobotShoot = new SequentialCommandGroup( - // TEST NEW AUTO ALIGN - //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), - new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake), - new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(3.5), - IntakeRetracted, - new WaitCommand(5), - new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), - new InstantCommand(()-> m_robotShooter.spinUpIdle(), m_robotShooter) - ); + public RobotContainer() { - configureButtonBindings(); + configureSINGLEBindings(); // Called on first robot enable DeferredBlock.addBlock(() -> { @@ -193,15 +124,6 @@ public class RobotContainer { m_robotShooter.io.updateGains(); }, true); - NamedCommands.registerCommand("Robot Rev Up", RobotRev); - NamedCommands.registerCommand("Zero Encoder", ZeroEncoder); - NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); - NamedCommands.registerCommand("Robot Shoot", RobotShoot); - // NamedCommands.registerCommand("Lidar Intake", LidarIntake); - NamedCommands.registerCommand("Intake Extended", IntakeExtended); - NamedCommands.registerCommand("Labubu Growl", LabubuGrowl); - NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); - NamedCommands.registerCommand("Intake Reference", WaitIntakeReference); NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed)); NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter)); NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)); @@ -244,57 +166,14 @@ public class RobotContainer { } - - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - - //Driver controls - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - - // manually shoot from climb post/ feed balls - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotShooter.spinUpFeeding(); - m_robotIntake.rollerStop(); - })) - .onFalse(new InstantCommand(() -> { - m_robotShooter.spinUpIdle(); - })); - - - - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ExtendingRolling); - })); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot); - })); - - - } private void configureSINGLEBindings() { - - - //Driver controls + + String controllerInstructions = "Single Controller: \n- A: Reset Gyro \n- Right Bumper: Shift Up \n- Left Bumper: Shift Down \n- X Button: Roller On \n- Y Button: Roller Off \n- B Button: Labubu Growl \n- Back Button: Manual shoot \n- Menu Button: Expels balls"; + + SmartDashboard.putString("Controller Binds", controllerInstructions); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); @@ -303,10 +182,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - // manually shoot from climb post/ feed balls - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.spinUpFeeding(); m_robotIntake.rollerStop(); @@ -315,18 +192,27 @@ public class RobotContainer { m_robotShooter.spinUpIdle(); })); - - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ExtendingRolling); + m_robotShooter.spinUpFeeding(); + m_robotIntake.rollerStop(); + })); + + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.RollerOn); })); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot); + m_robotIntake.setMode(IntakeMode.RollerOff); })); + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.LabubuGrowl); + })); } //.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 946d103..d2ec612 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 241; - public static final String GIT_SHA = "6316daedf7fea6d03ec620bc433364a6881e3316"; - public static final String GIT_DATE = "2026-04-17 17:27:23 MDT"; - public static final String GIT_BRANCH = "demo"; - public static final String BUILD_DATE = "2026-04-17 18:31:24 MDT"; - public static final long BUILD_UNIX_TIME = 1776472284468L; + public static final int GIT_REVISION = 242; + public static final String GIT_SHA = "a15a6d08bf31c3ec80ed3e4327cbf214cc9781f5"; + public static final String GIT_DATE = "2026-04-17 18:42:57 MDT"; + public static final String GIT_BRANCH = "DemoBranch-(No-Intake)"; + public static final String BUILD_DATE = "2026-04-27 23:44:57 MDT"; + public static final long BUILD_UNIX_TIME = 1777355097369L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 62ea042..9cc937c 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -1,8 +1,5 @@ package frc4388.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Rotations; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -10,7 +7,6 @@ import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.subsystems.swerve.SwerveDrive; @@ -31,19 +27,11 @@ public class Intake extends SubsystemBase { } public enum IntakeMode { - ExtendingIdle, - ExtendingRolling, - - EncoderFix, - Retracting, - ArmIdleRollingNot, - - Idle, - RectractTorque, - RectractAuto, - Bouncing, + RollerOn, + RollerOff, ExpelBalls, - LabubuGrowl + LabubuGrowl, + Idle } private boolean overCompressed = false; @@ -51,15 +39,6 @@ public class Intake extends SubsystemBase { public void setMode(IntakeMode mode) { this.mode = mode; - - switch (mode) { - case Bouncing: - // When bounce is enabled: set the bounce timer - this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); - break; - default: - break; - } } public IntakeMode getMode() { @@ -119,91 +98,25 @@ public class Intake extends SubsystemBase { // getCurrentTime switch (mode) { - case ExtendingIdle: - // io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, 0); - break; - - - case ExtendingRolling: - // io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed)); + case RollerOn: + io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); break; - case EncoderFix: - // io.armFix(IntakeConstants.FIX_ARM_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, 0); - break; - - case Retracting: - // io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); - - // if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { - io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); - // } else { - // io.setRollerOutput(state, 0); - // } - break; - case ArmIdleRollingNot: - // io.armOutput(0); - io.setRollerOutput(state, 0); - break; - case Bouncing: - // io.setRollerOutput(state, 0); - - // if( - // state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() - // // Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() - // ) { - // this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); - // } - - // // Get the time delta from the last bounce time update - // double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime; - // // Get the percentage through the bounce period (0 output means one half period has passed) - // double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get(); - // // Clamp the output of the motor to some value - // percentOutput = -Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()); - - // io.armOutput(percentOutput); - - // if(percentOutput < 0) { - // io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); - // } else { - // io.setRollerOutput(state, 0); - // } - // break; - // case RectractTorque: - // io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); - // if (!overCompressed){ - // io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get()); - // } else { - // io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get()); - // } - - // if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { - // io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); - // } else { - // io.setRollerOutput(state, 0); - // } - break; - case RectractAuto: - io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); - // io.armOutput(IntakeConstants.ARM_AUTO_OUTPUT.get()); - break; - case Idle: - // io.armOutput(0); + case RollerOff: io.setRollerOutput(state, 0); break; case ExpelBalls: - // io.armOutput(0); io.setRollerOutput(state, IntakeConstants.ROLLER_EJECT_PERCENT_OUTPUT.get()); break; + case LabubuGrowl: - // io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_LABUBU_GROWL_PERCENT_OUTPUT.get()); break; + case Idle: + io.armOutput(0); + io.setRollerOutput(state, 0); + break; } // if (state.retractedLimit){ // this.mode = IntakeMode.Retracted; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index b31e451..a70abbe 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,8 +1,5 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import org.littletonrobotics.junction.AutoLogOutput; @@ -12,9 +9,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.intake.Intake; @@ -162,12 +156,6 @@ public class Shooter extends SubsystemBase { badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); - //revtime calculations - // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond); - double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT)); - // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend)); - Logger.recordOutput("Time to rev", revTime); switch (mode) { case Shooting: @@ -211,9 +199,9 @@ public class Shooter extends SubsystemBase { break; case ManualShoot: if(io.demoSpeed(state)){ - io.setIndexerOutput(state, -0.5); + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); }else{ - io.setIndexerOutput(state, 0.2); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); } io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_OVERRIDE_VELOCITY.get())); m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 5aeaab5..d46e77c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -54,7 +54,7 @@ public interface ShooterIO { public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {} public default boolean demoSpeed(ShooterState state) { boolean demo = false; - if((Math.abs(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))-Math.abs(state.motor2TargetVelocity.in(RotationsPerSecond))) < 3)){ + if((Math.abs(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))-Math.abs(state.motor1Velocity.in(RotationsPerSecond))) < 3)){ demo = true; } return demo;