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

Merge branch 'DenverAutos' into New-Intake

Browse Source
This commit is contained in:
Michael Mikovsky
2026-04-04 11:16:38 -06:00
committed by GitHub
parent d40cf208e8 0a2559bb4b
commit 6d55c96f6a
38 changed files with 2003 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+29 -16
View File
@@ -81,7 +81,7 @@ public class RobotContainer {
//Testing of Colors
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive);
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
@@ -109,7 +109,7 @@ public class RobotContainer {
private Command IntakeExtended = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME), m_robotIntake)
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake)
);
// private Command LidarIntake = new SequentialCommandGroup(
@@ -146,7 +146,7 @@ public class RobotContainer {
);
private Command IntakeRetracted = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RetractedREMOVEME), m_robotIntake)
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake)
);
private Command RobotShoot = new SequentialCommandGroup(
@@ -195,17 +195,17 @@ public class RobotContainer {
NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> {
if (TimesNegativeOne.isRed) {
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED_FRONT);
} else {
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE_FRONT);
}
}));
NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> {
if (!TimesNegativeOne.isRed) {
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
NamedCommands.registerCommand("BumpOffsetBackward", new InstantCommand(() -> {
if (TimesNegativeOne.isRed) {
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED_BACK);
} else {
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE_BACK);
}
}));
@@ -304,8 +304,8 @@ public class RobotContainer {
FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get()
);
}, m_robotSwerveDrive)
);
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
// D-PAD fine alignment
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
@@ -423,6 +423,18 @@ public class RobotContainer {
// m_robotIntake.setMode(IntakeMode.Idle);
// }));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
// .whileTrue(
// new PathPlannerAuto("Right_AutoClimb")
// )
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
// .whileTrue(
// new PathPlannerAuto("Left_AutoClimb")
// )
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> {
// m_robotClimber.toggleDeployed();
@@ -435,11 +447,11 @@ public class RobotContainer {
private void configureSINGLEBindings() {
//Driver controls
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
@@ -596,7 +608,8 @@ public class RobotContainer {
for (String auto : autos) {
if (auto.endsWith(".auto"))
autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
if (auto.startsWith("X. "))
autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
// System.out.println(auto);
}
src/main/java/frc4388/robot/constants/FieldConstants.java
+13 -2
View File
@@ -11,17 +11,28 @@ public final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
public static final double BUMP_OFFSET = 0.4;
public static final Transform2d BUMP_OFFSET_RED = new Transform2d(
public static final Transform2d BUMP_OFFSET_RED_FRONT = new Transform2d(
Meters.of(BUMP_OFFSET),
Meters.of(0),
new Rotation2d()
);
public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d(
public static final Transform2d BUMP_OFFSET_BLUE_FRONT = new Transform2d(
Meters.of(-BUMP_OFFSET),
Meters.of(0),
new Rotation2d()
);
public static final Transform2d BUMP_OFFSET_RED_BACK = new Transform2d(
Meters.of(-BUMP_OFFSET),
Meters.of(0),
new Rotation2d()
);
public static final Transform2d BUMP_OFFSET_BLUE_BACK = new Transform2d(
Meters.of(BUMP_OFFSET),
Meters.of(0),
new Rotation2d()
);
// Test april tag field layout
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
// Arrays.asList(new AprilTag[] {
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+9 -5
View File
@@ -2,7 +2,6 @@ package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.function.Supplier;
@@ -11,19 +10,23 @@ 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;
public class Intake extends SubsystemBase {
public IntakeIO io;
IntakeStateAutoLogged state = new IntakeStateAutoLogged();
SwerveDrive m_SwerveDrive;
Supplier<Pose2d> m_swervePoseSupplier;
public Intake(
IntakeIO io
IntakeIO io,
SwerveDrive m_SwerveDrive
// Supplier<Pose2d> swervePoseSupplier
) {
this.io = io;
this.m_SwerveDrive = m_SwerveDrive;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
@@ -93,8 +96,9 @@ public class Intake extends SubsystemBase {
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
// System.out.println(m_armLimitSwitch.get());
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
Logger.processInputs("Intake", state);
Logger.recordOutput("Intake/IntakeState", this.mode);
@@ -126,7 +130,7 @@ public class Intake extends SubsystemBase {
case ExtendingRolling:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants. ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
break;
case Retracting:
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+9 -3
View File
@@ -6,6 +6,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.units.measure.AngularVelocity;
import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
import com.revrobotics.spark.config.LimitSwitchConfig.Type;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -59,8 +62,9 @@ public class IntakeConstants {
public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .20);
public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .20);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50);
public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40);
public static final ConfigurableDouble ROLLER_MULTIPLIER_CONST = new ConfigurableDouble("Roller Multiplier Constant", 0.4);
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
@@ -79,7 +83,9 @@ public class IntakeConstants {
// public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
// public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
public static double getTargetRollerSpeed(double chassisSpeed) {
return ROLLER_PERCENT_OUTPUT.get() * (1 + ROLLER_MULTIPLIER_CONST.get() * chassisSpeed);
}
// 0 is paralell to the ground, 90 is directly up