diff --git a/src/main/java/training/GeneralConstants.java b/src/main/java/training/GeneralConstants.java new file mode 100644 index 00000000..8c6417ee --- /dev/null +++ b/src/main/java/training/GeneralConstants.java @@ -0,0 +1,7 @@ +package training; + +public class GeneralConstants { + + public static RobotType ROBOT_TYPE = RobotType.SIMULATION; + +} diff --git a/src/main/java/training/RobotType.java b/src/main/java/training/RobotType.java new file mode 100644 index 00000000..22088752 --- /dev/null +++ b/src/main/java/training/RobotType.java @@ -0,0 +1,8 @@ +package training; + +public enum RobotType { + + SIMULATION, + SYNCOPA; + +} diff --git a/src/main/java/training/TrainingRobotManager.java b/src/main/java/training/TrainingRobotManager.java index 9a3b7de5..78a8be69 100644 --- a/src/main/java/training/TrainingRobotManager.java +++ b/src/main/java/training/TrainingRobotManager.java @@ -1,6 +1,11 @@ package training; +import edu.wpi.first.math.geometry.Rotation2d; +import training.commands.elbow.MoveElbowToAngle; +import training.subsystems.RobotArm.elbow.Elbow; import utils.DefaultRobotManager; +import utils.joysticks.JoystickPorts; +import utils.joysticks.SmartJoystick; public class TrainingRobotManager extends DefaultRobotManager { @@ -19,6 +24,12 @@ public void trainingPeriodic() { @Override public void teleopInit() { // schedule your command... + SmartJoystick smartJoystick = new SmartJoystick(JoystickPorts.MAIN); + + smartJoystick.A.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(30), Elbow.getInstance())); + smartJoystick.Y.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(50), Elbow.getInstance())); + smartJoystick.B.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(80), Elbow.getInstance())); + smartJoystick.X.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(100), Elbow.getInstance())); } 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..dc08953a --- /dev/null +++ b/src/main/java/training/commands/elbow/MoveElbowToAngle.java @@ -0,0 +1,33 @@ +package training.commands.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import training.subsystems.RobotArm.elbow.Elbow; + +public class MoveElbowToAngle extends Command { + + private Rotation2d position; + private Elbow elbow; + + public MoveElbowToAngle(Rotation2d position, Elbow elbow) { + this.position = position; + this.elbow = elbow; + } + + @Override + public void execute() { + elbow.goToAngle(position); + } + + + @Override + public boolean isFinished() { + return elbow.isAtPosition(position); + } + + @Override + public void end(boolean interrupted) { + elbow.stayInPlace(position); + } + +} \ No newline at end of file diff --git a/src/main/java/training/commands/roller/MoveRollerAtSpeed.java b/src/main/java/training/commands/roller/MoveRollerAtSpeed.java new file mode 100644 index 00000000..db974391 --- /dev/null +++ b/src/main/java/training/commands/roller/MoveRollerAtSpeed.java @@ -0,0 +1,28 @@ +package training.commands.roller; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import training.subsystems.RobotArm.roller.Roller; + +public class MoveRollerAtSpeed extends Command { + + private Roller roller; + private Rotation2d velocity; + + public MoveRollerAtSpeed(Rotation2d velocity) { + this.velocity = velocity; + } + + public void initialize() { + Roller.getInstance().moveAtSpeed(velocity.getDegrees()); + } + + public boolean isFinished() { + return roller.isAtSpeed(velocity); + } + + public void end(boolean interrupted) { + roller.moveAtSpeed(0); + } + +} 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..750b3efc --- /dev/null +++ b/src/main/java/training/commands/wrist/MoveWristToAngle.java @@ -0,0 +1,32 @@ +package training.commands.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import training.subsystems.RobotArm.wrist.Wrist; + +public class MoveWristToAngle extends Command { + + private Wrist wristSubsystem; + private Rotation2d wantedAngle; + + public MoveWristToAngle(Rotation2d wantedAngle, Wrist wristSubsystem) { + this.wantedAngle = wantedAngle; + this.wristSubsystem = wristSubsystem; + } + + @Override + public void execute() { + wristSubsystem.goToPosition(wantedAngle); + } + + @Override + public boolean isFinished() { + return wristSubsystem.isAtPosition(wantedAngle); + } + + @Override + public void end(boolean interrupted) { + wristSubsystem.stayInPlace(); + } + +} diff --git a/src/main/java/training/subsystems/RobotArm/elbow/Elbow.java b/src/main/java/training/subsystems/RobotArm/elbow/Elbow.java new file mode 100644 index 00000000..d7215e11 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/Elbow.java @@ -0,0 +1,58 @@ +package training.subsystems.RobotArm.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; +import utils.GBSubsystem; + +public class Elbow extends GBSubsystem { + + private ElbowInputsAutoLogged inputs; + private Rotation2d position; + private static Elbow instance; + private final IElbow iElbow; + + private Elbow() { + this.iElbow = ElbowFactory.create(); + this.position = new Rotation2d(ElbowConstants.BEGINNING_POSITION); + this.inputs = new ElbowInputsAutoLogged(); + } + + public static Elbow getInstance() { + if (instance == null) + instance = new Elbow(); + return instance; + } + + public void goToAngle(Rotation2d position) { + iElbow.goToAngle(position); + } + + public Rotation2d getPosition() { + return inputs.position; + } + + public Rotation2d getVelocity() { + return inputs.velocity; + } + + public boolean isAtPosition(Rotation2d position) { + return Math.abs(getPosition().minus(position).getDegrees()) <= ElbowConstants.TOLERANCE.getDegrees(); + } + + public void stayInPlace(Rotation2d position) { + iElbow.stayInPlace(); + } + + @Override + protected String getLogPath() { + return "Elbow/"; + } + + @Override + protected void subsystemPeriodic() { + iElbow.updateInputs(inputs); + Logger.processInputs(getLogPath(), inputs); + } + +} + diff --git a/src/main/java/training/subsystems/RobotArm/elbow/ElbowConstants.java b/src/main/java/training/subsystems/RobotArm/elbow/ElbowConstants.java new file mode 100644 index 00000000..dceaf73f --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/ElbowConstants.java @@ -0,0 +1,40 @@ +package training.subsystems.RobotArm.elbow; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; + +public class ElbowConstants { + + public static final double KP = 30, KI = 0, KD = 15, KS = 0, KA = 0, KV = 0, KG = 0, ARE_FEED_FORWARDS = 0; + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3); + public static final double BEGINNING_POSITION = 0; + + 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; + } + + } + + protected static ArmFeedforward ARM_FEED_FORWARD = new ArmFeedforward(KS, KG, KV, KA); + + public static final int MOTOR_ID = 0, PID_SLOT = 0; + + public static final double ARM_LENGTH = 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 double ARM_MASS_KG = 0.44; + +} diff --git a/src/main/java/training/subsystems/RobotArm/elbow/ElbowFactory.java b/src/main/java/training/subsystems/RobotArm/elbow/ElbowFactory.java new file mode 100644 index 00000000..558d2765 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/ElbowFactory.java @@ -0,0 +1,14 @@ +package training.subsystems.RobotArm.elbow; + +import training.GeneralConstants; + +public class ElbowFactory { + + public static IElbow create() { + return switch (GeneralConstants.ROBOT_TYPE) { + case SYNCOPA -> new ElbowNEO(); + case SIMULATION -> new ElbowSimulation(); + }; + } + +} diff --git a/src/main/java/training/subsystems/RobotArm/elbow/ElbowInputs.java b/src/main/java/training/subsystems/RobotArm/elbow/ElbowInputs.java new file mode 100644 index 00000000..5e022361 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/ElbowInputs.java @@ -0,0 +1,13 @@ +package training.subsystems.RobotArm.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/RobotArm/elbow/ElbowNEO.java b/src/main/java/training/subsystems/RobotArm/elbow/ElbowNEO.java new file mode 100644 index 00000000..8fe1f9d8 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/ElbowNEO.java @@ -0,0 +1,50 @@ +package training.subsystems.RobotArm.elbow; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; + +public class ElbowNEO implements IElbow { + + private final CANSparkMax motor; + private Rotation2d position; + + public ElbowNEO() { + this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + this.position = new Rotation2d(ElbowConstants.BEGINNING_POSITION); + motor.getPIDController().setP(ElbowConstants.KP); + motor.getPIDController().setI(ElbowConstants.KI); + motor.getPIDController().setD(ElbowConstants.KD); + } + + @Override + public void goToAngle(Rotation2d position) { + motor.getPIDController() + .setReference( + position.getDegrees(), + CANSparkBase.ControlType.kPosition, + ElbowConstants.PID_SLOT, + ElbowConstants.ARM_FEED_FORWARD.calculate(getPosition().getDegrees(), getVelocity().getDegrees()) + ); + } + + public Rotation2d getPosition() { + return Rotation2d.fromDegrees(motor.getEncoder().getPosition()); + } + + public Rotation2d getVelocity() { + return Rotation2d.fromDegrees(motor.getEncoder().getVelocity()); + } + + @Override + public void stayInPlace() { + goToAngle(position); + } + + @Override + public void updateInputs(ElbowInputsAutoLogged inputs) { + inputs.position = getPosition(); + } + +} diff --git a/src/main/java/training/subsystems/RobotArm/elbow/ElbowSimulation.java b/src/main/java/training/subsystems/RobotArm/elbow/ElbowSimulation.java new file mode 100644 index 00000000..e77af756 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/ElbowSimulation.java @@ -0,0 +1,62 @@ +package training.subsystems.RobotArm.elbow; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +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 org.littletonrobotics.junction.Logger; +import utils.simulation.SingleJointedArmSimulation; + +public class ElbowSimulation implements IElbow { + + private SingleJointedArmSimulation motor; + private Rotation2d position; + private PIDController controller; + + public ElbowSimulation() { + SingleJointedArmSim armSim = new SingleJointedArmSim( + DCMotor.getFalcon500(ElbowSimulationConstants.NUMBER_OF_MOTORS), + ElbowSimulationConstants.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() + ) { + + }; + this.motor = new SingleJointedArmSimulation(armSim); + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = ElbowConstants.KP; + config.Slot0.kI = ElbowConstants.KI; + config.Slot0.kD = ElbowConstants.KD; + motor.applyConfiguration(config); + } + + public Rotation2d getPosition() { + return motor.getPosition(); + } + + public void goToAngle(Rotation2d targetAngle) { + PositionVoltage pos = new PositionVoltage(targetAngle.getRotations()); + motor.setControl(pos); + } + + public void stop() { + goToAngle(getPosition()); + } + + @Override + public void stayInPlace() { + goToAngle(getPosition()); + } + + @Override + public void updateInputs(ElbowInputsAutoLogged inputs) { + inputs.position = getPosition(); + } + +} diff --git a/src/main/java/training/subsystems/RobotArm/elbow/ElbowSimulationConstants.java b/src/main/java/training/subsystems/RobotArm/elbow/ElbowSimulationConstants.java new file mode 100644 index 00000000..2df447f4 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/ElbowSimulationConstants.java @@ -0,0 +1,10 @@ +package training.subsystems.RobotArm.elbow; + +public class ElbowSimulationConstants { + + public static final int NUMBER_OF_MOTORS = 1; + + public static final double GEAR_RATIO = 1; + final static double P = 5, I = 0, D = 0; + +} diff --git a/src/main/java/training/subsystems/RobotArm/elbow/IElbow.java b/src/main/java/training/subsystems/RobotArm/elbow/IElbow.java new file mode 100644 index 00000000..823fceaf --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/elbow/IElbow.java @@ -0,0 +1,13 @@ +package training.subsystems.RobotArm.elbow; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IElbow { + + void goToAngle(Rotation2d position); + + void stayInPlace(); + + void updateInputs(ElbowInputsAutoLogged inputs); + +} diff --git a/src/main/java/training/subsystems/RobotArm/roller/IRoller.java b/src/main/java/training/subsystems/RobotArm/roller/IRoller.java new file mode 100644 index 00000000..fbdafe82 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/roller/IRoller.java @@ -0,0 +1,11 @@ +package training.subsystems.RobotArm.roller; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IRoller { + + void moveAtSpeed(double velocity); + + void updateInputs(RollerInputsAutoLogged inputs); + +} diff --git a/src/main/java/training/subsystems/RobotArm/roller/Roller.java b/src/main/java/training/subsystems/RobotArm/roller/Roller.java new file mode 100644 index 00000000..f4418f0c --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/roller/Roller.java @@ -0,0 +1,48 @@ +package training.subsystems.RobotArm.roller; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; +import utils.GBSubsystem; + +public class Roller extends GBSubsystem { + + private RollerInputsAutoLogged inputs; + private CANSparkMax motor; + private static Roller instance; + + private Roller() { + this.motor = new CANSparkMax(RollerConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + motor.getPIDController().setP(RollerConstants.KP); + motor.getPIDController().setI(RollerConstants.KI); + motor.getPIDController().setD(RollerConstants.KD); + } + + public static Roller getInstance() { + if (instance != null) + instance = new Roller(); + return instance; + } + + public void moveAtSpeed(double velocity) { + motor.getPIDController().setReference(velocity, CANSparkBase.ControlType.kVelocity, RollerConstants.PID_SLOT); + } + + + public Rotation2d getVelocity() { + return Rotation2d.fromDegrees(motor.getEncoder().getVelocity()); + } + + + public boolean isAtSpeed(Rotation2d velocity) { + return (Math.abs(getVelocity().minus(velocity).getDegrees()) <= RollerConstants.TOLERANCE.getDegrees()); + } + + protected String getLogPath() { + return "Roller/"; + } + + protected void subsystemPeriodic() {} + +} diff --git a/src/main/java/training/subsystems/RobotArm/roller/RollerConstants.java b/src/main/java/training/subsystems/RobotArm/roller/RollerConstants.java new file mode 100644 index 00000000..7d8bef23 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/roller/RollerConstants.java @@ -0,0 +1,11 @@ +package training.subsystems.RobotArm.roller; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class RollerConstants { + + public static final double KP = 20, KI = 0, KD = 0, KS = 0, KA = 0, KV = 0, KG = 0, ARM_FEED_FOWORDS = 0; + public static final int MOTOR_ID = 0, PID_SLOT = 0; + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3); + +} diff --git a/src/main/java/training/subsystems/RobotArm/roller/RollerInputs.java b/src/main/java/training/subsystems/RobotArm/roller/RollerInputs.java new file mode 100644 index 00000000..43f42ad9 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/roller/RollerInputs.java @@ -0,0 +1,13 @@ +package training.subsystems.RobotArm.roller; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +@AutoLog +public class RollerInputs { + + public Rotation2d position; + + public Rotation2d velocity; + +} diff --git a/src/main/java/training/subsystems/RobotArm/roller/RollerNEO.java b/src/main/java/training/subsystems/RobotArm/roller/RollerNEO.java new file mode 100644 index 00000000..14646cf3 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/roller/RollerNEO.java @@ -0,0 +1,45 @@ +package training.subsystems.RobotArm.roller; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; + +public class RollerNEO implements IRoller { + + private CANSparkMax motor; + private static RollerNEO instance; + + private RollerNEO() { + this.motor = new CANSparkMax(RollerConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + motor.getPIDController().setP(RollerConstants.KP); + motor.getPIDController().setI(RollerConstants.KI); + motor.getPIDController().setD(RollerConstants.KD); + } + + + public void moveAtSpeed(double velocity) { + motor.getPIDController().setReference(velocity, CANSparkBase.ControlType.kVelocity, RollerConstants.PID_SLOT); + } + + + public Rotation2d getPosition() { + return Rotation2d.fromDegrees(motor.getEncoder().getPosition()); + } + + public Rotation2d getVelocity() { + return Rotation2d.fromDegrees(motor.getEncoder().getVelocity()); + } + + @Override + public void updateInputs(RollerInputsAutoLogged inputs) { + inputs.position = getPosition(); + } + + protected String getLogPath() { + return "Roller/"; + } + + protected void subsystemPeriodic() {} + +} diff --git a/src/main/java/training/subsystems/RobotArm/wrist/IWrist.java b/src/main/java/training/subsystems/RobotArm/wrist/IWrist.java new file mode 100644 index 00000000..1d316801 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/wrist/IWrist.java @@ -0,0 +1,9 @@ +package training.subsystems.RobotArm.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IWrist { + + void goToPosition(Rotation2d velocity); + +} diff --git a/src/main/java/training/subsystems/RobotArm/wrist/Wrist.java b/src/main/java/training/subsystems/RobotArm/wrist/Wrist.java new file mode 100644 index 00000000..fe11dc26 --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/wrist/Wrist.java @@ -0,0 +1,51 @@ +package training.subsystems.RobotArm.wrist; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.math.geometry.Rotation2d; +import utils.GBSubsystem; + +public class Wrist extends GBSubsystem { + + private TalonSRX Wrist; + private Rotation2d position; + private static Wrist instance; + + public Wrist() { + this.Wrist = new TalonSRX(WristConstans.MOTOR_ID); + this.position = new Rotation2d(); + Wrist.configAllSettings(WristConstans.TALON_SRX_CONFIGURATION); + Wrist.configSelectedFeedbackSensor( + FeedbackDevice.CTRE_MagEncoder_Relative, + WristConstans.PID_SLOT, + WristConstans.TIME_OUT_FOR_CONFIGS_SET + ); + } + + public Rotation2d getPosition() { + return Rotation2d.fromDegrees(Wrist.getSelectedSensorPosition()); + } + + public void goToPosition(Rotation2d position) { + Wrist.selectProfileSlot(WristConstans.PID_SLOT, 0); + Wrist.set(ControlMode.Position, position.getRotations() * WristConstans.MAG_ENCODER_CONVERSION_FACTOR); + } + + public boolean isAtPosition(Rotation2d position) { + return (position == getPosition()); + } + + public void stayInPlace() { + goToPosition(getPosition()); + } + + @Override + protected String getLogPath() { + return "Wrist/"; + } + + @Override + protected void subsystemPeriodic() {} + +} diff --git a/src/main/java/training/subsystems/RobotArm/wrist/WristConstans.java b/src/main/java/training/subsystems/RobotArm/wrist/WristConstans.java new file mode 100644 index 00000000..20fcdb4e --- /dev/null +++ b/src/main/java/training/subsystems/RobotArm/wrist/WristConstans.java @@ -0,0 +1,18 @@ +package training.subsystems.RobotArm.wrist; + +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; + +public class WristConstans { + + public static final double KP = 20, KI = 0, KD = 0, KS = 0, KA = 0, KV = 0, KG = 0, ARB_FEED_FOWORDS = 0, + MAG_ENCODER_CONVERSION_FACTOR = 8192; + public static final int MOTOR_ID = 0, PID_SLOT = 0, TIME_OUT_FOR_CONFIGS_SET = 0; + public static final TalonSRXConfiguration TALON_SRX_CONFIGURATION = new TalonSRXConfiguration(); + + static { + TALON_SRX_CONFIGURATION.slot0.kP = KP; + TALON_SRX_CONFIGURATION.slot0.kI = KI; + TALON_SRX_CONFIGURATION.slot0.kD = KD; + } + +} 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..ec72acb4 --- /dev/null +++ b/src/main/java/utils/joysticks/ButtonID.java @@ -0,0 +1,34 @@ +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); + } + +} diff --git a/src/main/python/__pycache__/NetworkTableManager.cpython-312.pyc b/src/main/python/__pycache__/NetworkTableManager.cpython-312.pyc new file mode 100644 index 00000000..b2d9407f Binary files /dev/null and b/src/main/python/__pycache__/NetworkTableManager.cpython-312.pyc differ