diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a16ba8b..471fa8c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,9 +41,17 @@ 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.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.ExpireAlgue; +import frc.robot.commands.requin.L1Requin; +import frc.robot.commands.requin.aspire; import frc.robot.commands.requin.exspire; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -84,7 +92,7 @@ public class RobotContainer { Pose2d pose = new Pose2d(); Grimpeur Grimpeur = new Grimpeur(); Requin requin = new Requin(); - + CorailAspir corailAspir = new CorailAspir(pince, bougie); public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("New Auto"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -117,7 +125,7 @@ public class RobotContainer { manette1.rightBumper().whileTrue(new StationPince(pince, elevateur)); manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie)); manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); - + manette1.povRight().whileTrue(new AlgueExpire(pince, bougie)); //elevateur manette1.a().whileTrue(new Depart(elevateur, pince)); manette1.b().whileTrue(new L2(elevateur,pince)); @@ -128,11 +136,12 @@ public class RobotContainer { //requin manette2.a().whileTrue(new CorailAspir(pince,bougie)); manette2.b().whileTrue(new Algue_inspire(pince,bougie)); - manette2.y().whileTrue(new BalayeuseHaut(requin)); - manette2.x().whileTrue(new BalayeuseBas(requin)); - manette2.rightTrigger().whileTrue(new exspire(requin,bougie)); - manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); - + manette2.y().whileTrue(new BalayeuseAlgue(requin,bougie)); + manette2.x().whileTrue(new BalayeuseHaut(requin)); + manette2.rightTrigger().whileTrue(new ExpireAlgue(requin, bougie)); + manette2.leftTrigger().whileTrue(new BalayeuseCoral(requin, bougie)); + manette1.povUp().whileTrue(new L1Requin(requin, bougie)); + manette2.povUp().whileTrue(new aspire(requin, bougie)); //Pince manuel pince.setDefaultCommand(new RunCommand(()->{ pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY()*manette2.getRightY(), 0.05)); @@ -149,8 +158,10 @@ public class RobotContainer { //Reset encodeur manette2.start().whileTrue(new reset(elevateur, pince)); - //grimpeur manuel + //grimpeur manette2.povDown().whileTrue(new GrimpeurManuelhaut(Grimpeur, bougie)); + manette2.povLeft().whileTrue(new GrimperHaut(Grimpeur, bougie)); + manette2.povRight().whileTrue(new GrimpeurBas(Grimpeur)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Elevateur/Depart.java b/src/main/java/frc/robot/commands/Elevateur/Depart.java index 56f684b..91c48a9 100644 --- a/src/main/java/frc/robot/commands/Elevateur/Depart.java +++ b/src/main/java/frc/robot/commands/Elevateur/Depart.java @@ -30,7 +30,7 @@ public class Depart extends Command { elevateur.reset(); } else{ - elevateur.vitesse(.5); + elevateur.vitesse(.3); } } diff --git a/src/main/java/frc/robot/commands/Elevateur/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java index f1f1e7e..f7d8e79 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -26,24 +26,15 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position()<=elevateur.encodeurelevateurL2bas() && elevateur.position()>=elevateur.encodeurelevateurL2haut()){ - elevateur.vitesse(0); - } - else if(elevateur.position()>=elevateur.encodeurelevateurL2bas()){ - elevateur.vitesse(-0.2); - } - else{ - elevateur.vitesse(.2); - // } - // if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){ - // pince.pivote(0); + if(pince.encodeurpivot()>=15.6 && pince.encodeurpivot()<=16){ + pince.pivote(0); - // } - // else if(pince.encodeurpivot()>=510){ - // pince.pivote(0.2); - // } - // else{ - // pince.pivote(-0.2); + } + else if(pince.encodeurpivot()>=16){ + pince.pivote(-0.1); + } + else{ + pince.pivote(0.1); } } diff --git a/src/main/java/frc/robot/commands/Elevateur/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java index b8a4bf2..3d2afe9 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -32,20 +32,20 @@ public class L3 extends Command { elevateur.vitesse(0); } else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){ - elevateur.vitesse(-0.2); + elevateur.vitesse(-0.5); } else{ - elevateur.vitesse(.2); + elevateur.vitesse(0.5); } - // if(pince.encodeurpivot()>=700 && pince.encodeurpivot()<=710){ - // pince.pivote(0); - // } - // else if(pince.encodeurpivot()>=710){ - // pince.pivote(0.2); - // } - // else{ - // pince.pivote(-0.2); - // } + 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 6663827..6d562f5 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L4.java +++ b/src/main/java/frc/robot/commands/Elevateur/L4.java @@ -29,22 +29,23 @@ public class L4 extends Command { public void execute() { if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){ elevateur.vitesse(0); + } else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){ - elevateur.vitesse(-0.2); + elevateur.vitesse(-0.5); } else{ - elevateur.vitesse(.2); + elevateur.vitesse(.5); } - // if(pince.encodeurpivot()>=800 && pince.encodeurpivot()<=809){ - // pince.pivote(0); - // } - // else if(pince.encodeurpivot()>=810){ - // pince.pivote(0.2); - // } - // else{ - // pince.pivote(-0.2); - // } + 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 05c1ebd..f7d7f67 100644 --- a/src/main/java/frc/robot/commands/Elevateur/StationPince.java +++ b/src/main/java/frc/robot/commands/Elevateur/StationPince.java @@ -31,23 +31,14 @@ public class StationPince extends Command { @Override public void execute() { pince.aspirecoral(0.5); - if(pince.encodeurpivot()<=elevateur.encodeurelevateurstationbas() && pince.encodeurpivot()>=elevateur.encodeurelevateurstationhaut()){ + if(pince.encodeurpivot()<=9.8 && pince.encodeurpivot()>=12){ pince.pivote(0); } - else if(pince.encodeurpivot()>=elevateur.encodeurelevateurstationbas()){ - pince.pivote(0.2); + else if(pince.encodeurpivot()>=9.8){ + pince.pivote(-0.1); } else{ - pince.pivote(-0.2); - } - if(elevateur.position()>=400 && elevateur.position()<=410){ - elevateur.vitesse(0); - } - else if(elevateur.position()>=410){ - elevateur.vitesse(0.3); - } - else{ - elevateur.vitesse(-.3); + pince.pivote(0.1); } } diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java index b63010c..e5ad0a4 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java @@ -48,10 +48,10 @@ public class AprilTag3G extends Command { double a = limelight3g.getX(); if(limelight3g.getV() == true){ drivetrain.setControl(drive. - withRotationalRate(-a/5). + withRotationalRate(-a/7). withVelocityX(x.getAsDouble()). withVelocityY(y.getAsDouble())); - System.out.println(a/5); + System.out.println(a/7); } else{ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/commands/Pince/CorailAspir.java b/src/main/java/frc/robot/commands/Pince/CorailAspir.java index 5cb5e13..444b5de 100644 --- a/src/main/java/frc/robot/commands/Pince/CorailAspir.java +++ b/src/main/java/frc/robot/commands/Pince/CorailAspir.java @@ -27,11 +27,14 @@ public class CorailAspir extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(pince.emperagecoral() > 60){ + if(pince.emperagecoral() > 15){ pince.aspirecoral(0); bougie.Bleu(); } - pince.aspirecoral(0.5); + else{ + pince.aspirecoral(0.5); + } + } // Called once the command ends or is interrupted. @@ -43,6 +46,6 @@ public class CorailAspir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return pince.emperagecoral()>13; } } diff --git a/src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java b/src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java index 7dbb65c..8455d38 100644 --- a/src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java +++ b/src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java @@ -23,10 +23,10 @@ public class GrimpeurBas extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(grimpeur.encodeur()>=-38.5 && grimpeur.encodeur()<=-39.19){ + if(grimpeur.encodeur()>=-43.3 && grimpeur.encodeur()<=-42.5){ grimpeur.grimpe(0); } - else if(grimpeur.encodeur()>=-38.5){ + else if(grimpeur.encodeur()>=-43.3){ grimpeur.grimpe(-0.5); } else{grimpeur.grimpe(0.5); diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java index 212583b..5f9ce05 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java @@ -27,7 +27,7 @@ public class BalayeuseAlgue extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(requin.encodeur()>=500 && requin.encodeur()<=510) + if(requin.encodeur()<=715 && requin.encodeur()>=700) { requin.rotationer(0); if(requin.amp()> 60){ @@ -36,10 +36,10 @@ public class BalayeuseAlgue extends Command { } else { - requin.balaye(0.5); + requin.balaye(-0.5); } } - else if(requin.encodeur()>=510){ + else if(requin.encodeur()>=700){ requin.rotationer(0.5); } else{ diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseBas.java b/src/main/java/frc/robot/commands/requin/BalayeuseBas.java index 67d0687..da6b3de 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.5); } // 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 5b12ada..3cb837f 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java @@ -27,7 +27,7 @@ public class BalayeuseCoral extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(requin.encodeur()>=100 && requin.encodeur()<=110){ + if(requin.encodeur()<=1200 && requin.encodeur()>=1050){ requin.rotationer(0); if(requin.amp()>60){ requin.balaye(0); @@ -36,18 +36,18 @@ public class BalayeuseCoral extends Command { requin.rotationer(0); } else{ - requin.rotationer(0.5); + requin.rotationer(-0.5); } } else{ requin.balaye(0.5); } } - else if(requin.encodeur()>=110){ - requin.rotationer(0.5); + else if(requin.encodeur()>=1200){ + requin.rotationer(-0.5); } else{ - requin.rotationer(-0.5); + requin.rotationer(0.5); } } diff --git a/src/main/java/frc/robot/commands/requin/L1Requin.java b/src/main/java/frc/robot/commands/requin/L1Requin.java index c65e1cc..23a73f6 100644 --- a/src/main/java/frc/robot/commands/requin/L1Requin.java +++ b/src/main/java/frc/robot/commands/requin/L1Requin.java @@ -28,7 +28,7 @@ public class L1Requin extends Command { @Override public void execute() { - if(requin.encodeur()>=800 && requin.encodeur()<=810){ + if(requin.encodeur()<=645 && requin.encodeur()>=600){ requin.rotationer(0); if(requin.amp()>60){ requin.balaye(-0.5); @@ -38,11 +38,11 @@ public class L1Requin extends Command { bougie.Rouge(); } } - else if(requin.encodeur()>=810){ - requin.rotationer(0.5); + else if(requin.encodeur()>=645){ + requin.rotationer(-0.5); } else{ - requin.rotationer(-0.5); + requin.rotationer(0.5); } diff --git a/src/main/java/frc/robot/commands/requin/aspire.java b/src/main/java/frc/robot/commands/requin/aspire.java index dee5e1e..3be18f1 100644 --- a/src/main/java/frc/robot/commands/requin/aspire.java +++ b/src/main/java/frc/robot/commands/requin/aspire.java @@ -33,7 +33,7 @@ public class aspire extends Command { } else { - requin.balaye(0.5); + requin.balaye(-0.5); } } diff --git a/src/main/java/frc/robot/commands/requin/exspire.java b/src/main/java/frc/robot/commands/requin/exspire.java index dd45048..12ee35d 100644 --- a/src/main/java/frc/robot/commands/requin/exspire.java +++ b/src/main/java/frc/robot/commands/requin/exspire.java @@ -30,12 +30,12 @@ public class exspire extends Command { @Override public void execute() { if(requin.amp()> 60){ - requin.balaye(-0.5); + requin.balaye(0.5); } else { bougie.Rouge(); - requin.balaye(-0.5); + requin.balaye(0.5); } } diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index c906975..f78578d 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", -2.9).getEntry(); + teb.add("encodeurelevateurL3bas", -1.9).getEntry(); private GenericEntry encodeurelevateurL3haut = - teb.add("encodeurelevateurL3haut", -3).getEntry(); + teb.add("encodeurelevateurL3haut", -2.11).getEntry(); private GenericEntry encodeurelevateurL4bas = - teb.add("encodeurelevateurL4bas", -6.4).getEntry(); + teb.add("encodeurelevateurL4bas", -6.6).getEntry(); private GenericEntry encodeurelevateurL4haut = - teb.add("encodeurelevateurL4haut", -6.5).getEntry(); + teb.add("encodeurelevateurL4haut", -6.3).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(-2.9); + return encodeurelevateurL3bas.getDouble(-1.9); } public double encodeurelevateurL3haut(){ - return encodeurelevateurL3haut.getDouble(-3); + return encodeurelevateurL3haut.getDouble(-2.11); } public double encodeurelevateurL4bas(){ - return encodeurelevateurL4bas.getDouble(-6.4); + return encodeurelevateurL4bas.getDouble(-6.6); } public double encodeurelevateurL4haut(){ - return encodeurelevateurL4haut.getDouble(-6.5); + return encodeurelevateurL4haut.getDouble(-6.3); } public double encodeurelevateurstationbas(){ return encodeurelevateurstationbas.getDouble(-0.5); diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index 759c4b3..1f94b20 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -31,7 +31,7 @@ public class Grimpeur extends SubsystemBase { return grimpeur.getEncoder().getPosition(); } public void reset(){ - grimpeur.getEncoder().setPosition(0); + grimpeur.getEncoder().setPosition(135.11); } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Pince.java b/src/main/java/frc/robot/subsystems/Pince.java index 957b45b..4aaa92a 100644 --- a/src/main/java/frc/robot/subsystems/Pince.java +++ b/src/main/java/frc/robot/subsystems/Pince.java @@ -17,6 +17,8 @@ public class Pince extends SubsystemBase { public Pince() { teb.addBoolean("limit pince",this::position); teb.addDouble("encodeur pince", this::encodeurpivot); + teb.addDouble("amperage corail", this::emperagecoral); + teb.addDouble("amperage algue", this::emperagealgue); } final SparkMax coral = new SparkMax(20, MotorType.kBrushless); final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless); @@ -30,7 +32,7 @@ public void aspirecoral(double vitesse){ } public void pivote(double vitesse){ if (position()) { - if (vitesse > 0) { + if (vitesse < 0) { pivoti.set(0); } else{ diff --git a/src/main/java/frc/robot/subsystems/Requin.java b/src/main/java/frc/robot/subsystems/Requin.java index db0e3c4..58e0c60 100644 --- a/src/main/java/frc/robot/subsystems/Requin.java +++ b/src/main/java/frc/robot/subsystems/Requin.java @@ -17,6 +17,8 @@ public class Requin extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); public Requin() { teb.addBoolean("limit requin", this::enHaut); + teb.addDouble("amparge requin", this::amp); + teb.addDouble("encodeur requin", this::encodeur); } final SparkFlex balaye = new SparkFlex(15, MotorType.kBrushless);