From feda40a2661680f36bfdecb3483595d3bd427616 Mon Sep 17 00:00:00 2001
From: willitcode <91231142+willitcode@users.noreply.github.com>
Date: Mon, 29 Jan 2024 17:00:03 -0600
Subject: [PATCH] bring back drive commands

---
 .../commands/drive/DriveFieldOriented.java    | 56 +++++++++++++++++++
 .../commands/drive/DriveRobotOriented.java    | 56 +++++++++++++++++++
 2 files changed, 112 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/drive/DriveFieldOriented.java
 create mode 100644 src/main/java/frc/robot/commands/drive/DriveRobotOriented.java

diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java
new file mode 100644
index 00000000..9a3a7266
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java
@@ -0,0 +1,56 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+/** Drives the robot in field-oriented mode. */
+public class DriveFieldOriented extends Command {
+  private final Drive drive;
+
+  /** Creates a new DriveFieldOriented. */
+  public DriveFieldOriented() {
+    this.drive = RobotContainer.drive;
+    addRequirements(drive);
+  }
+
+  // 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() {
+    double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED;
+    double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED;
+    double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED;
+    Translation2d translation = new Translation2d(x, y);
+
+    drive.driveFieldOriented(translation, RobotContainer.getControllerRightXAxis());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drive.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java
new file mode 100644
index 00000000..74c6a1c8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java
@@ -0,0 +1,56 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+/** Drives the robot in robot-oriented mode. Default command for {@link Drive} subsystem. */
+public class DriveRobotOriented extends Command {
+  private final Drive drive;
+
+  /** Creates a new DriveRobotOriented. */
+  public DriveRobotOriented() {
+    this.drive = RobotContainer.drive;
+    addRequirements(drive);
+  }
+
+  // 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() {
+     double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED;
+    double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED;
+    double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED;
+    Translation2d translation = new Translation2d(x, y);
+
+    drive.driveRobotOriented(translation, RobotContainer.getControllerRightXAxis());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drive.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}