From c81d4cfbd1543aca2b88234a60ddde0b7427d589 Mon Sep 17 00:00:00 2001 From: EdwardFaucher Date: Sat, 18 Mar 2023 12:58:08 -0400 Subject: [PATCH] zdfgbg --- networktables.json | 27 ++++++++++++++++++- networktables.json.bck | 1 + src/main/java/frc/robot/RobotContainer.java | 18 ++++++------- src/main/java/frc/robot/commands/Reculer.java | 2 +- .../robot/commands/bras/PivoteBrasBas.java | 2 +- .../commands/bras/PivoteBrasMilieux.java | 4 +-- .../java/frc/robot/subsystems/Limelight.java | 2 +- 7 files changed, 41 insertions(+), 15 deletions(-) create mode 100644 networktables.json.bck diff --git a/networktables.json b/networktables.json index 8b13789..6cf6543 100644 --- a/networktables.json +++ b/networktables.json @@ -1 +1,26 @@ - +[ + { + "name": "/Shuffleboard/teb/auto/reculerdistancesortir", + "type": "double", + "value": -66.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Shuffleboard/teb/auto/reculerdistancebalance", + "type": "double", + "value": -31.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Shuffleboard/teb/auto/avancer", + "type": "double", + "value": -35.0, + "properties": { + "persistent": true + } + } +] diff --git a/networktables.json.bck b/networktables.json.bck new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/networktables.json.bck @@ -0,0 +1 @@ + diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5eb7222..2bbc6ca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -93,9 +93,9 @@ 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); +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); @@ -119,8 +119,8 @@ public RobotContainer() { manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); manette1.x().onTrue(brakeOuvre); manette1.b().onTrue(brakeFerme); - manette1.leftBumper().toggleOnTrue(aprilTag); - manette1.rightBumper().toggleOnTrue(tape); + manette1.leftBumper().whileTrue(aprilTag); + manette1.rightBumper().whileTrue(tape); manette1.povUp().whileTrue(pivoteBrasHaut); manette1.povDown().whileTrue(pivoteBrasBas); manette1.povRight().whileTrue(pivoteBrasMilieux); @@ -128,8 +128,8 @@ public RobotContainer() { //manette 2 manette2.povDown().onTrue(pivotChercheBas); manette2.povUp().onTrue(pivotChercheHaut); - manette2.rightBumper().toggleOnTrue(cube); - manette2.leftBumper().toggleOnTrue(cone); + manette2.rightBumper().whileTrue(cube); + manette2.leftBumper().whileTrue(cone); manette2.y().whileTrue(gyro); manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro)); manette2.a().whileTrue(gratteMonte); @@ -154,9 +154,9 @@ public RobotContainer() { new PivotBrasRentre(brasTelescopique , pivot).unless(()->chooser.getSelected().equals(nulpart)), Commands.waitSeconds(1), Commands.either(reculers, reculerb,()-> autosortir.getBoolean(true)), - avancer.unless(()->!autosortir.getBoolean(true)|| !autobalance.getBoolean(false)), + avancer.unless(()->!autosortir.getBoolean(true)|| autobalance.getBoolean(false)), Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true)) - )).andThen(new InstantCommand(basePilotable::BrakeOuvre)); + )).andThen(new InstantCommand(basePilotable::BrakeOuvre,basePilotable)); } diff --git a/src/main/java/frc/robot/commands/Reculer.java b/src/main/java/frc/robot/commands/Reculer.java index b19b241..ecc904f 100644 --- a/src/main/java/frc/robot/commands/Reculer.java +++ b/src/main/java/frc/robot/commands/Reculer.java @@ -30,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(-0.5,0); + basePilotable.drive(-0.4,0); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java index 130619b..a6015d7 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java @@ -65,7 +65,7 @@ public class PivoteBrasBas extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return brasTelescopique.distance()>-13.5 && brasTelescopique.distance() <-15.5 && pivot.distance()<8.5 && pivot.distance()>10.5; + return brasTelescopique.distance()>-13.5 && brasTelescopique.distance() <-14.5 && pivot.distance()<8.5 && pivot.distance()>9.5; } } diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java index 8d80105..764050b 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java @@ -41,10 +41,10 @@ public class PivoteBrasMilieux extends CommandBase { brasTelescopique.ouvrir(); } if (pivot.distance()<43.5){ - pivot.monteDescendre(0.7); + pivot.monteDescendre(0.6); } else if(pivot.distance()>44.5) { - pivot.monteDescendre(-0.7); + pivot.monteDescendre(-0.6); } else{ pivot.monteDescendre(0); diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 7ed44b6..f6ff621 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -47,7 +47,7 @@ public class Limelight extends SubsystemBase { public double getYaw() { var result = limelight.getLatestResult(); if(result.hasTargets()){ - return result.getBestTarget().getYaw(); + return -result.getBestTarget().getYaw()/60; } return 0;