diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 21e30aa..1206c23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,8 +45,7 @@ public class Constants { // Limit switch public static int guideurhaut = 0; public static int guideurbas = 1; - public static int limitacc = 2; - public static int limitacc2 = 3; + 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 4ba1d18..239c7f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,10 +12,11 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Manette - +import edu.wpi.first.wpilibj2.command.WaitCommand; // Manettes import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -29,6 +30,8 @@ import frc.robot.command.GuiderHaut; import frc.robot.command.Lancer; import frc.robot.command.LancerNote; import frc.robot.command.Lancerampli; +import frc.robot.command.Limelight_tracker; +import frc.robot.command.Pistongrimpeur; // Subsystems import frc.robot.subsystem.Accumulateur; import frc.robot.subsystem.Balayeuse; @@ -37,6 +40,7 @@ import frc.robot.subsystem.Grimpeur; import frc.robot.subsystem.Guideur; import frc.robot.subsystem.LED; import frc.robot.subsystem.Lanceur; +import frc.robot.subsystem.Limelight; import frc.robot.subsystem.Pixy; @@ -48,19 +52,23 @@ public class RobotContainer { Grimpeur grimpeur = new Grimpeur(); Guideur guideur = new Guideur(); Lanceur lanceur = new Lanceur(); - Balayer balayer = new Balayer(balayeuse, accumulateur); - GuiderHaut guiderHaut = new GuiderHaut(guideur); - GuiderBas guiderBas = new GuiderBas(guideur); - Lancer lancer = new Lancer(lanceur); - LancerNote lancernote = new LancerNote(lanceur, accumulateur); - Lancerampli lancerampli = new Lancerampli(lanceur); + Limelight limelight = new Limelight(); + LED LED = new LED(); + Pixy pixy = new Pixy(); CommandJoystick joystick = new CommandJoystick(0); CommandXboxController manette = new CommandXboxController(1); + //command + Limelight_tracker limelight_tracker = new Limelight_tracker(limelight,drive); + Balayer balayer = new Balayer(balayeuse, accumulateur); + GuiderHaut guiderHaut = new GuiderHaut(guideur); + GuiderBas guiderBas = new GuiderBas(guideur); + 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::getLeftY); - LED LED = new LED(); AllumeLED allumeLED = new AllumeLED(LED); - Pixy pixy = new Pixy(); + Pistongrimpeur pistongrimpeur = new Pistongrimpeur(grimpeur, LED); public RobotContainer() { NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur)); NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur)); @@ -70,6 +78,9 @@ public class RobotContainer { manette.a().whileTrue(guiderBas); manette.b().whileTrue(guiderHaut); joystick.button(3).toggleOnTrue(balayer); + joystick.button(1).whileTrue(lancernote); + joystick.button(4).whileTrue(new ParallelCommandGroup(lancer,limelight_tracker)); + joystick.button(2).whileTrue(new ParallelCommandGroup(lancerampli,limelight_tracker,guiderHaut)); configureBindings(); drive.setDefaultCommand(new RunCommand(()->{ @@ -79,6 +90,7 @@ public class RobotContainer { grimpeur.droit(manette.getLeftX());} ,grimpeur)); LED.setDefaultCommand(allumeLED); + } private void configureBindings() { diff --git a/src/main/java/frc/robot/command/GuiderHaut.java b/src/main/java/frc/robot/command/GuiderHaut.java index 1eb052e..74719ca 100644 --- a/src/main/java/frc/robot/command/GuiderHaut.java +++ b/src/main/java/frc/robot/command/GuiderHaut.java @@ -40,6 +40,6 @@ public class GuiderHaut extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return guideur.haut()==true; +return guideur.haut()==true; } } diff --git a/src/main/java/frc/robot/command/Lancer.java b/src/main/java/frc/robot/command/Lancer.java index de4314e..01eb49c 100644 --- a/src/main/java/frc/robot/command/Lancer.java +++ b/src/main/java/frc/robot/command/Lancer.java @@ -9,14 +9,17 @@ package frc.robot.command; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystem.Lanceur; +import frc.robot.subsystem.Limelight; public class Lancer extends Command { /** Creates a new Lanceur. */ + private Limelight limelight; private Lanceur lanceur; - public Lancer(Lanceur lanceur) { + public Lancer(Lanceur lanceur,Limelight limelight) { + this.limelight = limelight; this.lanceur = lanceur; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(lanceur); + addRequirements(lanceur,limelight); } // Called when the command is initially scheduled. @@ -26,7 +29,9 @@ public class Lancer extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(limelight.getv()){ lanceur.lancerspeaker(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/command/LancerNote.java b/src/main/java/frc/robot/command/LancerNote.java index d653184..133492b 100644 --- a/src/main/java/frc/robot/command/LancerNote.java +++ b/src/main/java/frc/robot/command/LancerNote.java @@ -15,6 +15,7 @@ public class LancerNote extends Command { public LancerNote(Lanceur lancer, Accumulateur accumulateur) { this.lancer = lancer; this.accumulateur = accumulateur; + addRequirements(lancer); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/command/Lancerampli.java b/src/main/java/frc/robot/command/Lancerampli.java index 450c87a..4a9b3c8 100644 --- a/src/main/java/frc/robot/command/Lancerampli.java +++ b/src/main/java/frc/robot/command/Lancerampli.java @@ -9,15 +9,16 @@ package frc.robot.command; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystem.Lanceur; +import frc.robot.subsystem.Limelight; public class Lancerampli extends Command { /** Creates a new Lanceur. */ - private Lanceur lanceur; - - public Lancerampli(Lanceur lanceur) { + private Limelight limelight; + public Lancerampli(Lanceur lanceur,Limelight limelight) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(lanceur); + addRequirements(lanceur, limelight); + this.limelight = limelight; this.lanceur = lanceur; } @@ -28,7 +29,10 @@ public class Lancerampli extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - lanceur.lanceramp(); + if(limelight.getv()){ + lanceur.lanceramp(); + } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/command/Limelight_tracker.java b/src/main/java/frc/robot/command/Limelight_tracker.java new file mode 100644 index 0000000..78d0f91 --- /dev/null +++ b/src/main/java/frc/robot/command/Limelight_tracker.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.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.Drive; +import frc.robot.subsystem.Limelight; + +public class Limelight_tracker extends Command { + /** Creates a new Limelight_tracker. */ + private Limelight tracker; + private Drive drive; + + public Limelight_tracker(Limelight tracker, Drive drive) { + this.drive = drive; + this.tracker = tracker; + addRequirements(tracker,drive); + } + + // 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 (tracker.getv()){ + drive.drive(0,0, tracker.getx()); + } + else{ + drive.drive(0, 0, 0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drive.drive(0, 0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/Pistongrimpeur.java b/src/main/java/frc/robot/command/Pistongrimpeur.java new file mode 100644 index 0000000..2bb6bbc --- /dev/null +++ b/src/main/java/frc/robot/command/Pistongrimpeur.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.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.Grimpeur; +import frc.robot.subsystem.LED; + +public class Pistongrimpeur extends Command { + private LED LED; + private Grimpeur grimpeur; + /** Creates a new Pistongrimpeur. */ + public Pistongrimpeur(Grimpeur grimpeur,LED LED) { + this.grimpeur = grimpeur; + this.LED = LED; + addRequirements(grimpeur,LED); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + grimpeur.pistondroiteouvre(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if(grimpeur.pistondgaucheouvre()){ + LED.couleur(0, 0, 255); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + LED.couleur(0, 0, 0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystem/Accumulateur.java b/src/main/java/frc/robot/subsystem/Accumulateur.java index 82ade1e..2c43a06 100644 --- a/src/main/java/frc/robot/subsystem/Accumulateur.java +++ b/src/main/java/frc/robot/subsystem/Accumulateur.java @@ -24,7 +24,7 @@ public class Accumulateur extends SubsystemBase { .getEntry(); final WPI_TalonSRX Moteuracc2 = new WPI_TalonSRX(Constants.Moteuracc2); final WPI_TalonSRX Moteuracc = new WPI_TalonSRX(Constants.Moteuracc); - final DigitalInput limitacc = new DigitalInput(Constants.limitacc); + final DigitalInput photocellacc = new DigitalInput(Constants.photocellacc); public void Deaccumuler(double vitesse){ Moteuracc2.set(vitesse); @@ -34,7 +34,7 @@ public class Accumulateur extends SubsystemBase { Moteuracc.setInverted(true); } public boolean limitswitch(){ - return limitacc.get(); + return photocellacc.get(); } public Accumulateur() { diff --git a/src/main/java/frc/robot/subsystem/Grimpeur.java b/src/main/java/frc/robot/subsystem/Grimpeur.java index 7d6ce03..eb41d70 100644 --- a/src/main/java/frc/robot/subsystem/Grimpeur.java +++ b/src/main/java/frc/robot/subsystem/Grimpeur.java @@ -8,10 +8,13 @@ import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -19,7 +22,8 @@ import frc.robot.Constants; public class Grimpeur extends SubsystemBase { /** Creates a new Acrocheur. */ // moteur - public Grimpeur() {} + + ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless); final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); // limit switch @@ -28,6 +32,23 @@ public class Grimpeur extends SubsystemBase { final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondroiteouvre); final Solenoid pistondgauche = new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondgaucheouvre); //fonction + public Grimpeur() { + dashboard.add("limitgrimpeurd", droite()) + .withSize(1, 1) + .withPosition(1, 5); + dashboard.add("limitgrimpeurd", droite()) + .withSize(1, 1) + .withPosition(1, 4); + dashboard.add("grimpeurencodeurd", encoderd()) + .withSize(1, 1) + .withPosition(1, 3); + dashboard.add("grimpeurencodeurg", encoderg()) + .withSize(1, 1) + .withPosition(1, 2); + dashboard.add("pitchgyrogrimpeur", getpitch()) + .withSize(1, 1) + .withPosition(1, 1); + } public void droit(double vitesse){ grimpeurd.set(vitesse); } @@ -56,14 +77,13 @@ public AHRS gyroscope = new AHRS(); public double getpitch(){ return gyroscope.getPitch(); } - public void pistonouvre(){ - pistondroite.get(); - pistondgauche.get(); + public boolean pistondroiteouvre(){ + return pistondroite.get(); } - public void pistonferme(){ - pistondroite.get(); - pistondgauche.get(); + public boolean pistondgaucheouvre(){ + return pistondgauche.get(); } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystem/Guideur.java b/src/main/java/frc/robot/subsystem/Guideur.java index 359a04c..c801259 100644 --- a/src/main/java/frc/robot/subsystem/Guideur.java +++ b/src/main/java/frc/robot/subsystem/Guideur.java @@ -7,17 +7,22 @@ package frc.robot.subsystem; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; 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; import frc.robot.Constants; public class Guideur extends SubsystemBase { /** Creates a new Guideur. */ - public Guideur() {} + ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); final WPI_TalonSRX guideur = new WPI_TalonSRX(Constants.guideur); final DigitalInput guideurhaut = new DigitalInput(Constants.guideurhaut); final DigitalInput guideurbas = new DigitalInput(Constants.guideurbas); - + public Guideur() { + dashboard.add("limitguideurhaut", haut()); + dashboard.add("limitguideurbas", bas()); + } public void guider(double vitesse){ guideur.set(vitesse); } diff --git a/src/main/java/frc/robot/subsystem/Limelight.java b/src/main/java/frc/robot/subsystem/Limelight.java index 9fb068b..ab2358f 100644 --- a/src/main/java/frc/robot/subsystem/Limelight.java +++ b/src/main/java/frc/robot/subsystem/Limelight.java @@ -11,27 +11,44 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; - public class Limelight extends SubsystemBase { - /** Creates a new Limelight. */ - public Limelight() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty"); NetworkTableEntry ta = table.getEntry("ta"); + NetworkTableEntry pipeline = table.getEntry("pipeline"); + NetworkTableEntry tv = table.getEntry("tv"); + NetworkTableEntry camMode = table.getEntry("camMode"); + NetworkTableEntry tid = table.getEntry("tid"); - double x = tx.getDouble(0.0); - double y = ty.getDouble(0.0); - double area = ta.getDouble(0.0); - - SmartDashboard.putNumber("LimelightX", x); - SmartDashboard.putNumber("LimelightY", y); - SmartDashboard.putNumber("LimelightArea", area); - -} + /** Creates a new Limelight. */ + public Limelight() { + } + public double getx(){ + return tx.getDouble(0); + } + public double gety(){ + return ty.getDouble(0); + } + /* public double geta() { + return ta.getDouble(0); + }*/ + public boolean getv(){ + return tv.getBoolean(false); + } + public void setpipeline(){ + pipeline.setNumber(0); + } + public void setcamMode(){ + camMode.setNumber(0); + } + public double getTid(){ + return tid.getDouble(0); + } + @Override public void periodic() { // This method will be called once per scheduler run