diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 64d3d04..50c6926 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -47,7 +47,9 @@ public void run(Context context) { // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - PlacementPosition placementPosition = null; + PlacementPosition placementPosition = PlacementPosition.NONE; + boolean elevatorManual = false; + boolean wristManual = false; while (true) { context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); @@ -126,7 +128,9 @@ public void run(Context context) { } // look for button presses to queue placement of intake/wrist/elevator superstructure - if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { + if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) { + placementPosition = PlacementPosition.NONE; + } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) { placementPosition = PlacementPosition.LOW_NODE; // TODO: update lights } else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) { @@ -153,6 +157,8 @@ public void run(Context context) { // release to retract if (boxopGamepad.getButtonPressed(InputConstants.BUTTON_EXTEND_WRISTVATOR)) { switch (placementPosition) { + case NONE: + break; case LOW_NODE: new ExtendWristvatorToLow().run(context); break; @@ -181,25 +187,25 @@ public void run(Context context) { // look for elevator nudges double elevatorNudgeAxis = boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT); if (Math.abs(elevatorNudgeAxis) > 0.05) { + elevatorManual = true; context.takeOwnership(Robot.elevator); - if (elevatorNudgeAxis > 0) { - Robot.elevator.nudgeUp(); - } else if (elevatorNudgeAxis < 0) { - Robot.elevator.nudgeDown(); - } + Robot.elevator.nudgeNoPID(elevatorNudgeAxis); context.releaseOwnership(Robot.elevator); + } else if (elevatorManual) { + Robot.elevator.stopElevator(); + elevatorManual = false; } // look for wrist nudges double wristNudgeAxis = boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT); if (Math.abs(wristNudgeAxis) > 0.05) { + wristManual = true; context.takeOwnership(Robot.wrist); - if (wristNudgeAxis > 0) { - Robot.wrist.nudgeUp(); - } else if (wristNudgeAxis < 0) { - Robot.wrist.nudgeDown(); - } + Robot.wrist.nudgeNoPID(wristNudgeAxis); context.releaseOwnership(Robot.wrist); + } else if (wristManual) { + Robot.wrist.stopWrist(); + wristManual = true; } } } diff --git a/src/main/java/com/team766/robot/PlacementPosition.java b/src/main/java/com/team766/robot/PlacementPosition.java index db03cb4..9a369c8 100644 --- a/src/main/java/com/team766/robot/PlacementPosition.java +++ b/src/main/java/com/team766/robot/PlacementPosition.java @@ -1,6 +1,7 @@ package com.team766.robot; public enum PlacementPosition { + NONE, LOW_NODE, MID_NODE, HIGH_NODE, diff --git a/src/main/java/com/team766/robot/constants/InputConstants.java b/src/main/java/com/team766/robot/constants/InputConstants.java index 9f9df3f..3655791 100644 --- a/src/main/java/com/team766/robot/constants/InputConstants.java +++ b/src/main/java/com/team766/robot/constants/InputConstants.java @@ -41,6 +41,8 @@ public final class InputConstants { public static final int POV_UP = 0; public static final int POV_DOWN = 180; + // RB + public static final int BUTTON_PLACEMENT_NONE = 6; // X/A/B/Y public static final int BUTTON_PLACEMENT_HUMAN_PLAYER = 1; // X public static final int BUTTON_PLACEMENT_HIGH = 4; // Y diff --git a/src/main/java/com/team766/robot/mechanisms/Elevator.java b/src/main/java/com/team766/robot/mechanisms/Elevator.java index 4143c1f..169042e 100644 --- a/src/main/java/com/team766/robot/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/mechanisms/Elevator.java @@ -10,6 +10,7 @@ import com.team766.library.RateLimiter; import com.team766.library.ValueProvider; import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import static com.team766.robot.constants.ConfigConstants.*; @@ -45,6 +46,7 @@ private int getHeight() { } private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.25; private final CANSparkMax leftMotor; private final CANSparkMax rightMotor; @@ -100,6 +102,18 @@ public double getHeight() { return EncoderUtils.elevatorRotationsToHeight(leftMotor.getEncoder().getPosition()); } + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + leftMotor.set(clampedValue); + } + + public void stopElevator() { + checkContextOwnership(); + leftMotor.set(0); + } + public void nudgeUp() { double height = getHeight(); // NOTE: this could artificially limit nudge range diff --git a/src/main/java/com/team766/robot/mechanisms/Wrist.java b/src/main/java/com/team766/robot/mechanisms/Wrist.java index c1ad3fb..51b4442 100644 --- a/src/main/java/com/team766/robot/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/mechanisms/Wrist.java @@ -7,6 +7,7 @@ import com.team766.library.RateLimiter; import com.team766.library.ValueProvider; import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxPIDController; @@ -48,6 +49,7 @@ private double getAngle() { } private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_DAMPENER = 0.25; private final CANSparkMax motor; private final SparkMaxPIDController pidController; @@ -91,6 +93,18 @@ public double getAngle() { return EncoderUtils.wristRotationsToDegrees(motor.getEncoder().getPosition()); } + public void nudgeNoPID(double value) { + checkContextOwnership(); + double clampedValue = MathUtil.clamp(value, -1, 1); + clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear + motor.set(clampedValue); + } + + public void stopWrist() { + checkContextOwnership(); + motor.set(0); + } + public void nudgeUp() { double angle = getAngle(); double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.UP.getAngle());