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);