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

auto align wackiness

Browse Source
This commit is contained in:
Shikhar
2026-02-24 22:55:28 -07:00
parent 0af963284a
commit 826a87371c
3 changed files with 24 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+2 -6
View File
@@ -11,9 +11,7 @@ import java.io.File;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
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.DriverStation;
@@ -29,7 +27,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
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;
@@ -41,7 +38,6 @@ import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.shooter.Shooter;
import frc4388.robot.subsystems.shooter.ShooterConstants;
import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.DeferredBlock;
@@ -124,8 +120,8 @@ public class RobotContainer {
);
private Command RobotShoot = new SequentialCommandGroup(
new InstantCommand(() ->
new AutoAlign(m_robotSwerveDrive, m_vision, true)),
// TEST NEW AUTO ALIGN
//new AutoAlign(m_robotSwerveDrive, m_vision),
new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter),
new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
new WaitCommand(3),
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+20 -62
View File
@@ -14,101 +14,59 @@ 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 PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive;
Vision vision;
boolean waitVelocityZero;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) {
public AutoAlign(SwerveDrive swerveDrive, Vision vision) {
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();
rotPID.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;
}
Pose2d robotPose = vision.getPose2d();
if (robotPose == null) return;
double dx = targetpos.getX() - robotPose.getX();
double dy = targetpos.getY() - robotPose.getY();
Rotation2d desiredHeading = new Rotation2d(Math.atan2(dy, dx));
roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
if (roterr > 180) roterr -= 360;
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());
}
swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
}
@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)
boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE;
if (finished) {
swerveDrive.softStop();
}
return finished;
}
private class PID {
protected Gains gains;
private double output = 0;
src/main/java/frc4388/robot/constants/Constants.java
+2 -2
View File
@@ -47,7 +47,7 @@ public final class Constants {
public static final class AutoConstants {
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
public static final Gains XY_GAINS = new Gains(8,0,0.0);
public static final Gains ROT_GAINS = new Gains(8,0,0.0);
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
@@ -61,7 +61,7 @@ public final class Constants {
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
public static final double ROT_TOLERANCE = 10; // Degrees
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;