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

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));
}
}