vfdrty

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