diff --git a/src/main/java/frc/robot/Commands/Desaccumuler.java b/src/main/java/frc/robot/Commands/Desaccumuler.java index e3899e5..4301dec 100644 --- a/src/main/java/frc/robot/Commands/Desaccumuler.java +++ b/src/main/java/frc/robot/Commands/Desaccumuler.java @@ -22,7 +22,8 @@ public class Desaccumuler extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - accumulateur.desaccumule(0.1); + if(accumulateur.photocell()){accumulateur.desaccumule(0.1);} + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a82b460..47d0d47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,16 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Commands.Desaccumuler; +import frc.robot.Commands.FollowAprilTag; +import frc.robot.Commands.Lancer; import frc.robot.Subsystems.Accumulateur; import frc.robot.Subsystems.Drive; import frc.robot.Subsystems.Lanceur; @@ -19,6 +25,7 @@ public class RobotContainer { Limelight3G limelight3G = new Limelight3G(); Drive drive = new Drive(); ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); + CommandXboxController manette = new CommandXboxController(0); public RobotContainer() { dashboard.addCamera("limelight3G", "limelight3G","limelight.local:5800") .withSize(3,4) @@ -27,9 +34,19 @@ public class RobotContainer { .withSize(3,4) .withPosition(3,0); configureBindings(); + drive.setDefaultCommand(new RunCommand(()->{ + drive.drive(-MathUtil.applyDeadband(manette.getRightX(),0.2), MathUtil.applyDeadband(-manette.getRightY(),0.2), MathUtil.applyDeadband(-manette.getRightX(), 0.2)); + },drive)); + } + + private void configureBindings() { + Lancer lancer= new Lancer(lanceur); + Desaccumuler desaccumuler = new Desaccumuler(accumulateur); + FollowAprilTag aprilTag = new FollowAprilTag(limelight3G, lanceur); + manette.x().whileTrue(new Lancer(lanceur)); + manette.leftBumper().toggleOnTrue(new FollowAprilTag(limelight3G, lanceur)); + manette.a().whileTrue(new Desaccumuler(accumulateur)); } - - private void configureBindings() {} public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/Subsystems/Accumulateur.java b/src/main/java/frc/robot/Subsystems/Accumulateur.java index f7ba6af..ad890d2 100644 --- a/src/main/java/frc/robot/Subsystems/Accumulateur.java +++ b/src/main/java/frc/robot/Subsystems/Accumulateur.java @@ -12,9 +12,8 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class Accumulateur extends SubsystemBase { /** Creates a new Accumulateur. */ - public Accumulateur() {dashboard.addBoolean("photocellacc", this::limitswitch) - .withSize(1, 1) - .withPosition(0, 1); + public Accumulateur() {dashboard.addBoolean("photocellacc", this::photocell).withSize(1, 1).withPosition(0, 1); + } ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); @@ -29,8 +28,8 @@ public class Accumulateur extends SubsystemBase { final DigitalInput photocell = new DigitalInput(94); public void encodeur(){ } - public boolean limitswitch(){ - return !photocell.get(); + public boolean photocell(){ + return photocell.get(); } public void desaccumule(double vitesse){ accumulateur1.set(vitesse); diff --git a/src/main/java/frc/robot/Subsystems/Lanceur.java b/src/main/java/frc/robot/Subsystems/Lanceur.java index 45d5f34..7b14e92 100644 --- a/src/main/java/frc/robot/Subsystems/Lanceur.java +++ b/src/main/java/frc/robot/Subsystems/Lanceur.java @@ -30,6 +30,11 @@ public class Lanceur extends SubsystemBase { .withSize(0,0) .withPosition(1, 4) .getEntry(); + private GenericEntry rotation = + dashboard.add("rottourel", 0.2) + .withSize(0,0) + .withPosition(1, 5) + .getEntry(); public void encodeur(double distance){ lanceur1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); lanceur2.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); @@ -40,14 +45,17 @@ public class Lanceur extends SubsystemBase { lanceur2.setInverted(true); } public void lance(double vitesse){ - lanceur1.set(vitesse); + lanceur1.set(vitesse);lanceur2.set(vitesse); } - public void lance(){ + public void lance(){ lance(vitesse.getDouble(0.2)); } public void tourelRotation(double x, double y, double rotation){ tourelle.set(rotation); } + public void tourelRotation(){ + tourelle.set(rotation.getDouble(0.1)); + } public double vitessetourel(){ return(tourelle.getEncoder().getVelocity()); } diff --git a/src/main/java/frc/robot/Subsystems/Limelight3G.java b/src/main/java/frc/robot/Subsystems/Limelight3G.java index 2fdba8f..b11532c 100644 --- a/src/main/java/frc/robot/Subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/Subsystems/Limelight3G.java @@ -6,11 +6,15 @@ package frc.robot.Subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; public class Limelight3G extends SubsystemBase { + ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty");