diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eba36bf..93e0d5c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,7 +85,7 @@ public class RobotContainer { private final CommandXboxController manette2 = new CommandXboxController(1); private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2 public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - private final SendableChooser autoChooser; + //private final SendableChooser autoChooser; public double getAngle() { return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot } @@ -104,16 +104,16 @@ public class RobotContainer { public RobotContainer() { CameraServer.startAutomaticCapture(); configureBindings(); - NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); - NamedCommands.registerCommand("Station",new StationPince(pince, elevateur,bougie)); - NamedCommands.registerCommand("L4", new L4(elevateur, pince)); - NamedCommands.registerCommand("L3", new L3(elevateur, pince)); - NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie)); - NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie)); - NamedCommands.registerCommand("baleeuse",new L1Requin(requin, bougie)); - NamedCommands.registerCommand("baleeuse sort", new ExpireCorail(requin, bougie)); - autoChooser = AutoBuilder.buildAutoChooser("New Auto"); - SmartDashboard.putData("Auto Mode", autoChooser); + // NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); + // NamedCommands.registerCommand("Station",new StationPince(pince, elevateur,bougie)); + // NamedCommands.registerCommand("L4", new L4(elevateur, pince)); + // NamedCommands.registerCommand("L3", new L3(elevateur, pince)); + // NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie)); + // NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie)); + // NamedCommands.registerCommand("baleeuse",new L1Requin(requin, bougie)); + // NamedCommands.registerCommand("baleeuse sort", new ExpireCorail(requin, bougie)); + //autoChooser = AutoBuilder.buildAutoChooser("New Auto"); + //SmartDashboard.putData("Auto Mode", autoChooser); } private void configureBindings() { diff --git a/src/main/java/frc/robot/commands/AvancerAuto.java b/src/main/java/frc/robot/commands/AvancerAuto.java index de51667..50e95ec 100644 --- a/src/main/java/frc/robot/commands/AvancerAuto.java +++ b/src/main/java/frc/robot/commands/AvancerAuto.java @@ -6,6 +6,7 @@ package frc.robot.commands; import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; @@ -16,7 +17,8 @@ public class AvancerAuto extends Command { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1); + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); /** Creates a new AvancerAuto. */ public AvancerAuto(SwerveRequest.RobotCentric drive) { // Use addRequirements() here to declare subsystem dependencies. @@ -29,7 +31,7 @@ public class AvancerAuto extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drive.withVelocityX(0.2); + drive.withVelocityX(0.2*MaxSpeed); } // Called once the command ends or is interrupted.