From 9d09af20b032dca8cc7676c6e92b9a1cccd20c86 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 31 Mar 2026 19:11:09 -0400 Subject: [PATCH] vfdrty --- .../autos/Copy of DepotTirerReservoir.auto | 4 +- .../pathplanner/autos/DepotTirerMur.auto | 4 +- .../autos/DepotTirerReservoir.auto | 4 +- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/commands/Limelighter.java | 61 ++++++++------ .../commands/ModeAuto/BougerDroiteAuto.java | 80 +++++++++++++++++++ .../commands/ModeAuto/BougerGaucheAuto.java | 79 ++++++++++++++++++ .../robot/commands/ModeAuto/GrimperMur.java | 5 +- .../commands/ModeAuto/GrimperReservoir.java | 7 +- .../commands/ModeAuto/LimelighterAuto.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 13 ++- src/main/java/frc/robot/subsystems/Led.java | 13 +-- .../frc/robot/subsystems/Limelight3G.java | 2 +- 13 files changed, 232 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java create mode 100644 src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java diff --git a/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto index de04b62..2b0de62 100644 --- a/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto @@ -79,9 +79,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterReservoir" + "name": "gauche" } }, { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index dc157ad..e157521 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -78,9 +78,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterMur" + "name": "droite" } }, { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto index 15e36ff..62f3ffc 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto @@ -78,9 +78,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterReservoir" + "name": "gauche" } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db5966a..ce353e8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,8 @@ import frc.robot.commands.ModeOposerDemeleur; import frc.robot.commands.MonterBalyeuse; import frc.robot.commands.MonterGrimpeur; import frc.robot.commands.ModeAuto.AspirerAuto; +import frc.robot.commands.ModeAuto.BougerDroiteAuto; +import frc.robot.commands.ModeAuto.BougerGaucheAuto; import frc.robot.commands.ModeAuto.GrimperMur; import frc.robot.commands.ModeAuto.GrimperReservoir; import frc.robot.commands.ModeAuto.LancerAuto; @@ -73,6 +75,8 @@ public class RobotContainer { public RobotContainer() { + NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain)); + NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain)); NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); @@ -126,7 +130,8 @@ public class RobotContainer { manette1.b().whileTrue(new ModeOposer(lanceur)); manette1.a().whileTrue(new ModeOposerDemeleur(lanceur)); manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); - manette1.povUp().whileTrue(new LancerAuto(lanceur, limeLight3G)); + manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain)); + manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 1d0acbc..811f3ec 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -53,7 +53,7 @@ public class Limelighter extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { - // BotPose = limelight3g.getBotPoseBlue(); + BotPose = limelight3g.getBotPoseBlue(); if (!alliance.isPresent()) { return; } @@ -65,36 +65,47 @@ public class Limelighter extends Command { x = 11.915394; BotPose = limelight3g.getBotPoseRed(); } - botx = BotPose[1]; - boty = BotPose[0]; - angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(calcul < -5 && calcul > -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + if(calcul > -5 && calcul < 5){ + drivetrain.setControl( + drive.withRotationalRate(0)); } - else if(calcul > 5 && calcul < 180){ + else if(calcul > 5){ drivetrain.setControl( drive.withRotationalRate(-0.5*(2*Math.PI))); } else if(calcul < -5){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); - } - else if(calcul <= -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); - } - else{ - drivetrain.setControl( - drive.withRotationalRate(0)); - } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(angle); - if (calcul < 0.2 && calcul > -0.2) { - drivetrain.setControl(drive.withRotationalRate(0)); + drivetrain.setControl(drive.withRotationalRate(-0.5*(2*Math.PI))); } + // botx = BotPose[1]; + // boty = BotPose[0]; + // angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + // calcul = limelight3g.Calcule(botx, x, boty, 4, angle); + // if(calcul < -5 && calcul > -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else if(calcul > 5 && calcul < 180){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul < -5){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul <= -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else{ + // drivetrain.setControl( + // drive.withRotationalRate(0)); + // } + // drivetrain.setControl( + // drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); + // System.out.println(angle); + // if (calcul < 0.2 && calcul > -0.2) { + // drivetrain.setControl(drive.withRotationalRate(0)); + // } } else{ drivetrain.setControl(drive.withRotationalRate(0)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java new file mode 100644 index 0000000..72952e7 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java @@ -0,0 +1,80 @@ +// 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.ModeAuto; + +import static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 BougerDroiteAuto extends Command { + CommandSwerveDrivetrain drivetrain; + Timer timer = new Timer(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Optional alliance; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new AvanceAuto. */ + public BougerDroiteAuto(CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + alliance = DriverStation.getAlliance(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(alliance.get() == Alliance.Blue){ + if(timer.get() < .75){ + System.out.println("8765"); + drivetrain.setControl(drive.withVelocityY(-1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + else{ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityY(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > 1; + } +} diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java new file mode 100644 index 0000000..e4a3748 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java @@ -0,0 +1,79 @@ +// 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.ModeAuto; + +import static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 BougerGaucheAuto extends Command { + CommandSwerveDrivetrain drivetrain; + Timer timer = new Timer(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Optional alliance; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new AvanceAuto. */ + public BougerGaucheAuto(CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + alliance = DriverStation.getAlliance(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(alliance.get() == Alliance.Blue){ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + else{ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(-1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityY(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > 1; + } +} diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index b7e1255..5eb1bf1 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -48,7 +48,10 @@ public class GrimperMur extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - alliance = DriverStation.getAlliance(); + // alliance = DriverStation.getAlliance(); + // if(drivetrain.Equipe()){ + // angle+=180; + // } } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 5dbdba1..62e4cb7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -49,7 +49,10 @@ public class GrimperReservoir extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - alliance = DriverStation.getAlliance(); + // alliance = DriverStation.getAlliance(); + // if(drivetrain.Equipe()){ + // drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); + // } } // Called every time the scheduler runs while the command is scheduled. @@ -85,7 +88,7 @@ public class GrimperReservoir extends Command { x = 1.11; angle = 0; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 358 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 2){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)).withRotationalRate(0)); + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 21cb1e8..8175fc7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -78,7 +78,7 @@ public class LimelighterAuto extends Command { } else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(0.5*(2*Math.PI))); } else if(calcul < -5){ drivetrain.setControl( diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9b70aea..c20b5f1 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -22,10 +22,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -54,8 +58,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + ShuffleboardTab teb = Shuffleboard.getTab("teb"); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - + private GenericEntry equipe = + teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -221,7 +227,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } configureAutoBuilder(); } - + public boolean Equipe(){ + return equipe.getBoolean(false); + } /** * Returns a command that applies the specified control request to this swerve drivetrain. * @@ -254,7 +262,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su else if(getPigeon2().getYaw().getValueAsDouble() < 0){ getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360); } - /* * Periodically try to apply the operator perspective. * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index f628715..8cacbf2 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -16,8 +16,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Led extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - private GenericEntry equipe = - teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); @SuppressWarnings("removal") CANdle CANDle = new CANdle(17); @SuppressWarnings("removal") @@ -91,9 +89,6 @@ public class Led extends SubsystemBase { public void RainBowStop(){ CANDle.animate(null); } - public boolean Equipe(){ - return equipe.getBoolean(true); - } /** Creates a new Led. */ public Led() {} @@ -104,7 +99,7 @@ public class Led extends SubsystemBase { if(temps > 20 && temps < 30){ Vert1(); } - if(Equipe()){ + // if(Equipe()){ if(temps > 30 && temps < 55){ bleu(); } @@ -120,8 +115,8 @@ public class Led extends SubsystemBase { else{ RainBow(); } - } - else{ + // } + // else{ if(temps > 30 && temps < 55){ Rouge(); } @@ -140,6 +135,6 @@ public class Led extends SubsystemBase { } // This method will be called once per scheduler run - } + // } } diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index 5bc3bb6..1303b8f 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -72,7 +72,7 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle; + return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } } @Override