diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 294a6d2..12bb4c1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,11 +13,11 @@ public class Constants { public static int lancer4 = 15; // Ballayeuse public static int roue = 16; - public static int etoile = 19; + public static int etoile = 21; // Accumulateur public static int Moteuracc2 = 20; - public static int Moteuracc = 21; + public static int Moteuracc = 19; // Guideur public static int guideur = 22; @@ -43,8 +43,8 @@ public class Constants { public static int ArriereGauche = 6; // Limit switch - public static int guideurhaut = 0; - public static int guideurbas = 1; + 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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0b43edf..22d113d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,8 +65,8 @@ public class RobotContainer { Lancer lancer = new Lancer(lanceur,limelight); LancerNote lancernote = new LancerNote(lanceur, accumulateur); Lancerampli lancerampli = new Lancerampli(lanceur,limelight); - GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX); - GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getRightX); + GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftY); + GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getRightY); AllumeLED allumeLED = new AllumeLED(LED, accumulateur); Pistongrimpeur pistongrimpeur = new Pistongrimpeur(grimpeur, LED); public RobotContainer() { @@ -80,7 +80,7 @@ public class RobotContainer { manette.a().whileTrue(new GuiderBas(guideur)); manette.b().whileTrue(new GuiderHaut(guideur)); manette.x().whileTrue(new PistonFerme(grimpeur)); - joystick.button(3).whileTrue(new Balayer(balayeuse, accumulateur)); + joystick.button(3).whileTrue(balayer); joystick.button(1).whileTrue(new LancerNote(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))); @@ -93,8 +93,8 @@ public class RobotContainer { // grimpeur manuel grimpeur.setDefaultCommand(new RunCommand(()->{ - grimpeur.droit(manette.getLeftX()); - grimpeur.gauche(manette.getRightX());} + grimpeur.droit(manette.getLeftY()); + grimpeur.gauche(manette.getRightY());} ,grimpeur)); LED.setDefaultCommand(allumeLED); diff --git a/src/main/java/frc/robot/command/Balayer.java b/src/main/java/frc/robot/command/Balayer.java index 01a230d..9b3e158 100644 --- a/src/main/java/frc/robot/command/Balayer.java +++ b/src/main/java/frc/robot/command/Balayer.java @@ -26,7 +26,7 @@ public class Balayer extends Command { public void execute() { if(accumulateur.limitswitch()){ balayeuse.balaye(0); - accumulateur.Accumuler(); + accumulateur.Accumuler(0); } else{ balayeuse.balaye(0.3); diff --git a/src/main/java/frc/robot/command/LancerNote.java b/src/main/java/frc/robot/command/LancerNote.java index 15b74c1..8cc3a4a 100644 --- a/src/main/java/frc/robot/command/LancerNote.java +++ b/src/main/java/frc/robot/command/LancerNote.java @@ -26,10 +26,10 @@ public class LancerNote extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = 100; + double vitesse = 0.5; lancer.lancerspeaker(); if(lancer.vitesse(vitesse)>vitesse){ - accumulateur.Accumuler(0.6); + accumulateur.Accumuler(); } else{ accumulateur.Accumuler(0); diff --git a/src/main/java/frc/robot/subsystem/Accumulateur.java b/src/main/java/frc/robot/subsystem/Accumulateur.java index 1e7a558..22611c4 100644 --- a/src/main/java/frc/robot/subsystem/Accumulateur.java +++ b/src/main/java/frc/robot/subsystem/Accumulateur.java @@ -18,7 +18,11 @@ public class Accumulateur extends SubsystemBase { ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); private GenericEntry vitesse = +<<<<<<< HEAD dashboard.add("vitesseacc", 0.1) +======= + dashboard.add("vitesseacc", 0.5) +>>>>>>> 9d55ab9afa804a6ffb304aee298f28ad470c396d .withSize(1, 1) .withPosition(4, 3) .getEntry(); @@ -27,11 +31,11 @@ public class Accumulateur extends SubsystemBase { final DigitalInput photocellacc = new DigitalInput(Constants.photocellacc); public void Deaccumuler(double vitesse){ - Moteuracc2.set(vitesse); + Moteuracc2.set(-vitesse); + Moteuracc.set(-vitesse); } public void moteuraccfollow(){ - Moteuracc.follow(Moteuracc2); - Moteuracc.setInverted(true); + } public boolean limitswitch(){ return photocellacc.get(); diff --git a/src/main/java/frc/robot/subsystem/Balayeuse.java b/src/main/java/frc/robot/subsystem/Balayeuse.java index 5ebdd36..4dc3991 100644 --- a/src/main/java/frc/robot/subsystem/Balayeuse.java +++ b/src/main/java/frc/robot/subsystem/Balayeuse.java @@ -15,13 +15,8 @@ public class Balayeuse extends SubsystemBase { public Balayeuse() {} final WPI_TalonSRX roue = new WPI_TalonSRX(Constants.roue); final WPI_TalonSRX etoile = new WPI_TalonSRX(Constants.etoile); - - public void Accumulation(double vitesse){ - roue.set(vitesse); - } public void balayeuse(){ - etoile.follow(roue); - etoile.setInverted(true); + } public void balaye(double vitesse){ roue.set(vitesse); diff --git a/src/main/java/frc/robot/subsystem/Lanceur.java b/src/main/java/frc/robot/subsystem/Lanceur.java index bb100d7..bd4f7fc 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", 1) + dashboard.add("vitesse", 0.5) .withSize(1, 1) .withPosition(3, 3) .getEntry(); private GenericEntry vitesseamp = - dashboard.add("vitesseamp", 1) + dashboard.add("vitesseamp", 0.4) .withSize(1, 1) .withPosition(3, 4) .getEntry(); @@ -36,23 +36,22 @@ public class Lanceur extends SubsystemBase { lanceur1.getPIDController(); } public void lancer(double vitesse){ - lanceur1.set(vitesse); + lanceur1.set(-vitesse); + lanceur2.set(-vitesse); + lanceur3.set(vitesse); + lanceur4.set(-vitesse); } public void lancerspeaker(){ - lancer(vitesse.getDouble(0.1)); + lancer(vitesse.getDouble(0.5)); } public void lanceramp(){ - lancer(vitesseamp.getDouble(0.5)); + lancer(vitesseamp.getDouble(0.4)); } public double vitesse(double vitesse){ return lanceur1.getEncoder().getVelocity(); } public void lanceur(){ - lanceur2.follow(lanceur1); - lanceur3.follow(lanceur1); - lanceur4.follow(lanceur1); - lanceur3.setInverted(true); - lanceur4.setInverted(true); + } @Override