From a7fb3108ea5e034e4af3a81211dec9051149e496 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Sat, 21 Mar 2026 16:11:24 -0400 Subject: [PATCH] debug de samedi --- src/main/java/frc/robot/Robot.java | 11 ++++ src/main/java/frc/robot/RobotContainer.java | 39 ++++++++----- src/main/java/frc/robot/commands/Aspirer.java | 40 ++----------- .../frc/robot/commands/DescendreBalyeuse.java | 2 +- .../frc/robot/commands/DescendreGrimpeur.java | 2 +- src/main/java/frc/robot/commands/Lancer.java | 58 ++++++------------- .../frc/robot/commands/LancerAspirer.java | 4 +- .../frc/robot/commands/LancerBaseVitesse.java | 16 ++--- .../java/frc/robot/commands/Limelighter.java | 55 ++++++++++++------ .../robot/commands/ModeAuto/AspirerAuto.java | 6 +- .../java/frc/robot/commands/ModeOposer.java | 7 +-- .../robot/commands/ModeOposerBalayeuse.java | 44 ++++++++++++++ .../frc/robot/commands/MonterBalyeuse.java | 2 +- .../frc/robot/commands/MonterGrimpeur.java | 2 +- .../java/frc/robot/subsystems/Balayeuse.java | 10 ++-- .../java/frc/robot/subsystems/Lanceur.java | 3 +- .../java/frc/robot/subsystems/LimeLight3.java | 14 ++--- .../frc/robot/subsystems/Limelight3G.java | 13 +---- 18 files changed, 178 insertions(+), 150 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ModeOposerBalayeuse.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index be21731..72f45e3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -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 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6659b6d..11141f5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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() { diff --git a/src/main/java/frc/robot/commands/Aspirer.java b/src/main/java/frc/robot/commands/Aspirer.java index ffe751d..5c029f6 100644 --- a/src/main/java/frc/robot/commands/Aspirer.java +++ b/src/main/java/frc/robot/commands/Aspirer.java @@ -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. diff --git a/src/main/java/frc/robot/commands/DescendreBalyeuse.java b/src/main/java/frc/robot/commands/DescendreBalyeuse.java index d7decb4..0d22cb8 100644 --- a/src/main/java/frc/robot/commands/DescendreBalyeuse.java +++ b/src/main/java/frc/robot/commands/DescendreBalyeuse.java @@ -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); diff --git a/src/main/java/frc/robot/commands/DescendreGrimpeur.java b/src/main/java/frc/robot/commands/DescendreGrimpeur.java index 4078f0b..c8da33e 100644 --- a/src/main/java/frc/robot/commands/DescendreGrimpeur.java +++ b/src/main/java/frc/robot/commands/DescendreGrimpeur.java @@ -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(); diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 7d3677f..3333d12 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -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. diff --git a/src/main/java/frc/robot/commands/LancerAspirer.java b/src/main/java/frc/robot/commands/LancerAspirer.java index 9b9967f..4184065 100644 --- a/src/main/java/frc/robot/commands/LancerAspirer.java +++ b/src/main/java/frc/robot/commands/LancerAspirer.java @@ -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()); } diff --git a/src/main/java/frc/robot/commands/LancerBaseVitesse.java b/src/main/java/frc/robot/commands/LancerBaseVitesse.java index ccf8a34..f8d1427 100644 --- a/src/main/java/frc/robot/commands/LancerBaseVitesse.java +++ b/src/main/java/frc/robot/commands/LancerBaseVitesse.java @@ -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. diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index dde9b48..becd06b 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -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; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java index 9cfa5fb..70b9ffd 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java @@ -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. diff --git a/src/main/java/frc/robot/commands/ModeOposer.java b/src/main/java/frc/robot/commands/ModeOposer.java index dc69a1e..840c782 100644 --- a/src/main/java/frc/robot/commands/ModeOposer.java +++ b/src/main/java/frc/robot/commands/ModeOposer.java @@ -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. } diff --git a/src/main/java/frc/robot/commands/ModeOposerBalayeuse.java b/src/main/java/frc/robot/commands/ModeOposerBalayeuse.java new file mode 100644 index 0000000..2bf7abb --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeOposerBalayeuse.java @@ -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; + } +} diff --git a/src/main/java/frc/robot/commands/MonterBalyeuse.java b/src/main/java/frc/robot/commands/MonterBalyeuse.java index 599cdac..de69081 100644 --- a/src/main/java/frc/robot/commands/MonterBalyeuse.java +++ b/src/main/java/frc/robot/commands/MonterBalyeuse.java @@ -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(); diff --git a/src/main/java/frc/robot/commands/MonterGrimpeur.java b/src/main/java/frc/robot/commands/MonterGrimpeur.java index fd9b8d6..a8a3e84 100644 --- a/src/main/java/frc/robot/commands/MonterGrimpeur.java +++ b/src/main/java/frc/robot/commands/MonterGrimpeur.java @@ -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 { diff --git a/src/main/java/frc/robot/subsystems/Balayeuse.java b/src/main/java/frc/robot/subsystems/Balayeuse.java index a3fc48e..36ea83e 100644 --- a/src/main/java/frc/robot/subsystems/Balayeuse.java +++ b/src/main/java/frc/robot/subsystems/Balayeuse.java @@ -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() { diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index 44a7aba..5fc74ce 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -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 = diff --git a/src/main/java/frc/robot/subsystems/LimeLight3.java b/src/main/java/frc/robot/subsystems/LimeLight3.java index 780b275..7454e86 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-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); diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index 5f2fed9..9ac3bd1 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -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() {