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

Debug autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-25 14:47:23 -07:00
parent a7ac44c25d
commit 1b9b7ed00c
23 changed files with 272 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -7
View File
@@ -230,7 +230,7 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(3,0,0);
public static final Gains XY_GAINS = new Gains(3,0.01,0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
@@ -240,7 +240,7 @@ public final class Constants {
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
public static final double XY_TOLERANCE = 0.05;
public static final double XY_TOLERANCE = 0.1;
public static final double ROT_TOLERANCE = 1;
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
@@ -249,8 +249,8 @@ public final class Constants {
public static final class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help.
public static final double CLOSED_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help.
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help.
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help.
public static final double NEUTRAL_DEADBAND = 0.04;
// POWER! (limiting)
@@ -282,7 +282,7 @@ public final class Constants {
// new ClosedLoopRampsConfigs()
// .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
);
private static final double SLIP_CURRENT = 100; // TODO: Tune???
private static final double SLIP_CURRENT = 20; // TODO: Tune???
}
// No mans land
@@ -343,7 +343,7 @@ public final class Constants {
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
public static final double MIN_ESTIMATION_DISTANCE = 0; // Meters
// Photonvision thing
// The standard deviations of our vision estimated poses, which affect correction rate
@@ -354,7 +354,8 @@ public final class Constants {
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
public static final double HORISONTAL_SCORING_POSITION_OFFSET = 0;
public static final double HORISONTAL_SCORING_POSITION_OFFSET = .2794
;
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
src/main/java/frc4388/robot/Robot.java
+7
View File
@@ -143,13 +143,20 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.stop();
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().cancel(m_autonomousCommand);
m_autonomousCommand.cancel();
m_autonomousCommand.end(true);
System.out.println("NOT Null!!");
} else {
System.out.println("Null!!");
}
m_robotTime.startMatchTime();
}
src/main/java/frc4388/robot/RobotContainer.java
+13 -6
View File
@@ -92,6 +92,10 @@ public class RobotContainer {
// public List<Subsystem> subsystems = new ArrayList<>();
// ! Teleop Commands
public void stop() {
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;;
m_robotSwerveDrive.stopModules();
}
// ! /* Autos */
private String lastAutoName = "defualt.auto";
@@ -104,8 +108,10 @@ public class RobotContainer {
private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision);
private Command placeCoral = new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
new InstantCommand(() -> System.out.println("Placing Some Coral")),
new WaitCommand(3)
new WaitCommand(3),
new InstantCommand(() -> System.out.println("Done placing Some Coral"))
);
private Command aprilAlign = new SequentialCommandGroup(
@@ -115,7 +121,8 @@ public class RobotContainer {
private Command grabCoral = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Yoinking some coral...")),
new WaitCommand(2)
new WaitCommand(2),
new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
);
/**
@@ -210,10 +217,10 @@ public class RobotContainer {
// ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
.onTrue(AutoGotoPosition);
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
.onTrue(new InstantCommand(()->{}, m_robotSwerveDrive));
// creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time
// ? /* Programer Buttons (Controller 3)*/
@@ -300,10 +307,10 @@ public class RobotContainer {
//return autoPlayback;
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
return autoChooser.getSelected();
return autoChooser.getSelected().andThen(new InstantCommand(() -> System.err.println("Auto Ded!")));
// try{
// // Load the path you want to follow using its name in the GUI
// return new PathPlannerAuto("New Auto");
// return new PathPlannerAuto("New Auto");
// } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
// return Commands.none();
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+5 -3
View File
@@ -37,14 +37,14 @@ public class GotoPositionCommand extends Command {
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
this.swerveDrive = swerveDrive;
this.vision = vision;
addRequirements(swerveDrive);
// addRequirements(swerveDrive);
}
@Override
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get());
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
}
double xerr;
@@ -84,7 +84,9 @@ public class GotoPositionCommand extends Command {
@Override
public final boolean isFinished() {
return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
// System.out.println(finished);
return finished;
// this statement is a boolean in and of itself
}