Add PID and Motion Magic Commands

This commit is contained in:
Aarav
2020-01-16 20:22:50 -07:00
parent 6fc414ccb8
commit 9be99be0c3
4 changed files with 71 additions and 6 deletions
@@ -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.DriveToDistanceMM;
import frc4388.robot.commands.DriveToDistancePID;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LED;
@@ -74,6 +75,9 @@ public class RobotContainer {
/* PID Test Command */
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveToDistancePID(m_robotDrive, 5000));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new DriveToDistanceMM(m_robotDrive, 5000));
}
/**