From 4ffbe3f5954581f07ac26ee6174113c909cdbaa7 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 19 Mar 2026 18:32:02 -0600 Subject: [PATCH] add testing changes --- .../java/frc4388/robot/RobotContainer.java | 44 +++++----- .../robot/constants/BuildConstants.java | 10 +-- .../robot/subsystems/shooter/Shooter.java | 5 +- .../subsystems/shooter/ShooterConstants.java | 8 +- .../robot/subsystems/shooter/ShooterIO.java | 2 +- .../robot/subsystems/shooter/ShooterReal.java | 85 ++++++++++--------- .../robot/subsystems/swerve/SwerveDrive.java | 10 ++- 7 files changed, 90 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a9a600b..97c86ba 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_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -231,22 +231,25 @@ public class RobotContainer { m_robotSwerveDrive.shiftDownRot(); })); - //TEST - > Defense: X position on wheels and swerve drive pid on position + //TEST - > X positino on wheels new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .whileTrue(new RunCommand(() -> { m_robotSwerveDrive.defenseXPosition(); }, m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> { m_robotSwerveDrive.stopDefenseXPosition(); + })); - // .onTrue(new InstantCommand(() -> { - // currentPose = m_robotSwerveDrive.getCurrentPose(); - // })) - // .whileTrue(new RunCommand(() -> { - // m_stayInPosition.goToTargetPose(currentPose); - // }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.softStop(); + //TEST - > PID positinon + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); })); @@ -254,19 +257,14 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - // () -> m_robotSwerveDrive.driveFacingPosition( - // getDeadbandedDriverController().getLeft(), - // FieldPositions.HUB_POSITION, - // ShooterConstants.AIM_LEAD_TIME.get() - // ), m_robotSwerveDrive) - () -> { - m_robotSwerveDrive.driveFacingVelocity( + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + })) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get(), - m_robotShooter.getBallVelocity(), - m_robotShooter.getDistanceToHub() + ShooterConstants.AIM_LEAD_TIME.get() ); }, m_robotSwerveDrive) ); @@ -315,7 +313,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(); + m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -352,7 +350,7 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.indexerStalled(); })) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b8c41f5..0eed6eb 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 = 167; - public static final String GIT_SHA = "85fe11c8bdd51d8671e98047f88a2d42276f7094"; - public static final String GIT_DATE = "2026-03-18 13:39:27 MDT"; + 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 String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-18 16:02:33 MDT"; - public static final long BUILD_UNIX_TIME = 1773871353359L; + public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT"; + public static final long BUILD_UNIX_TIME = 1773965797063L; 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 e0d32a8..dd1b78a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -32,7 +32,7 @@ public class Shooter extends SubsystemBase { // Supplier m_swervePoseSupplier; public boolean badShooterVelocity; public double distanceToHub = 5; - + public double chassisXSpeed = 0; public Shooter( ShooterIO io, @@ -66,7 +66,8 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting() { + public void spinUpShooting(double chassisXSpeed) { + this.chassisXSpeed = chassisXSpeed; this.mode = ShooterMode.Shooting; } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 4ef195a..01be9f6 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -35,7 +35,7 @@ public class ShooterConstants { public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); - public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); @@ -82,8 +82,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.05) - .withKI(0.20) + .withKP(0.02) + .withKI(0.15) .withKD(0.0); @@ -91,7 +91,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index b30bd2d..96cbd19 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -21,7 +21,7 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(40); // For modeling purposes + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); double indexerTargetOutput = 0; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 3f46ae0..8b7c445 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -29,6 +30,8 @@ public class ShooterReal implements ShooterIO { private boolean m_shooterStalling = false; private boolean m_indexerStalling = false; private boolean m_rollerStalling = false; + public String motorStall = ""; + public ShooterReal( TalonFX shooter1Motor, @@ -52,46 +55,52 @@ 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)) > 20) { - // if (!m_shooterStalling) { - // m_shooterStalling = true; - // m_stallTimerShooter.restart(); - // } - // if (m_stallTimerShooter.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // } - // } else { - // m_shooterStalling = false; - // m_stallTimerShooter.stop(); - // } + 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(); + } - // if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.2) { - // if (!m_indexerStalling) { - // m_indexerStalling = true; - // m_stallTimerIndexer.restart(); - // } - // if (m_stallTimerIndexer.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // } - // } else { - // m_indexerStalling = false; - // m_stallTimerIndexer.stop(); - // } + if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) { + if (!m_indexerStalling) { + m_indexerStalling = true; + m_stallTimerIndexer.restart(); + } + 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)); + } + } else { + m_indexerStalling = false; + m_stallTimerIndexer.reset(); + } - // if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.2) { - // if (!m_rollerStalling) { - // m_rollerStalling = true; - // m_stallTimerRoller.restart(); - // } - // if (m_stallTimerRoller.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // } - // } else { - // m_rollerStalling = false; - // m_stallTimerRoller.stop(); - // } + if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) { + if (!m_rollerStalling) { + m_rollerStalling = true; + m_stallTimerRoller.restart(); + } + 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())); + } + } else { + m_rollerStalling = false; + m_stallTimerRoller.reset(); + } + Logger.recordOutput("Stalled Motor: ", motorStall); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 7e3ce17..4ebbf14 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -166,6 +166,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // module.setDesiredState(state); // } + public double chassisXSpeeds(){ + if (TimesNegativeOne.isRed) { + return chassisSpeeds.vxMetersPerSecond; + } else { + return -chassisSpeeds.vxMetersPerSecond; + } + } + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve @@ -412,7 +420,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Simple", aimOffset); + Logger.recordOutput("Offset Value", aimOffset); }