diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 7a3b569..4e0c9bb 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -8,17 +8,18 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.LimeLight3; +import frc.robot.subsystems.Limelight3G; /* 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 Lancer extends Command { private Lanceur lanceur; private PIDController pidController; - private LimeLight3 limeLight3; + private Limelight3G limeLight3G; private double output; /** Creates a new Lancer. */ public Lancer(Lanceur lanceur, LimeLight3 limeLight3) { this.lanceur = lanceur; - this.limeLight3 = new LimeLight3(); + this.limeLight3G = new Limelight3G(); addRequirements(lanceur); // Use addRequirements() here to declare subsystem dependencies. } @@ -32,7 +33,7 @@ public class Lancer extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double vitesse = (100-limeLight3.getTA())/100; + double vitesse = (100-limeLight3G.getTA())/100; double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); if(lanceur.Vitesse() >= vitesse){ diff --git a/src/main/java/frc/robot/subsystems/Balayeuse.java b/src/main/java/frc/robot/subsystems/Balayeuse.java index 14d27f6..519e3b3 100644 --- a/src/main/java/frc/robot/subsystems/Balayeuse.java +++ b/src/main/java/frc/robot/subsystems/Balayeuse.java @@ -12,11 +12,13 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Balayeuse extends SubsystemBase { - SparkFlex Balaye = new SparkFlex(5, MotorType.kBrushless); + SparkFlex Balaye1 = new SparkFlex(5, MotorType.kBrushless); + SparkFlex Balaye2 = new SparkFlex(5, MotorType.kBrushless); SparkMax Pivot = new SparkMax(6, MotorType.kBrushless); DigitalInput limit = new DigitalInput(0); public void Balayer(double vitesse){ - Balaye.set(vitesse); + Balaye1.set(vitesse); + Balaye2.set(vitesse); } public void Pivoter(double vitesse){ Pivot.set(vitesse); diff --git a/src/main/java/frc/robot/subsystems/LimeLight3.java b/src/main/java/frc/robot/subsystems/LimeLight3.java index 6c6a97c..780b275 100644 --- a/src/main/java/frc/robot/subsystems/LimeLight3.java +++ b/src/main/java/frc/robot/subsystems/LimeLight3.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; public class LimeLight3 extends SubsystemBase { - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry pipeline = table.getEntry("pipeline"); /** Creates a new LimeLight3. */ public LimeLight3() { diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java new file mode 100644 index 0000000..5f2fed9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -0,0 +1,68 @@ +// 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.subsystems; + +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; + +public class Limelight3G extends SubsystemBase { +NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry pipeline = table.getEntry("pipeline"); + /** Creates a new LimeLight3. */ + public Limelight3G() { + for(int port = 5800; port <=5807; port++){ + PortForwarder.add(port, "limelight.local", port); + } + } + public double[] getBotPoseBlue(){ + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + public double[] getBotPoseRed(){ + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + public double getTx(){ + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry tx = table.getEntry("tx"); + return tx.getDouble(0.0); + } + public double getTId(){ + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry tid = table.getEntry("tid"); + return tid.getDouble(0.0); + } + public double getTA(){ + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag"); + NetworkTableEntry ta = table.getEntry("ta"); + return ta.getDouble(0.0); + } + public boolean getV(){ + return LimelightHelpers.getTV("limelight-tag"); + } + public double Calcule(double x1, double x2, double y1, double y2, double angle) + { + if (x1 > 4) + { + return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90; + } + else + { + return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90; + } + } + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}