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

Fixing the shooter command and adding alignment

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-24 18:43:16 -08:00
parent 34a7ed050d
commit 0af963284a
7 changed files with 174 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+18 -26
View File
@@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.alignment.AutoAlign;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
@@ -98,7 +99,7 @@ public class RobotContainer {
private Command RobotIntakeDown = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended))
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
);
private Command LidarIntake = new SequentialCommandGroup(
@@ -117,34 +118,24 @@ public class RobotContainer {
.andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
);
private Command RobotShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted)),
new InstantCommand(() -> m_robotShooter.setShooterReady()),
new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
new WaitCommand(3),
new InstantCommand(()->m_robotShooter.setShooterShoot()),
new WaitCommand(5),
new InstantCommand(()->m_robotShooter.setShooterNOTShoot()),
new InstantCommand(() -> m_robotShooter.setShooterNotReady())
private Command RobotReadyToShoot = new SequentialCommandGroup(
new InstantCommand(()-> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake),
new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter)
);
// private Command RobotShoot = new SequentialCommandGroup(
// new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
// new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED),
// new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
// new WaitCommand(5),
// new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED),
// new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
// );
// private Command RobotShoot = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
// );
// private Command RobotIntake = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
// );
private Command RobotShoot = new SequentialCommandGroup(
new InstantCommand(() ->
new AutoAlign(m_robotSwerveDrive, m_vision, true)),
new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter),
new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
new WaitCommand(3),
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake),
new WaitCommand(2),
new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter),
new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter)
);
public RobotContainer() {
configureButtonBindings();
@@ -163,6 +154,7 @@ public class RobotContainer {
m_robotShooter.io.updateGains();
}, true);
NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot);
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
NamedCommands.registerCommand("Lidar Intake", LidarIntake);
NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown);
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+143
View File
@@ -0,0 +1,143 @@
package frc4388.robot.commands.alignment;
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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.FieldPositions;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.structs.Gains;
public class AutoAlign extends Command {
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive;
Vision vision;
boolean waitVelocityZero;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.waitVelocityZero = waitVelocityZero && false;
addRequirements(swerveDrive);
}
public static double tagRelativeXError = -1;
private static void setTagRelativeXError(double val){
tagRelativeXError = val;
}
@Override
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0));
}
double xerr;
double yerr;
double roterr;
double xoutput;
double youtput;
double rotoutput;
@Override
public void execute() {
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
if(roterr > 180){
roterr -= 360;
}else if(roterr < -180){
roterr += 360;
}
SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr);
SmartDashboard.putNumber("PID Rot Error", roterr);
xoutput = xPID.update(xerr);
youtput = yPID.update(yerr);
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1)
);
setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation())
.getCos());
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
}
@Override
public final boolean isFinished() {
boolean finished = (
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
);
if(finished)
swerveDrive.softStop();
return finished;
}
private class PID {
protected Gains gains;
private double output = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(Gains gains, double tolerance) {
this.gains = gains;
}
// Called when the command is initially scheduled.
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
public double update(double error) {
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
return output;
}
}
}
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_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 86;
public static final String GIT_SHA = "b3f3ae444324c4d339e4aa5dc59fe75bd3d56558";
public static final String GIT_DATE = "2026-02-23 23:28:25 MST";
public static final int GIT_REVISION = 87;
public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01";
public static final String GIT_DATE = "2026-02-24 17:49:26 MST";
public static final String GIT_BRANCH = "AutoTesting";
public static final String BUILD_DATE = "2026-02-24 17:44:43 MST";
public static final long BUILD_UNIX_TIME = 1771980283653L;
public static final String BUILD_DATE = "2026-02-24 18:48:38 MST";
public static final long BUILD_UNIX_TIME = 1771984118729L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+1
View File
@@ -69,6 +69,7 @@ public class Shooter extends SubsystemBase {
this.mode = ShooterMode.Ready;
}
public void setShooterReadyFeeder() {
this.mode = ShooterMode.ReadyFeeder;
}