diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index abee378..3b58775 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,22 +26,24 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.TunerConstants.TunerConstants; import frc.robot.commands.AprilTag3; import frc.robot.commands.AprilTag3G; -import frc.robot.commands.Depart; -import frc.robot.commands.ElevateurManuel; import frc.robot.commands.Forme3; -import frc.robot.commands.L2; -import frc.robot.commands.L3; -import frc.robot.commands.L4; -import frc.robot.commands.PinceManuel; -import frc.robot.commands.PinceManuel2; import frc.robot.commands.RainBow; import frc.robot.commands.StationPince; import frc.robot.commands.reset; +import frc.robot.commands.Elevateur.Depart; +import frc.robot.commands.Elevateur.ElevateurManuel; +import frc.robot.commands.Elevateur.L2; +import frc.robot.commands.Elevateur.L3; +import frc.robot.commands.Elevateur.L4; import frc.robot.commands.Pince.AlgueExpire; import frc.robot.commands.Pince.Algue_inspire; import frc.robot.commands.Pince.CorailAspir; import frc.robot.commands.Pince.CoralAlgueInspire; import frc.robot.commands.Pince.CoralExpire; +import frc.robot.commands.Pince.PinceManuel; +import frc.robot.commands.requin.BalayeuseBas; +import frc.robot.commands.requin.BalayeuseHaut; +import frc.robot.commands.requin.exspire; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Elevateur; @@ -49,6 +51,7 @@ import frc.robot.subsystems.Grimpeur; import frc.robot.subsystems.Limelight3; import frc.robot.subsystems.Limelight3G; import frc.robot.subsystems.Pince; +import frc.robot.subsystems.Requin; public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -74,12 +77,12 @@ public class RobotContainer { Pince pince = new Pince(); ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY); PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY); - PinceManuel2 pinceManuel2 = new PinceManuel2(pince); Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); Limelight3 limelight3 = new Limelight3(); Pose2d pose = new Pose2d(); Grimpeur Grimpeur = new Grimpeur(); + Requin requin = new Requin(); public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("New Auto"); @@ -93,58 +96,58 @@ public class RobotContainer { NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie)); } private void configureBindings() { - + drivetrain.registerTelemetry(logger::telemeterize); drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(MathUtil.applyDeadband(Math.pow(-manette1.getLeftX(), 2)*MaxSpeed, 0.1)) // Drive forward with negative Y (forward) - .withVelocityY(MathUtil.applyDeadband(Math.pow(-manette1.getLeftY(), 2)*MaxSpeed, 0.10000)) // Drive left with negative X (left) - .withRotationalRate(MathUtil.applyDeadband(Math.pow(-manette1.getRightX(), 2)*manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left) + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftX()*manette1.getLeftX()*manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(manette1.getLeftY()*manette1.getLeftY()*manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*manette1.getRightX()*manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left) ) ); - // Elevateur manuel - drivetrain.registerTelemetry(logger::telemeterize); - elevateur.setDefaultCommand(new RunCommand(()->{ - elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.1)); - }, elevateur)); - - //Pince manuel - pince.setDefaultCommand(new RunCommand(()->{ - pince.pivote(MathUtil.applyDeadband(manette2.getRightY(), 0.1)); - }, pince)); + - - // manette1 - + /* Manette 1 */ // reset the field-centric heading on start press manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - //elevateur - manette1.a().whileTrue(new Depart(elevateur, pince)); - manette1.b().whileTrue(new L2(elevateur,pince)); - manette1.x().whileTrue(new L3(elevateur, pince)); - manette1.y().whileTrue(new L4(elevateur, pince)); - //pince manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightBumper().whileTrue(new StationPince(pince, elevateur)); manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie)); - //manette2 + //elevateur + manette1.a().whileTrue(new Depart(elevateur, pince)); + manette1.b().whileTrue(new L2(elevateur,pince)); + manette1.x().whileTrue(new L3(elevateur, pince)); + manette1.y().whileTrue(new L4(elevateur, pince)); + /* Manette 2 */ //pince manette2.a().whileTrue(new CorailAspir(pince)); - manette2.start().whileTrue(new reset(elevateur, pince)); manette2.b().whileTrue(new Algue_inspire(pince)); - manette2.start().whileTrue(new reset(elevateur,pince)); + manette2.y().whileTrue(new BalayeuseHaut(requin)); + manette2.x().whileTrue(new BalayeuseBas(requin)); + manette2.rightTrigger().whileTrue(new exspire(requin)); manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); + //Pince manuel + pince.setDefaultCommand(new RunCommand(()->{ + pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY()*manette2.getRightY(), 0.05)); + }, pince)); + + //Elevateur manuel + elevateur.setDefaultCommand(new RunCommand(()->{ + elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05)); + }, elevateur)); + //limelight manette2.leftBumper().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette2.rightBumper().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + + //Reset encodeur + manette2.start().whileTrue(new reset(elevateur, pince)); } - - // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); public Command getAutonomousCommand() { return new SequentialCommandGroup(Commands.runOnce(()->{ diff --git a/src/main/java/frc/robot/commands/Depart.java b/src/main/java/frc/robot/commands/Elevateur/Depart.java similarity index 88% rename from src/main/java/frc/robot/commands/Depart.java rename to src/main/java/frc/robot/commands/Elevateur/Depart.java index f5322dd..56f684b 100644 --- a/src/main/java/frc/robot/commands/Depart.java +++ b/src/main/java/frc/robot/commands/Elevateur/Depart.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.Elevateur; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevateur; @@ -11,12 +11,10 @@ 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 Depart extends Command { private Elevateur elevateur; - private Pince pince; /** Creates a new L2. */ public Depart(Elevateur elevateur, Pince pince) { - this.pince = pince; this.elevateur = elevateur; - addRequirements(elevateur,pince); + addRequirements(elevateur); // Use addRequirements() here to declare subsystem dependencies. } @@ -41,12 +39,11 @@ public class Depart extends Command { @Override public void end(boolean interrupted) { elevateur.vitesse(0); - pince.pivote(0); } // Returns true when the command should end. @Override public boolean isFinished() { - return elevateur.limit2()==true; + return elevateur.limit2() == true; } } diff --git a/src/main/java/frc/robot/commands/ElevateurManuel.java b/src/main/java/frc/robot/commands/Elevateur/ElevateurManuel.java similarity index 94% rename from src/main/java/frc/robot/commands/ElevateurManuel.java rename to src/main/java/frc/robot/commands/Elevateur/ElevateurManuel.java index f86268a..772df78 100644 --- a/src/main/java/frc/robot/commands/ElevateurManuel.java +++ b/src/main/java/frc/robot/commands/Elevateur/ElevateurManuel.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.Elevateur; import java.util.function.DoubleSupplier; @@ -28,7 +28,10 @@ public class ElevateurManuel extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (elevateur.limit2()) + if (elevateur.limit2()){ + + + } elevateur.vitesse(doubleSupplier.getAsDouble()/3.5); } diff --git a/src/main/java/frc/robot/commands/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java similarity index 98% rename from src/main/java/frc/robot/commands/L2.java rename to src/main/java/frc/robot/commands/Elevateur/L2.java index 65c8d18..ac4c59f 100644 --- a/src/main/java/frc/robot/commands/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -2,8 +2,7 @@ // 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; - +package frc.robot.commands.Elevateur; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; diff --git a/src/main/java/frc/robot/commands/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java similarity index 95% rename from src/main/java/frc/robot/commands/L3.java rename to src/main/java/frc/robot/commands/Elevateur/L3.java index 43635ac..7b77690 100644 --- a/src/main/java/frc/robot/commands/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.Elevateur; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; @@ -39,8 +39,8 @@ public class L3 extends Command { public void execute() { double encodeurHaute = encodeur1.get(); double encodeurBase = encodeur2.get(); - double encodeurbase = encodeur3.get(); - double encodeurhaute = encodeur4.get(); + double encodeurbasp = encodeur3.get(); + double encodeurhautp = encodeur4.get(); if(elevateur.position()<=-encodeurHaute && elevateur.position()>=encodeurBase){ elevateur.vitesse(0); } diff --git a/src/main/java/frc/robot/commands/L4.java b/src/main/java/frc/robot/commands/Elevateur/L4.java similarity index 98% rename from src/main/java/frc/robot/commands/L4.java rename to src/main/java/frc/robot/commands/Elevateur/L4.java index a395b10..e83f55d 100644 --- a/src/main/java/frc/robot/commands/L4.java +++ b/src/main/java/frc/robot/commands/Elevateur/L4.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.Elevateur; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.DoubleTopic; diff --git a/src/main/java/frc/robot/commands/DepartPince.java b/src/main/java/frc/robot/commands/Pince/DepartPince.java similarity index 97% rename from src/main/java/frc/robot/commands/DepartPince.java rename to src/main/java/frc/robot/commands/Pince/DepartPince.java index 12a69dd..bdfc343 100644 --- a/src/main/java/frc/robot/commands/DepartPince.java +++ b/src/main/java/frc/robot/commands/Pince/DepartPince.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.Pince; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Pince; diff --git a/src/main/java/frc/robot/commands/PinceManuel.java b/src/main/java/frc/robot/commands/Pince/PinceManuel.java similarity index 97% rename from src/main/java/frc/robot/commands/PinceManuel.java rename to src/main/java/frc/robot/commands/Pince/PinceManuel.java index 5b8b2c0..1382d39 100644 --- a/src/main/java/frc/robot/commands/PinceManuel.java +++ b/src/main/java/frc/robot/commands/Pince/PinceManuel.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.Pince; import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/commands/GrimperHaut.java b/src/main/java/frc/robot/commands/grimpeur/GrimperHaut.java similarity index 97% rename from src/main/java/frc/robot/commands/GrimperHaut.java rename to src/main/java/frc/robot/commands/grimpeur/GrimperHaut.java index ad0ebf0..ce3f65a 100644 --- a/src/main/java/frc/robot/commands/GrimperHaut.java +++ b/src/main/java/frc/robot/commands/grimpeur/GrimperHaut.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.grimpeur; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Bougie; diff --git a/src/main/java/frc/robot/commands/GrimpeurBas.java b/src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java similarity index 98% rename from src/main/java/frc/robot/commands/GrimpeurBas.java rename to src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java index 7837e24..4dfc3b0 100644 --- a/src/main/java/frc/robot/commands/GrimpeurBas.java +++ b/src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.grimpeur; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; diff --git a/src/main/java/frc/robot/commands/GrimpeurManuel.java b/src/main/java/frc/robot/commands/grimpeur/GrimpeurManuel.java similarity index 97% rename from src/main/java/frc/robot/commands/GrimpeurManuel.java rename to src/main/java/frc/robot/commands/grimpeur/GrimpeurManuel.java index 31a404b..60fdace 100644 --- a/src/main/java/frc/robot/commands/GrimpeurManuel.java +++ b/src/main/java/frc/robot/commands/grimpeur/GrimpeurManuel.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.grimpeur; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/commands/ResetGrimpeur.java b/src/main/java/frc/robot/commands/grimpeur/ResetGrimpeur.java similarity index 97% rename from src/main/java/frc/robot/commands/ResetGrimpeur.java rename to src/main/java/frc/robot/commands/grimpeur/ResetGrimpeur.java index b4e6978..5bf0bea 100644 --- a/src/main/java/frc/robot/commands/ResetGrimpeur.java +++ b/src/main/java/frc/robot/commands/grimpeur/ResetGrimpeur.java @@ -2,7 +2,7 @@ // 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; +package frc.robot.commands.grimpeur; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Grimpeur; diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java new file mode 100644 index 0000000..212583b --- /dev/null +++ b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java @@ -0,0 +1,63 @@ +// 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.requin; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; +import frc.robot.subsystems.Requin; + +/* 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 BalayeuseAlgue extends Command { + private Requin requin; + private Bougie bougie; + /** Creates a new Balayeuse. */ + public BalayeuseAlgue(Requin requin, Bougie bougie) { + this.requin = requin; + this.bougie =bougie; + addRequirements(requin,bougie); + // 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() { + if(requin.encodeur()>=500 && requin.encodeur()<=510) + { + requin.rotationer(0); + if(requin.amp()> 60){ + requin.balaye(0); + bougie.Vert(); + } + else + { + requin.balaye(0.5); + } + } + else if(requin.encodeur()>=510){ + requin.rotationer(0.5); + } + else{ + requin.rotationer(-0.5); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + requin.rotationer(0); + requin.balaye(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseBas.java b/src/main/java/frc/robot/commands/requin/BalayeuseBas.java new file mode 100644 index 0000000..67d0687 --- /dev/null +++ b/src/main/java/frc/robot/commands/requin/BalayeuseBas.java @@ -0,0 +1,40 @@ +// 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.requin; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Requin; + +/* 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 BalayeuseBas extends Command { + private Requin requin; + /** Creates a new Balayeuse. */ + public BalayeuseBas(Requin requin) { + this.requin = requin; + addRequirements(requin); + // 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() { + requin.rotationer(-0.5); + } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + requin.rotationer(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java new file mode 100644 index 0000000..5b12ada --- /dev/null +++ b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java @@ -0,0 +1,67 @@ +// 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.requin; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; +import frc.robot.subsystems.Requin; + +/* 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 BalayeuseCoral extends Command { + private Requin requin; + private Bougie bougie; + /** Creates a new Balayeuse. */ + public BalayeuseCoral(Requin requin, Bougie bougie) { + this.requin = requin; + this.bougie = bougie; + addRequirements(requin,bougie); + // 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() { + if(requin.encodeur()>=100 && requin.encodeur()<=110){ + requin.rotationer(0); + if(requin.amp()>60){ + requin.balaye(0); + bougie.Vert(); + if(requin.enHaut()){ + requin.rotationer(0); + } + else{ + requin.rotationer(0.5); + } + } + else{ + requin.balaye(0.5); + } + } + else if(requin.encodeur()>=110){ + requin.rotationer(0.5); + } + else{ + requin.rotationer(-0.5); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + requin.rotationer(0); + requin.balaye(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/PinceManuel2.java b/src/main/java/frc/robot/commands/requin/BalayeuseHaut.java similarity index 68% rename from src/main/java/frc/robot/commands/PinceManuel2.java rename to src/main/java/frc/robot/commands/requin/BalayeuseHaut.java index bf8d1e4..eea2d7d 100644 --- a/src/main/java/frc/robot/commands/PinceManuel2.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseHaut.java @@ -2,22 +2,18 @@ // 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; +package frc.robot.commands.requin; + import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Pince; +import frc.robot.subsystems.Requin; /* 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 PinceManuel2 extends Command { - private Pince pince; - //private DoubleSupplier doubleSupplier; - /** Creates a new PinceManuel. */ - public PinceManuel2(Pince pince - - //,DoubleSupplier doubleSupplier - ) { - this.pince = pince; - // this.doubleSupplier = doubleSupplier; - addRequirements(pince); +public class BalayeuseHaut extends Command { + private Requin requin; + /** Creates a new Balayeuse. */ + public BalayeuseHaut(Requin requin) { + this.requin = requin; + addRequirements(requin); // Use addRequirements() here to declare subsystem dependencies. } @@ -28,15 +24,19 @@ public class PinceManuel2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - pince.pivote(-0.2); - + if(requin.enHaut()==true){ + requin.rotationer(0); + requin.reset(); + } + else{ + requin.rotationer(-0.5); + } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - pince.pivote(0); + requin.rotationer(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/requin/ExpireAlgue.java b/src/main/java/frc/robot/commands/requin/ExpireAlgue.java new file mode 100644 index 0000000..679c6ce --- /dev/null +++ b/src/main/java/frc/robot/commands/requin/ExpireAlgue.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.requin; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Requin; + +/* 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 ExpireAlgue extends Command { + private Requin requin; + /** Creates a new ExpireAlgue. */ + public ExpireAlgue(Requin requin) { + this.requin = requin; + addRequirements(requin); + // 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() { + requin.balaye(0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + requin.balaye(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/requin/L1Requin.java b/src/main/java/frc/robot/commands/requin/L1Requin.java new file mode 100644 index 0000000..1a7d127 --- /dev/null +++ b/src/main/java/frc/robot/commands/requin/L1Requin.java @@ -0,0 +1,65 @@ +// 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.requin; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; +import frc.robot.subsystems.Requin; + +/* 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 L1Requin extends Command { + private Requin requin; + private Bougie bougie; + /** Creates a new Balayeuse. */ + public L1Requin(Requin requin,Bougie bougie) { + this.requin = requin; + this.bougie = bougie; + addRequirements(requin,bougie); + // 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() { + + if(requin.encodeur()>=800 && requin.encodeur()<=810){ + requin.rotationer(0); + if(requin.amp()>8){ + requin.balaye(-0.5); + } + else{ + requin.balaye(0); + bougie.Rouge(); + } + } + else if(requin.encodeur()>=810){ + requin.rotationer(0.5); + } + else{ + requin.rotationer(-0.5); + } + + + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + requin.rotationer(0); + requin.balaye(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Pince/Algue1Test.java b/src/main/java/frc/robot/commands/requin/aspire.java similarity index 75% rename from src/main/java/frc/robot/commands/Pince/Algue1Test.java rename to src/main/java/frc/robot/commands/requin/aspire.java index ab5e886..5105d83 100644 --- a/src/main/java/frc/robot/commands/Pince/Algue1Test.java +++ b/src/main/java/frc/robot/commands/requin/aspire.java @@ -2,19 +2,19 @@ // 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.Pince; +package frc.robot.commands.requin; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Pince; +import frc.robot.subsystems.Requin; /* 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); +public class aspire extends Command { + /** Creates a new aspire. */ + private Requin requin; + public aspire(Requin requin) { // Use addRequirements() here to declare subsystem dependencies. + this.requin = requin; + addRequirements(requin); } // Called when the command is initially scheduled. @@ -24,13 +24,13 @@ public class Algue1Test extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - pince.algue1Test(0.2); + requin.balaye(0.3); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - pince.algue1Test(0); + requin.balaye(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Pince/Algue2Test.java b/src/main/java/frc/robot/commands/requin/exspire.java similarity index 75% rename from src/main/java/frc/robot/commands/Pince/Algue2Test.java rename to src/main/java/frc/robot/commands/requin/exspire.java index 386e4f1..9c57fe0 100644 --- a/src/main/java/frc/robot/commands/Pince/Algue2Test.java +++ b/src/main/java/frc/robot/commands/requin/exspire.java @@ -2,19 +2,19 @@ // 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.Pince; +package frc.robot.commands.requin; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Pince; +import frc.robot.subsystems.Requin; /* 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); +public class exspire extends Command { + /** Creates a new aspire. */ + private Requin requin; + public exspire(Requin requin) { // Use addRequirements() here to declare subsystem dependencies. + this.requin = requin; + addRequirements(requin); } // Called when the command is initially scheduled. @@ -24,13 +24,13 @@ public class Algue2Test extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - pince.algue2Test(0.2); + requin.balaye(-0.3); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - pince.algue2Test(0); + requin.balaye(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/requin/requin_manuel.java b/src/main/java/frc/robot/commands/requin/requin_manuel.java new file mode 100644 index 0000000..9a3fa3d --- /dev/null +++ b/src/main/java/frc/robot/commands/requin/requin_manuel.java @@ -0,0 +1,49 @@ +// 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.requin; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Requin; + +/* 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 requin_manuel extends Command { + /** Creates a new requin_manuel. */ + private Requin requin; + private DoubleSupplier x; + public requin_manuel(Requin requin) { + // Use addRequirements() here to declare subsystem dependencies. + this.requin = requin; + addRequirements(requin); + } + + // 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() { + if(requin.enHaut()){ + requin.rotationer(0); + } + else{ + requin.rotationer(x.getAsDouble()); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + requin.rotationer(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Bougie.java b/src/main/java/frc/robot/subsystems/Bougie.java index 02cd2a8..4c2dcb5 100644 --- a/src/main/java/frc/robot/subsystems/Bougie.java +++ b/src/main/java/frc/robot/subsystems/Bougie.java @@ -43,8 +43,12 @@ public class Bougie extends SubsystemBase { candle.setLEDs(255, 215, 0,0,48,8); candle.setLEDs(255, 215, 0,0,64,8); } - public void RainBow(){candle.animate(rainbowAnim);} - public void RainBowStop(){candle.animate(null);} + public void RainBow(){ + candle.animate(rainbowAnim); + } + public void RainBowStop(){ + candle.animate(null); + } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 311b4ca..335ef61 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; /** @@ -44,71 +43,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per secondĀ², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - private void configureAutoBuilder() { try { var config = RobotConfig.fromGUISettings(); @@ -140,9 +76,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return 0; }); } - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer; /** * Constructs a CTRE SwerveDrivetrain using the specified constants. @@ -234,28 +167,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public Command applyRequest(Supplier requestSupplier) { return run(() -> this.setControl(requestSupplier.get())); } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 607dae9..9562af3 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -3,8 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; - -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -25,7 +23,17 @@ public class Elevateur extends SubsystemBase { return monte.getEncoder().getPosition(); } public void vitesse(double vitesse){ - monte.set(vitesse); + if (limit2()) { + if (vitesse > 0) { + monte.set(0); + } + else{ + monte.set(vitesse); + } + } + else{ + monte.set(vitesse); + } } public boolean limit2(){ return limit2.get(); diff --git a/src/main/java/frc/robot/subsystems/Pince.java b/src/main/java/frc/robot/subsystems/Pince.java index f07f601..957b45b 100644 --- a/src/main/java/frc/robot/subsystems/Pince.java +++ b/src/main/java/frc/robot/subsystems/Pince.java @@ -6,8 +6,6 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; - -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -27,11 +25,21 @@ public class Pince extends SubsystemBase { final DigitalInput limit6 = new DigitalInput(9); - public void aspirecoral(double vitesse){ - coral.set(vitesse); - } +public void aspirecoral(double vitesse){ + coral.set(vitesse); +} public void pivote(double vitesse){ - pivoti.set(vitesse); + if (position()) { + if (vitesse > 0) { + pivoti.set(0); + } + else{ + pivoti.set(vitesse); + } + } + else{ + pivoti.set(vitesse); + } } public void aspirealgue(double vitesse){ algue2.set(-vitesse); @@ -52,12 +60,7 @@ 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() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/Requin.java b/src/main/java/frc/robot/subsystems/Requin.java new file mode 100644 index 0000000..db0e3c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Requin.java @@ -0,0 +1,48 @@ +// 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.subsystems; + +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Requin extends SubsystemBase { + /** Creates a new Requin. */ + ShuffleboardTab teb = Shuffleboard.getTab("teb"); + public Requin() { + teb.addBoolean("limit requin", this::enHaut); + } + + final SparkFlex balaye = new SparkFlex(15, MotorType.kBrushless); + final SparkMax rotatione = new SparkMax(17, MotorType.kBrushless); + final DigitalInput limit3 = new DigitalInput(1); + + public void balaye(double vitesse){ + balaye.set(vitesse); + } + public void rotationer(double vitesse){ + rotatione.set(vitesse); + } + public boolean enHaut(){ + return limit3.get(); + } + public double encodeur(){ + return rotatione.getEncoder().getPosition(); + } + public void reset(){ + rotatione.getEncoder().setPosition(0); + } + public double amp(){ + return balaye.getOutputCurrent(); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}