From 5bc17643ec88f6e23506d8608eeeb42c696aedfe Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 17 Dec 2024 18:32:47 -0500 Subject: [PATCH] test --- src/main/java/frc/robot/Commands/Lancer.java | 1 - .../frc/robot/Commands/LancerModeAuto.java | 48 +++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 61 +++++++++---------- .../Subsystems/CommandSwerveDrivetrain.java | 7 +-- 4 files changed, 81 insertions(+), 36 deletions(-) create mode 100644 src/main/java/frc/robot/Commands/LancerModeAuto.java diff --git a/src/main/java/frc/robot/Commands/Lancer.java b/src/main/java/frc/robot/Commands/Lancer.java index 248310e..d7589b4 100644 --- a/src/main/java/frc/robot/Commands/Lancer.java +++ b/src/main/java/frc/robot/Commands/Lancer.java @@ -4,7 +4,6 @@ package frc.robot.Commands; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Subsystems.Accumulateur; import frc.robot.Subsystems.Lanceur; diff --git a/src/main/java/frc/robot/Commands/LancerModeAuto.java b/src/main/java/frc/robot/Commands/LancerModeAuto.java new file mode 100644 index 0000000..9426bd3 --- /dev/null +++ b/src/main/java/frc/robot/Commands/LancerModeAuto.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Commands; +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.Subsystems.Accumulateur; +import frc.robot.Subsystems.Lanceur; + +public class LancerModeAuto extends Command { + private Lanceur lanceur; + private Accumulateur accumulateur; + /** Creates a new Lancer. */ + public LancerModeAuto(Lanceur lanceur,Accumulateur accumulateur) { + this.lanceur = lanceur; + this.accumulateur = accumulateur; + addRequirements(lanceur, accumulateur); + // Use addRequirements() here to declare subsystem dependencies. + } + // Called when the command is initially scheduled. + @Override + public void initialize() { + lanceur.PIDlanceur(0, 0, 0); + } + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + lanceur.lance(); + accumulateur.Petitlanceur(0.9); + accumulateur.desaccumule(0.2); + lanceur.masterslave(); + accumulateur.rouesbleue(0.7); + } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + lanceur.lance(0); + accumulateur.desaccumule(0); + accumulateur.Petitlanceur(0); + accumulateur.rouesbleue(0); + } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9000847..6cb8770 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,20 +10,18 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Commands.Desaccumuler; -import frc.robot.Commands.FollowAprilTag; import frc.robot.Commands.LEDactive; import frc.robot.Commands.Lancer; +import frc.robot.Commands.LancerModeAuto; import frc.robot.Subsystems.Accumulateur; import frc.robot.Subsystems.CommandSwerveDrivetrain; import frc.robot.Subsystems.LED; @@ -48,6 +46,33 @@ public class RobotContainer { private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); private final Telemetry logger = new Telemetry(MaxSpeed); + + + // private final SendableChooser autoChooser; + Lanceur lanceur= new Lanceur(); + Accumulateur accumulateur = new Accumulateur(); + Limelight3G limelight3G = new Limelight3G(); + LED led = new LED(); + //Drive drive = new Drive(); + ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); + + public RobotContainer() { + // NamedCommands.registerCommand("lancer", lanceur.lance()); + //autoChooser = AutoBuilder.buildAutoChooser(); + dashboard.addCamera("limelight3G", "limelight3G","limelight.local:5800") + .withSize(3,4) + .withPosition(5,0); + dashboard.addCamera("limelight3", "limelight3","limelight.local:5800") + .withSize(3,4) + .withPosition(2,0); + + + manette.x().whileTrue(new Lancer(lanceur,accumulateur)); + //manette.leftBumper().toggleOnTrue(new FollowAprilTag(limelight3G, lanceur)); + manette.a().whileTrue(new Desaccumuler(accumulateur)); + manette.y().whileTrue(new LEDactive(accumulateur, led)); + configureBindings(); + } private void configureBindings() { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> drive.withVelocityX(-manette.getLeftY() * MaxSpeed) // Drive forward with @@ -67,34 +92,8 @@ public class RobotContainer { } drivetrain.registerTelemetry(logger::telemeterize); } - - // private final SendableChooser autoChooser; - Lanceur lanceur= new Lanceur(); - Accumulateur accumulateur = new Accumulateur(); - Limelight3G limelight3G = new Limelight3G(); - LED led = new LED(); - //Drive drive = new Drive(); - ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); - - public RobotContainer() { - //NamedCommands.registerCommand("lancer", new Lancer(lanceur, accumulateur)); - //NamedCommands.registerCommand("Desaccumuler", new Desaccumuler(accumulateur)); - //autoChooser = AutoBuilder.buildAutoChooser(); - dashboard.addCamera("limelight3G", "limelight3G","limelight.local:5800") - .withSize(3,4) - .withPosition(5,0); - dashboard.addCamera("limelight3", "limelight3","limelight.local:5800") - .withSize(3,4) - .withPosition(2,0); - - - manette.x().whileTrue(new Lancer(lanceur,accumulateur)); - manette.leftBumper().toggleOnTrue(new FollowAprilTag(limelight3G, lanceur)); - manette.a().whileTrue(new Desaccumuler(accumulateur)); - manette.y().whileTrue(new LEDactive(accumulateur, led)); - } public Command getAutonomousCommand() { - return null; + return new SequentialCommandGroup(new LancerModeAuto(lanceur, accumulateur)); // return autoChooser.getSelected(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java index 4931dd9..bc1c6b9 100644 --- a/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; - /** * Class that extends the Phoenix SwerveDrivetrain class and implements * subsystem so it can be used in command-based projects easily. @@ -31,20 +30,20 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); /* Keep track if we've ever applied the operator perspective before or not */ private boolean hasAppliedOperatorPerspective = false; - public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { super(driveTrainConstants, OdometryUpdateFrequency, modules); if (Utils.isSimulation()) { startSimThread(); } } + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { super(driveTrainConstants, modules); if (Utils.isSimulation()) { startSimThread(); } } - + public Command applyRequest(Supplier requestSupplier) { return run(() -> this.setControl(requestSupplier.get())); } @@ -63,7 +62,7 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst }); m_simNotifier.startPeriodic(kSimLoopPeriod); } - + @Override public void periodic() { /* Periodically try to apply the operator perspective */