diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fac6bd7..202f647 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -83,6 +84,12 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); + + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + + new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) + .whenPressed(new InstantCommand(null, m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java new file mode 100644 index 0000000..70e54dd --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveAtVelocityPID extends CommandBase { + Drive m_drive; + double m_targetVel; + double m_leftTarget; + double m_rightTarget; + /** + * Creates a new DriveAtVelocityPID. + */ + public DriveAtVelocityPID(Drive subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_targetVel = targetVel; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_leftTarget = m_targetVel; + m_rightTarget = -m_targetVel; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.runVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); + } + + // 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/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 6c8e1cc..3fe0ea0 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; @@ -31,6 +32,8 @@ public class DriveToDistancePID extends CommandBase { public void initialize() { m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); + SmartDashboard.putNumber("Left Target", m_leftTarget); + SmartDashboard.putNumber("Right Target", m_rightTarget); } // Called every time the scheduler runs while the command is scheduled.