diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ec0c08..923373b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,11 @@ 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.Algue1Test; +import frc.robot.commands.Algue2Test; import frc.robot.commands.AlgueExpire; +import frc.robot.commands.CorailAspir; +import frc.robot.commands.CorailTest; import frc.robot.commands.CoralAlgueInspire; import frc.robot.commands.CoralExpire; import frc.robot.commands.Depart; @@ -38,7 +42,7 @@ public class RobotContainer { public RobotContainer() { configureBindings(); elevateur.setDefaultCommand(new RunCommand(()->{ - elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); + elevateur.vitesse(MathUtil.applyDeadband(-manette2.getLeftY(), 0.2)); }, elevateur)); pince.setDefaultCommand(new RunCommand(()->{ pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2)); @@ -57,13 +61,17 @@ public class RobotContainer { manette1.b().whileTrue(new L2(elevateur,pince)); manette1.x().whileTrue(new L3(elevateur, pince)); manette1.y().whileTrue(new L4(elevateur, pince)); - manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie)); manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightBumper().whileTrue(new StationPince(pince, elevateur)); //manette2 manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); manette2.povUp().whileTrue(new PinceManuel(pince)); manette2.povDown().whileTrue(new PinceManuel2(pince)); + manette2.a().whileTrue(new CorailAspir(pince)); + manette2.b().whileTrue(new CoralExpire(pince, bougie)); + manette2.povLeft().whileTrue(new Algue1Test(pince)); + manette2.povRight().whileTrue(new Algue2Test(pince)); + manette2.x().whileTrue(new CorailTest(pince)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Algue1Test.java b/src/main/java/frc/robot/commands/Algue1Test.java new file mode 100644 index 0000000..f79faa6 --- /dev/null +++ b/src/main/java/frc/robot/commands/Algue1Test.java @@ -0,0 +1,41 @@ +// 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 edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pince; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class Algue1Test extends Command { + private Pince pince; + /** Creates a new AlgueTest. */ + public Algue1Test(Pince pince) { + this.pince = pince; + addRequirements(pince); + // 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() { + pince.algue1Test(0.2); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + pince.algue1Test(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Algue2Test.java b/src/main/java/frc/robot/commands/Algue2Test.java new file mode 100644 index 0000000..5f0e758 --- /dev/null +++ b/src/main/java/frc/robot/commands/Algue2Test.java @@ -0,0 +1,41 @@ +// 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 edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pince; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class Algue2Test extends Command { + private Pince pince; + /** Creates a new AlgueTest. */ + public Algue2Test(Pince pince) { + this.pince = pince; + addRequirements(pince); + // 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() { + pince.algue2Test(0.2); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + pince.algue2Test(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/CorailAspir.java b/src/main/java/frc/robot/commands/CorailAspir.java new file mode 100644 index 0000000..8199c75 --- /dev/null +++ b/src/main/java/frc/robot/commands/CorailAspir.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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pince; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class CorailAspir extends Command { + /** Creates a new CorailAspir. */ + private Pince pince; + public CorailAspir(Pince pince) { + // Use addRequirements() here to declare subsystem dependencies. + this.pince = pince; + addRequirements(pince); + } + + // 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() { + + pince.aspirecoral(0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + pince.aspirecoral(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/CorailTest.java b/src/main/java/frc/robot/commands/CorailTest.java new file mode 100644 index 0000000..5e947d2 --- /dev/null +++ b/src/main/java/frc/robot/commands/CorailTest.java @@ -0,0 +1,41 @@ +// 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 edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pince; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class CorailTest extends Command { + private Pince pince; + /** Creates a new AlgueTest. */ + public CorailTest(Pince pince) { + this.pince = pince; + addRequirements(pince); + // 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() { + pince.aspirecoral(0.2); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + pince.aspirecoral(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ElevateurManuel.java b/src/main/java/frc/robot/commands/ElevateurManuel.java index 0a52684..ce00534 100644 --- a/src/main/java/frc/robot/commands/ElevateurManuel.java +++ b/src/main/java/frc/robot/commands/ElevateurManuel.java @@ -31,7 +31,7 @@ public class ElevateurManuel extends Command { if(elevateur.limit2()==true){ elevateur.vitesse(0); } - elevateur.vitesse(doubleSupplier.getAsDouble()); + elevateur.vitesse(doubleSupplier.getAsDouble()/5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/PinceManuel.java b/src/main/java/frc/robot/commands/PinceManuel.java index 164fd42..acb39a3 100644 --- a/src/main/java/frc/robot/commands/PinceManuel.java +++ b/src/main/java/frc/robot/commands/PinceManuel.java @@ -3,16 +3,12 @@ // 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.wpilibj2.command.Command; import frc.robot.subsystems.Pince; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class PinceManuel extends Command { private Pince pince; - private DoubleSupplier doubleSupplier; /** Creates a new PinceManuel. */ public PinceManuel(Pince pince //, DoubleSupplier doubleSupplier @@ -30,17 +26,19 @@ public class PinceManuel extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(pince.position()){ - pince.pivote(0); - } - else{ + // if(pince.position()){ + // pince.pivote(0); + // } + // else{ pince.pivote(0.2); - } + // } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + pince.pivote(0); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/PinceManuel2.java b/src/main/java/frc/robot/commands/PinceManuel2.java index 03edffa..bf8d1e4 100644 --- a/src/main/java/frc/robot/commands/PinceManuel2.java +++ b/src/main/java/frc/robot/commands/PinceManuel2.java @@ -3,9 +3,6 @@ // 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.wpilibj2.command.Command; import frc.robot.subsystems.Pince; @@ -38,7 +35,9 @@ public class PinceManuel2 extends Command { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + pince.pivote(0); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/StationPince.java b/src/main/java/frc/robot/commands/StationPince.java index 9e11f99..59527d5 100644 --- a/src/main/java/frc/robot/commands/StationPince.java +++ b/src/main/java/frc/robot/commands/StationPince.java @@ -33,19 +33,19 @@ public class StationPince extends Command { pince.pivote(0); } else if(pince.encodeurpivot()>=910){ - pince.pivote(-0.2); + pince.pivote(0.2); } else{ - pince.pivote(0.2); + pince.pivote(-0.2); } if(elevateur.position()>=400 && elevateur.position()<=410){ elevateur.vitesse(0); } else if(elevateur.position()>=410){ - elevateur.vitesse(-0.3); + elevateur.vitesse(0.3); } else{ - elevateur.vitesse(.3); + elevateur.vitesse(-.3); } } diff --git a/src/main/java/frc/robot/subsystems/Pince.java b/src/main/java/frc/robot/subsystems/Pince.java index 2428fbd..3e04f48 100644 --- a/src/main/java/frc/robot/subsystems/Pince.java +++ b/src/main/java/frc/robot/subsystems/Pince.java @@ -42,6 +42,12 @@ public double emperagecoral(){ } public double emperagealgue(){ return algue1.getOutputCurrent(); +} +public void algue1Test(double vitesse){ + algue1.set(vitesse); +} +public void algue2Test(double vitesse){ + algue2.set(vitesse); } @Override public void periodic() {