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

Work on Autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-05 19:42:36 -07:00
parent aeedebd095
commit a6daadb537
15 changed files with 191 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -193,7 +193,7 @@ public final class Constants {
.withKS(0).withKV(0.124);
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0.2, 0.1); // TODO: TEST
}
public static final class Configurations {
@@ -294,7 +294,7 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(5,0.4,0.0);
public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
@@ -316,10 +316,10 @@ public final class Constants {
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
src/main/java/frc4388/robot/RobotContainer.java
+20 -13
View File
@@ -317,12 +317,19 @@ public class RobotContainer {
private Boolean operatorManualMode = false;
public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
@@ -334,15 +341,15 @@ public class RobotContainer {
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0, 1),
new Translation2d(), 400, true
new Translation2d(), 300, true
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> {
m_robotElevator.transitionState(CoordinationState.PrimedFour);
src/main/java/frc4388/robot/commands/GotoLastApril.java
+7 -6
View File
@@ -95,12 +95,13 @@ public class GotoLastApril extends Command {
roterr += 360;
}
SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID error", roterr);
// SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
// SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
// SmartDashboard.putNumber("Rotational PID error", roterr);
// SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr);
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);
@@ -128,7 +129,7 @@ public class GotoLastApril extends Command {
.rotateBy(targetpos.getRotation())
.getCos());
swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation());
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
}
src/main/java/frc4388/robot/commands/LidarAlign.java
+1 -1
View File
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.headedRight = (GotoLastApril.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
src/main/java/frc4388/robot/commands/WhileTrueCommand.java
+6 -4
View File
@@ -34,24 +34,26 @@ public class WhileTrueCommand extends Command {
*/
@SuppressWarnings("this-escape")
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand");
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
CommandScheduler.getInstance().registerComposedCommands(whileTrue);
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
addRequirements(whileTrue.getRequirements());
// addRequirements(whileTrue.getRequirements());
}
@Override
public void initialize() {
if(m_condition.getAsBoolean())
m_whileTrue.initialize();
m_whileTrue.initialize();
}
@Override
public void execute() {
m_whileTrue.execute();
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
if(!m_whileTrue.isFinished())
return;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -2
View File
@@ -141,7 +141,7 @@ public class SwerveDrive extends Subsystem {
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
stopped = false;
if (fieldRelative) {
@@ -213,13 +213,28 @@ public class SwerveDrive extends Subsystem {
.withTargetDirection(rightStick.getAngle()));
}
public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
leftStick = leftStick.rotateBy(heading);
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,