-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
116 changed files
with
3,446 additions
and
3,884 deletions.
There are no files selected for viewing
8 changes: 0 additions & 8 deletions
8
src/main/java/com/team766/ViSIONbase/AprilTagGeneralUncheckedException.java
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,109 +1,108 @@ | ||
| package com.team766.robot.common; | ||
|
|
||
| import com.team766.framework.Context; | ||
| import com.team766.framework.OIFragment; | ||
| import static com.team766.framework3.RulePersistence.*; | ||
|
|
||
| import com.team766.framework3.Conditions; | ||
| import com.team766.framework3.Rule; | ||
| import com.team766.framework3.RuleEngine; | ||
| import com.team766.hal.JoystickReader; | ||
| import com.team766.robot.common.constants.ControlConstants; | ||
| import com.team766.robot.common.constants.InputConstants; | ||
| import com.team766.robot.common.mechanisms.SwerveDrive; | ||
| import java.util.Set; | ||
|
|
||
| public class DriverOI extends OIFragment { | ||
|
|
||
| protected static final double FINE_DRIVING_COEFFICIENT = 0.25; | ||
|
|
||
| protected final SwerveDrive drive; | ||
| protected final JoystickReader leftJoystick; | ||
| protected final JoystickReader rightJoystick; | ||
| protected double rightJoystickY = 0; | ||
| protected double leftJoystickX = 0; | ||
| protected double leftJoystickY = 0; | ||
| protected boolean isCross = false; | ||
|
|
||
| private final OICondition movingJoysticks; | ||
|
|
||
| public DriverOI(SwerveDrive drive, JoystickReader leftJoystick, JoystickReader rightJoystick) { | ||
| this.drive = drive; | ||
| this.leftJoystick = leftJoystick; | ||
| this.rightJoystick = rightJoystick; | ||
|
|
||
| movingJoysticks = | ||
| new OICondition( | ||
| () -> | ||
| !isCross | ||
| && Math.abs(leftJoystickX) | ||
| + Math.abs(leftJoystickY) | ||
| + Math.abs(rightJoystickY) | ||
| > 0); | ||
| } | ||
|
|
||
| public void handleOI(Context context) { | ||
|
|
||
| // Negative because forward is negative in driver station | ||
| leftJoystickX = | ||
| -createJoystickDeadzone(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) | ||
| * ControlConstants.MAX_POSITIONAL_VELOCITY; // For fwd/rv | ||
| // Negative because left is negative in driver station | ||
| leftJoystickY = | ||
| -createJoystickDeadzone(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) | ||
| * ControlConstants.MAX_POSITIONAL_VELOCITY; // For left/right | ||
| // Negative because left is negative in driver station | ||
| rightJoystickY = | ||
| -createJoystickDeadzone(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) | ||
| * ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer | ||
| public class DriverOI { | ||
| public DriverOI( | ||
| RuleEngine oi, | ||
| JoystickReader leftJoystick, | ||
| JoystickReader rightJoystick, | ||
| SwerveDrive drive) { | ||
| leftJoystick.setAllAxisDeadzone(ControlConstants.JOYSTICK_DEADZONE); | ||
| rightJoystick.setAllAxisDeadzone(ControlConstants.JOYSTICK_DEADZONE); | ||
|
|
||
| if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) { | ||
| drive.resetGyro(); | ||
| } | ||
|
|
||
| if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_POS)) { | ||
| drive.resetCurrentPosition(); | ||
| } | ||
| oi.addRule( | ||
| Rule.create( | ||
| "Reset Gyro", | ||
| () -> leftJoystick.getButton(InputConstants.BUTTON_RESET_GYRO)) | ||
| .withOnTriggeringProcedure(ONCE, Set.of(drive), () -> drive.resetGyro())); | ||
| oi.addRule( | ||
| Rule.create( | ||
| "Reset Pos", | ||
| () -> leftJoystick.getButton(InputConstants.BUTTON_RESET_POS)) | ||
| .withOnTriggeringProcedure( | ||
| ONCE, Set.of(drive), () -> drive.resetCurrentPosition())); | ||
|
|
||
| // Sets the wheels to the cross position if the cross button is pressed | ||
| if (rightJoystick.getButtonPressed(InputConstants.BUTTON_CROSS_WHEELS)) { | ||
| if (!isCross) { | ||
| context.takeOwnership(drive); | ||
| drive.stopDrive(); | ||
| drive.setCross(); | ||
| } | ||
| isCross = !isCross; | ||
| } | ||
| oi.addRule( | ||
| Rule.create( | ||
| "Cross Wheels", | ||
| new Conditions.Toggle( | ||
| () -> | ||
| rightJoystick.getButton( | ||
| InputConstants.BUTTON_CROSS_WHEELS))) | ||
| .withOnTriggeringProcedure( | ||
| ONCE_AND_HOLD, | ||
| Set.of(drive), | ||
| () -> drive.setRequest(new SwerveDrive.SetCross()))); | ||
|
|
||
| // Moves the robot if there are joystick inputs | ||
| if (movingJoysticks.isTriggering()) { | ||
| double drivingCoefficient = 1; | ||
| // If a button is pressed, drive is just fine adjustment | ||
| if (rightJoystick.getButton(InputConstants.BUTTON_FINE_DRIVING)) { | ||
| drivingCoefficient = FINE_DRIVING_COEFFICIENT; | ||
| } | ||
| context.takeOwnership(drive); | ||
| drive.controlFieldOriented( | ||
| (drivingCoefficient | ||
| * curvedJoystickPower( | ||
| leftJoystickX, ControlConstants.TRANSLATIONAL_CURVE_POWER)), | ||
| (drivingCoefficient | ||
| * curvedJoystickPower( | ||
| leftJoystickY, ControlConstants.TRANSLATIONAL_CURVE_POWER)), | ||
| (drivingCoefficient | ||
| * curvedJoystickPower( | ||
| rightJoystickY, ControlConstants.ROTATIONAL_CURVE_POWER))); | ||
| } else { | ||
| context.takeOwnership(drive); | ||
| drive.stopDrive(); | ||
| drive.setCross(); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Helper method to ignore joystick values below JOYSTICK_DEADZONE | ||
| * @param joystickValue the value to trim | ||
| * @return the trimmed joystick value | ||
| */ | ||
| private double createJoystickDeadzone(double joystickValue) { | ||
| return Math.abs(joystickValue) > ControlConstants.JOYSTICK_DEADZONE ? joystickValue : 0; | ||
| oi.addRule( | ||
| Rule.create( | ||
| "Joysticks moved", | ||
| () -> | ||
| leftJoystick.isAxisMoved( | ||
| InputConstants.AXIS_FORWARD_BACKWARD) | ||
| || leftJoystick.isAxisMoved( | ||
| InputConstants.AXIS_LEFT_RIGHT) | ||
| || rightJoystick.isAxisMoved( | ||
| InputConstants.AXIS_LEFT_RIGHT)) | ||
| .withOnTriggeringProcedure( | ||
| REPEATEDLY, | ||
| Set.of(drive), | ||
| () -> { | ||
| // For fwd/rv | ||
| // Negative because forward is negative in driver station | ||
| final double leftJoystickX = | ||
| -leftJoystick.getAxis( | ||
| InputConstants.AXIS_FORWARD_BACKWARD) | ||
| * ControlConstants.MAX_TRANSLATIONAL_VELOCITY; | ||
| // For left/right | ||
| // Negative because left is negative in driver station | ||
| final double leftJoystickY = | ||
| -leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT) | ||
| * ControlConstants.MAX_TRANSLATIONAL_VELOCITY; | ||
| // For steer | ||
| // Negative because left is negative in driver station | ||
| final double rightJoystickY = | ||
| -rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT) | ||
| * ControlConstants.MAX_ROTATIONAL_VELOCITY; | ||
| // If a button is pressed, drive is just fine adjustment | ||
| final double drivingCoefficient = | ||
| rightJoystick.getButton( | ||
| InputConstants.BUTTON_FINE_DRIVING) | ||
| ? ControlConstants.FINE_DRIVING_COEFFICIENT | ||
| : 1; | ||
| drive.setRequest( | ||
| new SwerveDrive.FieldOrientedVelocity( | ||
| (drivingCoefficient | ||
| * curvedJoystickPower( | ||
| leftJoystickX, | ||
| ControlConstants | ||
| .TRANSLATIONAL_CURVE_POWER)), | ||
| (drivingCoefficient | ||
| * curvedJoystickPower( | ||
| leftJoystickY, | ||
| ControlConstants | ||
| .TRANSLATIONAL_CURVE_POWER)), | ||
| (drivingCoefficient | ||
| * curvedJoystickPower( | ||
| rightJoystickY, | ||
| ControlConstants | ||
| .ROTATIONAL_CURVE_POWER)))); | ||
| })); | ||
| } | ||
|
|
||
| private double curvedJoystickPower(double value, double power) { | ||
| private static double curvedJoystickPower(double value, double power) { | ||
| return Math.signum(value) * Math.pow(Math.abs(value), power); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.