diff --git a/build.gradle b/build.gradle index 322a386..f55c06c 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/java/frc/robot/Commandes.java b/src/main/java/frc/robot/Commandes.java new file mode 100644 index 0000000..0a23edd --- /dev/null +++ b/src/main/java/frc/robot/Commandes.java @@ -0,0 +1,67 @@ +package frc.robot; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Balayeuse; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Demeleur; +import frc.robot.subsystems.Grimpeur; +import frc.robot.subsystems.Lanceur; +import frc.robot.subsystems.PivotBalayeuse; + +public class Commandes { + + private final CommandSwerveDrivetrain drivetrain; + private final Lanceur lanceur; + private final Demeleur demeleur; + private final Balayeuse balayeuse; + private final Grimpeur grimpeur; + private final PivotBalayeuse pivotBalayeuse; + + public Commandes(CommandSwerveDrivetrain drivetrain, + Lanceur lanceur, + Demeleur demeleur, + Balayeuse balayeuse, + Grimpeur grimpeur, + PivotBalayeuse pivotBalayeuse) { + this.drivetrain = drivetrain; + this.lanceur = lanceur; + this.demeleur = demeleur; + this.balayeuse = balayeuse; + this.grimpeur = grimpeur; + this.pivotBalayeuse = pivotBalayeuse; + } + + public double vitesseLanceur() { + return Coordinates.diffFromHub(drivetrain.getState().Pose.getTranslation()).getNorm() * 410.57 + 2250; + } + + public Command viserLancer() { + return viserLancer(() -> 0.0, () -> 0.0); + } + + public Command viserLancer(Supplier vitesseX, Supplier vitesseY) { + return Commands.parallel( + drivetrain.viserReservoir(vitesseX, vitesseY), + this.lancer()); + } + + public Command lancer() { + return lancer(this::vitesseLanceur); + } + + public Command lancer(Supplier vitesse) { + return Commands.parallel( + lanceur.lancer(vitesse), + Commands.waitUntil(lanceur::setPointAtteint).andThen(demeleur.run())); + } + + public Command lancerAspirer() { + return Commands.parallel( + this.lancer(), + balayeuse.aspirer()); + } + +} diff --git a/src/main/java/frc/robot/Coordinates.java b/src/main/java/frc/robot/Coordinates.java new file mode 100644 index 0000000..1a7c6c5 --- /dev/null +++ b/src/main/java/frc/robot/Coordinates.java @@ -0,0 +1,41 @@ +package frc.robot; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public final class Coordinates { + public static final Translation2d kRedHub = new Translation2d(Meters.of(16.5 - 4.6), Meters.of(4.03)); + public static final Translation2d kBlueHub = new Translation2d(Meters.of(4.6), Meters.of(4.03)); + + public static final Translation2d kGrimpeDepotRouge = new Translation2d(Meters.of(16.5 - 1.1), Meters.of(4.9)); + public static final Translation2d kGrimpeMurRouge = new Translation2d(Meters.of(16.5 - 1.1), Meters.of(3.7)); + public static final Translation2d kGrimpeDepotBleu = new Translation2d(Meters.of(1.1), Meters.of(3.1)); + public static final Translation2d kGrimpeMurBleu = new Translation2d(Meters.of(1.1), Meters.of(4.4)); + + public static Translation2d diffFromHub(Translation2d robotTranslation) { + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { + return kBlueHub.minus(robotTranslation); + } else { + return kRedHub.minus(robotTranslation); + } + } + + public static Translation2d diffFromGrimpeMur(Translation2d robotTranslation) { + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { + return kGrimpeMurBleu.minus(robotTranslation); + } else { + return kGrimpeMurRouge.minus(robotTranslation); + } + } + + public static Translation2d diffFromGrimpeDepot(Translation2d robotTranslation) { + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { + return kGrimpeDepotBleu.minus(robotTranslation); + } else { + return kGrimpeDepotRouge.minus(robotTranslation); + } + } +} diff --git a/src/main/java/frc/robot/LimeLight3.java b/src/main/java/frc/robot/LimeLight3.java new file mode 100644 index 0000000..44bc5f7 --- /dev/null +++ b/src/main/java/frc/robot/LimeLight3.java @@ -0,0 +1,75 @@ +// 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; + +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class LimeLight3 { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); + NetworkTableEntry pipeline = table.getEntry("pipeline"); + + /** Creates a new LimeLight3. */ + public LimeLight3() { + for (int port = 5800; port <= 5807; port++) { + PortForwarder.add(port, "limelight.local", port); + } + } + + public double[] getBotPoseBlue() { + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + + public double[] getBotPoseRed() { + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + + public double getTx() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); + NetworkTableEntry tx = table.getEntry("tx"); + return tx.getDouble(0.0); + } + + public double getTId() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); + NetworkTableEntry tid = table.getEntry("tid"); + return tid.getDouble(0.0); + } + + public double getTA() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); + NetworkTableEntry ta = table.getEntry("ta"); + return ta.getDouble(0.0); + } + + public boolean getV() { + return LimelightHelpers.getTV("limelight-balaie"); + } + + public void AprilTag() { + pipeline.setNumber(0); + } + + public void Forme() { + pipeline.setNumber(1); + } + + public double Calcule(double x1, double x2, double y1, double y2, double angle) { + if (x1 > 4) { + return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle) / 90; + } else { + return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1 / 90; + } + } + +} diff --git a/src/main/java/frc/robot/Limelight3G.java b/src/main/java/frc/robot/Limelight3G.java new file mode 100644 index 0000000..83500ff --- /dev/null +++ b/src/main/java/frc/robot/Limelight3G.java @@ -0,0 +1,74 @@ +// 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; + +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Limelight3G { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry pipeline = table.getEntry("pipeline"); + + /** Creates a new LimeLight3. */ + public Limelight3G() { + for (int port = 5800; port <= 5807; port++) { + PortForwarder.add(port, "limelight.local", port); + } + } + + public double[] getBotPoseBlue() { + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + + public double[] getBotPoseRed() { + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + + public double getTx() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry tx = table.getEntry("tx"); + return tx.getDouble(0.0); + } + + public double getTId() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry tid = table.getEntry("tid"); + return tid.getDouble(0.0); + } + + public double getTA() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry ta = table.getEntry("ta"); + return ta.getDouble(0.0); + } + + public boolean getV() { + return LimelightHelpers.getTV("limelight-tag"); + } + + public double Calcule(double x1, double x2, double y1, double y2, double angle) { + if (x1 > x2) { + if (y1 > y2) { + return Math.atan(90 - ((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle; + } else { + return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) + 90 - angle; + } + } else { + if (y1 > y2) { + return Math.atan(90 - ((x2 - x1) / (y2 - y1))) * (180 / Math.PI) + 270 - angle; + } else { + return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) + 180 - angle; + } + } + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9ceb3be..dc73c09 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; @@ -27,19 +28,28 @@ public class Robot extends TimedRobot { LimelightHelpers.SetRobotOrientation("limelight_tag", headingDeg, 0, 0, 0, 0, 0); var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight_tag"); + var stdDevs = LimelightHelpers.getLimelightDoubleArrayEntry("limelight_tag", "stddevs"); + var limelightStdDevs = VecBuilder.fill(.5, .5, 9999999); + if (stdDevs.exists()) { + limelightStdDevs = VecBuilder.fill(stdDevs.get()[6], stdDevs.get()[7], stdDevs.get()[12]); + } if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) { - m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds); + m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds, + limelightStdDevs); } } @Override - public void disabledInit() {} + public void disabledInit() { + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } @Override - public void disabledExit() {} + public void disabledExit() { + } @Override public void autonomousInit() { @@ -51,10 +61,12 @@ public class Robot extends TimedRobot { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override - public void autonomousExit() {} + public void autonomousExit() { + } @Override public void teleopInit() { @@ -64,10 +76,12 @@ public class Robot extends TimedRobot { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override - public void teleopExit() {} + public void teleopExit() { + } @Override public void testInit() { @@ -75,8 +89,10 @@ public class Robot extends TimedRobot { } @Override - public void testPeriodic() {} + public void testPeriodic() { + } @Override - public void testExit() {} + public void testExit() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59e696e..2e093e5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,9 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -12,28 +14,11 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.MathUtil; 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.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.Aspirer; -import frc.robot.commands.DescendreBalyeuse; -import frc.robot.commands.DescendreGrimpeur; -import frc.robot.commands.Lancer; -import frc.robot.commands.LancerAspirer; -import frc.robot.commands.LancerBaseVitesse; -import frc.robot.commands.Limelighter; -import frc.robot.commands.ModeOposer; -import frc.robot.commands.ModeOposerBalayeuse; -import frc.robot.commands.ModeOposerDemeleur; -import frc.robot.commands.MonterBalyeuse; -import frc.robot.commands.MonterGrimpeur; -import frc.robot.commands.ModeAuto.AspirerAuto; -import frc.robot.commands.ModeAuto.GrimperMur; -import frc.robot.commands.ModeAuto.GrimperReservoir; -import frc.robot.commands.ModeAuto.LancerAuto; -import frc.robot.commands.ModeAuto.LimelighterAuto; import frc.robot.commands.ModeAuto.RetourMilieuDroite; import frc.robot.commands.ModeAuto.RetourMilieuGauche; import frc.robot.commands.ModeAuto.TournerVersMur; @@ -41,91 +26,89 @@ import frc.robot.commands.ModeAuto.TournerVersReservoir; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Balayeuse; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Demeleur; import frc.robot.subsystems.Grimpeur; import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.Led; -import frc.robot.subsystems.LimeLight3; -import frc.robot.subsystems.Limelight3G; +import frc.robot.subsystems.PivotBalayeuse; public class RobotContainer { private final SendableChooser autoChooser; - Balayeuse balayeuse = new Balayeuse(); - Grimpeur grimpeur = new Grimpeur(); - Lanceur lanceur = new Lanceur(); - LimeLight3 limeLight3 = new LimeLight3(); - Limelight3G limeLight3G = new Limelight3G(); - Led led = new Led(); - CommandXboxController manette = new CommandXboxController(0); - CommandXboxController manette1 = new CommandXboxController(1); + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private final Balayeuse balayeuse = new Balayeuse(); + private final PivotBalayeuse pivotBalayeuse = new PivotBalayeuse(); + private final Demeleur demeleur = new Demeleur(); + private final Grimpeur grimpeur = new Grimpeur(); + private final Lanceur lanceur = new Lanceur(); + private final Commandes commandes = new Commandes(drivetrain, lanceur, demeleur, balayeuse, grimpeur, pivotBalayeuse); + private final LimeLight3 limeLight3 = new LimeLight3(); + private final Limelight3G limeLight3G = new Limelight3G(); + private final Led led = new Led(); + private final CommandXboxController manette = new CommandXboxController(0); + private final CommandXboxController manette1 = new CommandXboxController(1); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max + // angular velocity - /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + private final Telemetry logger = new Telemetry(MaxSpeed); public RobotContainer() { - NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); - NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); - NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); - NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G)); + NamedCommands.registerCommand("GrimperMur", drivetrain.allerGrimpeMur()); + NamedCommands.registerCommand("GrimperReservoir", drivetrain.allerGrimpeDepot()); + NamedCommands.registerCommand("Lancer", commandes.lancer()); + NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain, limeLight3G)); NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G)); - NamedCommands.registerCommand("Limelighter", new LimelighterAuto(limeLight3G,drivetrain)); - NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse)); - NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse)); + NamedCommands.registerCommand("Limelighter", commandes.viserLancer()); + NamedCommands.registerCommand("DescendreBalayeuse", pivotBalayeuse.descendre()); + NamedCommands.registerCommand("Aspirer", balayeuse.aspirer()); NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain)); NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain)); - NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur)); - NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur)); + NamedCommands.registerCommand("MonterGrimpeur", grimpeur.sortir()); + NamedCommands.registerCommand("DescendreGrimpeur", grimpeur.rentrer()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - + CameraServer.startAutomaticCapture(); - + configureBindings(); } private void configureBindings() { + + // Manette 0 (pilote) drivetrain.setDefaultCommand( - drivetrain.applyRequest(() -> - drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05)) - .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed*0.7, 0.05)) - .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) - ) - ); - //manette 1 - // manette1.povUp().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - // manette1.povDown().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - // manette1.povRight().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); - // manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - manette.povUp().whileTrue(new MonterGrimpeur(grimpeur)); - manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur)); - manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G)); - manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain)); - manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse)); - manette.rightBumper().whileTrue(new Aspirer(balayeuse)); - manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain)); - manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain)); + drivetrain.applyRequest(() -> drive + .withVelocityY(-manette.getLeftX() * MaxSpeed * 0.7) + .withVelocityX(-manette.getLeftY() * MaxSpeed * 0.7) + .withRotationalRate(-manette.getRightX() * MaxAngularRate))); + manette.rightBumper().whileTrue(balayeuse.aspirer()); + manette.povUp().whileTrue(grimpeur.sortir()); + manette.povDown().whileTrue(grimpeur.rentrer()); + manette.rightTrigger().whileTrue(commandes.lancer()); + manette.leftTrigger().whileTrue( + commandes.viserLancer(() -> -manette.getLeftY() * MaxSpeed * 0.7, () -> -manette.getLeftX() * MaxSpeed * 0.7)); + manette.leftBumper().whileTrue(pivotBalayeuse.descendre()); + manette.b().whileTrue(drivetrain.allerGrimpeDepot()); + manette.x().whileTrue(drivetrain.allerGrimpeMur()); - //manette 2 - manette1.rightTrigger().whileTrue(new Aspirer(balayeuse)); - manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse)); - manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur)); - manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse)); - manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G)); - manette1.b().whileTrue(new ModeOposer(lanceur)); - manette1.a().whileTrue(new ModeOposerDemeleur(lanceur)); - manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); + // Manette 1 (co-pilote) + manette1.rightTrigger().whileTrue(balayeuse.aspirer()); + + manette1.rightBumper().whileTrue(pivotBalayeuse.monter()); + manette1.leftTrigger().whileTrue(commandes.lancer(() -> lanceur.ntBasseVitesse())); + manette1.leftBumper().whileTrue(pivotBalayeuse.descendre()); + manette1.x().whileTrue(commandes.lancerAspirer()); + manette1.b().whileTrue(Commands.parallel(lanceur.inverse(), demeleur.inverse())); + manette1.a().whileTrue(demeleur.inverse()); + manette1.y().whileTrue(balayeuse.ejecter()); + + drivetrain.registerTelemetry(logger::telemeterize); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Aspirer.java b/src/main/java/frc/robot/commands/Aspirer.java deleted file mode 100644 index cb5bfe8..0000000 --- a/src/main/java/frc/robot/commands/Aspirer.java +++ /dev/null @@ -1,44 +0,0 @@ -// 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.Balayeuse; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class Aspirer extends Command { - private Balayeuse balayeuse; - /** Creates a new Aspirer. */ - public Aspirer(Balayeuse balayeuse) { - this.balayeuse = balayeuse; - addRequirements(balayeuse); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - balayeuse.BalayerEnbas(-0.5); - balayeuse.BalayerPadle(-0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - balayeuse.BalayerEnbas(0); - balayeuse.BalayerPadle(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/DescendreBalyeuse.java b/src/main/java/frc/robot/commands/DescendreBalyeuse.java deleted file mode 100644 index 5c83a98..0000000 --- a/src/main/java/frc/robot/commands/DescendreBalyeuse.java +++ /dev/null @@ -1,46 +0,0 @@ -// 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.Balayeuse; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class DescendreBalyeuse extends Command { - private Balayeuse balayeuse; - /** Creates a new Descendre. */ - public DescendreBalyeuse(Balayeuse balayeuse) { - this.balayeuse = balayeuse; - addRequirements(balayeuse); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){ - balayeuse.Pivoter(-0.5); - } - else{ - balayeuse.Pivoter(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - balayeuse.Pivoter(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return Math.abs(balayeuse.Distance()) > balayeuse.EncodeurBalayeuse(); - } -} diff --git a/src/main/java/frc/robot/commands/DescendreGrimpeur.java b/src/main/java/frc/robot/commands/DescendreGrimpeur.java deleted file mode 100644 index c8da33e..0000000 --- a/src/main/java/frc/robot/commands/DescendreGrimpeur.java +++ /dev/null @@ -1,46 +0,0 @@ -// 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.Grimpeur; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class DescendreGrimpeur extends Command { - private Grimpeur grimpeur; - /** Creates a new DescendreGrimpeur. */ - public DescendreGrimpeur(Grimpeur grimpeur) { - this.grimpeur = grimpeur; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(!grimpeur.Limit()){ - grimpeur.Grimper(-0.4); - } - else{ - grimpeur.Reset(); - grimpeur.Grimper(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - grimpeur.Grimper(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java deleted file mode 100644 index f1df279..0000000 --- a/src/main/java/frc/robot/commands/Lancer.java +++ /dev/null @@ -1,90 +0,0 @@ -// 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 java.util.Optional; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lanceur; -import frc.robot.subsystems.Limelight3G; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class Lancer extends Command { - private Lanceur lanceur; - private PIDController pidController; - private Limelight3G limeLight3G; - private Timer timer; - double vitesse = 0.5; - double botx = 0; - double boty = 0; - Optional alliance = DriverStation.getAlliance(); - - /** Creates a new Lancer. */ - public Lancer(Lanceur lanceur, Limelight3G limeLight3G) { - this.lanceur = lanceur; - this.timer = new Timer(); - this.limeLight3G = limeLight3G; - addRequirements(lanceur, limeLight3G); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - if(limeLight3G.getV()){} - pidController = new PIDController(0.0007, 0,0, 0.001); - timer.reset(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double[] BotPose = new double[6]; - if(limeLight3G.getV()){ - if(!alliance.isPresent()){return;} - if(alliance.get() == Alliance.Blue){ - BotPose = limeLight3G.getBotPoseBlue(); - } - else{ - BotPose = limeLight3G.getBotPoseRed(); - } - botx = BotPose[0]; - boty = BotPose[1]; - vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; - } - if(limeLight3G.getV()){ - System.out.println(vitesse); - - double output = pidController.calculate(lanceur.Vitesse(),vitesse); - lanceur.Lancer(output); - if(lanceur.Vitesse() >= vitesse-800){ - timer.start(); - if(timer.get() >1){ - lanceur.Demeler(1); - } - - } - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - lanceur.Demeler(0); - lanceur.Lancer(0); - timer.reset(); - timer.stop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/LancerAspirer.java b/src/main/java/frc/robot/commands/LancerAspirer.java deleted file mode 100644 index 64a5b06..0000000 --- a/src/main/java/frc/robot/commands/LancerAspirer.java +++ /dev/null @@ -1,23 +0,0 @@ -// 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.ParallelCommandGroup; -import frc.robot.subsystems.Balayeuse; -import frc.robot.subsystems.Lanceur; -import frc.robot.subsystems.Limelight3G; - - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class LancerAspirer extends ParallelCommandGroup { - /** Creates a new LacerAspirer. */ - public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, Limelight3G limeLight3G) { - addCommands(new Lancer(lanceur, limeLight3G), new Aspirer(balayeuse)); - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - } -} diff --git a/src/main/java/frc/robot/commands/LancerBaseVitesse.java b/src/main/java/frc/robot/commands/LancerBaseVitesse.java deleted file mode 100644 index ef1d8a0..0000000 --- a/src/main/java/frc/robot/commands/LancerBaseVitesse.java +++ /dev/null @@ -1,63 +0,0 @@ -// 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.math.controller.PIDController; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lanceur; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class LancerBaseVitesse extends Command { - private Lanceur lanceur; - private PIDController pidController; - private Timer timer; - double tempsDebut = 0; - /** Creates a new Lancer. */ - public LancerBaseVitesse(Lanceur lanceur) { - this.lanceur = lanceur; - this.timer = new Timer(); - addRequirements(lanceur); - //this.temp = 0; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - pidController = new PIDController(0.0007, 0,0, 0.001); - timer.reset(); - timer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - System.out.println(DriverStation.getMatchTime()); - double vitesse = lanceur.vitesseDemander(); - double output = pidController.calculate(lanceur.Vitesse(),vitesse); - lanceur.Lancer(output); - if(timer.get() > 1){ - lanceur.Demeler(1); - } - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - lanceur.Demeler(0); - lanceur.Lancer(0); - timer.reset(); - timer.stop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java deleted file mode 100644 index ce3c27f..0000000 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ /dev/null @@ -1,103 +0,0 @@ -// 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 com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Limelight3G; -import static edu.wpi.first.units.Units.*; - -import java.util.Optional; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class Limelighter extends Command { - Limelight3G limelight3g; - CommandSwerveDrivetrain drivetrain; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - double botx; - double boty; - double angle; - double calcul; - Optional alliance = DriverStation.getAlliance(); - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - /** Creates a new Limelighter. */ - public Limelighter(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) { - this.limelight3g = limelight3g; - this.drivetrain = drivetrain; - addRequirements(drivetrain, limelight3g); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double[] BotPose = new double[6]; - System.out.println("e"); - if (limelight3g.getV()) { - BotPose = limelight3g.getBotPoseBlue(); - - botx = BotPose[1]; - boty = BotPose[0]; - angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle); - if(calcul < -5 && calcul > -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(calcul > 5 && calcul < 180){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(calcul >= 180){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(calcul <= -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); - } - else{ - drivetrain.setControl( - drive.withRotationalRate(0)); - } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(angle); - if (calcul < 0.2 && calcul > -0.2) { - drivetrain.setControl(drive.withRotationalRate(0)); - } - } - else{ - drivetrain.setControl(drive.withRotationalRate(0)); - } - } - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - drivetrain.setControl(drive.withRotationalRate(0)); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java deleted file mode 100644 index 70b9ffd..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java +++ /dev/null @@ -1,43 +0,0 @@ -// 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.ModeAuto; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Balayeuse; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class AspirerAuto extends Command { - Balayeuse balayeuse; - /** Creates a new AspirerAuto. */ - public AspirerAuto(Balayeuse balayeuse) { - this.balayeuse = balayeuse; - addRequirements(balayeuse); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - balayeuse.BalayerEnbas(-0.5); - balayeuse.BalayerPadle(-0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - balayeuse.BalayerEnbas(0); - balayeuse.BalayerPadle(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java deleted file mode 100644 index 768028c..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ /dev/null @@ -1,108 +0,0 @@ -// 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.ModeAuto; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import java.util.Optional; - -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.LimeLight3; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class GrimperMur extends Command { - LimeLight3 limeLight3; - CommandSwerveDrivetrain drivetrain; - double[] BotPose = new double[6]; - double botx; - double boty; - double x; - double y; - double angle; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - Optional alliance = DriverStation.getAlliance(); - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /** Creates a new GrimperMur. */ - public GrimperMur(LimeLight3 limeLight3, CommandSwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - this.limeLight3 = limeLight3; - addRequirements(limeLight3,drivetrain); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - BotPose = limeLight3.getBotPoseBlue(); - botx = BotPose[0]; - boty = BotPose[1]; - System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); - if(angle < 0){ - angle = angle + 360; - } - if(alliance.get() == Alliance.Blue){ - y = 2.961328; - x = 1.11; - angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - } - } - else{ - x = 11.45966; - y = 6.959326; - angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - } - } - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java deleted file mode 100644 index 381b213..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ /dev/null @@ -1,108 +0,0 @@ -// 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.ModeAuto; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import java.util.Optional; - -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Limelight3G; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class GrimperReservoir extends Command { - Limelight3G limeLight3G; - CommandSwerveDrivetrain drivetrain; - double[] BotPose = new double[6]; - double botx; - double boty; - double x; - double y; - double angle; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - Optional alliance = DriverStation.getAlliance(); - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /** Creates a new GrimperMur. */ - public GrimperReservoir(Limelight3G limeLight3G, CommandSwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - this.limeLight3G = limeLight3G; - addRequirements(limeLight3G,drivetrain); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - BotPose = limeLight3G.getBotPoseBlue(); - botx = BotPose[0]; - boty = BotPose[1]; - System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); - if(angle < 0){ - angle = angle + 360; - } - if(alliance.get() == Alliance.Red){ - y = 6.959326; - x = 13.57966; - angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - } - } - else{ - y = 5.081328; - x = 1.11; - angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - } - } - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java deleted file mode 100644 index 2205a84..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ /dev/null @@ -1,92 +0,0 @@ -// 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.ModeAuto; - -import java.util.Optional; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lanceur; -import frc.robot.subsystems.Limelight3G; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class LancerAuto extends Command { - Lanceur lanceur; - Timer timer; - private PIDController pidController; - Limelight3G limelight3g; - double botx = 0; - double boty = 0; - Optional alliance = DriverStation.getAlliance(); - - /** Creates a new LancerAuto. */ - public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) { - this.lanceur = lanceur; - timer = new Timer(); - this.limelight3g = limelight3g; - addRequirements(lanceur, limelight3g); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - pidController = new PIDController(0.0007, 0, 0, 0.001); - timer.reset(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double[] BotPose = new double[6]; - if (limelight3g.getV()) { - if (!alliance.isPresent()) { - return; - } - if (alliance.get() == Alliance.Blue) { - BotPose = limelight3g.getBotPoseBlue(); - } else { - BotPose = limelight3g.getBotPoseRed(); - } - - botx = BotPose[0]; - boty = BotPose[1]; - } - double vitesse = 0.5; - if (limelight3g.getV()) { - vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594 - botx), 2) + Math.pow(Math.abs(4.034536 - boty), 2)))) - + 2250; - System.out.println("lancer"); - - double output = pidController.calculate(lanceur.Vitesse(), vitesse); - lanceur.Lancer(output); - if (lanceur.Vitesse() >= vitesse - 800) { - timer.start(); - if (timer.get() > 1) { - lanceur.Demeler(1); - } - } - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - lanceur.Lancer(0); - lanceur.Demeler(0); - timer.reset(); - timer.stop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return timer.get() > 4; - // return false; - } -} diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java deleted file mode 100644 index 3e749da..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ /dev/null @@ -1,104 +0,0 @@ -// 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.ModeAuto; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Limelight3G; -import static edu.wpi.first.units.Units.*; - -import java.util.Optional; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class LimelighterAuto extends Command { - Limelight3G limelight3g; - CommandSwerveDrivetrain drivetrain; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - double botx; - double boty; - double angle; - double calcul; - Optional alliance = DriverStation.getAlliance(); - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - /** Creates a new Limelighter. */ - public LimelighterAuto(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) { - this.limelight3g = limelight3g; - this.drivetrain = drivetrain; - addRequirements(drivetrain, limelight3g); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double[] BotPose = new double[6]; - System.out.println("e"); - if (limelight3g.getV()) { - BotPose = limelight3g.getBotPoseBlue(); - - botx = BotPose[1]; - boty = BotPose[0]; - angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle); - if(calcul < -5 && calcul > -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(calcul > 5 && calcul < 180){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(calcul >= 180){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(calcul <= -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); - } - else{ - drivetrain.setControl( - drive.withRotationalRate(0)); - } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(angle); - if (calcul < 0.2 && calcul > -0.2) { - drivetrain.setControl(drive.withRotationalRate(0)); - } - } - else{ - drivetrain.setControl(drive.withRotationalRate(0)); - } - } - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - drivetrain.setControl(drive.withRotationalRate(0)); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return calcul < 5 && calcul > -5; - } -} - diff --git a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java index 4c065a5..4698632 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java +++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java @@ -15,9 +15,9 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Limelight3G; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Limelight3G; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class RetourMilieuDroite extends Command { @@ -38,7 +38,7 @@ public class RetourMilieuDroite extends Command { public RetourMilieuDroite(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) { this.drivetrain = drivetrain; this.limelight3g = limelight3g; - addRequirements(drivetrain, limelight3g); + addRequirements(drivetrain); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java index 714390a..f0c53c5 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java +++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java @@ -15,9 +15,9 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Limelight3G; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Limelight3G; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class RetourMilieuGauche extends Command { @@ -38,7 +38,7 @@ public class RetourMilieuGauche extends Command { public RetourMilieuGauche(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) { this.drivetrain = drivetrain; this.limelight3g = limelight3g; - addRequirements(drivetrain, limelight3g); + addRequirements(drivetrain); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/ModeOposer.java b/src/main/java/frc/robot/commands/ModeOposer.java deleted file mode 100644 index 840c782..0000000 --- a/src/main/java/frc/robot/commands/ModeOposer.java +++ /dev/null @@ -1,43 +0,0 @@ -// 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.Lanceur; -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class ModeOposer extends Command { - private Lanceur lanceur; - /** Creates a new Lancer. */ - public ModeOposer(Lanceur lanceur) { - this.lanceur = lanceur; - addRequirements(lanceur); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - lanceur.Lancer(-0.2); - lanceur.Demeler(-0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - lanceur.Demeler(0); - lanceur.Lancer(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ModeOposerBalayeuse.java b/src/main/java/frc/robot/commands/ModeOposerBalayeuse.java deleted file mode 100644 index 9a3a2fb..0000000 --- a/src/main/java/frc/robot/commands/ModeOposerBalayeuse.java +++ /dev/null @@ -1,43 +0,0 @@ -// 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.Balayeuse; -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class ModeOposerBalayeuse extends Command { - private Balayeuse balayeuse; - /** Creates a new Lancer. */ - public ModeOposerBalayeuse(Balayeuse balayeuse) { - this.balayeuse = balayeuse; - addRequirements(balayeuse); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - balayeuse.BalayerEnbas(0.5); - balayeuse.BalayerPadle(0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - balayeuse.BalayerEnbas(0); - balayeuse.BalayerPadle(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ModeOposerDemeleur.java b/src/main/java/frc/robot/commands/ModeOposerDemeleur.java deleted file mode 100644 index 940308a..0000000 --- a/src/main/java/frc/robot/commands/ModeOposerDemeleur.java +++ /dev/null @@ -1,42 +0,0 @@ -// 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.Lanceur; -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class ModeOposerDemeleur extends Command { - private Lanceur lanceur; - /** Creates a new Lancer. */ - public ModeOposerDemeleur(Lanceur lanceur) { - this.lanceur = lanceur; - addRequirements(lanceur); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - lanceur.Demeler(-0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - lanceur.Demeler(0); - lanceur.Lancer(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/MonterBalyeuse.java b/src/main/java/frc/robot/commands/MonterBalyeuse.java deleted file mode 100644 index de69081..0000000 --- a/src/main/java/frc/robot/commands/MonterBalyeuse.java +++ /dev/null @@ -1,47 +0,0 @@ -// 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.Balayeuse; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class MonterBalyeuse extends Command { - private Balayeuse balayeuse; - /** Creates a new MonterBalyeuse. */ - public MonterBalyeuse(Balayeuse balayeuse) { - this.balayeuse = balayeuse; - addRequirements(balayeuse); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(!balayeuse.GetLimiSwtich()){ - balayeuse.Pivoter(0.5); - } - else{ - balayeuse.Reset(); - balayeuse.Pivoter(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - balayeuse.Pivoter(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/MonterGrimpeur.java b/src/main/java/frc/robot/commands/MonterGrimpeur.java deleted file mode 100644 index a8a3e84..0000000 --- a/src/main/java/frc/robot/commands/MonterGrimpeur.java +++ /dev/null @@ -1,47 +0,0 @@ -// 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.Grimpeur; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class MonterGrimpeur extends Command { - private Grimpeur grimpeur; - /** Creates a new MonterGrimpeur. */ - public MonterGrimpeur(Grimpeur grimpeur) { - this.grimpeur = grimpeur; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){ - grimpeur.Grimper(0.5); - System.out.println("monte"); - } - else { - grimpeur.Grimper(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - grimpeur.Grimper(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/Balayeuse.java b/src/main/java/frc/robot/subsystems/Balayeuse.java index 36ea83e..9b0134f 100644 --- a/src/main/java/frc/robot/subsystems/Balayeuse.java +++ b/src/main/java/frc/robot/subsystems/Balayeuse.java @@ -5,61 +5,42 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Balayeuse extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - SparkFlex Balaye1 = new SparkFlex(2, MotorType.kBrushless); - SparkFlex Balaye2 = new SparkFlex(20, MotorType.kBrushless); - SparkMax Pivot = new SparkMax(8, MotorType.kBrushless); - DigitalInput limit = new DigitalInput(9); - private GenericEntry EncodeurBalayeuse = - teb.add("Position bas balayeuse", 1.8).getEntry(); - private GenericEntry AmpBaleyeuse = - teb.add("Ampérage Baleyeuse", 40).getEntry(); - public void BalayerEnbas(double vitesse){ - Balaye1.set(vitesse); + SparkFlex moteurBas = new SparkFlex(2, MotorType.kBrushless); + SparkFlex moteurHaut = new SparkFlex(20, MotorType.kBrushless); + + private final double vitesseMoteurBas = -0.5; + private final double vitesseMoteurHaut = -0.2; + + public Command balaye(double vitesseBas, double vitesseHaut) { + return startEnd( + () -> { + moteurBas.set(vitesseBas); + moteurHaut.set(vitesseHaut); + }, () -> { + moteurBas.set(0); + moteurHaut.set(0); + }); } - public void BalayerPadle(double vitesse){ - Balaye2.set(vitesse); + + public Command aspirer() { + return balaye(vitesseMoteurBas, vitesseMoteurHaut); } - public void Pivoter(double vitesse){ - Pivot.set(vitesse); - } - public double Distance(){ - return Pivot.getEncoder().getPosition(); - } - public void Reset(){ - Pivot.getEncoder().setPosition(0.6); - } - public boolean GetLimiSwtich(){ - return !limit.get(); - } - public double Amp(){ - return Balaye2.getOutputCurrent(); - } - public double AmpMax(){ - return AmpBaleyeuse.getDouble(40); - } - public void Temps(){ - Timer timer = new Timer(); - timer.start(); - } - public double EncodeurBalayeuse(){ - return EncodeurBalayeuse.getDouble(1.8); + + public Command ejecter() { + return balaye(-vitesseMoteurBas, -vitesseMoteurHaut); } + /** Creates a new Balayeuse. */ public Balayeuse() { - teb.addBoolean("limit balayeuse", this::GetLimiSwtich); - teb.addDouble("encodeur balayeuse", this::Distance); } @Override diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9b70aea..189d108 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,6 +1,8 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; import java.util.Optional; import java.util.function.Supplier; @@ -9,8 +11,10 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -19,6 +23,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Matrix; 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.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -29,7 +34,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - +import frc.robot.Coordinates; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; /** @@ -56,107 +61,107 @@ 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(); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + /* + * SysId routine for characterizing translation. This is used to find PID gains + * for the drive motors. + */ + @SuppressWarnings("unused") private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this)); - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + /* + * SysId routine for characterizing steer. This is used to find PID gains for + * the steer motors. + */ + @SuppressWarnings("unused") private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this)); /* * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + * This is used to find PID gains for the FieldCentricFacingAngle + * HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on + * importing the log to SysId. */ + @SuppressWarnings("unused") private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this)); private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - private void configureAutoBuilder() { + + private void configureAutoBuilder() { try { RobotConfig config = RobotConfig.fromGUISettings(); AutoBuilder.configure( - () -> getState().Pose, - this::resetPose, - () -> getState().Speeds, - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - new PIDConstants(10, 0, 0), - new PIDConstants(7, 0, 0) - ), - config, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this - ); + () -> getState().Pose, + this::resetPose, + () -> getState().Speeds, + (speeds, feedforwards) -> setControl( + m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + new PIDConstants(10, 0, 0), + new PIDConstants(7, 0, 0)), + config, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this); } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", + ex.getStackTrace()); } } + /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { super(drivetrainConstants, modules); if (Utils.isSimulation()) { startSimThread(); @@ -167,8 +172,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through * getters in the classes. * * @param drivetrainConstants Drivetrain-wide constants for the swerve drive @@ -178,10 +185,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su * @param modules Constants for each specific module */ public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { super(drivetrainConstants, odometryUpdateFrequency, modules); if (Utils.isSimulation()) { startSimThread(); @@ -192,30 +198,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on + * unspecified or set to 0 Hz, this is 250 Hz + * on * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters + * @param odometryStandardDeviation The standard deviation for odometry + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters + * @param visionStandardDeviation The standard deviation for vision + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters * and radians * @param modules Constants for each specific module */ public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, + modules); if (Utils.isSimulation()) { startSimThread(); } @@ -223,7 +237,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } /** - * Returns a command that applies the specified control request to this swerve drivetrain. + * Returns a command that applies the specified control request to this swerve + * drivetrain. * * @param request Function returning the request to apply * @return Command to run @@ -231,44 +246,26 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public Command applyRequest(Supplier request) { return run(() -> this.setControl(request.get())); } - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } @Override public void periodic() { - if(getPigeon2().getYaw().getValueAsDouble() > 360){ - getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()-360); - } - else if(getPigeon2().getYaw().getValueAsDouble() < 0){ - getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360); - } - /* * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + * If we haven't applied the operator perspective before, then we should apply + * it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts + * mid-match. + * Otherwise, only check and apply the operator perspective if the DS is + * disabled. + * This ensures driving behavior doesn't change until an explicit disable event + * occurs during testing. */ if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { DriverStation.getAlliance().ifPresent(allianceColor -> { setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); m_hasAppliedOperatorPerspective = true; }); } @@ -290,11 +287,14 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * Adds a vision measurement to the Kalman Filter. This will correct the + * odometry pose estimate * while still accounting for measurement noise. * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionRobotPoseMeters The pose of the robot as measured by the vision + * camera. + * @param timestampSeconds The timestamp of the vision measurement in + * seconds. */ @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { @@ -302,35 +302,74 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * Adds a vision measurement to the Kalman Filter. This will correct the + * odometry pose estimate * while still accounting for measurement noise. *

* Note that the vision measurement standard deviations passed into this method * will continue to apply to future measurements until a subsequent call to * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement - * in the form [x, y, theta]ᵀ, with units in meters and radians. + * @param visionRobotPoseMeters The pose of the robot as measured by the + * vision camera. + * @param timestampSeconds The timestamp of the vision measurement in + * seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement + * in the form [x, y, theta]ᵀ, with units in + * meters and radians. */ @Override public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs - ) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), + visionMeasurementStdDevs); } /** * Return the pose at a given timestamp, if the buffer is not empty. * * @param timestampSeconds The timestamp of the pose in seconds. - * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). + * @return The pose at the given timestamp (or Optional.empty() if the buffer is + * empty). */ @Override public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } + + private SwerveRequest.FieldCentricFacingAngle fieldCentricFacingAngle = new FieldCentricFacingAngle() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage).withHeadingPID(7, 0, 0); + + public Command viserReservoir(Supplier velocityX, Supplier velocityY) { + return runEnd(() -> { + Translation2d diff = Coordinates.diffFromHub(getState().Pose.getTranslation()); + Rotation2d angle = diff.getAngle(); + setControl(fieldCentricFacingAngle + .withTargetDirection(angle) + .withVelocityX(velocityX.get()) + .withVelocityY(velocityY.get())); + }, () -> setControl(fieldCentricFacingAngle.withVelocityX(0).withVelocityY(0))); + } + + public Command allerGrimper(Supplier diffSupplier) { + return runEnd(() -> { + Translation2d diff = diffSupplier.get(); + Rotation2d angle = diff.getAngle().rotateBy(new Rotation2d(Degrees.of(-90))); + setControl(fieldCentricFacingAngle + .withTargetDirection(angle) + .withVelocityX(diff.getX()) + .withVelocityY(diff.getY())); + }, () -> setControl(fieldCentricFacingAngle.withVelocityX(0).withVelocityY(0))); + } + + public Command allerGrimpeMur() { + return allerGrimper(() -> Coordinates.diffFromGrimpeMur(getState().Pose.getTranslation())); + } + + public Command allerGrimpeDepot() { + return allerGrimper(() -> Coordinates.diffFromGrimpeDepot(getState().Pose.getTranslation())); + } } diff --git a/src/main/java/frc/robot/subsystems/Demeleur.java b/src/main/java/frc/robot/subsystems/Demeleur.java new file mode 100644 index 0000000..5b4281c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Demeleur.java @@ -0,0 +1,33 @@ +// 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.subsystems; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Demeleur extends SubsystemBase { + + SparkMax moteur = new SparkMax(19, MotorType.kBrushless); + + /** Creates a new Demeleur. */ + public Demeleur() { + } + + public Command run() { + return startEnd(() -> moteur.set(1), () -> moteur.set(0)); + } + + public Command inverse() { + return startEnd(() -> moteur.set(-0.2), () -> moteur.set(0)); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index 1ba82c3..df45810 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -4,48 +4,76 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Grimpeur extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless); - SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless); - SparkMaxConfig slaveConfig = new SparkMaxConfig(); + SparkMax moteur1 = new SparkMax(3, MotorType.kBrushless); + SparkMax moteur2 = new SparkMax(12, MotorType.kBrushless); + RelativeEncoder encoder1 = moteur1.getEncoder(); + RelativeEncoder encoder2 = moteur2.getEncoder(); + + private double vitesseSortir = 0.5; + private double vitesseEntrer = -0.4; + + private GenericEntry ntEncodeurGrimpeur = teb.add("Position haut grimpeur", 101).getEntry(); + + private double maxEncodeur = ntEncodeurGrimpeur.getDouble(101); + DigitalInput limit = new DigitalInput(0); - private GenericEntry EncodeurGrimpeur = - teb.add("Position haut grimpeur", 101).getEntry(); - public void Grimper(double vitesse){ - grimpeur1.set(vitesse); - grimpeur2.set(vitesse); - } - public double Position(){ - return grimpeur1.getEncoder().getPosition(); - } - public void Reset(){ - grimpeur1.getEncoder().setPosition(0); - } - public boolean Limit(){ - return limit.get(); - } - public double PositionFinal(){ - return EncodeurGrimpeur.getDouble(101); - } - /** Creates a new Grimpeur. */ + public Grimpeur() { - teb.addDouble("encodeur grimpeur", this::Position); - teb.addBoolean("limit grimpeur", this::Limit); + teb.addDouble("encodeur grimpeur", encoder1::getPosition); + teb.addBoolean("limit grimpeur", limit::get); + configMoteurs(); + } + + private void configMoteurs() { + SparkBaseConfig configMoteur = new SparkMaxConfig().apply( + new SoftLimitConfig().forwardSoftLimit(maxEncodeur).forwardSoftLimitEnabled(true) + .reverseSoftLimit(0).reverseSoftLimitEnabled(true)) + .openLoopRampRate(0.1) + .idleMode(IdleMode.kBrake); + moteur1.configure(configMoteur, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + moteur2.configure(configMoteur.follow(moteur1), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public Command sortir() { + return startEnd(() -> moteur1.set(vitesseSortir), () -> moteur1.set(0)) + .until(() -> moteur1.getForwardSoftLimit().isReached() || moteur2.getForwardSoftLimit().isReached()); + } + + public Command rentrer() { + return startEnd(() -> moteur1.set(vitesseEntrer), () -> moteur1.set(0)) + .until(() -> moteur1.getReverseSoftLimit().isReached() || moteur2.getReverseSoftLimit().isReached()); } @Override public void periodic() { - // This method will be called once per scheduler run + if (limit.get()) { + encoder1.setPosition(0); + encoder2.setPosition(0); + } + + double newEncoderMax = ntEncodeurGrimpeur.getDouble(maxEncodeur); + if (newEncoderMax != maxEncodeur) { + maxEncodeur = newEncoderMax; + configMoteurs(); + } } } diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index 5fc74ce..f5b493e 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -4,48 +4,71 @@ package frc.robot.subsystems; +import java.util.function.Supplier; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Lanceur extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); + SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless); SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless); - SparkMax Demeleur = new SparkMax(19, MotorType.kBrushless); - GenericEntry vitesse = - teb.add("vitesse lanceur",4000).getEntry(); - GenericEntry AmpLanceur = - teb.add("ampérage lanceur",30).getEntry(); - - public void Lancer(double vitesse){ - moteur1.set(vitesse); - moteur2.set(vitesse); - } - public void Demeler(double vitesse){ - Demeleur.set(vitesse); - } - public double Vitesse(){ - return moteur1.getEncoder().getVelocity(); - } - public double Amp(){ - return moteur1.getOutputCurrent(); - } - public double AmpBas(){ - return AmpLanceur.getDouble(30); - } - public double vitesseDemander(){ - return vitesse.getDouble(4000); - } + + RelativeEncoder encoder = moteur1.getEncoder(); + + GenericEntry vitesse = teb.add("vitesse lanceur", 4000).getEntry(); + GenericEntry AmpLanceur = teb.add("ampérage lanceur", 30).getEntry(); + /** Creates a new Lanceur. */ public Lanceur() { - teb.addDouble("amperage lanceur", this::Amp); - teb.addDouble("vitesse actuelle",this::Vitesse); + teb.addDouble("amperage lanceur", moteur1::getOutputCurrent); + teb.addDouble("vitesse actuelle", encoder::getVelocity); + configMoteurs(); + } + + private void configMoteurs() { + SparkBaseConfig configMoteur = new SparkMaxConfig().apply( + new ClosedLoopConfig().pid(0.0007, 0, 0)) + .openLoopRampRate(0.1) + .idleMode(IdleMode.kCoast); + moteur1.configure(configMoteur, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + moteur2.configure(configMoteur.follow(moteur1), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public Command lancer(Supplier vitesse) { + return runEnd(() -> moteur1.getClosedLoopController().setSetpoint(vitesse.get(), ControlType.kVelocity), + () -> moteur1.set(0)); + } + + public Command inverse() { + return startEnd(() -> moteur1.set(-0.2), () -> moteur1.set(0)); + } + + public double getVitesse() { + return encoder.getVelocity(); + } + + public boolean setPointAtteint() { + return getVitesse() >= moteur1.getClosedLoopController().getSetpoint() * 0.95; + } + + public double ntBasseVitesse() { + return vitesse.getDouble(4000); } @Override @@ -53,4 +76,3 @@ public class Lanceur extends SubsystemBase { // This method will be called once per scheduler run } } - diff --git a/src/main/java/frc/robot/subsystems/LimeLight3.java b/src/main/java/frc/robot/subsystems/LimeLight3.java deleted file mode 100644 index 5a66220..0000000 --- a/src/main/java/frc/robot/subsystems/LimeLight3.java +++ /dev/null @@ -1,74 +0,0 @@ -// 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.subsystems; - -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.LimelightHelpers; - -public class LimeLight3 extends SubsystemBase { - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry pipeline = table.getEntry("pipeline"); - /** Creates a new LimeLight3. */ - public LimeLight3() { - for(int port = 5800; port <=5807; port++){ - PortForwarder.add(port, "limelight.local", port); - } - } - public double[] getBotPoseBlue(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue"); - double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); - return BotPose; - } - public double[] getBotPoseRed(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired"); - double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); - return BotPose; - } - public double getTx(){ - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry tx = table.getEntry("tx"); - return tx.getDouble(0.0); - } - public double getTId(){ - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry tid = table.getEntry("tid"); - return tid.getDouble(0.0); - } - public double getTA(){ - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry ta = table.getEntry("ta"); - return ta.getDouble(0.0); - } - public boolean getV(){ - return LimelightHelpers.getTV("limelight-balaie"); - } - public void AprilTag(){ - pipeline.setNumber(0); - } - public void Forme(){ - pipeline.setNumber(1); - } - public double Calcule(double x1, double x2, double y1, double y2, double angle) - { - if (x1 > 4) - { - return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90; - } - else - { - return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90; - } - } - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java deleted file mode 100644 index dbee931..0000000 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ /dev/null @@ -1,76 +0,0 @@ -// 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.subsystems; - -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.LimelightHelpers; - -public class Limelight3G extends SubsystemBase { -NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); - NetworkTableEntry pipeline = table.getEntry("pipeline"); - /** Creates a new LimeLight3. */ - public Limelight3G() { - for(int port = 5800; port <=5807; port++){ - PortForwarder.add(port, "limelight.local", port); - } - } - public double[] getBotPoseBlue(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); - double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); - return BotPose; - } - public double[] getBotPoseRed(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); - double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); - return BotPose; - } - public double getTx(){ - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); - NetworkTableEntry tx = table.getEntry("tx"); - return tx.getDouble(0.0); - } - public double getTId(){ - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); - NetworkTableEntry tid = table.getEntry("tid"); - return tid.getDouble(0.0); - } - public double getTA(){ - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); - NetworkTableEntry ta = table.getEntry("ta"); - return ta.getDouble(0.0); - } - public boolean getV(){ - return LimelightHelpers.getTV("limelight-tag"); - } - public double Calcule(double x1, double x2, double y1, double y2, double angle) - { - if(x1 > x2){ - if(y1 > y2){ - return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle; - } - else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle; - } - } - else{ - if(y1 > y2){ - return Math.atan(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle; - } - else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - angle; - } - } - } - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/PivotBalayeuse.java b/src/main/java/frc/robot/subsystems/PivotBalayeuse.java new file mode 100644 index 0000000..3bd0018 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PivotBalayeuse.java @@ -0,0 +1,82 @@ +// 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.subsystems; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class PivotBalayeuse extends SubsystemBase { + + ShuffleboardTab teb = Shuffleboard.getTab("teb"); + + SparkMax moteur = new SparkMax(8, MotorType.kBrushless); + RelativeEncoder encoder = moteur.getEncoder(); + DigitalInput limit = new DigitalInput(9); + + private GenericEntry ntEncodeurBalayeuse = teb.add("Position bas balayeuse", 1.8).getEntry(); + + private double maxEncodeur = ntEncodeurBalayeuse.getDouble(1.8); + + private double vitesseMonter = 0.5; + private double vitesseDescendre = -0.5; + + /** Creates a new PivotBalayeuse. */ + public PivotBalayeuse() { + configMoteurs(); + } + + private void configMoteurs() { + SparkBaseConfig configMoteur = new SparkMaxConfig().apply( + new SoftLimitConfig().forwardSoftLimit(maxEncodeur).forwardSoftLimitEnabled(true) + .reverseSoftLimit(0.6).reverseSoftLimitEnabled(true)) + .openLoopRampRate(0.1) + .idleMode(IdleMode.kBrake); + moteur.configure(configMoteur, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public Command monter() { + return startEnd(() -> moteur.set(vitesseMonter), () -> moteur.set(0)) + .until(() -> moteur.getForwardSoftLimit().isReached()); + } + + public Command descendre() { + return startEnd(() -> moteur.set(vitesseDescendre), () -> moteur.set(0)) + .until(() -> moteur.getReverseSoftLimit().isReached()); + } + + public double EncodeurBalayeuse() { + return ntEncodeurBalayeuse.getDouble(1.8); + } + + @Override + public void periodic() { + teb.addBoolean("limit balayeuse", limit::get); + teb.addDouble("encodeur balayeuse", encoder::getPosition); + + if (!limit.get()) { + encoder.setPosition(0.6); + } + + double newEncoderMax = ntEncodeurBalayeuse.getDouble(maxEncodeur); + if (newEncoderMax != maxEncodeur) { + maxEncodeur = newEncoderMax; + configMoteurs(); + } + } +}