diff options
Diffstat (limited to 'src/main/java/frc/robot/RobotContainer.java')
| -rw-r--r-- | src/main/java/frc/robot/RobotContainer.java | 57 |
1 files changed, 48 insertions, 9 deletions
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 64ef818..3580633 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,14 @@ package frc.robot; +import javax.smartcardio.CommandAPDU; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.subsystems.Drivetrain; public class RobotContainer @@ -29,16 +32,13 @@ public class RobotContainer private void configureBindings() { - // m_drive.setDefaultCommand(m_drive.run(() -> - // // m_drive.arcade( - // // deadzone(-1, 1, m_controller.getRawAxis(1) * Constants.DriveMaxSpeed), - // // deadzone(-1, 1, m_controller.getRawAxis(0) * Constants.DriveMaxAngular) - // // ))); - // m_drive.tank( - // deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed, - // deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed - // ))); + m_drive.setDefaultCommand(m_drive.run(() -> + m_drive.tank( + deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed, + deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed + ))); + // debug controls new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> { Pose2d pose = m_drive.pose2d(); System.out.println("x: " + pose.getX() +" y: "+ pose.getY()); @@ -48,6 +48,45 @@ public class RobotContainer System.out.println("Reset"); m_drive.reset(); })); + + + new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(8)) + .onTrue(Commands.sequence(Commands.runOnce(() -> + System.out.println("Quasistatic Forward")), + m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward))); + new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(7)) + .onTrue(Commands.sequence(Commands.runOnce(() -> + System.out.println("Quasistatic Reverse")), + m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse))); + new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(8)) + .onTrue(Commands.sequence(Commands.runOnce(() -> + System.out.println("Dynamic Forward")), + m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward))); + new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(7)) + .onTrue(Commands.sequence(Commands.runOnce(() -> + System.out.println("Dynamic Reverse")), + m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse))); + + // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.runOnce(() -> { + // System.out.println("Running Quasistatis routine"); + // if(m_controller.getRawButton(8)) { + // System.out.println("Reverse"); + // Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)); + // } else { + // System.out.println("Forward"); + // Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)); + // } + // })); + // new Trigger(() -> m_controller.getRawButton(4) && m_controller()).onTrue(() -> { + // System.out.println("Running Dynamic routine"); + // if(m_controller.getRawButton(8)) { + // System.out.println("Reverse"); + // return m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse); + // } else { + // System.out.println("Forward"); + // return m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward); + // } + // }()); } public Command getAutonomousCommand() |