From 69677d5ae9e63125a30857f5f7afa0756dfa9416 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Fri, 20 Mar 2026 15:40:07 -0600 Subject: [PATCH] Lots - shooting while moving corrected (vertical movements) - reverse indexer before shooting - defensive positions fixed values - stalling motors led fixed (except shooter) --- elastic-layout.json | 141 +++++++++++------- .../java/frc4388/robot/RobotContainer.java | 121 ++++++++++++++- .../robot/commands/Swerve/StayInPosition.java | 4 +- .../robot/constants/BuildConstants.java | 10 +- .../robot/subsystems/shooter/Shooter.java | 51 +++---- .../subsystems/shooter/ShooterConstants.java | 36 +++-- .../robot/subsystems/shooter/ShooterReal.java | 37 ++--- 7 files changed, 268 insertions(+), 132 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 53d53bb..8a56808 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 384.0, + "x": 896.0, "y": 0.0, "width": 256.0, - "height": 256.0, + "height": 384.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -55,27 +55,11 @@ } ], "containers": [ - { - "title": "MatchTime", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/DriverStation/MatchTime", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, { "title": "RetractedLimit", - "x": 0.0, - "y": 384.0, - "width": 128.0, + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -90,9 +74,9 @@ }, { "title": "Auto Chooser", - "x": 1024.0, - "y": 256.0, - "width": 384.0, + "x": 896.0, + "y": 384.0, + "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -103,9 +87,9 @@ }, { "title": "Roller Percent Output", - "x": 0.0, + "x": 512.0, "y": 256.0, - "width": 256.0, + "width": 384.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -120,8 +104,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 128.0, - "y": 384.0, + "x": 768.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -134,8 +118,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 256.0, - "y": 256.0, + "x": 512.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -151,8 +135,8 @@ }, { "title": "Mode", - "x": 256.0, - "y": 384.0, + "x": 512.0, + "y": 128.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -164,30 +148,49 @@ } }, { - "title": "Time to rev", - "x": 768.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Large Text Display", + "title": "IsActive", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Boolean Box", "properties": { - "topic": "/AdvantageKit/RealOutputs/Time to rev", + "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", "period": 0.06, - "data_type": "double" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Arm angle extended", - "x": 1152.0, - "y": 128.0, - "width": 256.0, + "title": "RemainingInShift", + "x": 0.0, + "y": 256.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Arm angle extended", + "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", "period": 0.06, "data_type": "double", - "show_submit_button": true + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Phase", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "period": 0.06, + "data_type": "string" } } ] @@ -527,8 +530,8 @@ }, { "title": "Shooter Idle max current", - "x": 384.0, - "y": 512.0, + "x": 1024.0, + "y": 384.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -555,8 +558,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 640.0, - "y": 512.0, + "x": 1024.0, + "y": 256.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -575,12 +578,40 @@ "grid_layout": { "layouts": [], "containers": [ + { + "title": "Stalled Motor: ", + "x": 512.0, + "y": 128.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter idle % output", + "x": 128.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shooter idle % output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, { "title": "Auto Chooser", - "x": 0.0, - "y": 0.0, - "width": 640.0, - "height": 768.0, + "x": 512.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, "type": "ComboBox Chooser", "properties": { "topic": "/SmartDashboard/Auto Chooser", diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97c86ba..53e4df3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // ); private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -149,7 +149,7 @@ public class RobotContainer { public RobotContainer() { - configureButtonBindings(); + configureSINGLEBindings(); // Called on first robot enable DeferredBlock.addBlock(() -> { @@ -313,7 +313,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); + m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -350,16 +350,125 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + + } + + + private void configureSINGLEBindings() { + + //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())); + + + + + // TEST-> the driver is holding the left trigger, drive slow and rotation up + // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + // .onTrue(new InstantCommand(() -> { + // m_robotSwerveDrive.setToSlow(); + // m_robotSwerveDrive.shiftUpRot(); + // })) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.setToFast(); + // m_robotSwerveDrive.shiftDownRot(); + // })); + + //TEST - > X positino on wheels + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + })); + + //TEST - > PID positinon + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.indexerStalled(); + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + + })); + + + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + })) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get() + ); + }, m_robotSwerveDrive) + ); + + // D-PAD fine alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + + //allow shooting with right trigger + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotShooter.allowShooting(); + })).onFalse(new InstantCommand(() -> { + m_robotShooter.denyShooting(); + })); + + + + //set shooter ready (rev) with left trigger hold + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + m_robotIntake.rollerStop(); + m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); })); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extending); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); - } + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracting); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); + + } //.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index afbfaa3..7b826a7 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -34,7 +34,7 @@ public class StayInPosition extends PID { //Same here translationY = 0.2; } - if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) { + if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) { driveTranslation = new Translation2d(); } else { //TODO: Tweak for best corrector against another bot @@ -42,8 +42,6 @@ public class StayInPosition extends PID { } drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); - // If above doesn't work - //drive.driveFieldAngle(driveTranslation, targetPose.getRotation()); } @Override diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 0eed6eb..9f111da 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 = 170; - public static final String GIT_SHA = "917aaa774650423095bb33033fe57b1eb1c12a1a"; - public static final String GIT_DATE = "2026-03-19 15:46:28 MDT"; + public static final int GIT_REVISION = 171; + public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7"; + public static final String GIT_DATE = "2026-03-19 18:32:02 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT"; - public static final long BUILD_UNIX_TIME = 1773965797063L; + public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT"; + public static final long BUILD_UNIX_TIME = 1774042307703L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index dd1b78a..52fbcd3 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -13,12 +13,14 @@ 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; import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.TimesNegativeOne; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -34,6 +36,8 @@ public class Shooter extends SubsystemBase { public double distanceToHub = 5; public double chassisXSpeed = 0; + + public Shooter( ShooterIO io, SwerveDrive swerveDrive, @@ -59,15 +63,13 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { Shooting, ManualShoot, - Idle, - IndexerStall + Idle } private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting(double chassisXSpeed) { - this.chassisXSpeed = chassisXSpeed; + public void spinUpShooting() { this.mode = ShooterMode.Shooting; } @@ -87,10 +89,6 @@ public class Shooter extends SubsystemBase { shooterButtonReady = true; } - public void indexerStalled() { - mode = ShooterMode.IndexerStall; - } - public void denyShooting() { shooterButtonReady = false; } @@ -122,26 +120,20 @@ public class Shooter extends SubsystemBase { // Get robot positon and speeds ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI)); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + if (TimesNegativeOne.isRed) { + chassisXSpeed = chassisSpeeds.vxMetersPerSecond; + } else { + chassisXSpeed = -chassisSpeeds.vxMetersPerSecond; + } - // Calculate aim lead - // Get the current speed of the robot - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - // Calculate a point to aim ahead of the actual position. - Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); - + Translation2d fieldPos = robotPose2d.getTranslation(); // Get the robot's aim distance to hub - distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm()); - //Center of hub to cameras in inches + //Center of hub to cameras in meters Logger.recordOutput("Hub Dist", distanceToHub); boolean driverError = @@ -158,14 +150,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); + 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: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); int bitmask = ( (shooterButtonReady ? 1 : 0) + @@ -175,18 +167,18 @@ public class Shooter extends SubsystemBase { switch (bitmask) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, 0); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; - case 0b001: // No errors and shoot button is pressed + case 0b001: // No errors and shoot button is pressed io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, 0); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); break; @@ -253,11 +245,6 @@ public class Shooter extends SubsystemBase { io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - case IndexerStall: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get())); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); - m_robotLED.setMode(Constants.LEDConstants.INDEXER_REVERSE); - break; } io.motorStalled(state, m_Intake, m_robotLED); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 01be9f6..3198f2e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -2,6 +2,8 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; @@ -31,10 +33,14 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); - public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.4); + public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2); public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); + public static final double NEG_OFFSET = 8.; + public static final double POS_OFFSET = 8.; + + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); // Shoot mode tolerances @@ -49,21 +55,23 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { - // Model derived from points - // double speed = - // 1.11576*hubDistMeters*hubDistMeters + - // 0.318464*hubDistMeters + - // 30.6293; - double speed = - 0.0593402*hubDistMeters*hubDistMeters + + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { + double speed = 0; + + if (Math.abs(chassisXSpeed) < 0.1){ + speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + 30.35696 + MODEL_TRIM.get(); - - // double speed = - // 0.00610938*hubDistMeters*hubDistMeters - // 5.65235*hubDistMeters + - // 22.82825; + } else if (chassisXSpeed > 0){ + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get(); + + } else { // Negative is closer to hub + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get(); + } double max = SHOOTER_MAX_VELOCITY.get(); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 8b7c445..f78a10a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -55,21 +55,24 @@ public class ShooterReal implements ShooterIO { } @Override - public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { - if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { - if (!m_shooterStalling) { - m_shooterStalling = true; - m_stallTimerShooter.restart(); - } - if (m_stallTimerShooter.hasElapsed(5.0)) { - m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - motorStall = "Shooter Stalled"; - System.out.println(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))); - } - } else { - m_shooterStalling = false; - m_stallTimerShooter.reset(); - } + public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { + motorStall = ""; + // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { + // if (!m_shooterStalling) { + // m_shooterStalling = true; + // m_stallTimerShooter.restart(); + // } + // if (m_stallTimerShooter.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // motorStall = "Shooter Stalled"; + // System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)))); + // System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))); + // System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond))); + // } + // } else { + // m_shooterStalling = false; + // m_stallTimerShooter.reset(); + // } if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) { if (!m_indexerStalling) { @@ -79,7 +82,7 @@ public class ShooterReal implements ShooterIO { if (m_stallTimerIndexer.hasElapsed(5.0)) { m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); motorStall = "Indexer Stalled"; - System.out.println(Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)); + System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput))); } } else { m_indexerStalling = false; @@ -94,7 +97,7 @@ public class ShooterReal implements ShooterIO { if (m_stallTimerRoller.hasElapsed(5.0)) { m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); motorStall = "Roller Stalled"; - System.out.println(Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())); + System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()))); } } else { m_rollerStalling = false;