diff --git a/simgui.json b/simgui.json index 1d8ea9a..28a4c44 100644 --- a/simgui.json +++ b/simgui.json @@ -1,7 +1,9 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/Shuffleboard/teb/DifferentialDrive[1]": "DifferentialDrive", + "/Shuffleboard/teb/auto/choix hauteur": "String Chooser" } }, "NetworkTables": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b2ce20d..13b0abe 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,31 +1,33 @@ package frc.robot; public class Constants { - public static int avantdroit = 1; - public static int avantgauche = 4; + public static int avantdroit = 4; + public static int avantgauche = 6; public static int arrieredroit = 3; public static int arrieregauche = 5; - public static int BrasTelescopique = 2; - public static int pivot = 0; + public static int BrasTelescopique = 1; + public static int pivot = 2; //moteur - public static int leverGratte = 0; - public static int baiserGratte = 1; + + + public static int GratteD = 7; + public static int GratteG = 8; + // pneumatique public static int pistonpinceouvre = 0; public static int pistonpinceferme = 1; - public static int actuateur = 8; - public static int brakedroit = 3; - public static int brakegauche = 4; - public static int brakewinchf = 5; - public static int brakewinchb = 5; + public static int brakeouvre = 5; + public static int brakeferme = 4; + public static int brakewinchf = 3; + public static int brakewinchb = 2; // DIO - public static int limitbd = 0; - public static int limitbg = 2; - public static int limithd = 3; - public static int limithg = 1; - public static int photocell = 4; + public static int limitbd = 2; + public static int limitbg = 4; + public static int limithd = 1; + public static int limithg = 3; + public static int photocell = 0; public static int limitpivot = 5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5a5bc2..a68925a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; import java.util.Map; +import java.util.function.DoubleSupplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.networktables.GenericEntry; @@ -14,8 +15,9 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; //subsystems @@ -25,17 +27,25 @@ import frc.robot.subsystems.bras.BrasTelescopique; import frc.robot.subsystems.bras.Pince; import frc.robot.subsystems.bras.Pivot; import frc.robot.subsystems.Limelight; +import frc.robot.commands.Apriltag; +import frc.robot.commands.Avancer; // command import frc.robot.commands.BrakeFerme; import frc.robot.commands.BrakeOuvre; import frc.robot.commands.Cone; +import frc.robot.commands.Cube; import frc.robot.commands.GratteBaisser; import frc.robot.commands.GratteMonte; 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.FermePince; 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; @@ -45,6 +55,19 @@ public class RobotContainer { CommandXboxController manette1 = new CommandXboxController(0); CommandXboxController manette2 = new CommandXboxController(1); + +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(); // subsystems BasePilotable basePilotable = new BasePilotable(); Gratte gratte = new Gratte(); @@ -65,7 +88,17 @@ 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 reculer = new Reculer(basePilotable); +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); + SendableChooser chooser = new SendableChooser<>(); String enhaut = "en bas"; @@ -82,47 +115,51 @@ public RobotContainer() { chooser.addOption(aumilieux, aumilieux); chooser.addOption(nulpart, nulpart); layoutauto.add("choix hauteur",chooser); - - configureBindings(); - + CameraServer.startAutomaticCapture(); basePilotable.setDefaultCommand(new RunCommand(() -> { - basePilotable.drive(-manette1.getLeftY(), manette1.getLeftX()); - },basePilotable)); - -} - + basePilotable.drive(-manette1.getLeftY(), -manette1.getLeftX()); + }, basePilotable)); + 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.y().whileTrue(gyro); - manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable)); - /*manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte)); - manette2.a().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasHaut, brasTelescopique::pivoteBrasHaut)); - manette2.b().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasBas, brasTelescopique::pivoteBrasBas, brasTelescopique)); - manette2.x().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasMilieux, brasTelescopique::pivoteBrasMilieux, brasTelescopique)); - manette2.y().toggleOnTrue(Commands.startEnd(brasTelescopique::pivotBrasRentre, brasTelescopique::pivotBrasRentre, brasTelescopique)); - manette2.povRight().toggleOnTrue(Commands.startEnd(brasTelescopique::PivotChercheBas, brasTelescopique::PivotChercheBas, brasTelescopique)); - manette2.povLeft().toggleOnTrue(Commands.startEnd(brasTelescopique::PivotChercheHaut, brasTelescopique::PivotChercheHaut, brasTelescopique)); - manette2.rightBumper().toggleOnTrue(Commands.startEnd(null, null, null)); - manette2.leftBumper().toggleOnTrue(Commands.startEnd(null, null, null));**/ - manette2.leftBumper().toggleOnTrue(Commands.startEnd(null, null, null)); - + 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); + manette2.y().whileTrue(gyro); + manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro)); + manette2.a().whileTrue(gratteMonte); + manette2.b().whileTrue(gratteBaisser); + } public Command getAutonomousCommand() { chooser.getSelected(); autobalance.getBoolean(true); return new SequentialCommandGroup( - Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true)), - Commands.either(reculer, Commands.none(),()-> autosortir.getBoolean(true)), Commands.select(Map.ofEntries( Map.entry(enhaut,pivoteBrasHaut), - Map.entry(aumilieux,null), - Map.entry(enbas,null), - Map.entry(nulpart,null) - ), chooser::getSelected) - ); + Map.entry(aumilieux,pivoteBrasMilieux), + Map.entry(enbas,pivoteBrasBas), + Map.entry(nulpart,pivotBrasRentre) + ), chooser::getSelected), + ouvrePince.unless(()->chooser.getSelected().equals(nulpart)), + Commands.either(reculers, reculerb,()-> autosortir.getBoolean(true)), + avancer.unless(()->!autosortir.getBoolean(true)|| !autobalance.getBoolean(false)), + Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true)) + ).deadlineWith(Commands.waitSeconds(14.6)).andThen(brakeOuvre); } diff --git a/src/main/java/frc/robot/commands/Apriltag.java b/src/main/java/frc/robot/commands/Apriltag.java index 3ef6b06..9ef54b3 100644 --- a/src/main/java/frc/robot/commands/Apriltag.java +++ b/src/main/java/frc/robot/commands/Apriltag.java @@ -32,7 +32,7 @@ public class Apriltag extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0); + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Avancer.java b/src/main/java/frc/robot/commands/Avancer.java new file mode 100644 index 0000000..0b94017 --- /dev/null +++ b/src/main/java/frc/robot/commands/Avancer.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.BasePilotable; + +public class Avancer extends CommandBase { + BasePilotable basePilotable; + DoubleSupplier distance; + /** Creates a new Reculer. */ + public Avancer(BasePilotable basePilotable, DoubleSupplier distance) { + this.basePilotable = basePilotable; + this.distance = distance; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(basePilotable); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + basePilotable.Reset(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + basePilotable.drive(0.3,0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return basePilotable.distance()>distance.getAsDouble(); + } +} diff --git a/src/main/java/frc/robot/commands/Cone.java b/src/main/java/frc/robot/commands/Cone.java index 8121b19..d780293 100644 --- a/src/main/java/frc/robot/commands/Cone.java +++ b/src/main/java/frc/robot/commands/Cone.java @@ -32,7 +32,7 @@ public class Cone extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0); + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Cube.java b/src/main/java/frc/robot/commands/Cube.java index 2bd023f..d54839e 100644 --- a/src/main/java/frc/robot/commands/Cube.java +++ b/src/main/java/frc/robot/commands/Cube.java @@ -32,7 +32,7 @@ public class Cube extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0); + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/GratteBaisser.java b/src/main/java/frc/robot/commands/GratteBaisser.java index 61c7240..9d1292f 100644 --- a/src/main/java/frc/robot/commands/GratteBaisser.java +++ b/src/main/java/frc/robot/commands/GratteBaisser.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Gratte; @@ -26,16 +27,16 @@ public class GratteBaisser extends CommandBase { @Override public void execute() { if(gratte.basd()){ - gratte.baiser(0); + gratte.baiserd(0); } else{ - gratte.baiser(0.5); + gratte.baiserd(0.5); } if(gratte.basg()){ - gratte.baiser(0); + gratte.baiserg(0); } else{ - gratte.baiser(0.5); + gratte.baiserg(0.5); } @@ -43,7 +44,10 @@ public class GratteBaisser extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + gratte.baiserd(0); + gratte.baiserg(0); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/GratteMonte.java b/src/main/java/frc/robot/commands/GratteMonte.java index 507b90d..553e5e0 100644 --- a/src/main/java/frc/robot/commands/GratteMonte.java +++ b/src/main/java/frc/robot/commands/GratteMonte.java @@ -5,6 +5,7 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Gratte; @@ -27,22 +28,25 @@ public class GratteMonte extends CommandBase { @Override public void execute() { if(gratte.hautd()){ - gratte.Lever(0.5); + gratte.Leverd(0); } else{ - gratte.Lever(0.5); + gratte.Leverd(0.5); } if(gratte.hautg()) { - gratte.Lever(0.5); + gratte.Leverg(0); } else{ - gratte.Lever(0.5); + gratte.Leverg(0.5); } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + gratte.Leverd(0); + gratte.Leverg(0); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/Gyro.java b/src/main/java/frc/robot/commands/Gyro.java index 6a758d0..0917326 100644 --- a/src/main/java/frc/robot/commands/Gyro.java +++ b/src/main/java/frc/robot/commands/Gyro.java @@ -10,12 +10,9 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.BasePilotable; public class Gyro extends CommandBase { - ShuffleboardTab teb = Shuffleboard.getTab("teb"); private BasePilotable basePilotable; /** Creates a new Gyro. */ public Gyro(BasePilotable basePilotable) { - teb.add("angleGyro", 0.1); - teb.add("vitesseGyro", 0.1); this.basePilotable = basePilotable; // Use addRequirements() here to declare subsystem dependencies. addRequirements(basePilotable); @@ -28,22 +25,17 @@ public class Gyro extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(basePilotable.getpitch()>10) + if(basePilotable.getpitch()>6) { - basePilotable.drive(0.3*basePilotable.getpitch()/12, 0, 0); + basePilotable.drive(0.3*basePilotable.getpitch()/12, 0); } - else if(basePilotable.getpitch()<-10) + else if(basePilotable.getpitch()<-6) { - basePilotable.drive(0.3*basePilotable.getpitch()/12, 0, 0); - basePilotable.drive(0.3*basePilotable.getpitch()/15, 0, 0); - } - else if(basePilotable.getpitch()<-4) - { - basePilotable.drive(0.3*basePilotable.getpitch()/15, 0, 0); + basePilotable.drive(0.3*basePilotable.getpitch()/12, 0); } else { - basePilotable.drive(0, 0, 0); + basePilotable.drive(0, 0); } } diff --git a/src/main/java/frc/robot/commands/Reculer.java b/src/main/java/frc/robot/commands/Reculer.java index 674ef40..e9d245b 100644 --- a/src/main/java/frc/robot/commands/Reculer.java +++ b/src/main/java/frc/robot/commands/Reculer.java @@ -5,15 +5,18 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.BasePilotable; public class Reculer extends CommandBase { BasePilotable basePilotable; + DoubleSupplier distance; /** Creates a new Reculer. */ - public Reculer(BasePilotable basePilotable) { + public Reculer(BasePilotable basePilotable, DoubleSupplier distance) { this.basePilotable = basePilotable; + this.distance = distance; // Use addRequirements() here to declare subsystem dependencies. addRequirements(basePilotable); } @@ -27,7 +30,7 @@ public class Reculer extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(SmartDashboard.getNumber("vitesse auto", -0.3), 0, 0); + basePilotable.drive(-0.3,0); } // Called once the command ends or is interrupted. @@ -37,6 +40,6 @@ public class Reculer extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return basePilotable.distance()1){ - brasTelescopique.AvanceRecule(0.5); - brasTelescopique.fermer(); - } - if (pivot.distance()>1){ - pivot.monteDescendre(0.5); - } - else if(brasTelescopique.photocell()){ + if(brasTelescopique.photocell()){ + brasTelescopique.ouvrir(); brasTelescopique.Reset(); brasTelescopique.AvanceRecule(0); - brasTelescopique.ouvrir(); } else{ brasTelescopique.AvanceRecule(0.5); } - if(pivot.limitpivot()){ + if(pivot.limitpivot()){ pivot.Reset(); pivot.monteDescendre(0); } else{ - pivot.monteDescendre(0.5); + pivot.monteDescendre(-0.3); } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + brasTelescopique.AvanceRecule(0); + pivot.monteDescendre(0); + brasTelescopique.ouvrir(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheBas.java b/src/main/java/frc/robot/commands/bras/PivotChercheBas.java index 76367e4..d52b893 100644 --- a/src/main/java/frc/robot/commands/bras/PivotChercheBas.java +++ b/src/main/java/frc/robot/commands/bras/PivotChercheBas.java @@ -22,27 +22,29 @@ public class PivotChercheBas extends CommandBase { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + brasTelescopique.fermer(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(brasTelescopique.distance()<10){ - brasTelescopique.AvanceRecule(0.5); + if(brasTelescopique.distance()>-17.5){ + brasTelescopique.AvanceRecule(-0.2); brasTelescopique.fermer(); } - else if(brasTelescopique.distance()>11) { - brasTelescopique.AvanceRecule(-0.5); + else if(brasTelescopique.distance()<-19.5) { + brasTelescopique.AvanceRecule(0.2); } else { brasTelescopique.AvanceRecule(0); brasTelescopique.ouvrir(); } - if (pivot.distance()<10){ - pivot.monteDescendre(0.5); + if (pivot.distance()<8.5){ + pivot.monteDescendre(0.3); } - else if(pivot.distance()>11) { - pivot.monteDescendre(-0.5); + else if(pivot.distance()>10.5) { + pivot.monteDescendre(-0.3); } else{ pivot.monteDescendre(0); @@ -51,7 +53,11 @@ public class PivotChercheBas extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + brasTelescopique.AvanceRecule(0); + pivot.monteDescendre(0); + brasTelescopique.fermer(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java b/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java index 1bdd45a..bd77d22 100644 --- a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java +++ b/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java @@ -22,27 +22,26 @@ public class PivotChercheHaut extends CommandBase { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + brasTelescopique.fermer(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(brasTelescopique.distance()<10){ - brasTelescopique.AvanceRecule(0.5); - brasTelescopique.fermer(); - } - else if(brasTelescopique.distance()>11) { - brasTelescopique.AvanceRecule(-0.5); - } - else { - brasTelescopique.AvanceRecule(0); + if(brasTelescopique.photocell()){ brasTelescopique.ouvrir(); + brasTelescopique.Reset(); + brasTelescopique.AvanceRecule(0); + } + else{ + brasTelescopique.AvanceRecule(0.3); } - if (pivot.distance()<10){ - pivot.monteDescendre(0.5); + if (pivot.distance()<43.5){ + pivot.monteDescendre(0.4); } - else if(pivot.distance()>11) { - pivot.monteDescendre(-0.5); + else if(pivot.distance()>44.5) { + pivot.monteDescendre(-0.4); } else{ pivot.monteDescendre(0); @@ -51,7 +50,11 @@ public class PivotChercheHaut extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + brasTelescopique.AvanceRecule(0); + pivot.monteDescendre(0); + brasTelescopique.ouvrir(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/bras/PivotManuel.java b/src/main/java/frc/robot/commands/bras/PivotManuel.java new file mode 100644 index 0000000..53576af --- /dev/null +++ b/src/main/java/frc/robot/commands/bras/PivotManuel.java @@ -0,0 +1,42 @@ +// 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 java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.bras.Pivot; + +public class PivotManuel extends CommandBase { + private Pivot pivot; + private DoubleSupplier doubleSupplier; + /** Creates a new PivotManuel. */ + public PivotManuel(Pivot pivot,DoubleSupplier doubleSupplier) { + this.pivot = pivot; + this.doubleSupplier = doubleSupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(pivot); + } + + // 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() { + pivot.monteDescendre(doubleSupplier.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java index 226454e..4634d75 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java @@ -24,27 +24,29 @@ public class PivoteBrasBas extends CommandBase { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + brasTelescopique.fermer(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(brasTelescopique.distance()<10){ - brasTelescopique.AvanceRecule(0.5); + if(brasTelescopique.distance()>-13.5){ + brasTelescopique.AvanceRecule(-0.2); brasTelescopique.fermer(); } - else if(brasTelescopique.distance()>11) { - brasTelescopique.AvanceRecule(-0.5); + else if(brasTelescopique.distance()<-15.5) { + brasTelescopique.AvanceRecule(0.2); } else { brasTelescopique.AvanceRecule(0); brasTelescopique.ouvrir(); } - if (pivot.distance()<10){ - pivot.monteDescendre(0.5); + if (pivot.distance()<8.5){ + pivot.monteDescendre(0.3); } - else if(pivot.distance()>11) { - pivot.monteDescendre(-0.5); + else if(pivot.distance()>10.5) { + pivot.monteDescendre(-0.3); } else{ pivot.monteDescendre(0); @@ -55,7 +57,11 @@ public class PivoteBrasBas extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + brasTelescopique.AvanceRecule(0); + pivot.monteDescendre(0); + brasTelescopique.ouvrir(); + } // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java index 1968013..cf41d39 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java @@ -22,27 +22,29 @@ public class PivoteBrasHaut extends CommandBase { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + brasTelescopique.fermer(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(brasTelescopique.distance()<10){ - brasTelescopique.AvanceRecule(0.5); + if(brasTelescopique.distance()>-39.5){ + brasTelescopique.AvanceRecule(-0.15); brasTelescopique.fermer(); } - else if(brasTelescopique.distance()>11) { - brasTelescopique.AvanceRecule(-0.5); + else if(brasTelescopique.distance()<41.5) { + brasTelescopique.AvanceRecule(-0.15); } else { brasTelescopique.AvanceRecule(0); brasTelescopique.ouvrir(); } - if (pivot.distance()<10){ - pivot.monteDescendre(0.5); + if (pivot.distance()<50.5){ + pivot.monteDescendre(0.4); } - else if(pivot.distance()>11) { - pivot.monteDescendre(-0.5); + else if(pivot.distance()>52.5) { + pivot.monteDescendre(-0.4); } else{ pivot.monteDescendre(0); @@ -51,7 +53,11 @@ public class PivoteBrasHaut extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + brasTelescopique.AvanceRecule(0); + pivot.monteDescendre(0); + brasTelescopique.ouvrir(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java index c230cfc..f5abb78 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java @@ -22,27 +22,29 @@ public class PivoteBrasMilieux extends CommandBase { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + brasTelescopique.fermer(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(brasTelescopique.distance()<10){ - brasTelescopique.AvanceRecule(0.5); + if(brasTelescopique.distance()>-16.5){ + brasTelescopique.AvanceRecule(-0.2); brasTelescopique.fermer(); } - else if(brasTelescopique.distance()>11) { - brasTelescopique.AvanceRecule(-0.5); + else if(brasTelescopique.distance()<-17.5) { + brasTelescopique.AvanceRecule(0.2); } else { brasTelescopique.AvanceRecule(0); brasTelescopique.ouvrir(); } - if (pivot.distance()<10){ - pivot.monteDescendre(0.5); + if (pivot.distance()<43.5){ + pivot.monteDescendre(0.4); } - else if(pivot.distance()>11) { - pivot.monteDescendre(-0.5); + else if(pivot.distance()>44.5) { + pivot.monteDescendre(-0.4); } else{ pivot.monteDescendre(0); @@ -52,7 +54,11 @@ public class PivoteBrasMilieux extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + brasTelescopique.AvanceRecule(0); + pivot.monteDescendre(0); + brasTelescopique.ouvrir(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 46ce7c2..8d0a0f1 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -13,9 +13,7 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -30,23 +28,21 @@ public class BasePilotable extends SubsystemBase { final DifferentialDrive drive = new DifferentialDrive(gauche, droit); //piston - private DoubleSolenoid brakedroit = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakedroit, Constants.brakedroit); - private DoubleSolenoid brakegauche = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakegauche, Constants.brakegauche); + private DoubleSolenoid brake = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakeouvre, Constants.brakeferme); //gyro ShuffleboardTab teb = Shuffleboard.getTab("teb"); - ShuffleboardLayout layout = Shuffleboard.getTab("teb") - .getLayout ("encodeurs base pilotable", BuiltInLayouts.kList) - .withSize(2, 2); - private AHRS gyroscope = new AHRS();public double getangle() {return gyroscope.getAngle();} + + double pitchoffset = 0; + + private AHRS gyroscope = new AHRS(); public double getpitch() { - return gyroscope.getPitch(); + return -gyroscope.getPitch() + pitchoffset; } - public void drive(double xSpeed, double zRotation, int i){ + public void drive(double xSpeed, double zRotation){ drive.arcadeDrive(xSpeed, zRotation); } public double distance(){ - teb .add ("distance",0.1); return (-avantdroit.getEncoder().getPosition() +avantgauche.getEncoder().getPosition() -arrieredroit.getEncoder().getPosition() @@ -59,27 +55,25 @@ public class BasePilotable extends SubsystemBase { arrieregauche.getEncoder().setPosition(0); } public void resetGyro(){ - {gyroscope.reset(); - } + pitchoffset = gyroscope.getPitch(); } public void BrakeOuvre(){ - brakedroit.set(Value.kForward); - brakegauche.set(Value.kForward); + brake.set(Value.kForward); } public void BrakeFerme(){ - brakedroit.set(Value.kReverse); - brakegauche.set(Value.kReverse); + brake.set(Value.kReverse); } /** Creates a new BasePilotable. */ public BasePilotable() { droit.setInverted(true); + teb.add(drive); + teb.addDouble("distancerobot",this::distance); + teb.addDouble("angle gyro", this::getpitch); } @Override public void periodic() { } -public void drive(double d, double leftX) { -} } diff --git a/src/main/java/frc/robot/subsystems/Gratte.java b/src/main/java/frc/robot/subsystems/Gratte.java index a90d859..d5920cf 100644 --- a/src/main/java/frc/robot/subsystems/Gratte.java +++ b/src/main/java/frc/robot/subsystems/Gratte.java @@ -13,12 +13,13 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Constants; public class Gratte extends SubsystemBase { + ShuffleboardTab teb = Shuffleboard.getTab("teb"); ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb") .getLayout("limitswitchsgratte", BuiltInLayouts.kList) .withSize(2, 2); - private WPI_TalonSRX Gratted = new WPI_TalonSRX(Constants.leverGratte); - private WPI_TalonSRX Gratteg = new WPI_TalonSRX(Constants.baiserGratte); + private WPI_TalonSRX Gratted = new WPI_TalonSRX(Constants.GratteD); + private WPI_TalonSRX Gratteg = new WPI_TalonSRX(Constants.GratteG); private DigitalInput limithd = new DigitalInput(Constants.limithd); private DigitalInput limithg = new DigitalInput(Constants.limithg); private DigitalInput limitbd = new DigitalInput(Constants.limitbd); @@ -49,19 +50,25 @@ ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb") } public Gratte() { - limitswitchgratte.add ("limitbd", 0.1); - limitswitchgratte.add ("limithg", 0.1); - limitswitchgratte.add ("limithd", 0.1); - limitswitchgratte.add ("limitbg", 0.1); + limitswitchgratte.addBoolean ("limitbd", this::basd); + limitswitchgratte.addBoolean ("limithg", this::hautg); + limitswitchgratte.addBoolean ("limithd", this::hautd); + limitswitchgratte.addBoolean ("limitbg", this::basg); } - public void Lever(double vitesse){ - Gratted.set(vitesse); - Gratteg.set(vitesse); - } - public void baiser(double vitesse){ + public void Leverd(double vitesse){ Gratted.set(-vitesse); + + } + public void Leverg(double vitesse){ Gratteg.set(-vitesse); } + public void baiserd(double vitesse){ + Gratted.set(vitesse); + } + public void baiserg(double vitesse){ + Gratteg.set(vitesse); + } + @Override public void periodic(){ } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index abdddaf..7ed44b6 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -5,8 +5,7 @@ package frc.robot.subsystems; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + import org.photonvision.PhotonCamera; import org.photonvision.common.hardware.VisionLEDMode; @@ -17,12 +16,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Limelight extends SubsystemBase { - ShuffleboardTab teb = Shuffleboard.getTab("teb"); + PhotonCamera limelight = new PhotonCamera("limelight"); /** Creates a new Limelight. */ public Limelight() { CameraServer.startAutomaticCapture(); - teb .add("limelight", 0.1); PortForwarder.add(5800, "photonvision.local", 5800); } diff --git a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java index 0e627cb..12b82b9 100644 --- a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java +++ b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java @@ -15,24 +15,19 @@ import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class BrasTelescopique extends SubsystemBase { ShuffleboardLayout layout = Shuffleboard.getTab("teb") - .getLayout("layout", BuiltInLayouts.kList) + .getLayout("bras", BuiltInLayouts.kList) .withSize(2, 2); -ShuffleboardLayout bras = Shuffleboard.getTab("teb") -.getLayout("bras", BuiltInLayouts.kList) -.withSize(2, 2); - ShuffleboardTab teb = Shuffleboard.getTab("teb"); /** Creates a new BrasTelescopique. */ public BrasTelescopique() { - teb .add("photocell",0.1); - teb .add("winch",0.1); - teb .add("encodeur",0.1);} + layout .addBoolean("photocell",this::photocell); + layout .addDouble("encodeur",this::distance); + } final CANSparkMax Winch = new CANSparkMax(Constants.BrasTelescopique,MotorType.kBrushless); private DigitalInput photocell = new DigitalInput(Constants.photocell); private DoubleSolenoid brakewinch = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakewinchf, Constants.brakewinchb); diff --git a/src/main/java/frc/robot/subsystems/bras/Pivot.java b/src/main/java/frc/robot/subsystems/bras/Pivot.java index 2cd74d2..6fc13b9 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pivot.java +++ b/src/main/java/frc/robot/subsystems/bras/Pivot.java @@ -14,6 +14,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; public class Pivot extends SubsystemBase { + ShuffleboardTab teb = Shuffleboard.getTab("teb"); // moteur @@ -25,8 +26,8 @@ public class Pivot extends SubsystemBase { pivot.set (vitesse); } public Pivot(){ - teb .add ("encodeurpivot",0.1); - teb .add ("limitpivot",0.1); + teb.addDouble("encodeurpivot",this::distance); + teb .addBoolean ("limitpivot",this::limitpivot); } // encodeur public double distance(){ @@ -37,7 +38,7 @@ public class Pivot extends SubsystemBase { } public boolean limitpivot(){ - return limitpivot.get(); + return !limitpivot.get(); } @Override public void periodic() { @@ -46,9 +47,8 @@ public class Pivot extends SubsystemBase { - { - teb.add ("encodeur pivot",0.1); - } + + }