From 99fe2c7f93be3ee45ba22e235edae27223d1fcd7 Mon Sep 17 00:00:00 2001 From: Olivier Demers Date: Mon, 20 Mar 2023 22:28:06 +0000 Subject: [PATCH] bras --- src/main/java/frc/robot/RobotContainer.java | 171 +++++++++--------- ...{PivotChercheHaut.java => Bougerbras.java} | 45 ++--- ...{PivotBrasRentre.java => Bougerpivot.java} | 49 ++--- .../commands/bras/DescendrePivotBras.java | 25 +++ .../robot/commands/bras/MonterPivotBras.java | 28 +++ .../robot/commands/bras/PivotChercheBas.java | 67 ------- .../subsystems/bras/BrasTelescopique.java | 2 +- .../java/frc/robot/subsystems/bras/Pivot.java | 1 + 8 files changed, 177 insertions(+), 211 deletions(-) rename src/main/java/frc/robot/commands/bras/{PivotChercheHaut.java => Bougerbras.java} (58%) rename src/main/java/frc/robot/commands/bras/{PivotBrasRentre.java => Bougerpivot.java} (53%) create mode 100644 src/main/java/frc/robot/commands/bras/DescendrePivotBras.java create mode 100644 src/main/java/frc/robot/commands/bras/MonterPivotBras.java delete mode 100644 src/main/java/frc/robot/commands/bras/PivotChercheBas.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ba220d..4efd8e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; + import java.util.Map; import edu.wpi.first.cameraserver.CameraServer; @@ -39,75 +40,63 @@ import frc.robot.commands.Gyro; import frc.robot.commands.Reculer; import frc.robot.commands.Tape; import frc.robot.commands.bras.BrasManuel; +import frc.robot.commands.bras.DescendrePivotBras; import frc.robot.commands.bras.FermePince; +import frc.robot.commands.bras.MonterPivotBras; import frc.robot.commands.bras.OuvrePince; -import frc.robot.commands.bras.PivotBrasRentre; -import frc.robot.commands.bras.PivotChercheBas; -import frc.robot.commands.bras.PivotChercheHaut; import frc.robot.commands.bras.PivotManuel; -import frc.robot.commands.bras.PivoteBrasBas; -import frc.robot.commands.bras.PivoteBrasHaut; -import frc.robot.commands.bras.PivoteBrasMilieux; - public class RobotContainer { - -CommandXboxController manette1 = new CommandXboxController(0); -CommandXboxController manette2 = new CommandXboxController(1); -// dashboard -SendableChooser chooser = new SendableChooser<>(); -String enhaut = "en haut"; -String aumilieux = "au milieux"; -String enbas = "en bas"; -String nulpart = "nul part"; -ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto",BuiltInLayouts.kList) -.withSize(2, 2).withProperties(Map.of("Label position","LEFT")); -GenericEntry autobalance = layoutauto.add("choix balance",true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); -GenericEntry autosortir = layoutauto.add("choix sortir",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); -GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir",0).getEntry(); -GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", 0).getEntry(); -GenericEntry avancerdistance = layoutauto.addPersistent("avancer",0).getEntry(); + CommandXboxController manette1 = new CommandXboxController(0); + CommandXboxController manette2 = new CommandXboxController(1); -// subsystems -BasePilotable basePilotable = new BasePilotable(); -Gratte gratte = new Gratte(); -BrasTelescopique brasTelescopique = new BrasTelescopique(); -Pince pince = new Pince(); -Pivot pivot = new Pivot(); -Limelight limelight = new Limelight(); + // dashboard + SendableChooser chooser = new SendableChooser<>(); + String enhaut = "en haut"; + String aumilieux = "au milieux"; + String enbas = "en bas"; + String nulpart = "nul part"; + ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList) + .withSize(2, 2).withProperties(Map.of("Label position", "LEFT")); + GenericEntry autobalance = layoutauto.add("choix balance", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); + GenericEntry autosortir = layoutauto.add("choix sortir", false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); + GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir", 0).getEntry(); + GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", 0).getEntry(); + GenericEntry avancerdistance = layoutauto.addPersistent("avancer", 0).getEntry(); -//commands -BrakeFerme brakeFerme = new BrakeFerme(basePilotable); -BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable); -GratteBaisser gratteBaisser = new GratteBaisser(gratte); -GratteMonte gratteMonte = new GratteMonte(gratte); -Gyro gyro = new Gyro(basePilotable); -FermePince fermePince = new FermePince(pince); -OuvrePince ouvrePince = new OuvrePince(pince); -PivotBrasRentre pivotBrasRentre = new PivotBrasRentre(brasTelescopique, pivot); -PivoteBrasBas pivoteBrasBas = new PivoteBrasBas(brasTelescopique, pivot); -PivoteBrasMilieux pivoteBrasMilieux = new PivoteBrasMilieux(brasTelescopique, pivot); -PivoteBrasHaut pivoteBrasHaut = new PivoteBrasHaut(brasTelescopique, pivot); -Cone cone = new Cone(limelight, basePilotable, ()->-manette1.getLeftY()); -Reculer reculers = new Reculer(basePilotable, ()->reculerdistances.getDouble(0)); -Reculer reculerb = new Reculer(basePilotable, ()->reculerdistanceb.getDouble(0)); -Avancer avancer = new Avancer(basePilotable, ()->avancerdistance.getDouble(0)); -PivotChercheBas pivotChercheBas = new PivotChercheBas(brasTelescopique, pivot); -PivotChercheHaut pivotChercheHaut = new PivotChercheHaut(brasTelescopique, pivot); -Cube cube = new Cube(limelight, basePilotable, null); -Apriltag aprilTag = new Apriltag(limelight, basePilotable, null); -Tape tape = new Tape(limelight, basePilotable, null); -PivotManuel pivotManuel = new PivotManuel(pivot,manette2::getLeftX); -BrasManuel brasManuel = new BrasManuel(brasTelescopique,manette2::getLeftY); + // subsystems + BasePilotable basePilotable = new BasePilotable(); + Gratte gratte = new Gratte(); + BrasTelescopique brasTelescopique = new BrasTelescopique(); + Pince pince = new Pince(); + Pivot pivot = new Pivot(); + Limelight limelight = new Limelight(); + // commands + BrakeFerme brakeFerme = new BrakeFerme(basePilotable); + BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable); + GratteBaisser gratteBaisser = new GratteBaisser(gratte); + GratteMonte gratteMonte = new GratteMonte(gratte); + Gyro gyro = new Gyro(basePilotable); + FermePince fermePince = new FermePince(pince); + OuvrePince ouvrePince = new OuvrePince(pince); + Cone cone = new Cone(limelight, basePilotable, () -> -manette1.getLeftY()); + Reculer reculers = new Reculer(basePilotable, () -> reculerdistances.getDouble(0)); + Reculer reculerb = new Reculer(basePilotable, () -> reculerdistanceb.getDouble(0)); + Avancer avancer = new Avancer(basePilotable, () -> avancerdistance.getDouble(0)); + Cube cube = new Cube(limelight, basePilotable, () -> manette1.getLeftY()); + Apriltag aprilTag = new Apriltag(limelight, basePilotable, () -> manette1.getLeftY()); + Tape tape = new Tape(limelight, basePilotable, () -> manette1.getLeftY()); + PivotManuel pivotManuel = new PivotManuel(pivot, manette2::getLeftX); + BrasManuel brasManuel = new BrasManuel(brasTelescopique, manette2::getLeftY); -public RobotContainer() { + public RobotContainer() { chooser.addOption(enhaut, enhaut); chooser.addOption(enbas, enbas); chooser.addOption(aumilieux, aumilieux); chooser.addOption(nulpart, nulpart); - layoutauto.add("choix hauteur",chooser); + layoutauto.add("choix hauteur", chooser); configureBindings(); CameraServer.startAutomaticCapture(); basePilotable.setDefaultCommand(new RunCommand(() -> { @@ -116,48 +105,58 @@ public RobotContainer() { brasTelescopique.setDefaultCommand(brasManuel); pivot.setDefaultCommand(pivotManuel); } + private void configureBindings() { // manette 1 - manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); - manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable)); - manette1.leftBumper().toggleOnTrue(aprilTag); - manette1.rightBumper().toggleOnTrue(tape); - manette1.povUp().whileTrue(pivoteBrasHaut); - manette1.povDown().whileTrue(pivoteBrasBas); - manette1.povRight().whileTrue(pivoteBrasMilieux); - manette1.povLeft().whileTrue(pivotBrasRentre); - //manette 2 - manette2.povDown().onTrue(pivotChercheBas); - manette2.povUp().onTrue(pivotChercheHaut); - manette2.rightBumper().toggleOnTrue(cube); - manette2.leftBumper().toggleOnTrue(cone); + manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer, pince)); + manette1.x().onTrue(brakeOuvre); + manette1.b().onTrue(brakeFerme); + manette1.leftBumper().whileTrue(aprilTag); + manette1.rightBumper().whileTrue(tape); + manette1.povUp().whileTrue(creerCommandBras(51, -40)); + manette1.povDown().whileTrue(creerCommandBras(9, -14)); + manette1.povRight().whileTrue(creerCommandBras(44, -17)); + manette1.povLeft().whileTrue(creerCommandBras(0, 0)); + // manette 2 + manette2.povDown().onTrue(creerCommandBras(9, -18)); + manette2.povUp().onTrue(creerCommandBras(44, 0)); + manette2.rightBumper().whileTrue(cube); + manette2.leftBumper().whileTrue(cone); manette2.y().whileTrue(gyro); manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro)); manette2.a().whileTrue(gratteMonte); manette2.b().whileTrue(gratteBaisser); } - //mode auto + + private Command creerCommandBras(double distancePivot, double distanceBras) { + return Commands.either( + new MonterPivotBras(brasTelescopique, pivot, distanceBras, distancePivot), + new DescendrePivotBras(brasTelescopique, pivot, distanceBras, distancePivot), + () -> pivot.distance() < distancePivot); + } + public Command getAutonomousCommand() { chooser.getSelected(); autobalance.getBoolean(true); - return new SequentialCommandGroup( - Commands.select(Map.ofEntries( - Map.entry(enhaut,pivoteBrasHaut), - Map.entry(aumilieux,pivoteBrasMilieux), - Map.entry(enbas,pivoteBrasBas), - Map.entry(nulpart,pivotBrasRentre) - ), chooser::getSelected), - new OuvrePince(pince).unless(()->chooser.getSelected().equals(nulpart)), - Commands.waitSeconds(1), - new FermePince(pince).unless(()->chooser.getSelected().equals(nulpart)), - new PivotBrasRentre(brasTelescopique , pivot).unless(()->chooser.getSelected().equals(nulpart)), - Commands.waitSeconds(1), - Commands.either(reculers, reculerb,()-> autosortir.getBoolean(true)), - new Avancer(basePilotable,()-> avancerdistance.getDouble(0)).unless(()->!autosortir.getBoolean(true)|| autobalance.getBoolean(false)), - Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true)) - ).deadlineWith(Commands.waitSeconds(14.6)).andThen(brakeOuvre); - + return Commands.deadline( + Commands.waitSeconds(14.6), + new SequentialCommandGroup( + Commands.select(Map.ofEntries( + Map.entry(enhaut, creerCommandBras(51, -40)), + Map.entry(aumilieux, creerCommandBras(9, -14)), + Map.entry(enbas, creerCommandBras(44, -17)), + Map.entry(nulpart, creerCommandBras(0, 0))), chooser::getSelected), + new OuvrePince(pince).unless(() -> chooser.getSelected().equals(nulpart)), + Commands.waitSeconds(1), + fermePince.unless(() -> chooser.getSelected().equals(nulpart)), + creerCommandBras(0, 0).unless(() -> chooser.getSelected().equals(nulpart)), + Commands.waitSeconds(1), + Commands.either(reculers, reculerb, () -> autosortir.getBoolean(true)), + new Avancer(basePilotable, () -> avancerdistance.getDouble(0)) + .unless(() -> !autosortir.getBoolean(true) || !autobalance.getBoolean(false)), + Commands.either(gyro, Commands.none(), () -> autobalance.getBoolean(true)))) + .andThen(brakeOuvre); } } diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java b/src/main/java/frc/robot/commands/bras/Bougerbras.java similarity index 58% rename from src/main/java/frc/robot/commands/bras/PivotChercheHaut.java rename to src/main/java/frc/robot/commands/bras/Bougerbras.java index 19bfcac..6390c94 100644 --- a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java +++ b/src/main/java/frc/robot/commands/bras/Bougerbras.java @@ -6,45 +6,36 @@ package frc.robot.commands.bras; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.bras.BrasTelescopique; -import frc.robot.subsystems.bras.Pivot; -public class PivotChercheHaut extends CommandBase { - private BrasTelescopique brasTelescopique; - private Pivot pivot; - /** Creates a new PivotChercheHaut. */ - public PivotChercheHaut(BrasTelescopique brasTelescopique, Pivot pivot) { +public class Bougerbras extends CommandBase { + /** Creates a new bougerbras. */ + double distance; + BrasTelescopique brasTelescopique; + public Bougerbras(BrasTelescopique brasTelescopique,double distance) { this.brasTelescopique = brasTelescopique; - this.pivot = pivot; - // Use addRequirements() here to declare subsystem dependencies. + this.distance = distance; addRequirements(brasTelescopique); - addRequirements(pivot); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { - brasTelescopique.fermer(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(brasTelescopique.photocell()){ - brasTelescopique.ouvrir(); - brasTelescopique.Reset(); - brasTelescopique.AvanceRecule(0); - } - else{ + if(brasTelescopique.distance()>distance+0.5 ) { + brasTelescopique.AvanceRecule(-0.3); + + } + else if(brasTelescopique.distance()44.5) { - pivot.monteDescendre(-0.5); - } - else{ - pivot.monteDescendre(0); + else { + brasTelescopique.AvanceRecule(0); } } @@ -52,13 +43,13 @@ public class PivotChercheHaut extends CommandBase { @Override public void end(boolean interrupted) { brasTelescopique.AvanceRecule(0); - pivot.monteDescendre(0); - brasTelescopique.ouvrir(); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return brasTelescopique.distance()>distance-0.5 && brasTelescopique.distance()distance+0.5 ) { pivot.monteDescendre(-0.5); } + else if(pivot.distance()distance-0.5 && pivot.distance()pivot.distance()>8),new Bougerbras(brasTelescopique, distanceBras)), + new Bougerpivot(pivot, distancePivot) + + ); + } +} diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheBas.java b/src/main/java/frc/robot/commands/bras/PivotChercheBas.java deleted file mode 100644 index d52b893..0000000 --- a/src/main/java/frc/robot/commands/bras/PivotChercheBas.java +++ /dev/null @@ -1,67 +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.bras; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.bras.BrasTelescopique; -import frc.robot.subsystems.bras.Pivot; - -public class PivotChercheBas extends CommandBase { - private BrasTelescopique brasTelescopique; - private Pivot pivot; - /** Creates a new PivotChercheBas. */ - public PivotChercheBas(BrasTelescopique brasTelescopique, Pivot pivot) { - this.brasTelescopique = brasTelescopique; - this.pivot = pivot; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(brasTelescopique); - addRequirements(pivot); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - brasTelescopique.fermer(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(brasTelescopique.distance()>-17.5){ - brasTelescopique.AvanceRecule(-0.2); - brasTelescopique.fermer(); - } - else if(brasTelescopique.distance()<-19.5) { - brasTelescopique.AvanceRecule(0.2); - } - else { - brasTelescopique.AvanceRecule(0); - brasTelescopique.ouvrir(); - } - if (pivot.distance()<8.5){ - pivot.monteDescendre(0.3); - } - else if(pivot.distance()>10.5) { - pivot.monteDescendre(-0.3); - } - else{ - pivot.monteDescendre(0); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - brasTelescopique.AvanceRecule(0); - pivot.monteDescendre(0); - brasTelescopique.fermer(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java index 12b82b9..dc2d69b 100644 --- a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java +++ b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java @@ -51,6 +51,6 @@ public class BrasTelescopique extends SubsystemBase { } @Override public void periodic() { - + if(photocell())Reset(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/bras/Pivot.java b/src/main/java/frc/robot/subsystems/bras/Pivot.java index 6fc13b9..744020a 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pivot.java +++ b/src/main/java/frc/robot/subsystems/bras/Pivot.java @@ -42,6 +42,7 @@ public class Pivot extends SubsystemBase { } @Override public void periodic() { + if(limitpivot())Reset(); }