auto I MARCHE(pas)
This commit is contained in:
@@ -47,50 +47,32 @@ Translation2d backLeftLocation = new Translation2d(-0.3, +0.3);
|
|||||||
Translation2d backRightLocation = new Translation2d(-0.3, -0.3);
|
Translation2d backRightLocation = new Translation2d(-0.3, -0.3);
|
||||||
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
|
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
|
||||||
private void configureAutoBuilder() {
|
private void configureAutoBuilder() {
|
||||||
RobotConfig config;
|
try {
|
||||||
System.out.println("avant le mode auto est genere");
|
var config = RobotConfig.fromGUISettings();
|
||||||
try{
|
AutoBuilder.configure(
|
||||||
config = RobotConfig.fromGUISettings();
|
() -> getState().Pose, // Supplier of current robot pose
|
||||||
// Configure AutoBuilder last
|
this::resetPose, // Consumer for seeding pose against auto
|
||||||
AutoBuilder.configure(
|
() -> getState().Speeds, // Supplier of current robot speeds
|
||||||
() -> getState().Pose, // Robot pose supplier
|
// Consumer of ChassisSpeeds and feedforwards to drive the robot
|
||||||
(pose) -> {
|
|
||||||
System.out.println("reset Pose " + pose.getX() + " " + pose.getY());
|
|
||||||
this.resetPose(pose);
|
|
||||||
this.resetPose(pose);
|
|
||||||
}, // Method to reset odometry (will be called if your auto has a starting pose)
|
|
||||||
() -> getState().Speeds,
|
|
||||||
(speeds, feedforwards) -> setControl(
|
(speeds, feedforwards) -> setControl(
|
||||||
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
|
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
|
||||||
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
|
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
|
||||||
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
|
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
|
||||||
), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
|
|
||||||
//(speeds, feedforwards) -> this.applyRequest(()-> driveAuto.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
|
|
||||||
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
|
|
||||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
|
|
||||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
|
|
||||||
),
|
),
|
||||||
config, // The robot configuration
|
new PPHolonomicDriveController(
|
||||||
() -> {
|
// PID constants for translation
|
||||||
// Boolean supplier that controls when the path will be mirrored for the red alliance
|
new PIDConstants(10, 0, 0),
|
||||||
// This will flip the path being followed to the red side of the field.
|
// PID constants for rotation
|
||||||
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
|
new PIDConstants(7, 0, 0)
|
||||||
|
),
|
||||||
var alliance = DriverStation.getAlliance();
|
config,
|
||||||
if (alliance.isPresent()) {
|
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
|
||||||
return alliance.get() == DriverStation.Alliance.Red;
|
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
||||||
}
|
this // Subsystem for requirements
|
||||||
return false;
|
);
|
||||||
},
|
} catch (Exception ex) {
|
||||||
this // Reference to this subsystem to set requirements
|
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
||||||
);
|
}
|
||||||
System.out.println("le mode auto est genere");
|
|
||||||
}
|
|
||||||
catch (Exception e) {
|
|
||||||
// Handle exception as needed
|
|
||||||
e.printStackTrace();
|
|
||||||
System.out.println("le mode auto ne marche pas");
|
|
||||||
}
|
|
||||||
// try {
|
// try {
|
||||||
// var config = RobotConfig.fromGUISettings();
|
// var config = RobotConfig.fromGUISettings();
|
||||||
// AutoBuilder.configure(
|
// AutoBuilder.configure(
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
{
|
{
|
||||||
"fileName": "PathplannerLib-2025.2.7.json",
|
"fileName": "PathplannerLib-2025.2.6.json",
|
||||||
"name": "PathplannerLib",
|
"name": "PathplannerLib",
|
||||||
"version": "2025.2.7",
|
"version": "2025.2.6",
|
||||||
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||||
"frcYear": "2025",
|
"frcYear": "2025",
|
||||||
"mavenUrls": [
|
"mavenUrls": [
|
||||||
@@ -12,7 +12,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.pathplanner.lib",
|
"groupId": "com.pathplanner.lib",
|
||||||
"artifactId": "PathplannerLib-java",
|
"artifactId": "PathplannerLib-java",
|
||||||
"version": "2025.2.7"
|
"version": "2025.2.6"
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"jniDependencies": [],
|
"jniDependencies": [],
|
||||||
@@ -20,7 +20,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.pathplanner.lib",
|
"groupId": "com.pathplanner.lib",
|
||||||
"artifactId": "PathplannerLib-cpp",
|
"artifactId": "PathplannerLib-cpp",
|
||||||
"version": "2025.2.7",
|
"version": "2025.2.6",
|
||||||
"libName": "PathplannerLib",
|
"libName": "PathplannerLib",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": false,
|
"sharedLibrary": false,
|
||||||
Reference in New Issue
Block a user