From e4b1ea3ae51e2da8fe66be037b47564363de0380 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 5 Mar 2021 15:31:57 -0700
Subject: [PATCH] Updated TankDriveVelocity, WORKING (but maybe small problem
 with timing?)

---
 src/main/java/frc4388/robot/RobotContainer.java   |  3 ++-
 src/main/java/frc4388/robot/subsystems/Drive.java | 11 +++++++----
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 6bf8541..05e33b2 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -40,6 +40,7 @@ import frc4388.robot.commands.auto.EightBallAutoMiddle;
 import frc4388.robot.commands.auto.FiveBallAutoMiddle;
 import frc4388.robot.commands.auto.SixBallAutoMiddle;
 import frc4388.robot.commands.auto.Slalom;
+import frc4388.robot.commands.auto.TankDriveVelocity;
 import frc4388.robot.commands.auto.TenBallAutoMiddle;
 import frc4388.robot.commands.InterruptSubystem;
 import frc4388.robot.commands.auto.AutoPath1FromCenter;
@@ -191,7 +192,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000));
+            .whenPressed(new TankDriveVelocity(m_robotDrive, 5000, 5000, 2));
 
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fe96ca2..f834cd9 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -436,8 +436,8 @@ public class Drive extends SubsystemBase {
   /**
    * Controls the left and right sides of the drive with velocity targets.
    *
-   * @param leftSpeed  the commanded left speed
-   * @param rightSpeed the commanded right speed
+   * @param leftSpeed  the commanded left speed (currently measured in ticks/100 ms)
+   * @param rightSpeed the commanded right speed (currently measured in ticks/100 ms)
    */
   public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
     // DifferentialDriveWheelSpeeds wheelSpeeds = new
@@ -454,8 +454,11 @@ public class Drive extends SubsystemBase {
     // m_currentAngleYaw+(360));
     // double targetGyro = (m_kinematicsTargetAngle / 360) *
     // DriveConstants.TICKS_PER_GYRO_REV;
-    double moveVelLeft = inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
-    double moveVelRight = inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
+    //double moveVelLeft = inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
+    //double moveVelRight = inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
+
+    double moveVelLeft = leftSpeed;
+    double moveVelRight = rightSpeed;
 
     // SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
     // SmartDashboard.putNumber("Move Vel Right", moveVelRight);