diff options
Diffstat (limited to 'src/main/java/frc/robot/RobotContainer.java')
| -rw-r--r-- | src/main/java/frc/robot/RobotContainer.java | 39 |
1 files changed, 29 insertions, 10 deletions
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d91ff78..d2cede4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,34 +4,53 @@ package frc.robot; -import edu.wpi.first.math.filter.SlewRateLimiter; +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 frc.robot.subsystems.Drivetrain; public class RobotContainer { private final Drivetrain m_drive = new Drivetrain(); - private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3); - private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3); - - private final GenericHID m_controller = - new GenericHID(Constants.PortDriverController); + private final GenericHID m_controller = Constants.controller; public RobotContainer() { configureBindings(); } + private double deadzone(double low, double high, double val) { + if(val > low && val < high) return 0.0; + return val; + } + private void configureBindings() { m_drive.setDefaultCommand(m_drive.run(() -> - m_drive.drive( - m_speedlimiter.calculate(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed, - m_rotlimiter.calculate(m_controller.getRawAxis(2)) * Constants.DriveMaxAngular - ))); + // 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 + ))); + + new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> { + Pose2d pose = m_drive.pose2d(); + System.out.println("x: " + pose.getX() +" y: "+ pose.getY()); + System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees()); + })); + new Trigger(() -> m_controller.getRawButton(2)).onTrue(Commands.runOnce(() -> { + System.out.println("Reset"); + m_drive.reset(); + })); + // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.run(() -> { + // m_drive.tank(5, 5); + // })); } public Command getAutonomousCommand() |