From fe9eb1c40ca82fa70d2b5e3185a4d86730c45df0 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Fri, 6 Mar 2020 20:48:57 -0700
Subject: [PATCH] Merge Repairs

---
 .../robot/commands/drive/GotoCoordinates.java | 87 -------------------
 .../GotoCoordinatesFieldRelative.java         |  2 +-
 .../GotoCoordinatesRobotRelative.java         |  0
 3 files changed, 1 insertion(+), 88 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java
 rename src/main/java/frc4388/robot/commands/{ => drive}/GotoCoordinatesFieldRelative.java (96%)
 rename src/main/java/frc4388/robot/commands/{ => drive}/GotoCoordinatesRobotRelative.java (100%)

diff --git a/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java
deleted file mode 100644
index 5ffd58c..0000000
--- a/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java
+++ /dev/null
@@ -1,87 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.drive;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc4388.robot.commands.auto.Wait;
-import frc4388.robot.subsystems.Drive;
-import frc4388.robot.subsystems.Pneumatics;
-
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
-public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
-  Drive m_drive;
-  Pneumatics m_pneumatics;
-
-  double m_xTarget;
-  double m_yTarget;
-  double m_currentAngle;
-  double m_hypotDist;
-  double m_endAngle;
-
-  /**
-   * Creates a new GotoPosition.
-   */
-  public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());
-    m_drive = subsystem;
-    m_pneumatics = subsystem2;
-
-    m_xTarget = xTarget;
-    m_yTarget = yTarget;
-
-    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
-
-    m_currentAngle = calcAngle();
-
-    m_endAngle = endAngle;
-
-    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
-                  new Wait(m_drive, 0, 0),
-                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
-                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
-  }
-
-  public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
-    m_drive = subsystem;
-    m_pneumatics = subsystem2;
-
-    m_xTarget = xTarget;
-    m_yTarget = yTarget;
-
-    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
-
-    m_currentAngle = calcAngle();
-
-    m_endAngle = m_currentAngle;
-
-    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
-                  new Wait(m_drive, 0, 0),
-                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
-                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
-  }
-
-  public boolean isQuadrantThree() {
-    if ((m_xTarget < 0) && (m_yTarget < 0)) {
-      return true;
-    } else {
-      return false;
-    }
-  }
-
-  public double calcAngle() {
-    if (isQuadrantThree()) {
-      return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
-    } else {
-      return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
-    }
-  }
-
-}
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java
similarity index 96%
rename from src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java
rename to src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java
index 927d4b4..6165ffd 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java
+++ b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java
@@ -5,7 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc4388.robot.commands;
+package frc4388.robot.commands.drive;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.subsystems.Drive;
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java
similarity index 100%
rename from src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java
rename to src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java