diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 12bb4c1..cdb9911 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,9 +46,7 @@ public class Constants { public static int guideurhaut = 4; public static int guideurbas = 5; public static int photocellacc = 2; - public static int limithaut = 4; - public static int limitbas = 5; //piston - public static int pistondroiteouvre= 6; + public static int pistondroiteouvre= 7; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d89e6a4..869a55e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import frc.robot.command.GrimpeurGauche; import frc.robot.command.GuiderBas; import frc.robot.command.GuiderHaut; import frc.robot.command.Lancer; +import frc.robot.command.LancerAmp; import frc.robot.command.LancerNote; import frc.robot.command.Lancerampli; import frc.robot.command.Limelight_tracker; @@ -79,8 +80,10 @@ public class RobotContainer { CameraServer.startAutomaticCapture(); manette.a().whileTrue(new GuiderBas(guideur)); manette.b().whileTrue(new GuiderHaut(guideur)); + manette.x().whileTrue(new PistonFerme(grimpeur)); joystick.button(3).whileTrue(balayer); joystick.button(1).whileTrue(new LancerNote(lanceur, accumulateur)); + joystick.button(5).whileTrue(new LancerAmp(lanceur, accumulateur)); joystick.button(4).whileTrue(new ParallelCommandGroup(new Lancer(lanceur,limelight),new Limelight_tracker(limelight,drive))); joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive),new GuiderHaut(guideur))); diff --git a/src/main/java/frc/robot/command/GrimpeurDroit.java b/src/main/java/frc/robot/command/GrimpeurDroit.java index 162d692..cb53ba4 100644 --- a/src/main/java/frc/robot/command/GrimpeurDroit.java +++ b/src/main/java/frc/robot/command/GrimpeurDroit.java @@ -24,16 +24,14 @@ public class GrimpeurDroit extends Command { @Override public void initialize() { grimpeur.resetencodeurd(); - grimpeur.pistonferme(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { grimpeur.droit(doubleSupplier.getAsDouble()); - if(grimpeur.encoderd()>78){ - grimpeur.droit(0); - + if(grimpeur.encoderd()<73){ + grimpeur.droit(doubleSupplier.getAsDouble()); } else if(grimpeur.getpitch()<-15){ grimpeur.droit(-doubleSupplier.getAsDouble()); diff --git a/src/main/java/frc/robot/command/GrimpeurGauche.java b/src/main/java/frc/robot/command/GrimpeurGauche.java index ea9d860..df8891e 100644 --- a/src/main/java/frc/robot/command/GrimpeurGauche.java +++ b/src/main/java/frc/robot/command/GrimpeurGauche.java @@ -23,17 +23,17 @@ public class GrimpeurGauche extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - grimpeur.resetencodeurd(); grimpeur.resetencodeurg(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { grimpeur.gauche(doubleSupplier.getAsDouble()); - if(grimpeur.encoderg()>73){ - grimpeur.gauche(0); - } + if(grimpeur.encoderg()<78){ + grimpeur.gauche(doubleSupplier.getAsDouble()); + } else if(grimpeur.getpitch()<-15){ grimpeur.gauche(doubleSupplier.getAsDouble()); } @@ -43,10 +43,12 @@ public class GrimpeurGauche extends Command { else{ grimpeur.gauche(0); } - if(grimpeur.encoderd()>0){ + if(grimpeur.encoderg()<0){ + grimpeur.gauche(doubleSupplier.getAsDouble()); + } + else{ grimpeur.resetencodeurg(); grimpeur.gauche(0); - } } diff --git a/src/main/java/frc/robot/command/LancerAmp.java b/src/main/java/frc/robot/command/LancerAmp.java new file mode 100644 index 0000000..0392f21 --- /dev/null +++ b/src/main/java/frc/robot/command/LancerAmp.java @@ -0,0 +1,50 @@ +// 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.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.Accumulateur; +import frc.robot.subsystem.Lanceur; + +public class LancerAmp extends Command { + private Lanceur lancer; + private Accumulateur accumulateur; + /** Creates a new LancerNote. */ + public LancerAmp(Lanceur lancer, Accumulateur accumulateur) { + this.lancer = lancer; + this.accumulateur = accumulateur; + // 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() { + double vitesse = 0.1; + lancer.lanceramp(); + if(lancer.vitesse(vitesse)>vitesse){ + accumulateur.Accumuler(); + } + else{ + accumulateur.Accumuler(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + lancer.lancer(0); + accumulateur.Accumuler(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/LancerNote.java b/src/main/java/frc/robot/command/LancerNote.java index 8cc3a4a..1c32ce5 100644 --- a/src/main/java/frc/robot/command/LancerNote.java +++ b/src/main/java/frc/robot/command/LancerNote.java @@ -26,7 +26,7 @@ public class LancerNote extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = 0.5; + double vitesse = 0.8; lancer.lancerspeaker(); if(lancer.vitesse(vitesse)>vitesse){ accumulateur.Accumuler(); @@ -39,7 +39,7 @@ public class LancerNote extends Command { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - lancer.vitesse(0); + lancer.lancer(0); accumulateur.Accumuler(0); } diff --git a/src/main/java/frc/robot/command/PistonFerme.java b/src/main/java/frc/robot/command/PistonFerme.java index b4ef3fd..c072790 100644 --- a/src/main/java/frc/robot/command/PistonFerme.java +++ b/src/main/java/frc/robot/command/PistonFerme.java @@ -19,7 +19,7 @@ public class PistonFerme extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - grimpeur.pistonferme(); + grimpeur.pistonouvre(); } // Called every time the scheduler runs while the command is scheduled. @@ -28,7 +28,9 @@ public class PistonFerme extends Command { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + grimpeur.pistonferme(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystem/Accumulateur.java b/src/main/java/frc/robot/subsystem/Accumulateur.java index 11c5d3c..00d56b7 100644 --- a/src/main/java/frc/robot/subsystem/Accumulateur.java +++ b/src/main/java/frc/robot/subsystem/Accumulateur.java @@ -26,13 +26,7 @@ public class Accumulateur extends SubsystemBase { final WPI_TalonSRX Moteuracc = new WPI_TalonSRX(Constants.Moteuracc); final DigitalInput photocellacc = new DigitalInput(Constants.photocellacc); - public void Deaccumuler(double vitesse){ - Moteuracc2.set(-vitesse); - Moteuracc.set(-vitesse); - } - public void moteuraccfollow(){ - - } + public boolean limitswitch(){ return photocellacc.get(); } @@ -41,8 +35,8 @@ public class Accumulateur extends SubsystemBase { dashboard.addBoolean("limitacc", this::limitswitch); } public void Accumuler(double vitesse){ - Moteuracc.set(vitesse); - Moteuracc2.set(vitesse); + Moteuracc.set(-vitesse); + Moteuracc2.set(-vitesse); } public void Accumuler(){ Accumuler(vitesse.getDouble(0.3)); diff --git a/src/main/java/frc/robot/subsystem/Grimpeur.java b/src/main/java/frc/robot/subsystem/Grimpeur.java index 045a1be..e16af68 100644 --- a/src/main/java/frc/robot/subsystem/Grimpeur.java +++ b/src/main/java/frc/robot/subsystem/Grimpeur.java @@ -28,7 +28,7 @@ public class Grimpeur extends SubsystemBase { final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless); final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); // limit switch - final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondroiteouvre); + final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.REVPH, Constants.pistondroiteouvre); //fonction public Grimpeur() { pistonouvre(); diff --git a/src/main/java/frc/robot/subsystem/Lanceur.java b/src/main/java/frc/robot/subsystem/Lanceur.java index bd4f7fc..7730b2b 100644 --- a/src/main/java/frc/robot/subsystem/Lanceur.java +++ b/src/main/java/frc/robot/subsystem/Lanceur.java @@ -16,12 +16,12 @@ public class Lanceur extends SubsystemBase { /** Creates a new Lanceur. */ ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); private GenericEntry vitesse = - dashboard.add("vitesse", 0.5) + dashboard.add("vitesse", 0.8) .withSize(1, 1) .withPosition(3, 3) .getEntry(); private GenericEntry vitesseamp = - dashboard.add("vitesseamp", 0.4) + dashboard.add("vitesseamp", 0.1) .withSize(1, 1) .withPosition(3, 4) .getEntry(); @@ -42,13 +42,13 @@ public class Lanceur extends SubsystemBase { lanceur4.set(-vitesse); } public void lancerspeaker(){ - lancer(vitesse.getDouble(0.5)); + lancer(vitesse.getDouble(0.8)); } public void lanceramp(){ - lancer(vitesseamp.getDouble(0.4)); + lancer(vitesseamp.getDouble(0.1)); } public double vitesse(double vitesse){ - return lanceur1.getEncoder().getVelocity(); + return lanceur3.getEncoder().getVelocity(); } public void lanceur(){