sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug de samedi

Browse Source
This commit is contained in:
samuel desharnais
2026-03-21 16:11:24 -04:00
parent e0c1e26933
commit a7fb3108ea
18 changed files with 178 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/main/java/frc/robot/Robot.java
View File

@@ -4,9 +4,11 @@
package frc.robot;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.Limelight3G;
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
@@ -20,6 +22,15 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
var driveState = m_robotContainer.drivetrain.getState();
double headingDeg = driveState.Pose.getRotation().getDegrees();
double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond);
LimelightHelpers.SetRobotOrientation("limelight_tag", headingDeg, 0, 0, 0, 0, 0);
var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight_tag");
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
}
}
@Override

39
src/main/java/frc/robot/RobotContainer.java
View File

@@ -25,6 +25,7 @@ import frc.robot.commands.LancerAspirer;
import frc.robot.commands.LancerBaseVitesse;
import frc.robot.commands.Limelighter;
import frc.robot.commands.ModeOposer;
import frc.robot.commands.ModeOposerBalayeuse;
import frc.robot.commands.ModeOposerDemeleur;
import frc.robot.commands.MonterBalyeuse;
import frc.robot.commands.MonterGrimpeur;
@@ -55,7 +56,7 @@ public class RobotContainer {
Led led = new Led();
CommandXboxController manette = new CommandXboxController(0);
CommandXboxController manette1 = new CommandXboxController(1);
private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
/* Setting up bindings for necessary control of the swerve drive platform */
@@ -97,25 +98,31 @@ public class RobotContainer {
)
);
//manette 1
manette.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
manette.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
manette.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
manette.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
// manette1.povUp().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// manette1.povDown().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// manette1.povRight().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
// manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
manette.povUp().whileTrue(new LancerAuto(lanceur));
manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led));
manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3));
manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur));
manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
manette.povUp().whileTrue(new MonterGrimpeur(grimpeur));
manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur));
manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G));
manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain));
manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
manette.rightBumper().whileTrue(new Aspirer(balayeuse));
manette.y().whileTrue(new TournerAZero(drivetrain));
manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain));
manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain));
manette.b().whileTrue(new Aspirer(balayeuse,led));
//manette 2
manette1.povDown().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3, led));
manette1.y().whileTrue(new ModeOposer(lanceur, balayeuse));
manette1.x().whileTrue(new ModeOposerDemeleur(lanceur));
manette1.rightTrigger().whileTrue(new Aspirer(balayeuse));
manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur, limeLight3G));
manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G));
manette1.b().whileTrue(new ModeOposer(lanceur));
manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
}
public Command getAutonomousCommand() {

40
src/main/java/frc/robot/commands/Aspirer.java
View File

@@ -12,58 +12,30 @@ import frc.robot.subsystems.Led;
/* 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 Aspirer extends Command {
private Balayeuse balayeuse;
private Timer timer;
private Led led;
private double temp;
/** Creates a new Aspirer. */
public Aspirer(Balayeuse balayeuse, Led led) {
public Aspirer(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
this.led = led;
this.timer = new Timer();
addRequirements(balayeuse, led);
this.temp = 0;
addRequirements(balayeuse);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timer.reset();
temp = balayeuse.Amp();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
timer.start();
int nbFois = 0;
double moyenneAmp = 0;
if(timer.get() <3){
nbFois++;
moyenneAmp += balayeuse.Amp() / nbFois;
}
else{
nbFois++;
moyenneAmp -= temp;
moyenneAmp += balayeuse.Amp() / nbFois;
temp = balayeuse.Amp();
}
if(moyenneAmp < balayeuse.AmpMax()){
timer.reset();
balayeuse.Balayer(-0.5);
}
else{
balayeuse.Balayer(0);
led.Jaune2();
}
balayeuse.BalayerEnbas(-0.5);
balayeuse.BalayerPadle(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.Balayer(0);
timer.stop();
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.

2
src/main/java/frc/robot/commands/DescendreBalyeuse.java
View File

@@ -25,7 +25,7 @@ public class DescendreBalyeuse extends Command {
@Override
public void execute() {
if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
balayeuse.Pivoter(-0.2);
balayeuse.Pivoter(-0.5);
}
else{
balayeuse.Pivoter(0);

2
src/main/java/frc/robot/commands/DescendreGrimpeur.java
View File

@@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command {
@Override
public void execute() {
if(!grimpeur.Limit()){
grimpeur.Grimper(-0.3);
grimpeur.Grimper(-0.4);
}
else{
grimpeur.Reset();

58
src/main/java/frc/robot/commands/Lancer.java
View File

@@ -18,17 +18,13 @@ public class Lancer extends Command {
private PIDController pidController;
private Limelight3G limeLight3G;
private Timer timer;
private Balayeuse balayeuse;
private Led led;
private double temp;
/** Creates a new Lancer. */
public Lancer(Lanceur lanceur, Limelight3G limeLight3G, Balayeuse balayeuse,Led led) {
public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
this.balayeuse = balayeuse;
this.led = led;
this.timer = new Timer();
this.limeLight3G = new Limelight3G();
addRequirements(lanceur, balayeuse, led, limeLight3G);
addRequirements(lanceur, limeLight3G);
this.temp = 0;
// Use addRequirements() here to declare subsystem dependencies.
}
@@ -36,9 +32,8 @@ public class Lancer extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
pidController = new PIDController(1, 0,0, 0);
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
timer.start();
temp = lanceur.Amp();
}
@@ -52,40 +47,22 @@ public class Lancer extends Command {
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
}
int nbFois = 0;
double moyenneAmp = 0;
if(timer.get() < 3){
nbFois++;
moyenneAmp += balayeuse.Amp() / nbFois;
}
else{
nbFois++;
moyenneAmp -= temp;
moyenneAmp += balayeuse.Amp() / nbFois;
temp = balayeuse.Amp();
}
// if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
timer.reset();
balayeuse.Balayer(-0.5);
// led.Jaune2();
// }
// else{
}
double vitesse = 0.5;
if(limeLight3G.getV()){
//pythagore |
// \/
vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
}
double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/
lanceur.Lancer(output);
if(lanceur.Vitesse() >= vitesse){
lanceur.Demeler(0.1);
}
// }
vitesse = 3100 + 475 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2))) - 2);
System.out.println(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
if(lanceur.Vitesse() >= vitesse-800){
timer.start();
if(timer.get() >0.5){
lanceur.Demeler(1);
}
}
}
}
// Called once the command ends or is interrupted.
@@ -93,7 +70,8 @@ public class Lancer extends Command {
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
balayeuse.Pivoter(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.

4
src/main/java/frc/robot/commands/LancerAspirer.java
View File

@@ -17,8 +17,8 @@ import frc.robot.subsystems.Limelight3G;
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class LancerAspirer extends ParallelCommandGroup {
/** Creates a new LacerAspirer. */
public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, LimeLight3 limeLight3, Led led) {
addCommands(new LancerBaseVitesse(lanceur, limeLight3), new Aspirer(balayeuse, led));
public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, Limelight3G limeLight3G) {
addCommands(new Lancer(lanceur, limeLight3G), new Aspirer(balayeuse));
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
}

16
src/main/java/frc/robot/commands/LancerBaseVitesse.java
View File

@@ -19,9 +19,9 @@ public class LancerBaseVitesse extends Command {
private Timer timer;
private double temp;
/** Creates a new Lancer. */
public LancerBaseVitesse(Lanceur lanceur, LimeLight3 limeLight3) {
public LancerBaseVitesse(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
// this.timer = new Timer();
this.timer = new Timer();
this.limeLight3G = new Limelight3G();
addRequirements(lanceur, limeLight3G);
//this.temp = 0;
@@ -32,8 +32,8 @@ public class LancerBaseVitesse extends Command {
@Override
public void initialize() {
pidController = new PIDController(0.0007, 0,0, 0.001);
//timer.reset();
//timer.start();
timer.reset();
timer.start();
//temp = lanceur.Amp();
}
@@ -62,9 +62,9 @@ public class LancerBaseVitesse extends Command {
double vitesse = lanceur.vitesseDemander();
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
if(lanceur.Vitesse() >= 1200){
lanceur.Demeler(0.5);
}
if(timer.get() >1){
lanceur.Demeler(1);
}
// }
}
@@ -74,6 +74,8 @@ public class LancerBaseVitesse extends Command {
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.

55
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -21,41 +21,60 @@ public class Limelighter extends Command {
CommandSwerveDrivetrain drivetrain;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
double botx;
double boty;
double angle;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
double botx;
double boty;
double angle;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new Limelighter. */
public Limelighter(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain) {
public Limelighter(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) {
this.limelight3g = limelight3g;
this.drivetrain = drivetrain;
addRequirements(drivetrain,limelight3g);
addRequirements(drivetrain, limelight3g);
timer = new Timer();
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
timer.reset();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
drivetrain.setControl(drive.withRotationalRate(limelight3g.Calcule(4.625594, botx, 4.034536, boty, BotPose[5])/90));
if(limelight3g.Calcule(4.625594, botx, 4.034536, boty, angle)/90 < 0.1){
timer.start();
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle =- 360;
}
double calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 20;
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(calcul);
if (calcul < 0.2 && calcul > -0.2) {
// timer.start();
// } else {
// timer.reset();
// }
// } else {
// timer.stop();
drivetrain.setControl(drive.withRotationalRate(0));
}
}
else{
timer.reset();
drivetrain.setControl(drive.withRotationalRate(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
@@ -66,6 +85,6 @@ public class Limelighter extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() > 1;
return false;
}
}

6
src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java
View File

@@ -24,13 +24,15 @@ public class AspirerAuto extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
balayeuse.Balayer(0.5);
balayeuse.BalayerEnbas(-0.5);
balayeuse.BalayerPadle(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.Balayer(0);
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.

7
src/main/java/frc/robot/commands/ModeOposer.java
View File

@@ -5,17 +5,14 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.Lanceur;
/* 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 ModeOposer extends Command {
private Lanceur lanceur;
private Balayeuse balayeuse;
/** Creates a new Lancer. */
public ModeOposer(Lanceur lanceur, Balayeuse balayeuse) {
public ModeOposer(Lanceur lanceur) {
this.lanceur = lanceur;
this.balayeuse = balayeuse;
addRequirements(lanceur ,balayeuse);
addRequirements(lanceur);
// Use addRequirements() here to declare subsystem dependencies.
}

44
src/main/java/frc/robot/commands/ModeOposerBalayeuse.java Normal file
View File

@@ -0,0 +1,44 @@
// 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.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.Lanceur;
/* 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 ModeOposerBalayeuse extends Command {
private Balayeuse balayeuse;
/** Creates a new Lancer. */
public ModeOposerBalayeuse(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// 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() {
balayeuse.BalayerEnbas(0.5);
balayeuse.BalayerPadle(0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

2
src/main/java/frc/robot/commands/MonterBalyeuse.java
View File

@@ -25,7 +25,7 @@ public class MonterBalyeuse extends Command {
@Override
public void execute() {
if(!balayeuse.GetLimiSwtich()){
balayeuse.Pivoter(0.2);
balayeuse.Pivoter(0.5);
}
else{
balayeuse.Reset();

2
src/main/java/frc/robot/commands/MonterGrimpeur.java
View File

@@ -25,7 +25,7 @@ public class MonterGrimpeur extends Command {
public void execute() {
if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
grimpeur.Grimper(0.3);
grimpeur.Grimper(0.5);
System.out.println("monte");
}
else {

10
src/main/java/frc/robot/subsystems/Balayeuse.java
View File

@@ -22,11 +22,13 @@ public class Balayeuse extends SubsystemBase {
SparkMax Pivot = new SparkMax(8, MotorType.kBrushless);
DigitalInput limit = new DigitalInput(9);
private GenericEntry EncodeurBalayeuse =
teb.add("Position bas balayeuse", 10).getEntry();
teb.add("Position bas balayeuse", 1.8).getEntry();
private GenericEntry AmpBaleyeuse =
teb.add("Ampérage Baleyeuse", 40).getEntry();
public void Balayer(double vitesse){
public void BalayerEnbas(double vitesse){
Balaye1.set(vitesse);
}
public void BalayerPadle(double vitesse){
Balaye2.set(vitesse);
}
public void Pivoter(double vitesse){
@@ -36,7 +38,7 @@ public class Balayeuse extends SubsystemBase {
return Pivot.getEncoder().getPosition();
}
public void Reset(){
Pivot.getEncoder().setPosition(0);
Pivot.getEncoder().setPosition(0.6);
}
public boolean GetLimiSwtich(){
return !limit.get();
@@ -52,7 +54,7 @@ public class Balayeuse extends SubsystemBase {
timer.start();
}
public double EncodeurBalayeuse(){
return EncodeurBalayeuse.getDouble(10);
return EncodeurBalayeuse.getDouble(1.8);
}
/** Creates a new Balayeuse. */
public Balayeuse() {

3
src/main/java/frc/robot/subsystems/Lanceur.java
View File

@@ -6,6 +6,7 @@ package frc.robot.subsystems;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -16,7 +17,7 @@ public class Lanceur extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless);
SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless);
SparkFlex Demeleur = new SparkFlex(19, MotorType.kBrushless);
SparkMax Demeleur = new SparkMax(19, MotorType.kBrushless);
GenericEntry vitesse =
teb.add("vitesse lanceur",4000).getEntry();
GenericEntry AmpLanceur =

14
src/main/java/frc/robot/subsystems/LimeLight3.java
View File

@@ -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-balaie");
NetworkTableEntry pipeline = table.getEntry("pipeline");
/** Creates a new LimeLight3. */
public LimeLight3() {
@@ -21,34 +21,34 @@ public class LimeLight3 extends SubsystemBase {
}
}
public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
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-balon");
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
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-balon");
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry tx = table.getEntry("tx");
return tx.getDouble(0.0);
}
public double getTId(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry tid = table.getEntry("tid");
return tid.getDouble(0.0);
}
public double getTA(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0);
}
public boolean getV(){
return LimelightHelpers.getTV("limelight-balon");
return LimelightHelpers.getTV("limelight-balaie");
}
public void AprilTag(){
pipeline.setNumber(0);

13
src/main/java/frc/robot/subsystems/Limelight3G.java
View File

@@ -22,13 +22,13 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
}
public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_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");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
@@ -52,14 +52,7 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("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;
}
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
}
@Override
public void periodic() {