PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

path planner est de retour

Browse Source
This commit is contained in:
Samuel
2025-11-24 18:33:53 -05:00
parent a998218bd9
commit 6d3b467a86
3 changed files with 72 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/deploy/pathplanner/navgrid.json
View File

File diff suppressed because one or more lines are too long

68
src/main/java/frc/robot/RobotContainer.java
View File

@@ -10,6 +10,8 @@ import java.util.Map;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
@@ -18,6 +20,8 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -59,6 +63,7 @@ import frc.robot.commands.Elevateur.balonL2;
import frc.robot.commands.Elevateur.balonL3;
public class RobotContainer {
private final SendableChooser<Command> autoChooser;
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
.withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
@@ -95,7 +100,9 @@ public class RobotContainer {
Requin requin = new Requin();
CorailAspir corailAspir = new CorailAspir(pince, bougie);
public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser();
CameraServer.startAutomaticCapture();
SmartDashboard.putData("Auto Chooser", autoChooser);
configureBindings();
}
@@ -157,34 +164,35 @@ public class RobotContainer {
}
public Command getAutonomousCommand() {
return new SequentialCommandGroup(
drivetrain.applyRequest(()->
drive.withVelocityX(0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
drivetrain.applyRequest(()->
drive.withVelocityX(-0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
drivetrain.applyRequest(()->
drive.withVelocityX(-0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
drivetrain.applyRequest(()->
drive.withVelocityX(0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
drivetrain.applyRequest(()->
drive.withVelocityX(-0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
drivetrain.applyRequest(()->
drive.withVelocityX(0)
.withVelocityY(0)
.withRotationalRate(0)).withTimeout(0.1),
new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
new RainBow(bougie));
}
return autoChooser.getSelected();
// return new SequentialCommandGroup(
// drivetrain.applyRequest(()->
// drive.withVelocityX(0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
// drivetrain.applyRequest(()->
// drive.withVelocityX(-0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
// drivetrain.applyRequest(()->
// drive.withVelocityX(-0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
// drivetrain.applyRequest(()->
// drive.withVelocityX(0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
// drivetrain.applyRequest(()->
// drive.withVelocityX(-0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
// drivetrain.applyRequest(()->
// drive.withVelocityX(0)
// .withVelocityY(0)
// .withRotationalRate(0)).withTimeout(0.1),
// new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
// new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
// new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
// new RainBow(bougie));
}
}

66
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -45,42 +45,42 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
// private void configureAutoBuilder() {
// try {
// var config = RobotConfig.fromGUISettings();
// AutoBuilder.configure(
private void configureAutoBuilder() {
try {
var config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
// () -> getState().Pose, // Supplier of current robot pose
// this::resetPose, // Consumer for seeding pose against auto
// () -> getState().Speeds, // Supplier of current robot speeds
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
() -> getState().Speeds, // Supplier of current robot speeds
// // Consumer of ChassisSpeeds and feedforwards to drive the robot
// (speeds, feedforwards) -> setControl(
// m_pathApplyRobotSpeeds.withSpeeds(speeds)
// .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
// .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
// ),
// new PPHolonomicDriveController(
// // PID constants for translation
// new PIDConstants(63.167, 0, 0.54521),
// // // PID constants for rotation
// // new PIDConstants(7.9735, 0, 0.038499)
// // PID constants for rotation
// new PIDConstants(43.502,0,2.7353)
// ),
// config,
// // Assume the path needs to be flipped for Red vs Blue, this is normally the case
// () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
// this // Subsystem for requirements
// Consumer of ChassisSpeeds and feedforwards to drive the robot
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(speeds)
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(63.167, 0, 0.54521),
// // PID constants for rotation
// new PIDConstants(7.9735, 0, 0.038499)
// PID constants for rotation
new PIDConstants(43.502,0,2.7353)
),
config,
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this // Subsystem for requirements
// );
// } catch (Exception ex) {
// DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
// }
// PPHolonomicDriveController.overrideRotationFeedback(()->{
// return 0;
// });
// }
);
} catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
}
PPHolonomicDriveController.overrideRotationFeedback(()->{
return 0;
});
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.