diff --git a/networktables.json b/networktables.json new file mode 100644 index 00000000..fe51488c --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..69b1a3cb --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 00000000..5f9d2754 --- /dev/null +++ b/simgui.json @@ -0,0 +1,10 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/training/Robot.java b/src/main/java/training/Robot.java index 3fbb8a32..ec779899 100644 --- a/src/main/java/training/Robot.java +++ b/src/main/java/training/Robot.java @@ -1,11 +1,14 @@ package training; +import training.subsystems.elbow.Elbow; + public class Robot { // Enter your subsystems... public Robot() { // Boot your subsystems... + Elbow.init(); } // Add your subsystems getters... diff --git a/src/main/java/training/TrainingRobotManager.java b/src/main/java/training/TrainingRobotManager.java index 9a3b7de5..77c0a423 100644 --- a/src/main/java/training/TrainingRobotManager.java +++ b/src/main/java/training/TrainingRobotManager.java @@ -1,14 +1,28 @@ package training; +import edu.wpi.first.math.geometry.Rotation2d; +import training.commands.elbow.MoveElbowToAngle; +import training.subsystems.elbow.RobotTypes; import utils.DefaultRobotManager; +import utils.joysticks.JoystickPorts; +import utils.joysticks.SmartJoystick; public class TrainingRobotManager extends DefaultRobotManager { private Robot robot; + public static final RobotTypes ROBOT_TYPE = RobotTypes.SIMULATION; + @Override public void trainingInit() { this.robot = new Robot(); + configureBindings(); + } + + public void configureBindings() { + SmartJoystick smartJoystick = new SmartJoystick(JoystickPorts.MAIN); + smartJoystick.A.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(45))); + smartJoystick.B.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(90))); } @Override diff --git a/src/main/java/training/commands/elbow/MoveElbowToAngle.java b/src/main/java/training/commands/elbow/MoveElbowToAngle.java new file mode 100644 index 00000000..bbee4f32 --- /dev/null +++ b/src/main/java/training/commands/elbow/MoveElbowToAngle.java @@ -0,0 +1,31 @@ +package training.commands.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import training.subsystems.elbow.Elbow; + +public class MoveElbowToAngle extends Command { + + private Rotation2d targetAngle; + private Elbow elbow; + + public MoveElbowToAngle(Rotation2d targetAngle) { + this.targetAngle = targetAngle; + this.elbow = Elbow.getInstance(); + addRequirements(this.elbow); + } + + @Override + public void execute() { + elbow.goToAngle(targetAngle); + } + + public boolean isFinished() { + return elbow.isAtAngle(targetAngle); + } + + public void end(boolean interrupted) { + elbow.stayAtPosition(); + } + +} diff --git a/src/main/java/training/commands/roller/MoveRollerToSpeed.java b/src/main/java/training/commands/roller/MoveRollerToSpeed.java new file mode 100644 index 00000000..44288212 --- /dev/null +++ b/src/main/java/training/commands/roller/MoveRollerToSpeed.java @@ -0,0 +1,26 @@ +package training.commands.roller; + +import edu.wpi.first.wpilibj2.command.Command; +import training.subsystems.roller.Roller; + +public class MoveRollerToSpeed extends Command { + + private double targetSpeed; + private Roller roller; + + public MoveRollerToSpeed(double targetSpeed) { + this.targetSpeed = targetSpeed; + this.roller = Roller.getInstance(); + addRequirements(roller); + } + + @Override + public void execute() { + roller.goToSpeed(targetSpeed); + } + + public void end(boolean interrupted) { + roller.stop(); + } + +} diff --git a/src/main/java/training/commands/wrist/MoveWristToAngle.java b/src/main/java/training/commands/wrist/MoveWristToAngle.java new file mode 100644 index 00000000..6bfc46cb --- /dev/null +++ b/src/main/java/training/commands/wrist/MoveWristToAngle.java @@ -0,0 +1,31 @@ +package training.commands.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import training.subsystems.wrist.Wrist; + +public class MoveWristToAngle extends Command { + + private Rotation2d targetAngle; + private Wrist wrist; + + public MoveWristToAngle(Rotation2d targetAngle) { + this.targetAngle = targetAngle; + this.wrist = Wrist.getInstance(); + addRequirements(wrist); + } + + @Override + public void execute() { + wrist.goToAngle(targetAngle); + } + + public boolean isFinished() { + return wrist.isAtAngle(targetAngle); + } + + public void end(boolean interrupted) { + wrist.stayAtPosition(); + } + +} diff --git a/src/main/java/training/subsystems/elbow/Elbow.java b/src/main/java/training/subsystems/elbow/Elbow.java new file mode 100644 index 00000000..8f38b644 --- /dev/null +++ b/src/main/java/training/subsystems/elbow/Elbow.java @@ -0,0 +1,62 @@ +package training.subsystems.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; +import utils.GBSubsystem; + +public class Elbow extends GBSubsystem { + + private static Elbow instance; + + private final IElbow elbow; + + private ElbowInputsAutoLogged inputs; + + private Elbow() { + this.elbow = ElbowFactory.create(); + this.inputs = new ElbowInputsAutoLogged(); + } + + public static void init() { + if (instance == null) { + instance = new Elbow(); + } + } + + public static Elbow getInstance() { + init(); + return instance; + } + + public Rotation2d getPosition() { + return inputs.position; + } + + public Rotation2d getVelocity() { + return inputs.velocity; + } + + public void goToAngle(Rotation2d targetAngle) { + elbow.goToAngle(targetAngle); + } + + public boolean isAtAngle(Rotation2d targetAngle) { + return Math.abs(targetAngle.getRotations() - inputs.position.getRotations()) <= ElbowConstants.TOLERANCE.getRotations(); + } + + public void stayAtPosition() { + goToAngle(getPosition()); + } + + @Override + protected String getLogPath() { + return "Elbow/"; + } + + @Override + protected void subsystemPeriodic() { + elbow.updateInputs(inputs); + Logger.processInputs("Elbow/", inputs); + } + +} diff --git a/src/main/java/training/subsystems/elbow/ElbowConstants.java b/src/main/java/training/subsystems/elbow/ElbowConstants.java new file mode 100644 index 00000000..5fcb63fa --- /dev/null +++ b/src/main/java/training/subsystems/elbow/ElbowConstants.java @@ -0,0 +1,48 @@ +package training.subsystems.elbow; + + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; + +public class ElbowConstants { + + public enum PresetPositions { + + SAFE(Rotation2d.fromDegrees(0)), + INTAKE(Rotation2d.fromDegrees(0)), + SCORE(Rotation2d.fromDegrees(0)), + STARTING(Rotation2d.fromDegrees(0)), + TRANSFER(Rotation2d.fromDegrees(0)); + + public final Rotation2d ANGLE; + + PresetPositions(Rotation2d angle) { + this.ANGLE = angle; + } + + } + + public static final int MOTOR_ID = 5; + public static final int P = 10; + + public static final int I = 0; + + public static final int D = 1; + + public static final int PID_SLOT = 0; + + public static final ArmFeedforward ARM_FEEDFORWARD = new ArmFeedforward(0, 0, 0, 0); + + public static final double GEAR_RATIO = 1 / (28.0 * (60.0 / 16.0)); + + public static final double ARM_LENGTH = 0.44; + + public static final double ARM_MASS_KG = 0.44; + + public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-81); + + public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(90); + + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3); + +} diff --git a/src/main/java/training/subsystems/elbow/ElbowFactory.java b/src/main/java/training/subsystems/elbow/ElbowFactory.java new file mode 100644 index 00000000..37851ae9 --- /dev/null +++ b/src/main/java/training/subsystems/elbow/ElbowFactory.java @@ -0,0 +1,14 @@ +package training.subsystems.elbow; + +import training.TrainingRobotManager; + +public class ElbowFactory { + + public static IElbow create() { + return switch (TrainingRobotManager.ROBOT_TYPE) { + case SIMULATION -> new ElbowSimulation(); + case SYNCOPA -> new NeoElbow(); + }; + } + +} diff --git a/src/main/java/training/subsystems/elbow/ElbowInputs.java b/src/main/java/training/subsystems/elbow/ElbowInputs.java new file mode 100644 index 00000000..75250c0b --- /dev/null +++ b/src/main/java/training/subsystems/elbow/ElbowInputs.java @@ -0,0 +1,12 @@ +package training.subsystems.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +@AutoLog +public class ElbowInputs { + + public Rotation2d position; + public Rotation2d velocity; + +} diff --git a/src/main/java/training/subsystems/elbow/ElbowSimulation.java b/src/main/java/training/subsystems/elbow/ElbowSimulation.java new file mode 100644 index 00000000..c3e396df --- /dev/null +++ b/src/main/java/training/subsystems/elbow/ElbowSimulation.java @@ -0,0 +1,61 @@ +package training.subsystems.elbow; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import utils.simulation.SingleJointedArmSimulation; + +public class ElbowSimulation implements IElbow { + + private SingleJointedArmSimulation elbowSimulation; + private ArmFeedforward armFeedforward; + private PIDController controller; + private PositionVoltage positionVoltage; + + public ElbowSimulation() { + SingleJointedArmSim armSim = new SingleJointedArmSim( + DCMotor.getNEO(SimulationElbowConstants.NUMBER_OF_MOTORS), + SimulationElbowConstants.GEAR_RATIO, + SingleJointedArmSim.estimateMOI(ElbowConstants.ARM_LENGTH, ElbowConstants.ARM_MASS_KG), + ElbowConstants.ARM_LENGTH, + ElbowConstants.BACKWARD_ANGLE_LIMIT.getRadians(), + ElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(), + false, + ElbowConstants.PresetPositions.STARTING.ANGLE.getRadians() + ); + elbowSimulation = new SingleJointedArmSimulation(armSim); + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = ElbowConstants.P; + config.Slot0.kI = ElbowConstants.I; + config.Slot0.kD = ElbowConstants.D; + elbowSimulation.applyConfiguration(config); + this.positionVoltage = new PositionVoltage(0); + } + + public void goToAngle(Rotation2d targetAngle) { + elbowSimulation.setControl(positionVoltage.withPosition(targetAngle.getRotations())); + } + + public Rotation2d getPosition() { + return elbowSimulation.getPosition(); + } + + public Rotation2d getVelocity() { + return elbowSimulation.getVelocity(); + } + + public void stayAtPosition() { + goToAngle(getPosition()); + } + + @Override + public void updateInputs(ElbowInputsAutoLogged inputs) { + inputs.position = getPosition(); + inputs.velocity = getVelocity(); + } + +} diff --git a/src/main/java/training/subsystems/elbow/IElbow.java b/src/main/java/training/subsystems/elbow/IElbow.java new file mode 100644 index 00000000..ccba7dcb --- /dev/null +++ b/src/main/java/training/subsystems/elbow/IElbow.java @@ -0,0 +1,13 @@ +package training.subsystems.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IElbow { + + void stayAtPosition(); + + void goToAngle(Rotation2d targetAngle); + + void updateInputs(ElbowInputsAutoLogged inputs); + +} diff --git a/src/main/java/training/subsystems/elbow/NeoElbow.java b/src/main/java/training/subsystems/elbow/NeoElbow.java new file mode 100644 index 00000000..a61b057c --- /dev/null +++ b/src/main/java/training/subsystems/elbow/NeoElbow.java @@ -0,0 +1,52 @@ +package training.subsystems.elbow; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; + +public class NeoElbow implements IElbow { + + private CANSparkMax motor; + + public NeoElbow() { + this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + motor.getPIDController().setP(ElbowConstants.P); + motor.getPIDController().setI(ElbowConstants.I); + motor.getPIDController().setD(ElbowConstants.D); + } + + public Rotation2d getPosition() { + return Rotation2d.fromRotations(motor.getEncoder().getPosition()); + } + + public Rotation2d getVelocity() { + return Rotation2d.fromRotations(motor.getEncoder().getVelocity()); + } + + + public void goToAngle(Rotation2d targetAngle) { + motor.getPIDController() + .setReference( + targetAngle.getDegrees(), + CANSparkBase.ControlType.kPosition, + ElbowConstants.PID_SLOT, + ElbowConstants.ARM_FEEDFORWARD.calculate(getPosition().getRadians(), getVelocity().getRotations()) + ); + } + + public boolean isAtAngle(Rotation2d targetAngle) { + return Math.abs(targetAngle.getRotations() - getPosition().getRotations()) <= ElbowConstants.TOLERANCE.getRotations(); + } + + public void stayAtPosition() { + goToAngle(getPosition()); + } + + @Override + public void updateInputs(ElbowInputsAutoLogged inputs) { + inputs.position = getPosition(); + inputs.velocity = getVelocity(); + } + +} diff --git a/src/main/java/training/subsystems/elbow/RobotTypes.java b/src/main/java/training/subsystems/elbow/RobotTypes.java new file mode 100644 index 00000000..dc41143c --- /dev/null +++ b/src/main/java/training/subsystems/elbow/RobotTypes.java @@ -0,0 +1,9 @@ +package training.subsystems.elbow; + +public enum RobotTypes { + + SIMULATION, + + SYNCOPA; + +} diff --git a/src/main/java/training/subsystems/elbow/SimulationElbowConstants.java b/src/main/java/training/subsystems/elbow/SimulationElbowConstants.java new file mode 100644 index 00000000..33e722f2 --- /dev/null +++ b/src/main/java/training/subsystems/elbow/SimulationElbowConstants.java @@ -0,0 +1,16 @@ +package training.subsystems.elbow; + + +public class SimulationElbowConstants { + + public static final int NUMBER_OF_MOTORS = 1; + + public static final double GEAR_RATIO = 1 / ElbowConstants.GEAR_RATIO; + + public static final double P = 1; + + public static final double I = 0; + + public static final double D = 0; + +} diff --git a/src/main/java/training/subsystems/roller/IRoller.java b/src/main/java/training/subsystems/roller/IRoller.java new file mode 100644 index 00000000..ca669ef6 --- /dev/null +++ b/src/main/java/training/subsystems/roller/IRoller.java @@ -0,0 +1,15 @@ +package training.subsystems.roller; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IRoller { + + void goToSpeed(double targetSpeed); + + Rotation2d getVelocity(); + + void stayAtPosition(); + + Rotation2d getPosition(); + +} diff --git a/src/main/java/training/subsystems/roller/Roller.java b/src/main/java/training/subsystems/roller/Roller.java new file mode 100644 index 00000000..014ba89f --- /dev/null +++ b/src/main/java/training/subsystems/roller/Roller.java @@ -0,0 +1,64 @@ +package training.subsystems.roller; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; +import utils.GBSubsystem; + +public class Roller extends GBSubsystem { + + private static Roller instance; + private final CANSparkMax motor; + + private Roller() { + motor = new CANSparkMax(RollerConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + motor.getPIDController().setP(RollerConstants.P); + motor.getPIDController().setI(RollerConstants.I); + motor.getPIDController().setD(RollerConstants.D); + } + + public static void init() { + if (instance == null) { + instance = new Roller(); + } + } + + public static Roller getInstance() { + init(); + return instance; + } + + public Rotation2d getVelocity() { + return Rotation2d.fromRadians(motor.getEncoder().getVelocity()); + } + + public boolean isAtVelocity(Rotation2d targetVelocity) { + return Math.abs(targetVelocity.getRotations() - motor.getEncoder().getVelocity()) + <= RollerConstants.TOLERANCE.getRotations(); + } + + public void goToSpeed(double targetSpeed) { + motor.getPIDController().setReference(targetSpeed, CANSparkBase.ControlType.kVelocity); + } + + public Rotation2d getPosition() { + return getInstance().getPosition(); + } + + public void stop() { + motor.set(0); + } + + @Override + protected String getLogPath() { + return "Roller/"; + } + + @Override + protected void subsystemPeriodic() { + Logger.recordOutput("Roller/Speed", getVelocity().getRotations()); + } + +} diff --git a/src/main/java/training/subsystems/roller/RollerConstants.java b/src/main/java/training/subsystems/roller/RollerConstants.java new file mode 100644 index 00000000..d548cba1 --- /dev/null +++ b/src/main/java/training/subsystems/roller/RollerConstants.java @@ -0,0 +1,17 @@ +package training.subsystems.roller; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class RollerConstants { + + public static final int MOTOR_ID = 22; + + public static final int P = 1; + + public static final int I = 0; + + public static final int D = 0; + + public static final Rotation2d TOLERANCE = Rotation2d.fromRotations(0.04); + +} diff --git a/src/main/java/training/subsystems/wrist/IWrist.java b/src/main/java/training/subsystems/wrist/IWrist.java new file mode 100644 index 00000000..d378bdf5 --- /dev/null +++ b/src/main/java/training/subsystems/wrist/IWrist.java @@ -0,0 +1,11 @@ +package training.subsystems.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IWrist { + + void goToAngle(Rotation2d targetAngle); + + void stop(); + +} diff --git a/src/main/java/training/subsystems/wrist/Wrist.java b/src/main/java/training/subsystems/wrist/Wrist.java new file mode 100644 index 00000000..dc1da143 --- /dev/null +++ b/src/main/java/training/subsystems/wrist/Wrist.java @@ -0,0 +1,61 @@ +package training.subsystems.wrist; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; +import utils.GBSubsystem; + +import static edu.wpi.first.hal.simulation.PWMDataJNI.getPosition; + +public class Wrist extends GBSubsystem { + + private static Wrist instance; + private final CANSparkMax motor; + + private Wrist() { + motor = new CANSparkMax(WristConstant.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + motor.getPIDController().setP(WristConstant.P); + motor.getPIDController().setI(WristConstant.I); + motor.getPIDController().setD(WristConstant.D); + } + + public static Wrist getInstance() { + init(); + return instance; + } + + public static void init() { + if (instance == null) { + instance = new Wrist(); + } + } + + public Rotation2d getPosition() { + return Rotation2d.fromRadians(motor.getEncoder().getPosition()); + } + + public void goToAngle(Rotation2d targetAngle) { + motor.getPIDController().setReference(targetAngle.getDegrees(), CANSparkBase.ControlType.kPosition); + } + + public boolean isAtAngle(Rotation2d targetAngle) { + return Math.abs(targetAngle.getRotations() - motor.getEncoder().getPosition()) <= WristConstant.TOLERANCE.getRotations(); + } + + public void stayAtPosition() { + goToAngle(getPosition()); + } + + @Override + protected String getLogPath() { + return "Wrist/"; + } + + @Override + protected void subsystemPeriodic() { + Logger.recordOutput("Wrist/Position", getPosition().getDegrees()); + } + +} diff --git a/src/main/java/training/subsystems/wrist/WristConstant.java b/src/main/java/training/subsystems/wrist/WristConstant.java new file mode 100644 index 00000000..84e52480 --- /dev/null +++ b/src/main/java/training/subsystems/wrist/WristConstant.java @@ -0,0 +1,17 @@ +package training.subsystems.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class WristConstant { + + public static final int MOTOR_ID = 11; + + public static final int P = 1; + + public static final int I = 0; + + public static final int D = 0; + + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(10); + +} diff --git a/src/main/java/utils/joysticks/Axis.java b/src/main/java/utils/joysticks/Axis.java new file mode 100644 index 00000000..cbd28510 --- /dev/null +++ b/src/main/java/utils/joysticks/Axis.java @@ -0,0 +1,30 @@ +package utils.joysticks; + +import edu.wpi.first.wpilibj.Joystick; + +public enum Axis { + + LEFT_X(0, true), + LEFT_Y(1, true), + LEFT_TRIGGER(2, false), + RIGHT_TRIGGER(3, false), + RIGHT_X(4, false), + RIGHT_Y(5, true); + + private final int id; + private final int invertedSign; + + Axis(int id, boolean isInverted) { + this.id = id; + this.invertedSign = isInverted ? -1 : 1; + } + + AxisButton getAsButton(Joystick joystick, double threshold) { + return new AxisButton(joystick, id, threshold); + } + + double getValue(Joystick joystick) { + return invertedSign * joystick.getRawAxis(id); + } + +} diff --git a/src/main/java/utils/joysticks/AxisButton.java b/src/main/java/utils/joysticks/AxisButton.java new file mode 100644 index 00000000..1f996420 --- /dev/null +++ b/src/main/java/utils/joysticks/AxisButton.java @@ -0,0 +1,12 @@ +package utils.joysticks; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class AxisButton extends Trigger { + + public AxisButton(GenericHID joystick, int axis, double threshold) { + super(() -> Math.abs(joystick.getRawAxis(axis)) >= threshold); + } + +} diff --git a/src/main/java/utils/joysticks/ButtonID.java b/src/main/java/utils/joysticks/ButtonID.java new file mode 100644 index 00000000..3ddabae5 --- /dev/null +++ b/src/main/java/utils/joysticks/ButtonID.java @@ -0,0 +1,35 @@ +package utils.joysticks; + + +enum ButtonID { + + A(1), + B(2), + X(3), + Y(4), + + L1(5), + R1(6), + + BACK(7), + START(8), + + L3(9), + R3(10), + + POV_UP(0), + POV_RIGHT(90), + POV_DOWN(180), + POV_LEFT(270); + + private final int id; + + ButtonID(int id) { + this.id = id; + } + + public int getId() { + return id; + } + +} diff --git a/src/main/java/utils/joysticks/JoystickPorts.java b/src/main/java/utils/joysticks/JoystickPorts.java new file mode 100644 index 00000000..d8e32ccf --- /dev/null +++ b/src/main/java/utils/joysticks/JoystickPorts.java @@ -0,0 +1,22 @@ +package utils.joysticks; + +public enum JoystickPorts { + + MAIN(0), + SECOND(1), + THIRD(2), + FOURTH(3), + FIFTH(4), + SIXTH(5); + + private final int port; + + JoystickPorts(int port) { + this.port = port; + } + + public int getPort() { + return port; + } + +} diff --git a/src/main/java/utils/joysticks/SmartJoystick.java b/src/main/java/utils/joysticks/SmartJoystick.java new file mode 100644 index 00000000..2a051fcc --- /dev/null +++ b/src/main/java/utils/joysticks/SmartJoystick.java @@ -0,0 +1,94 @@ +package utils.joysticks; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; + +public class SmartJoystick { + + private static final double DEADZONE = 0.07; + private static final double DEFAULT_THRESHOLD_FOR_AXIS_BUTTON = 0.1; + private static final double SENSITIVE_AXIS_VALUE_POWER = 2; + + public final JoystickButton A, B, X, Y, L1, R1, START, BACK, L3, R3; + public final POVButton POV_UP, POV_RIGHT, POV_DOWN, POV_LEFT; + private final Joystick joystick; + private final double deadzone; + + public SmartJoystick(JoystickPorts joystickPort) { + this(joystickPort, DEADZONE); + } + + public SmartJoystick(JoystickPorts joystickPort, double deadzone) { + this(new Joystick(joystickPort.getPort()), deadzone); + } + + private SmartJoystick(Joystick joystick, double deadzone) { + this.deadzone = deadzone; + this.joystick = joystick; + + this.A = new JoystickButton(this.joystick, ButtonID.A.getId()); + this.B = new JoystickButton(this.joystick, ButtonID.B.getId()); + this.X = new JoystickButton(this.joystick, ButtonID.X.getId()); + this.Y = new JoystickButton(this.joystick, ButtonID.Y.getId()); + + this.L1 = new JoystickButton(this.joystick, ButtonID.L1.getId()); + this.R1 = new JoystickButton(this.joystick, ButtonID.R1.getId()); + + this.BACK = new JoystickButton(this.joystick, ButtonID.BACK.getId()); + this.START = new JoystickButton(this.joystick, ButtonID.START.getId()); + + this.L3 = new JoystickButton(this.joystick, ButtonID.L3.getId()); + this.R3 = new JoystickButton(this.joystick, ButtonID.R3.getId()); + + this.POV_UP = new POVButton(this.joystick, ButtonID.POV_UP.getId()); + this.POV_RIGHT = new POVButton(this.joystick, ButtonID.POV_RIGHT.getId()); + this.POV_DOWN = new POVButton(this.joystick, ButtonID.POV_DOWN.getId()); + this.POV_LEFT = new POVButton(this.joystick, ButtonID.POV_LEFT.getId()); + } + + /** + * @param power the power to rumble the joystick between [-1, 1] + */ + public void setRumble(GenericHID.RumbleType rumbleSide, double power) { + joystick.setRumble(rumbleSide, power); + } + + public void stopRumble(GenericHID.RumbleType rumbleSide) { + setRumble(rumbleSide, 0); + } + + /** + * Sample axis value with parabolic curve, allowing for finer control for smaller values. + */ + public double getSensitiveAxisValue(Axis axis) { + return sensitiveValue(getAxisValue(axis), SENSITIVE_AXIS_VALUE_POWER); + } + + private static double sensitiveValue(double axisValue, double power) { + return Math.pow(Math.abs(axisValue), power) * Math.signum(axisValue); + } + + public double getAxisValue(Axis axis) { + return isStickAxis(axis) ? applyDeadzone(axis.getValue(joystick), deadzone) : axis.getValue(joystick); + } + + private static double applyDeadzone(double power, double deadzone) { + return MathUtil.applyDeadband(power, deadzone); + } + + public AxisButton getAxisAsButton(Axis axis) { + return getAxisAsButton(axis, DEFAULT_THRESHOLD_FOR_AXIS_BUTTON); + } + + public AxisButton getAxisAsButton(Axis axis, double threshold) { + return axis.getAsButton(joystick, threshold); + } + + private static boolean isStickAxis(Axis axis) { + return (axis != Axis.LEFT_TRIGGER) && (axis != Axis.RIGHT_TRIGGER); + } + +}