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

vfdrty

Browse Source
This commit is contained in:
samuel desharnais
2026-03-31 19:11:09 -04:00
parent ae9a6bd046
commit 9d09af20b0
13 changed files with 232 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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));

80
src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java Normal file
View File

@@ -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> 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;
}
}

79
src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java Normal file
View File

@@ -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> 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;
}
}

5
src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
View File

@@ -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.

7
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
View File

@@ -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){

2
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -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(