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

Greatly Improved auto alignment.

Browse Source
This commit is contained in:
C4llSiqn
2025-03-07 21:46:25 -07:00
parent 5ae62c89e5
commit 63808a652e
6 changed files with 55 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -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.2, 0.1); // TODO: TEST
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1);
}
public static final class Configurations {
@@ -314,6 +314,8 @@ public final class Constants {
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;
public static final double VELOCITY_THRESHHOLD = 0.005;
// X is tangent to reef side
// Y is normal to reef side
@@ -324,10 +326,10 @@ public final class Constants {
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(15);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5);
public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
@@ -343,7 +345,7 @@ public final class Constants {
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547+(1*-1)), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
src/main/java/frc4388/robot/RobotContainer.java
+32 -17
View File
@@ -124,6 +124,8 @@ public class RobotContainer {
private Command autoCommand;
private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
// private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
private Command waitDebuger = new waitSupplier(() -> true);
// private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
// private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
@@ -136,7 +138,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -145,8 +147,10 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -170,7 +174,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -179,9 +183,10 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -207,14 +212,16 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL3Primed()),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -230,7 +237,7 @@ public class RobotContainer {
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
),() -> m_robotElevator.isL3Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
@@ -239,9 +246,11 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -255,10 +264,13 @@ public class RobotContainer {
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
waitDebuger.asProxy(),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -271,10 +283,13 @@ public class RobotContainer {
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
waitDebuger.asProxy(),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -289,7 +304,7 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
@@ -303,7 +318,7 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
@@ -482,8 +497,8 @@ public class RobotContainer {
), m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(thrustIntake.asProxy());
src/main/java/frc4388/robot/commands/GotoLastApril.java
+5 -2
View File
@@ -38,17 +38,19 @@ public class GotoLastApril extends Command {
Vision vision;
double distance;
Side side;
boolean waitVelocityZero;
/**
* Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.distance = distance;
this.side = side;
this.waitVelocityZero = waitVelocityZero;
// addRequirements(swerveDrive);
}
@@ -138,7 +140,8 @@ public class GotoLastApril extends Command {
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)
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
);
// System.out.println(finished);
src/main/java/frc4388/robot/commands/LidarAlign.java
+1 -1
View File
@@ -97,7 +97,7 @@ public class LidarAlign extends Command {
currentFinderTick = 0;
foundReef = false;
return false;
} else if (bounces >= 2) {
} else if (bounces >= 3) {
swerveDrive.stopModules();
return true;
} else {
src/main/java/frc4388/robot/subsystems/Elevator.java
+7 -7
View File
@@ -297,17 +297,17 @@ public class Elevator extends Subsystem {
// double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
// double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
// double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
// double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
}
// if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
// PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
// }
// This method will be called once per scheduler run
SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
// SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
// SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+4 -1
View File
@@ -429,8 +429,11 @@ public class SwerveDrive extends Subsystem {
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
if(curPose.isPresent() && lastPose.isPresent())
if(curPose.isPresent() && lastPose.isPresent()){
this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
SmartDashboard.putNumber("Speed", lastOdomSpeed);
}
}