From 737515dfd6928fc0592135de97b18505bb361f61 Mon Sep 17 00:00:00 2001
From: DrorMargalit <dropipopi@gmail,com>
Date: Thu, 27 Jun 2024 14:09:32 +0300
Subject: [PATCH] example

---
 .../robot/Commands/CommandGroupsFactory.java  | 74 +++++++++++++++++++
 1 file changed, 74 insertions(+)
 create mode 100644 src/main/java/frc/robot/Commands/CommandGroupsFactory.java

diff --git a/src/main/java/frc/robot/Commands/CommandGroupsFactory.java b/src/main/java/frc/robot/Commands/CommandGroupsFactory.java
new file mode 100644
index 0000000..415118c
--- /dev/null
+++ b/src/main/java/frc/robot/Commands/CommandGroupsFactory.java
@@ -0,0 +1,74 @@
+//example
+package frc.robot.Commands;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Misc;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
+import frc.robot.subsystems.swerve.TunerConstants;
+
+public class CommandGroupsFactory {
+    private static final CommandXboxController driverJoystick = RobotContainer.driverJoystick;
+
+    private static final CommandSwerveDrivetrain swerve = TunerConstants.Swerve; // My drivetrain
+
+    static double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed
+    static double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity
+
+    public static SwerveRequest.FieldCentric teleopDrive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+
+    public static SwerveRequest.FieldCentricFacingAngle driveAlignedToSpeaker = new SwerveRequest.FieldCentricFacingAngle()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
+            .withDriveRequestType(DriveRequestType.Velocity);
+
+    public static Command getSwitchDriveModeCommand() {
+        return new InstantCommand(() -> {
+            if (swerve.getDefaultCommand().getName() == getTeleopDriveCommand().getName()) {
+                swerve.setDefaultCommand(getDriveAlignedToSpeakerCommand());
+            } else {
+                swerve.setDefaultCommand(getTeleopDriveCommand());
+            }
+
+        });
+    }
+
+    public static Command getTeleopDriveCommand() {
+
+        Command teleopDriveCommand = swerve
+                .applyRequest(() -> teleopDrive.withVelocityX(-driverJoystick.getLeftY() * MaxSpeed * 0.1)
+                        .withVelocityY(-driverJoystick.getLeftX() * MaxSpeed * 0.1)
+                        .withRotationalRate(-driverJoystick.getRightX() * MaxAngularRate))
+                .ignoringDisable(true);
+        
+        teleopDriveCommand.setName("TeleopDriveCommand");
+
+        return teleopDriveCommand;
+    }
+
+    public static Command getDriveAlignedToSpeakerCommand() {
+        Translation2d speakerPose = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red
+                ? Misc.speakerPoseBlue
+                : Misc.speakerPoseRed;
+
+        Command driveAlignedCommand = swerve.applyRequest(() -> driveAlignedToSpeaker
+                .withVelocityX(-driverJoystick.getLeftY() * MaxSpeed)
+                .withVelocityY(-driverJoystick.getLeftX() * MaxSpeed)
+                .withTargetDirection(swerve.getPose().getTranslation().minus(speakerPose).getAngle()))
+                .ignoringDisable(true);
+        
+        driveAlignedCommand.setName("DriveAlignedToSpeakerCommand");
+
+        return driveAlignedCommand;
+    }
+
+}