Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

add testing changes

Browse Source
This commit is contained in:
mimigamin
2026-03-19 18:32:02 -06:00
parent 917aaa7746
commit 4ffbe3f595
7 changed files with 90 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+21 -23
View File
@@ -125,7 +125,7 @@ public class RobotContainer {
// ); // );
private Command RobotRev = new SequentialCommandGroup( private Command RobotRev = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter),
IntakeExtended, IntakeExtended,
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
); );
@@ -231,22 +231,25 @@ public class RobotContainer {
m_robotSwerveDrive.shiftDownRot(); 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) new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.whileTrue(new RunCommand(() -> { .whileTrue(new RunCommand(() -> {
m_robotSwerveDrive.defenseXPosition(); m_robotSwerveDrive.defenseXPosition();
}, m_robotSwerveDrive)) }, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> { .onFalse(new InstantCommand(() -> {
m_robotSwerveDrive.stopDefenseXPosition(); m_robotSwerveDrive.stopDefenseXPosition();
}));
// .onTrue(new InstantCommand(() -> { //TEST - > PID positinon
// currentPose = m_robotSwerveDrive.getCurrentPose(); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// })) .onTrue(new InstantCommand(() -> {
// .whileTrue(new RunCommand(() -> { currentPose = m_robotSwerveDrive.getCurrentPose();
// m_stayInPosition.goToTargetPose(currentPose); }))
// }, m_robotSwerveDrive)) .whileTrue(new RunCommand(() -> {
// .onFalse(new InstantCommand(() -> { m_stayInPosition.goToTargetPose(currentPose);
// m_robotSwerveDrive.softStop(); }, 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 // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.whileTrue(new RunCommand( .onTrue(new InstantCommand(() -> {
// () -> m_robotSwerveDrive.driveFacingPosition( m_robotSwerveDrive.setToSlow();
// getDeadbandedDriverController().getLeft(), }))
// FieldPositions.HUB_POSITION, .whileTrue(new RunCommand(() -> {
// ShooterConstants.AIM_LEAD_TIME.get() m_robotSwerveDrive.driveFacingPosition(
// ), m_robotSwerveDrive)
() -> {
m_robotSwerveDrive.driveFacingVelocity(
getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION, FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get(), ShooterConstants.AIM_LEAD_TIME.get()
m_robotShooter.getBallVelocity(),
m_robotShooter.getDistanceToHub()
); );
}, m_robotSwerveDrive) }, m_robotSwerveDrive)
); );
@@ -315,7 +313,7 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.setMode(IntakeMode.Idle);
m_robotIntake.rollerStop(); m_robotIntake.rollerStop();
m_robotShooter.spinUpShooting(); m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds());
})) }))
.onFalse(new InstantCommand(() -> { .onFalse(new InstantCommand(() -> {
m_robotShooter.spinUpIdle(); m_robotShooter.spinUpIdle();
@@ -352,7 +350,7 @@ public class RobotContainer {
// m_robotClimber.toggleDeployed(); // m_robotClimber.toggleDeployed();
// })); // }));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotShooter.indexerStalled(); m_robotShooter.indexerStalled();
})) }))
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = ""; public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified"; public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 167; public static final int GIT_REVISION = 170;
public static final String GIT_SHA = "85fe11c8bdd51d8671e98047f88a2d42276f7094"; public static final String GIT_SHA = "917aaa774650423095bb33033fe57b1eb1c12a1a";
public static final String GIT_DATE = "2026-03-18 13:39:27 MDT"; public static final String GIT_DATE = "2026-03-19 15:46:28 MDT";
public static final String GIT_BRANCH = "MiraOrg"; public static final String GIT_BRANCH = "MiraOrg";
public static final String BUILD_DATE = "2026-03-18 16:02:33 MDT"; public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT";
public static final long BUILD_UNIX_TIME = 1773871353359L; public static final long BUILD_UNIX_TIME = 1773965797063L;
public static final int DIRTY = 1; public static final int DIRTY = 1;
private BuildConstants(){} private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+3 -2
View File
@@ -32,7 +32,7 @@ public class Shooter extends SubsystemBase {
// Supplier<Pose2d> m_swervePoseSupplier; // Supplier<Pose2d> m_swervePoseSupplier;
public boolean badShooterVelocity; public boolean badShooterVelocity;
public double distanceToHub = 5; public double distanceToHub = 5;
public double chassisXSpeed = 0;
public Shooter( public Shooter(
ShooterIO io, ShooterIO io,
@@ -66,7 +66,8 @@ public class Shooter extends SubsystemBase {
private ShooterMode mode = ShooterMode.Idle; private ShooterMode mode = ShooterMode.Idle;
private boolean shooterButtonReady = false; private boolean shooterButtonReady = false;
public void spinUpShooting() { public void spinUpShooting(double chassisXSpeed) {
this.chassisXSpeed = chassisXSpeed;
this.mode = ShooterMode.Shooting; this.mode = ShooterMode.Shooting;
} }
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+4 -4
View File
@@ -35,7 +35,7 @@ public class ShooterConstants {
public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); 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 // Shoot mode tolerances
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
@@ -82,8 +82,8 @@ public class ShooterConstants {
// Motor Configuration // Motor Configuration
public static Slot0Configs SHOOTER_PID = new Slot0Configs() public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0) .withKV(0.0)
.withKP(0.05) .withKP(0.02)
.withKI(0.20) .withKI(0.15)
.withKD(0.0); .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_kI = new ConfigurableDouble("Shooter KI", 0.15);
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+1 -1
View File
@@ -21,7 +21,7 @@ public interface ShooterIO {
// Angle shooterTargetPitch = Rotations.of(0); // Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.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); AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0);
double indexerTargetOutput = 0; double indexerTargetOutput = 0;
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+47 -38
View File
@@ -3,6 +3,7 @@ package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
@@ -29,6 +30,8 @@ public class ShooterReal implements ShooterIO {
private boolean m_shooterStalling = false; private boolean m_shooterStalling = false;
private boolean m_indexerStalling = false; private boolean m_indexerStalling = false;
private boolean m_rollerStalling = false; private boolean m_rollerStalling = false;
public String motorStall = "";
public ShooterReal( public ShooterReal(
TalonFX shooter1Motor, TalonFX shooter1Motor,
@@ -52,46 +55,52 @@ public class ShooterReal implements ShooterIO {
} }
@Override @Override
public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { 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 (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 20) { if (!m_shooterStalling) {
// if (!m_shooterStalling) { m_shooterStalling = true;
// m_shooterStalling = true; m_stallTimerShooter.restart();
// m_stallTimerShooter.restart(); }
// } if (m_stallTimerShooter.hasElapsed(5.0)) {
// if (m_stallTimerShooter.hasElapsed(5.0)) { m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
// 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; } else {
// m_stallTimerShooter.stop(); m_shooterStalling = false;
// } m_stallTimerShooter.reset();
}
// if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.2) { if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) {
// if (!m_indexerStalling) { if (!m_indexerStalling) {
// m_indexerStalling = true; m_indexerStalling = true;
// m_stallTimerIndexer.restart(); m_stallTimerIndexer.restart();
// } }
// if (m_stallTimerIndexer.hasElapsed(5.0)) { if (m_stallTimerIndexer.hasElapsed(5.0)) {
// m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
// } motorStall = "Indexer Stalled";
// } else { System.out.println(Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput));
// m_indexerStalling = false; }
// m_stallTimerIndexer.stop(); } else {
// } m_indexerStalling = false;
m_stallTimerIndexer.reset();
}
// if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.2) { if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) {
// if (!m_rollerStalling) { if (!m_rollerStalling) {
// m_rollerStalling = true; m_rollerStalling = true;
// m_stallTimerRoller.restart(); m_stallTimerRoller.restart();
// } }
// if (m_stallTimerRoller.hasElapsed(5.0)) { if (m_stallTimerRoller.hasElapsed(5.0)) {
// m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
// } motorStall = "Roller Stalled";
// } else { System.out.println(Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()));
// m_rollerStalling = false; }
// m_stallTimerRoller.stop(); } else {
// } m_rollerStalling = false;
m_stallTimerRoller.reset();
}
Logger.recordOutput("Stalled Motor: ", motorStall);
} }
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+9 -1
View File
@@ -166,6 +166,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
// module.setDesiredState(state); // 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) { 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: 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 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){ if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
Logger.recordOutput("Offset Simple", aimOffset); Logger.recordOutput("Offset Value", aimOffset);
} }