From 77a03a5b34f0d90f6adc5a34c5a59eabf648a31a Mon Sep 17 00:00:00 2001 From: EdwardFaucher Date: Wed, 22 Mar 2023 18:16:48 -0400 Subject: [PATCH] drxgfh --- src/main/java/frc/robot/RobotContainer.java | 12 +++++------- src/main/java/frc/robot/commands/Apriltag.java | 2 +- src/main/java/frc/robot/commands/Tape.java | 2 +- .../java/frc/robot/subsystems/BasePilotable.java | 4 ++++ src/main/java/frc/robot/subsystems/Limelight.java | 3 ++- 5 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24fa703..b6d248b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,7 +87,7 @@ public class RobotContainer { 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()); + Tape tape = new Tape(limelight, basePilotable, () -> -manette1.getLeftY()); PivotManuel pivotManuel = new PivotManuel(pivot, manette1::getRightY); BrasManuel brasManuel = new BrasManuel(brasTelescopique, manette1::getRightX); ActiverLimeLight activerLimeLight = new ActiverLimeLight(limelight); @@ -110,8 +110,8 @@ public class RobotContainer { private void configureBindings() { // manette 1 manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); - manette1.x().onTrue(brakeOuvre); - manette1.b().onTrue(brakeFerme); + manette1.b().whileTrue(gratteMonte); + manette1.x().whileTrue(gratteBaisser); manette1.leftBumper().whileTrue(aprilTag); manette1.rightBumper().whileTrue(tape); manette1.povUp().whileTrue(creerCommandBras(51, -37)); @@ -122,12 +122,10 @@ public class RobotContainer { //manette 2 manette2.povDown().whileTrue(creerCommandBras(5, -12)); manette2.povUp().whileTrue(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); + manette2.a().onTrue(brakeOuvre); + manette2.b().onTrue(brakeFerme); } private Command creerCommandBras(double distancePivot, double distanceBras) { diff --git a/src/main/java/frc/robot/commands/Apriltag.java b/src/main/java/frc/robot/commands/Apriltag.java index 9ef54b3..3e83057 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()); + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(),false); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Tape.java b/src/main/java/frc/robot/commands/Tape.java index e1346f0..02a9a81 100644 --- a/src/main/java/frc/robot/commands/Tape.java +++ b/src/main/java/frc/robot/commands/Tape.java @@ -32,7 +32,7 @@ public class Tape extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw()); + basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(),false); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 8d0a0f1..a1e4c68 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.commands.bras.Bougerbras; public class BasePilotable extends SubsystemBase { final CANSparkMax avantdroit = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless); @@ -42,6 +43,9 @@ public class BasePilotable extends SubsystemBase { public void drive(double xSpeed, double zRotation){ drive.arcadeDrive(xSpeed, zRotation); } + public void drive(double xSpeed, double zRotation, boolean square){ + drive.arcadeDrive(xSpeed, zRotation, square); + } public double distance(){ return (-avantdroit.getEncoder().getPosition() +avantgauche.getEncoder().getPosition() diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index b869de3..eae8e0f 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -41,12 +41,13 @@ public class Limelight extends SubsystemBase { public void tape() { limelight.setLED(VisionLEDMode.kOn); limelight.setPipelineIndex(1); + } public double getYaw() { var result = limelight.getLatestResult(); if(result.hasTargets()){ - return -result.getBestTarget().getYaw()/30; + return -result.getBestTarget().getYaw()/45; } return 0;