diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f0cecc..2e84c89 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,6 +53,8 @@ public class RobotContainer { CommandXboxController manette = new CommandXboxController(1); public RobotContainer() { + NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur)); + NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur)); autoChooser = AutoBuilder.buildAutoChooser(); CameraServer.startAutomaticCapture(); @@ -62,8 +64,7 @@ public class RobotContainer { drive.setDefaultCommand(new RunCommand(()->{ drive.drive(-MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(-joystick.getX(),0.2), MathUtil.applyDeadband(-joystick.getZ(), 0.2)); },drive)); - NamedCommands.registerCommand("balayer",balayer); - NamedCommands.registerCommand("lancer", lancernote); + } private void configureBindings() { diff --git a/src/main/java/frc/robot/subsystem/Drive.java b/src/main/java/frc/robot/subsystem/Drive.java index 44bb959..7770387 100644 --- a/src/main/java/frc/robot/subsystem/Drive.java +++ b/src/main/java/frc/robot/subsystem/Drive.java @@ -7,10 +7,14 @@ package frc.robot.subsystem; import java.io.File; import java.io.IOException; - +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import swervelib.SwerveDrive; @@ -28,6 +32,17 @@ public class Drive extends SubsystemBase { } public Drive() { + AutoBuilder.configureHolonomic(swerveDrive::getPose, swerveDrive::resetOdometry, swerveDrive::getRobotVelocity, swerveDrive::setChassisSpeeds, new HolonomicPathFollowerConfig( + new PIDConstants(5,0,0), new PIDConstants(5,0,0), 4.5, + 0.275, + new ReplanningConfig() + ), ()->{ + var alliance = DriverStation.getAlliance(); + if(alliance.isPresent()){ + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, this); try { this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2); } catch (IOException e) {