diff --git a/simgui.json b/simgui.json index 7b653c9..28a4c44 100644 --- a/simgui.json +++ b/simgui.json @@ -2,6 +2,7 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/Shuffleboard/teb/DifferentialDrive[1]": "DifferentialDrive", "/Shuffleboard/teb/auto/choix hauteur": "String Chooser" } }, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5cc107f..13b0abe 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,35 +1,33 @@ package frc.robot; public class Constants { - public static int avantdroit = 1; - public static int avantgauche = 2; + 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 = 6; + public static int BrasTelescopique = 1; + public static int pivot = 2; //moteur - public static int baiserGratte = 8; - public static int leverGratte = 0; - public static int baisserGratte = 2; + 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 = 2; - public static int brakedroit = 3; - public static int brakegauche = 4; - public static int brakewinchf = 5; - public static int brakewinchb = 6; + 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 d211b64..aebdfab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,11 +30,13 @@ 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; @@ -92,7 +94,8 @@ 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); +BrasManuel brasManuel = new BrasManuel(brasTelescopique); public RobotContainer() { @@ -106,8 +109,8 @@ public RobotContainer() { configureBindings(); CameraServer.startAutomaticCapture(); basePilotable.setDefaultCommand(new RunCommand(() -> { - basePilotable.drive(-manette1.getLeftY(), manette1.getLeftX()); - })); + basePilotable.drive(-manette1.getLeftY(), -manette1.getLeftX()); + }, basePilotable)); } private void configureBindings() { // manette 1 @@ -115,18 +118,21 @@ public RobotContainer() { manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable)); manette1.leftBumper().toggleOnTrue(aprilTag); manette1.rightBumper().toggleOnTrue(tape); - manette1.povUp().onTrue(pivoteBrasHaut); - manette1.povDown().onTrue(pivoteBrasBas); - manette1.povRight().onTrue(pivoteBrasMilieux); - manette1.povLeft().onTrue(pivotBrasRentre); + manette1.povUp().whileTrue(pivoteBrasHaut); + manette1.povDown().whileTrue(pivoteBrasBas); + manette1.povRight().whileTrue(pivoteBrasMilieux); + manette1.povLeft().whileTrue(pivotBrasRentre); //manette 2 - manette2.povRight().onTrue(pivotChercheBas); - manette2.povLeft().onTrue(pivotChercheHaut); + 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.b().onTrue(Commands.either(gratteBaisser, gratteMonte, gratte::getenHaut)); + manette2.a().whileTrue(gratteMonte); + manette2.b().whileTrue(gratteBaisser); + manette2.leftStick().whileTrue(brasManuel); + manette2.rightStick().whileTrue(pivotManuel); } public Command getAutonomousCommand() { 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 d65a693..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,18 +25,13 @@ 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); } - else if(basePilotable.getpitch()<-10) + else if(basePilotable.getpitch()<-6) { basePilotable.drive(0.3*basePilotable.getpitch()/12, 0); - basePilotable.drive(0.3*basePilotable.getpitch()/15, 0); - } - else if(basePilotable.getpitch()<-4) - { - basePilotable.drive(0.3*basePilotable.getpitch()/15, 0); } else { diff --git a/src/main/java/frc/robot/commands/bras/BrasManuel.java b/src/main/java/frc/robot/commands/bras/BrasManuel.java new file mode 100644 index 0000000..2b8824e --- /dev/null +++ b/src/main/java/frc/robot/commands/bras/BrasManuel.java @@ -0,0 +1,37 @@ +// 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; + +public class BrasManuel extends CommandBase { + private BrasTelescopique brasTelescopique; + /** Creates a new BrasManuel. */ + public BrasManuel(BrasTelescopique brasTelescopique) { + this.brasTelescopique = brasTelescopique; + // 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() { + brasTelescopique.AvanceRecule(0.3); + } + + // 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/PivotBrasRentre.java b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java index 6efbb90..b35f2a8 100644 --- a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java +++ b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java @@ -22,38 +22,37 @@ public class PivotBrasRentre 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()>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..ee0e15a --- /dev/null +++ b/src/main/java/frc/robot/commands/bras/PivotManuel.java @@ -0,0 +1,38 @@ +// 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.Pivot; + +public class PivotManuel extends CommandBase { + private Pivot pivot; + /** Creates a new PivotManuel. */ + public PivotManuel(Pivot pivot) { + this.pivot = pivot; + // 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(0.3); + } + + // 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 2aa6cf9..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){ drive.arcadeDrive(xSpeed, zRotation); } public double distance(){ - teb .add ("distance",0.1); return (-avantdroit.getEncoder().getPosition() +avantgauche.getEncoder().getPosition() -arrieredroit.getEncoder().getPosition() @@ -59,21 +55,21 @@ 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 diff --git a/src/main/java/frc/robot/subsystems/Gratte.java b/src/main/java/frc/robot/subsystems/Gratte.java index 8b0a9a2..d5920cf 100644 --- a/src/main/java/frc/robot/subsystems/Gratte.java +++ b/src/main/java/frc/robot/subsystems/Gratte.java @@ -18,8 +18,8 @@ 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); @@ -50,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 50b77b9..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,18 @@ 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); diff --git a/src/main/java/frc/robot/subsystems/bras/Pivot.java b/src/main/java/frc/robot/subsystems/bras/Pivot.java index fdcae1b..6fc13b9 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pivot.java +++ b/src/main/java/frc/robot/subsystems/bras/Pivot.java @@ -38,7 +38,7 @@ public class Pivot extends SubsystemBase { } public boolean limitpivot(){ - return limitpivot.get(); + return !limitpivot.get(); } @Override public void periodic() {