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

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/deploy/pathplanner/autos/Stupid auto.auto
+31
View File
@@ -0,0 +1,31 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Stupid auto 1"
}
},
{
"type": "path",
"data": {
"pathName": "Stupid auto 2"
}
},
{
"type": "path",
"data": {
"pathName": "Stupid auto 3"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -122.12499844038754 "rotation": 60.35013649242443
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 178.97696981133208 "rotation": 0.0
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 59.82647997035557 "rotation": -121.27770286686648
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 54.32359177813805 "rotation": -126.57303097851931
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 121.75948008481289 "rotation": -57.4259428654274
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 179.30130561701642 "rotation": -0.9548412538721401
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 54.0394828033551 "rotation": -123.50343698241421
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 119.16761337957772 "rotation": -59.036243467926624
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Barge to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 179.65844947344297 "rotation": 0.5456575934157175
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 0.0
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path
+6 -6
View File
@@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 5.197222222222222, "x": 5.268371212121212,
"y": 5.52929292929293 "y": 5.417487373737374
}, },
"prevControl": { "prevControl": {
"x": 6.234912180385907, "x": 6.306061170284897,
"y": 5.781923450986739 "y": 5.670117895431184
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -121.60750224624891 "rotation": 60.851928154286895
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 179.44374777291927 "rotation": 1.1457628387483283
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 118.92642583525368 "rotation": -62.35402463626126
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 1.97493401088202
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 50.964487101253106 "rotation": -124.91183021661
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -178.99491399474581 "rotation": -1.6365770416167795
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Center Reef to Feed.path
+3 -3
View File
@@ -25,7 +25,7 @@
}, },
"nextControl": { "nextControl": {
"x": 3.388005050505051, "x": 3.388005050505051,
"y": 6.11881313131313 "y": 6.1188131313131295
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -58,13 +58,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -52.30575953331084 "rotation": 126.3843518158359
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -177.95459151111288 "rotation": 0.0
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Stupid auto 1.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.081502525252525,
"y": 7.267361111111111
},
"prevControl": null,
"nextControl": {
"x": 4.871969696969696,
"y": 8.070328282828283
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.081502525252525,
"y": 5.10239898989899
},
"prevControl": {
"x": 4.821148989898989,
"y": 5.10239898989899
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Stupid auto 2.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.101830808080808,
"y": 5.041414141414141
},
"prevControl": null,
"nextControl": {
"x": 7.10183080808081,
"y": 5.041414141414141
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.585795454545455,
"y": 6.149305555555555
},
"prevControl": {
"x": 8.175315656565658,
"y": 5.295517676767678
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Stupid auto 3.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.585795454545455,
"y": 6.200126262626262
},
"prevControl": null,
"nextControl": {
"x": 7.778914141414141,
"y": 7.033585858585859
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.081502525252525,
"y": 7.32834595959596
},
"prevControl": {
"x": 7.595959595959596,
"y": 7.724747474747475
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Taxi.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 179.47479945510887 "rotation": -1.9251837083231407
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 177.6140559696112 "rotation": -1.1306909512379957
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -120.0183674276091 "rotation": 60.255118703057796
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 179.30972280213487 "rotation": -2.223961240385466
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Feed to Reef.path
+10 -10
View File
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.263699494949495, "x": 1.0807449494949497,
"y": 7.135227272727272 "y": 6.972601010101011
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 1.6862557056350642, "x": 2.0869949494949496,
"y": 6.8478211564289 "y": 6.291603535353536
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 3.6217803030303033, "x": 3.764078282828282,
"y": 5.417487373737374 "y": 5.24469696969697
}, },
"prevControl": { "prevControl": {
"x": 3.111746968518841, "x": 2.9836492542425197,
"y": 5.776730048021274 "y": 5.685899280282051
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -60.228308784269046 "rotation": 121.34521414171566
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -55.124671655397854 "rotation": 124.23611780915067
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path
+2 -2
View File
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 120.25643716352924 "rotation": -54.659893078442295
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -179.06080905426438 "rotation": 0.0
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Top Reef to Feed.path
+6 -6
View File
@@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 1.192550505050505, "x": 1.2230429292929292,
"y": 7.0234217171717175 "y": 7.003093434343434
}, },
"prevControl": { "prevControl": {
"x": 0.9300747161944047, "x": 0.960567140436829,
"y": 7.106516384157039 "y": 7.086188101328755
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -52.56142842766699 "rotation": 126.86989764584399
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -121.75948008481281 "rotation": 57.21571913413089
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/settings.json
+1 -1
View File
@@ -16,7 +16,7 @@
"driveGearing": 5.143, "driveGearing": 5.143,
"maxDriveSpeed": 5.45, "maxDriveSpeed": 5.45,
"driveMotorType": "krakenX60", "driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0, "driveCurrentLimit": 40.0,
"wheelCOF": 1.2, "wheelCOF": 1.2,
"flModuleX": 0.273, "flModuleX": 0.273,
"flModuleY": 0.273, "flModuleY": 0.273,
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 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 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); 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 LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000; 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 double ROT_TOLERANCE = 1;
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); // 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 class Configurations {
public static final double OPEN_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.0; // 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; public static final double NEUTRAL_DEADBAND = 0.04;
// POWER! (limiting) // POWER! (limiting)
@@ -282,7 +282,7 @@ public final class Constants {
// new ClosedLoopRampsConfigs() // new ClosedLoopRampsConfigs()
// .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) // .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 // 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 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 // Photonvision thing
// The standard deviations of our vision estimated poses, which affect correction rate // 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 class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); 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 HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); 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 @Override
public void teleopInit() { public void teleopInit() {
m_robotContainer.stop();
// This makes sure that the autonomous stops running when // This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to // teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove // continue until interrupted by another command, remove
// this line or comment it out. // this line or comment it out.
if (m_autonomousCommand != null) { if (m_autonomousCommand != null) {
CommandScheduler.getInstance().cancel(m_autonomousCommand); CommandScheduler.getInstance().cancel(m_autonomousCommand);
m_autonomousCommand.cancel(); m_autonomousCommand.cancel();
m_autonomousCommand.end(true);
System.out.println("NOT Null!!");
} else {
System.out.println("Null!!");
} }
m_robotTime.startMatchTime(); 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<>(); // public List<Subsystem> subsystems = new ArrayList<>();
// ! Teleop Commands // ! Teleop Commands
public void stop() {
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;;
m_robotSwerveDrive.stopModules();
}
// ! /* Autos */ // ! /* Autos */
private String lastAutoName = "defualt.auto"; private String lastAutoName = "defualt.auto";
@@ -104,8 +108,10 @@ public class RobotContainer {
private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision);
private Command placeCoral = new SequentialCommandGroup( private Command placeCoral = new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
new InstantCommand(() -> System.out.println("Placing Some Coral")), 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( private Command aprilAlign = new SequentialCommandGroup(
@@ -115,7 +121,8 @@ public class RobotContainer {
private Command grabCoral = new SequentialCommandGroup( private Command grabCoral = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Yoinking some coral...")), 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 */ // ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); .onTrue(AutoGotoPosition);
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) 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 // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time
// ? /* Programer Buttons (Controller 3)*/ // ? /* Programer Buttons (Controller 3)*/
@@ -300,10 +307,10 @@ public class RobotContainer {
//return autoPlayback; //return autoPlayback;
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision) //return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
return autoChooser.getSelected(); return autoChooser.getSelected().andThen(new InstantCommand(() -> System.err.println("Auto Ded!")));
// try{ // try{
// // Load the path you want to follow using its name in the GUI // // 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) { // } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
// return Commands.none(); // 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) { public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
this.swerveDrive = swerveDrive; this.swerveDrive = swerveDrive;
this.vision = vision; this.vision = vision;
addRequirements(swerveDrive); // addRequirements(swerveDrive);
} }
@Override @Override
public void initialize() { public void initialize() {
xPID.initialize(); xPID.initialize();
yPID.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; double xerr;
@@ -84,7 +84,9 @@ public class GotoPositionCommand extends Command {
@Override @Override
public final boolean isFinished() { 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 // this statement is a boolean in and of itself
} }