From 500c113746562b049851ed106befeb712510fc49 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Mon, 22 Feb 2021 17:48:51 -0700
Subject: [PATCH] boilerplate for new BarrelEnd, TankDrive testing

Co-Authored-By: Nirvan Bhalala <78400306+nbhalala27@users.noreply.github.com>
---
 .../java/frc4388/robot/RobotContainer.java    |  5 ++-
 .../robot/commands/auto/BarrelEnd.java        | 40 +++++++++++++++++++
 .../java/frc4388/robot/subsystems/Drive.java  | 13 ++++--
 3 files changed, 54 insertions(+), 4 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/auto/BarrelEnd.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index e7ab476..1fee142 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -184,8 +184,11 @@ public class RobotContainer {
     private void configureButtonBindings() {
         /* Test Buttons */
         // A driver test button
+       // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+        //    .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000));
+
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000));
+            .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(6000, 6000), m_robotDrive));
 
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
diff --git a/src/main/java/frc4388/robot/commands/auto/BarrelEnd.java b/src/main/java/frc4388/robot/commands/auto/BarrelEnd.java
new file mode 100644
index 0000000..f6f415e
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/auto/BarrelEnd.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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.auto;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class BarrelEnd extends CommandBase {
+  /**
+   * Creates a new BarrelEnd.
+   */
+  public BarrelEnd() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fe96ca2..668f969 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -454,13 +454,18 @@ 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;
 
-    // SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
+    double moveVelLeft = leftSpeed;
+    double moveVelRight = rightSpeed;
+    
+    // SmartDashboard.putNumber("Move Vel Left", moveVelLeft)
     // SmartDashboard.putNumber("Move Vel Right", moveVelRight);
 
     // runDriveVelocityPID(moveVel*2, targetGyro);
+    
 
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
     m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -470,6 +475,8 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.follow(m_leftBackMotor);
     m_rightFrontMotor.follow(m_rightBackMotor);
 
+    SmartDashboard.putNumber("TankDrive Speed", inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
+
     m_driveTrain.feedWatchdog();
   }