diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 247e298..bc8097a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,6 +30,7 @@ public static class Intake { public static final double UNLOADING_WAIT_TIME = 2f; } + public final class Wrist { public static final int DRIVER_CONTROLLER_PORT = 0; public static final int WRIST_MOTOR_DEVICE_NUMBER = 16; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d81591a..9fa60b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,17 +4,14 @@ package frc.robot; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.commands.StopIntakeMotorCommand; -import frc.robot.commands.IntakeCommand; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.SequentialCommands; -import frc.robot.subsystems.WristSubsystem; import frc.robot.commands.AdvancedIntakeSequence; +import frc.robot.commands.SequentialCommands; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.WristSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -34,7 +31,7 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings - driverB.y().onTrue(new SequentialCommands(wristSubsystem)); + configureButtonBindings(); // } else if (driverB.leftTrigger().getAsBoolean()) { @@ -42,7 +39,7 @@ public RobotContainer() { // driverB.y().onTrue(new WristCommand(wristSubsystem, 0)); // driverB.a().onTrue(new WristCommand(wristSubsystem, 20)); // driverB.b().onTrue(new WristCommand(wristSubsystem, 40)); - driverB.x().onTrue(new InstantCommand(() -> {}, wristSubsystem)); + } /** @@ -78,6 +75,8 @@ public void containerMatchStarting() { */ private void configureButtonBindings() { // intake and outtake + driverB.y().onTrue(new SequentialCommands(wristSubsystem)); + driverB.x().onTrue(new InstantCommand(() -> {}, wristSubsystem)); driverB.rightTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, true)); driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true)); driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false)); diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java index e2ba11a..e643b08 100644 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java @@ -32,9 +32,9 @@ public WristSubsystem() { this.wrist_motor = new TalonFX(Constants.Wrist.WRIST_MOTOR_DEVICE_NUMBER); this.wrist_motor.configFactoryDefault(); this.wrist_motor.clearStickyFaults(); - this.wrist_motor.configForwardSoftLimitThreshold(0); - this.wrist_motor.configReverseSoftLimitThreshold(degreesToTicks(90)); - this.wrist_motor.configReverseSoftLimitEnable(true, 0); + //this.wrist_motor.configForwardSoftLimitThreshold(0); + //this.wrist_motor.configReverseSoftLimitThreshold(degreesToTicks(90)); + //this.wrist_motor.configReverseSoftLimitEnable(true, 0); this.wrist_motor.configForwardSoftLimitEnable(true, 0); this.wrist_motor.setNeutralMode(NeutralMode.Coast); this.wrist_motor.setSelectedSensorPosition(0); @@ -43,12 +43,13 @@ public WristSubsystem() { pidController = new PIDController(0.1, 0, 0); WristTab.add(pidController); WristTab.addNumber("Current Motor Position", wrist_motor::getSelectedSensorPosition); + WristTab.addNumber("Current motor angle", this::getCurrentAngle); WristTab.addBoolean("Is at target", this::nearTargetAngle); WristTab.addNumber("Target Angle", () -> this.targetAngle); -} + } public static double degreesToTicks(double angle) { - return (angle*360d)/((Wrist.GEAR_RATIO * Wrist.TICKS)); + return (angle * 360d) / ((Wrist.GEAR_RATIO * Wrist.TICKS)); } public void setTargetAngle(double targetAngle) { @@ -57,7 +58,7 @@ public void setTargetAngle(double targetAngle) { } public static double ticksToDegrees(double ticks) { - return ((ticks / Wrist.TICKS) * (Wrist.GEAR_RATIO/360)); + return ((ticks / Wrist.TICKS) * (Wrist.GEAR_RATIO) / 360); } private double getCurrentAngle() {