diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea864f5..276ef74 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,9 +44,7 @@ import frc.robot.commands.Pince.PinceManuel; import frc.robot.commands.grimpeur.GrimperHaut; import frc.robot.commands.grimpeur.GrimpeurBas; import frc.robot.commands.grimpeur.GrimpeurManuelhaut; -import frc.robot.commands.grimpeur.ResetGrimpeur; import frc.robot.commands.requin.BalayeuseAlgue; -import frc.robot.commands.requin.BalayeuseBas; import frc.robot.commands.requin.BalayeuseCoral; import frc.robot.commands.requin.BalayeuseHaut; import frc.robot.commands.requin.ExpireCorail; @@ -60,13 +58,16 @@ import frc.robot.subsystems.Limelight3G; import frc.robot.subsystems.Pince; import frc.robot.subsystems.Requin; import frc.robot.commands.requin.exspire; +import frc.robot.commands.Pince.DepartPince; +import frc.robot.commands.Elevateur.balonL2; +import frc.robot.commands.Elevateur.balonL3; public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage ); // Use open-loop control for drive motors @@ -106,34 +107,31 @@ public class RobotContainer { private void configureBindings() { drivetrain.registerTelemetry(logger::telemeterize); drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically + // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - 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) + drive.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*-manette1.getLeftX()*-manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward) + .withVelocityX(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) ) ); - - /* Manette 1 */ - // reset the field-centric heading on start press - manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - //pince manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightBumper().whileTrue(new StationPince(pince, elevateur,bougie)); - manette1.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); + manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie)); manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette1.povRight().whileTrue(new CoralExpire(pince, bougie)); - //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)); + manette1.leftTrigger().whileTrue(new DepartPince(pince)); + manette1.povDown().whileTrue(new Algue_inspire(pince,bougie)); + //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)); + manette1.povUp().whileTrue(new balonL2(elevateur)); + manette1.start().whileTrue(new balonL3(elevateur)); /* Manette 2 */ - manette2.a().whileTrue(new CorailAspir(pince,bougie)); - manette2.b().whileTrue(new Algue_inspire(pince,bougie)); //requin manette2.rightBumper().whileTrue(new BalayeuseAlgue(requin,bougie)); manette2.leftBumper().whileTrue(new L1Requin(requin, bougie)); @@ -141,12 +139,15 @@ public class RobotContainer { manette2.leftTrigger().whileTrue(new BalayeuseCoral(requin, bougie)); manette2.y().whileTrue(new exspire(requin, bougie)); manette2.x().whileTrue(new ExpireCorail(requin, bougie)); - + + //limelight + manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); //grimpeur manette2.povDown().whileTrue(new GrimpeurManuelhaut(Grimpeur, bougie)); manette2.povLeft().whileTrue(new GrimperHaut(Grimpeur, bougie)); manette2.povRight().whileTrue(new GrimpeurBas(Grimpeur)); + //Pince manuel pince.setDefaultCommand(new RunCommand(()->{ pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05)); @@ -157,8 +158,6 @@ public class RobotContainer { elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05)); }, elevateur)); - //limelight - manette2.povUp().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); //Reset encodeur manette2.start().whileTrue(new reset(elevateur, pince)); } diff --git a/src/main/java/frc/robot/commands/Elevateur/Depart.java b/src/main/java/frc/robot/commands/Elevateur/Depart.java index 91c48a9..75dbcec 100644 --- a/src/main/java/frc/robot/commands/Elevateur/Depart.java +++ b/src/main/java/frc/robot/commands/Elevateur/Depart.java @@ -11,10 +11,12 @@ 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.elevateur = elevateur; - addRequirements(elevateur); + this.pince = pince; + addRequirements(elevateur, pince); // Use addRequirements() here to declare subsystem dependencies. } @@ -25,20 +27,27 @@ public class Depart extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(pince.position()){ + pince.pivote(0); + pince.reset(); + } + else{ + pince.pivote(-0.2); + } if(elevateur.limit2()==true){ elevateur.vitesse(0); elevateur.reset(); } else{ - elevateur.vitesse(.3); + elevateur.vitesse(.5); } - } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { elevateur.vitesse(0); + pince.pivote(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Elevateur/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java index f7d8e79..279cd5e 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -26,11 +26,11 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(pince.encodeurpivot()>=15.6 && pince.encodeurpivot()<=16){ + if(pince.encodeurpivot()>=16 && pince.encodeurpivot()<=17){ pince.pivote(0); } - else if(pince.encodeurpivot()>=16){ + else if(pince.encodeurpivot()>=17){ pince.pivote(-0.1); } else{ diff --git a/src/main/java/frc/robot/commands/Elevateur/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java index 3d2afe9..f6634e7 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -30,22 +30,23 @@ public class L3 extends Command { public void execute() { if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ elevateur.vitesse(0); - } + pince.pivote(-0.15); + if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){ + pince.pivote(0); + } + else if(pince.encodeurpivot()>=20){ + pince.pivote(-0.15); + } + else{ + pince.pivote(0.15); + } + } else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){ - elevateur.vitesse(-0.5); + elevateur.vitesse(-0.7); } else{ - elevateur.vitesse(0.5); + elevateur.vitesse(0.25); } - if(pince.encodeurpivot()>=15.6 && pince.encodeurpivot()<=16){ - pince.pivote(0); - } - else if(pince.encodeurpivot()>=16){ - pince.pivote(-0.1); - } - else{ - pince.pivote(0.1); - } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Elevateur/L4.java b/src/main/java/frc/robot/commands/Elevateur/L4.java index 6d562f5..99c8499 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L4.java +++ b/src/main/java/frc/robot/commands/Elevateur/L4.java @@ -28,24 +28,24 @@ public class L4 extends Command { @Override public void execute() { if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){ - elevateur.vitesse(0); - + elevateur.vitesse(0); + pince.pivote(-0.1); + if(pince.encodeurpivot()>=22 && pince.encodeurpivot()<=23){ + pince.pivote(0); + } + else if(pince.encodeurpivot()>=22){ + pince.pivote(-0.15); + } + else{ + pince.pivote(0.15); + } } else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){ - elevateur.vitesse(-0.5); + elevateur.vitesse(-0.7); } else{ - elevateur.vitesse(.5); + elevateur.vitesse(.25); } - if(pince.encodeurpivot()>=21.76 && pince.encodeurpivot()<=22.3){ - pince.pivote(0); - } - else if(pince.encodeurpivot()>=22.3){ - pince.pivote(-0.1); - } - else{ - pince.pivote(0.1); - } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Elevateur/StationPince.java b/src/main/java/frc/robot/commands/Elevateur/StationPince.java index 0f5d460..c201b48 100644 --- a/src/main/java/frc/robot/commands/Elevateur/StationPince.java +++ b/src/main/java/frc/robot/commands/Elevateur/StationPince.java @@ -28,26 +28,37 @@ public class StationPince extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(pince.emperagecoral() > 60){ - pince.aspirecoral(0);bougie.Bleu(); - } - else{ - pince.aspirecoral(0.5); - } - if(pince.encodeurpivot()<=11 && pince.encodeurpivot()>=12.6){ - pince.pivote(0); - } - else if(pince.encodeurpivot()>=9.8){ - pince.pivote(-0.1); + if(pince.emperagecoral() >= 12){ + pince.x = true; } + if(pince.x){ + pince.aspirecoral(0); + bougie.Bleu(); + pince.pivote(-0.2); + if(pince.position()){ + pince.pivote(0); + } + } else{ - pince.pivote(0.1); + pince.aspirecoral(0.1); + } + if (!pince.x){ + if(pince.encodeurpivot()>=10.5 && pince.encodeurpivot()<=11.5){ + pince.pivote(0); + } + else if(pince.encodeurpivot()>=11){ + pince.pivote(-0.1); + } + else{ + pince.pivote(0.1); + } } } @@ -56,6 +67,7 @@ public class StationPince extends Command { public void end(boolean interrupted) { pince.pivote(0); pince.aspirecoral(0); + pince.x =false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Elevateur/balonL2.java b/src/main/java/frc/robot/commands/Elevateur/balonL2.java new file mode 100644 index 0000000..c643482 --- /dev/null +++ b/src/main/java/frc/robot/commands/Elevateur/balonL2.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.Elevateur; + +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.subsystems.Elevateur; +/* 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 balonL2 extends Command { + private Elevateur elevateur; + /** Creates a new L2. */ + public balonL2(Elevateur elevateur) { + this.elevateur = elevateur; + addRequirements(elevateur); + // 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(elevateur.position()<=-2 && elevateur.position()>=-2.2){ + elevateur.vitesse(0); + } + else if(elevateur.position()>= -1.95){ + elevateur.vitesse(-0.7); + } + else{ + elevateur.vitesse(0.25); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevateur.vitesse(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Elevateur/balonL3.java b/src/main/java/frc/robot/commands/Elevateur/balonL3.java new file mode 100644 index 0000000..9662961 --- /dev/null +++ b/src/main/java/frc/robot/commands/Elevateur/balonL3.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.Elevateur; + +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.subsystems.Elevateur; +/* 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 balonL3 extends Command { + private Elevateur elevateur; + /** Creates a new L2. */ + public balonL3(Elevateur elevateur) { + this.elevateur = elevateur; + addRequirements(elevateur); + // 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(elevateur.position()<=-4 && elevateur.position()>=-4.1){ + elevateur.vitesse(0); + } + else if(elevateur.position()>= -4){ + elevateur.vitesse(-0.7); + } + else{ + elevateur.vitesse(0.25); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevateur.vitesse(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java index e5ad0a4..af10b89 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java @@ -49,8 +49,8 @@ public class AprilTag3G extends Command { if(limelight3g.getV() == true){ drivetrain.setControl(drive. withRotationalRate(-a/7). - withVelocityX(x.getAsDouble()). - withVelocityY(y.getAsDouble())); + withVelocityY(x.getAsDouble()). + withVelocityX(-y.getAsDouble())); System.out.println(a/7); } else{ @@ -73,6 +73,6 @@ public class AprilTag3G extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return limelight3g.getX()<1 && limelight3g.getX()>-1; + return false; } } diff --git a/src/main/java/frc/robot/commands/Pince/Algue_inspire.java b/src/main/java/frc/robot/commands/Pince/Algue_inspire.java index c6319c3..6c76b6b 100644 --- a/src/main/java/frc/robot/commands/Pince/Algue_inspire.java +++ b/src/main/java/frc/robot/commands/Pince/Algue_inspire.java @@ -37,7 +37,6 @@ public class Algue_inspire extends Command { } else{ pince.aspirealgue(0.5); - } } diff --git a/src/main/java/frc/robot/commands/Pince/DepartPince.java b/src/main/java/frc/robot/commands/Pince/DepartPince.java index bdfc343..bc618ba 100644 --- a/src/main/java/frc/robot/commands/Pince/DepartPince.java +++ b/src/main/java/frc/robot/commands/Pince/DepartPince.java @@ -29,7 +29,7 @@ public class DepartPince extends Command { pince.reset(); } else{ - pince.pivote(.2); + pince.pivote(-0.2); } } diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseBas.java b/src/main/java/frc/robot/commands/requin/BalayeuseBas.java index da6b3de..b2ee059 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseBas.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseBas.java @@ -24,7 +24,7 @@ public class BalayeuseBas extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - requin.rotationer(0.5); + requin.rotationer(0.2); } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java index 5b8b3c6..d4e7c3b 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java @@ -29,28 +29,26 @@ public class BalayeuseCoral extends Command { public void execute() { if(requin.encodeur()<=1200 && requin.encodeur()>=1025){ requin.rotationer(0); - if(requin.amp()>60){ - requin.balaye(0); - bougie.Vert(); - if(requin.enHaut()){ - requin.rotationer(0); - } - else{ - requin.rotationer(-0.5); - } - } - else{ + if(requin.amp()>=12){ + requin.xRequin = true; + } + if(requin.xRequin){ + requin.balaye(0); + bougie.Vert(); + } + else{ requin.balaye(0.5); - } - } - else if(requin.encodeur()>=1200){ - requin.rotationer(-0.5); - } - else{ - requin.rotationer(0.5); - } - - } + } + } + if (!requin.xRequin) { + if(requin.encodeur()>=1200){ + requin.rotationer(-0.5); + } + else{ + requin.rotationer(0.5); + } + } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/requin/exspire.java b/src/main/java/frc/robot/commands/requin/exspire.java index 4517611..33a0fc0 100644 --- a/src/main/java/frc/robot/commands/requin/exspire.java +++ b/src/main/java/frc/robot/commands/requin/exspire.java @@ -29,7 +29,14 @@ public class exspire extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - requin.balaye(0.5); + if(requin.amp()> 15) + { + requin.balaye(0.5); + } + else{ + bougie.Rouge(); + requin.balaye(0.5); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index a59c533..5b0dc10 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -18,13 +18,13 @@ public class Elevateur extends SubsystemBase { private GenericEntry encodeurelevateurL2haut = teb.add("encodeurelevateurL2haut", -0.9).getEntry(); private GenericEntry encodeurelevateurL3bas = - teb.add("encodeurelevateurL3bas", -1.9).getEntry(); + teb.add("encodeurelevateurL3bas", -3).getEntry(); private GenericEntry encodeurelevateurL3haut = - teb.add("encodeurelevateurL3haut", -2.11).getEntry(); + teb.add("encodeurelevateurL3haut", -3.1).getEntry(); private GenericEntry encodeurelevateurL4bas = - teb.add("encodeurelevateurL4bas", -6.6).getEntry(); + teb.add("encodeurelevateurL4bas", -6.3).getEntry(); private GenericEntry encodeurelevateurL4haut = - teb.add("encodeurelevateurL4haut", -6.2).getEntry(); + teb.add("encodeurelevateurL4haut", -6.5).getEntry(); private GenericEntry encodeurelevateurstationbas = teb.add("encodeurelevateursationbas", -0.5).getEntry(); private GenericEntry encodeurelevateurstationhaut = @@ -67,16 +67,16 @@ public class Elevateur extends SubsystemBase { return encodeurelevateurL2haut.getDouble(-0.9); } public double encodeurelevateurL3bas(){ - return encodeurelevateurL3bas.getDouble(-1.9); + return encodeurelevateurL3bas.getDouble(-2.8); } public double encodeurelevateurL3haut(){ - return encodeurelevateurL3haut.getDouble(-2.11); + return encodeurelevateurL3haut.getDouble(-3); } public double encodeurelevateurL4bas(){ - return encodeurelevateurL4bas.getDouble(-6.6); + return encodeurelevateurL4bas.getDouble(-6.3); } public double encodeurelevateurL4haut(){ - return encodeurelevateurL4haut.getDouble(-6.2); + return encodeurelevateurL4haut.getDouble(-6.5); } public double encodeurelevateurstationbas(){ return encodeurelevateurstationbas.getDouble(-0.5); diff --git a/src/main/java/frc/robot/subsystems/Pince.java b/src/main/java/frc/robot/subsystems/Pince.java index 4aaa92a..4ddc0a7 100644 --- a/src/main/java/frc/robot/subsystems/Pince.java +++ b/src/main/java/frc/robot/subsystems/Pince.java @@ -62,6 +62,7 @@ public double emperagecoral(){ public double emperagealgue(){ return algue1.getOutputCurrent(); } +public boolean x = false; @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Requin.java b/src/main/java/frc/robot/subsystems/Requin.java index 58e0c60..844bee7 100644 --- a/src/main/java/frc/robot/subsystems/Requin.java +++ b/src/main/java/frc/robot/subsystems/Requin.java @@ -43,6 +43,7 @@ public class Requin extends SubsystemBase { public double amp(){ return balaye.getOutputCurrent(); } + public boolean xRequin = false; @Override public void periodic() { // This method will be called once per scheduler run