From 13fc601a908ea5e50e70254771ca9069d0f8a51f Mon Sep 17 00:00:00 2001 From: Ananth Date: Thu, 10 Sep 2026 18:28:12 -0400 Subject: [PATCH 1/3] added all folders (didn't add util) --- lib/build.gradle | 4 +- .../elevator/ElevatorConstants.java | 14 +- .../elevator/ElevatorIOTalonFXSim.java | 61 --- .../subsystems/elevator/ElevatorState.java | 7 - .../extension/ElevatorConstants.java | 48 ++ .../Extension.java} | 48 +- .../extension/ExtensionConstants.java | 48 ++ .../ExtensionIO.java} | 28 +- .../ExtensionIOSim.java} | 48 +- .../ExtensionIOTalonFX.java} | 28 +- .../extension/ExtensionIOTalonFXSim.java | 61 +++ .../subsystems/extension/ExtensionState.java | 7 + .../elevator/ElevatorStateTest.java | 19 - ...sTest.java => ExtensionConstantsTest.java} | 30 +- ...IOSimTest.java => ExtensionIOSimTest.java} | 30 +- ...st.java => ExtensionIOTalonFXSimTest.java} | 30 +- ...XTest.java => ExtensionIOTalonFXTest.java} | 30 +- ...evatorIOTest.java => ExtensionIOTest.java} | 12 +- .../elevator/ExtensionStateTest.java | 21 + .../{ElevatorTest.java => ExtensionTest.java} | 72 +-- src/main/java/first/robot/Constants.java | 0 src/main/java/first/robot/FieldConstants.java | 421 ++++++++++++++++++ .../first/robot/util/AllianceFlipUtil.java | 62 +++ .../V3_Horse_CV2_OPRCompositeCommands.java | 0 .../V3_Horse_CV2_OPRAutoLeftTrench.java | 0 .../subsystems/V3_Horse_CV2_OPRConstants.java | 0 .../V3_Horse_CV2_OPRMechanism3D.java | 0 .../V3_Horse_CV2_OPRRobotContainer.java | 0 .../V3_Horse_CV2_OPRRobotState.java | 0 .../V3_Horse_CV2_OPRTunerConstants.java | 0 .../V3_Horse_CV2_TRCompositeCommands.java | 0 .../V3_Horse_CV2_TRAutoLeftTrench.java | 0 .../subsystems/V3_Horse_CV2_TRConstants.java | 0 .../V3_Horse_CV2_TRMechanism3D.java | 0 .../V3_Horse_CV2_TRRobotContainer.java | 0 .../subsystems/V3_Horse_CV2_TRRobotState.java | 0 .../V3_Horse_CV2_TRTunerConstants.java | 0 .../V3_Horse_CV3_OPRCompositeCommands.java | 0 .../V3_Horse_CV3_OPRAutoLeftTrench.java | 0 .../subsystems/V3_Horse_CV3_OPRConstants.java | 0 .../V3_Horse_CV3_OPRMechanism3D.java | 0 .../V3_Horse_CV3_OPRRobotContainer.java | 0 .../V3_Horse_CV3_OPRRobotState.java | 0 .../V3_Horse_CV3_OPRTunerConstants.java | 0 .../V3_Horse_CV3_TRCompositeCommands.java | 0 .../V3_Horse_CV3_TRAutoLeftTrench.java | 0 .../subsystems/V3_Horse_CV3_TRConstants.java | 0 .../V3_Horse_CV3_TRMechanism3D.java | 0 .../V3_Horse_CV3_TRRobotContainer.java | 0 .../subsystems/V3_Horse_CV3_TRRobotState.java | 0 .../V3_Horse_CV3_TRTunerConstants.java | 0 51 files changed, 865 insertions(+), 264 deletions(-) delete mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java delete mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java create mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java rename lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/{elevator/Elevator.java => extension/Extension.java} (81%) create mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstants.java rename lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/{elevator/ElevatorIO.java => extension/ExtensionIO.java} (74%) rename lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/{elevator/ElevatorIOSim.java => extension/ExtensionIOSim.java} (76%) rename lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/{elevator/ElevatorIOTalonFX.java => extension/ExtensionIOTalonFX.java} (92%) create mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSim.java create mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionState.java delete mode 100644 lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorStateTest.java rename lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/{ElevatorConstantsTest.java => ExtensionConstantsTest.java} (71%) rename lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/{ElevatorIOSimTest.java => ExtensionIOSimTest.java} (82%) rename lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/{ElevatorIOTalonFXSimTest.java => ExtensionIOTalonFXSimTest.java} (88%) rename lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/{ElevatorIOTalonFXTest.java => ExtensionIOTalonFXTest.java} (90%) rename lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/{ElevatorIOTest.java => ExtensionIOTest.java} (84%) create mode 100644 lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionStateTest.java rename lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/{ElevatorTest.java => ExtensionTest.java} (66%) create mode 100644 src/main/java/first/robot/Constants.java create mode 100644 src/main/java/first/robot/FieldConstants.java create mode 100644 src/main/java/first/robot/util/AllianceFlipUtil.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java create mode 100644 src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java create mode 100644 src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java diff --git a/lib/build.gradle b/lib/build.gradle index 59f0480..33f11d5 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -194,7 +194,7 @@ jacocoTestReport { '**/SwerveDrive*.class', '**/SwerveModule*.class', '**/GenericFlywheel*.class', - '**/Elevator*.class', + '**/Extension*.class', '**/Arm*.class', '**/Vision*.class', '**/Gains*.class', @@ -261,7 +261,7 @@ jacocoTestCoverageVerification { '**/SwerveDrive*.class', '**/SwerveModule*.class', '**/GenericFlywheel*.class', - '**/Elevator*.class', + '**/Extension*.class', '**/Arm*.class', '**/Vision*.class', '**/Gains*.class', diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java index de74e2f..25a99d2 100644 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import com.ctre.phoenix6.CANBus; import edu.wpi.team190.gompeilib.core.utility.control.Gains; @@ -12,16 +12,16 @@ import org.wpilib.units.measure.Voltage; @Builder(setterPrefix = "with") -public class ElevatorConstants { +public class ExtensionConstants { @NonNull public final Integer leaderCANID; @NonNull public final CANBus canBus = new CANBus(); - @NonNull public final Double elevatorGearRatio; + @NonNull public final Double extensionGearRatio; @NonNull public final Double drumRadius; - @NonNull public final Double elevatorSupplyCurrentLimit; - @NonNull public final Double elevatorStatorCurrentLimit; + @NonNull public final Double extensionSupplyCurrentLimit; + @NonNull public final Double extensionStatorCurrentLimit; - @NonNull public final ElevatorParameters elevatorParameters; + @NonNull public final ExtensionParameters extensionParameters; @NonNull public final Gains slot0Gains; @Builder.Default public final Gains slot1Gains = Gains.builder().build(); @Builder.Default public final Gains slot2Gains = Gains.builder().build(); @@ -39,7 +39,7 @@ public class ElevatorConstants { @NonNull public final Distance heightOffsetStep; @Builder(setterPrefix = "with") - public record ElevatorParameters( + public record ExtensionParameters( @NonNull DCMotor ELEVATOR_MOTOR_CONFIG, @NonNull Double CARRIAGE_MASS_KG, @NonNull Distance MIN_HEIGHT, diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java deleted file mode 100644 index 21372bb..0000000 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java +++ /dev/null @@ -1,61 +0,0 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; - -import static org.wpilib.units.Units.*; -import static org.wpilib.units.Units.Meters; - -import com.ctre.phoenix6.sim.TalonFXSimState; -import edu.wpi.team190.gompeilib.core.GompeiLib; -import edu.wpi.team190.gompeilib.core.logging.Trace; -import org.wpilib.math.system.Models; -import org.wpilib.simulation.ElevatorSim; -import org.wpilib.system.RobotController; -import org.wpilib.units.measure.Angle; -import org.wpilib.units.measure.AngularVelocity; - -public class ElevatorIOTalonFXSim extends ElevatorIOTalonFX { - private final ElevatorSim elevatorSim; - - private final TalonFXSimState elevatorController; - - public ElevatorIOTalonFXSim(ElevatorConstants constants) { - super(constants); - elevatorSim = - new ElevatorSim( - Models.elevatorFromPhysicalConstants( - constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(), - constants.elevatorParameters.CARRIAGE_MASS_KG(), - constants.drumRadius, - constants.elevatorGearRatio), - constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(), - constants.elevatorParameters.MIN_HEIGHT().in(Meters), - constants.elevatorParameters.MAX_HEIGHT().in(Meters), - true, - constants.elevatorParameters.MIN_HEIGHT().in(Meters)); - - elevatorController = super.talonFX.getSimState(); - } - - @Override - @Trace - public void updateInputs(ElevatorIOInputs inputs) { - elevatorController.setSupplyVoltage(RobotController.getBatteryVoltage()); - double elevatorVoltage = elevatorController.getMotorVoltage(); - - elevatorSim.setInputVoltage(elevatorVoltage); - - elevatorSim.update(GompeiLib.getLoopPeriod()); - - Angle rotorPosition = - Angle.ofBaseUnits( - elevatorSim.getPosition() * constants.elevatorGearRatio * constants.drumRadius, - Radians); - AngularVelocity rotorVelocity = - AngularVelocity.ofBaseUnits( - elevatorSim.getVelocity() * constants.elevatorGearRatio * constants.drumRadius, - RadiansPerSecond); - elevatorController.setRawRotorPosition(rotorPosition); - elevatorController.setRotorVelocity(rotorVelocity); - - super.updateInputs(inputs); - } -} diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java deleted file mode 100644 index b09c07e..0000000 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java +++ /dev/null @@ -1,7 +0,0 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; - -public enum ElevatorState { - IDLE, - OPEN_LOOP_VOLTAGE_CONTROL, - CLOSED_LOOP_POSITION_CONTROL; -} diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java new file mode 100644 index 0000000..25a99d2 --- /dev/null +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java @@ -0,0 +1,48 @@ +package edu.wpi.team190.gompeilib.subsystems.extension; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.team190.gompeilib.core.utility.control.Gains; +import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; +import java.util.Set; +import lombok.Builder; +import lombok.NonNull; +import lombok.Singular; +import org.wpilib.math.system.DCMotor; +import org.wpilib.units.measure.Distance; +import org.wpilib.units.measure.Voltage; + +@Builder(setterPrefix = "with") +public class ExtensionConstants { + @NonNull public final Integer leaderCANID; + @NonNull public final CANBus canBus = new CANBus(); + @NonNull public final Double extensionGearRatio; + @NonNull public final Double drumRadius; + + @NonNull public final Double extensionSupplyCurrentLimit; + @NonNull public final Double extensionStatorCurrentLimit; + + @NonNull public final ExtensionParameters extensionParameters; + @NonNull public final Gains slot0Gains; + @Builder.Default public final Gains slot1Gains = Gains.builder().build(); + @Builder.Default public final Gains slot2Gains = Gains.builder().build(); + @NonNull public final LinearConstraints constraints; + + @Singular(value = "alignedFollowerCANID") + @NonNull + public final Set alignedFollowerCANIDs; + + @Singular(value = "opposedFollowerCANID") + @NonNull + public final Set opposedFollowerCANIDs; + + @NonNull public final Voltage voltageOffsetStep; + @NonNull public final Distance heightOffsetStep; + + @Builder(setterPrefix = "with") + public record ExtensionParameters( + @NonNull DCMotor ELEVATOR_MOTOR_CONFIG, + @NonNull Double CARRIAGE_MASS_KG, + @NonNull Distance MIN_HEIGHT, + @NonNull Distance MAX_HEIGHT, + @NonNull Integer NUM_MOTORS) {} +} diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/Elevator.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/Extension.java similarity index 81% rename from lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/Elevator.java rename to lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/Extension.java index 25efc44..86f4ad2 100644 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/Elevator.java +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/Extension.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.wpilib.units.Units.*; @@ -17,34 +17,34 @@ import org.wpilib.units.measure.Distance; import org.wpilib.units.measure.Voltage; -public class Elevator { - public final ElevatorIO io; - public final ElevatorIOInputsAutoLogged inputs; +public class Extension { + public final ExtensionIO io; + public final ExtensionIOInputsAutoLogged inputs; private final String aKitTopic; - private ElevatorState currentState; + private ExtensionState currentState; private Setpoint voltageGoal; private Setpoint positionGoal; private final SysIdRoutine characterizationRoutine; - public final ElevatorConstants constants; + public final ExtensionConstants constants; - public Elevator( - ElevatorConstants constants, + public Extension( + ExtensionConstants constants, Subsystem subsystem, int index, - ElevatorIO io, + ExtensionIO io, Setpoint positionGoal, Setpoint voltageGoal) { this.io = io; - this.inputs = new ElevatorIOInputsAutoLogged(); + this.inputs = new ExtensionIOInputsAutoLogged(); - aKitTopic = subsystem.getName() + "/Elevators" + index; + aKitTopic = subsystem.getName() + "/Extensions" + index; - currentState = ElevatorState.IDLE; + currentState = ExtensionState.IDLE; this.positionGoal = positionGoal; this.voltageGoal = voltageGoal; @@ -61,7 +61,7 @@ public Elevator( this.constants = constants; } - public Elevator(ElevatorConstants constants, Subsystem subsystem, int index, ElevatorIO io) { + public Extension(ExtensionConstants constants, Subsystem subsystem, int index, ExtensionIO io) { this( constants, subsystem, @@ -70,16 +70,16 @@ public Elevator(ElevatorConstants constants, Subsystem subsystem, int index, Ele new Setpoint<>( Meters.of(0), constants.heightOffsetStep, - constants.elevatorParameters.MIN_HEIGHT(), - constants.elevatorParameters.MAX_HEIGHT()), + constants.extensionParameters.MIN_HEIGHT(), + constants.extensionParameters.MAX_HEIGHT()), new Setpoint<>(Volts.of(0), constants.voltageOffsetStep, Volts.of(-12), Volts.of(12))); } - public Elevator( - ElevatorConstants constants, + public Extension( + ExtensionConstants constants, Subsystem subsystem, int index, - ElevatorIO io, + ExtensionIO io, Setpoint positionGoal) { this( constants, @@ -110,27 +110,27 @@ public void periodic() { } } - public Distance getElevatorPosition() { + public Distance getExtensionPosition() { return inputs.position; } public void setVoltageGoal(Voltage voltageGoal) { - currentState = ElevatorState.OPEN_LOOP_VOLTAGE_CONTROL; + currentState = ExtensionState.OPEN_LOOP_VOLTAGE_CONTROL; this.voltageGoal.setSetpoint(voltageGoal); } public void setPositionGoal(Distance positionGoal) { - currentState = ElevatorState.CLOSED_LOOP_POSITION_CONTROL; + currentState = ExtensionState.CLOSED_LOOP_POSITION_CONTROL; this.positionGoal.setSetpoint(positionGoal); } public void setVoltageGoal(Setpoint voltageGoal) { - currentState = ElevatorState.OPEN_LOOP_VOLTAGE_CONTROL; + currentState = ExtensionState.OPEN_LOOP_VOLTAGE_CONTROL; this.voltageGoal = voltageGoal; } public void setPositionGoal(Setpoint positionGoal) { - currentState = ElevatorState.CLOSED_LOOP_POSITION_CONTROL; + currentState = ExtensionState.CLOSED_LOOP_POSITION_CONTROL; this.positionGoal = positionGoal; } @@ -174,7 +174,7 @@ public void updateConstraints(LinearConstraints constraints) { public Command runSysIdRoutine() { return Commands.sequence( - Commands.runOnce(() -> currentState = ElevatorState.IDLE), + Commands.runOnce(() -> currentState = ExtensionState.IDLE), characterizationRoutine.quasistatic(SysIdRoutine.Direction.kForward), Commands.waitSeconds(1.0), characterizationRoutine.quasistatic(SysIdRoutine.Direction.kReverse), diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstants.java new file mode 100644 index 0000000..25a99d2 --- /dev/null +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstants.java @@ -0,0 +1,48 @@ +package edu.wpi.team190.gompeilib.subsystems.extension; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.team190.gompeilib.core.utility.control.Gains; +import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; +import java.util.Set; +import lombok.Builder; +import lombok.NonNull; +import lombok.Singular; +import org.wpilib.math.system.DCMotor; +import org.wpilib.units.measure.Distance; +import org.wpilib.units.measure.Voltage; + +@Builder(setterPrefix = "with") +public class ExtensionConstants { + @NonNull public final Integer leaderCANID; + @NonNull public final CANBus canBus = new CANBus(); + @NonNull public final Double extensionGearRatio; + @NonNull public final Double drumRadius; + + @NonNull public final Double extensionSupplyCurrentLimit; + @NonNull public final Double extensionStatorCurrentLimit; + + @NonNull public final ExtensionParameters extensionParameters; + @NonNull public final Gains slot0Gains; + @Builder.Default public final Gains slot1Gains = Gains.builder().build(); + @Builder.Default public final Gains slot2Gains = Gains.builder().build(); + @NonNull public final LinearConstraints constraints; + + @Singular(value = "alignedFollowerCANID") + @NonNull + public final Set alignedFollowerCANIDs; + + @Singular(value = "opposedFollowerCANID") + @NonNull + public final Set opposedFollowerCANIDs; + + @NonNull public final Voltage voltageOffsetStep; + @NonNull public final Distance heightOffsetStep; + + @Builder(setterPrefix = "with") + public record ExtensionParameters( + @NonNull DCMotor ELEVATOR_MOTOR_CONFIG, + @NonNull Double CARRIAGE_MASS_KG, + @NonNull Distance MIN_HEIGHT, + @NonNull Distance MAX_HEIGHT, + @NonNull Integer NUM_MOTORS) {} +} diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIO.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIO.java similarity index 74% rename from lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIO.java rename to lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIO.java index 1ea0f5b..2101b56 100644 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIO.java +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIO.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.wpilib.units.Units.*; @@ -8,10 +8,10 @@ import org.littletonrobotics.junction.AutoLog; import org.wpilib.units.measure.*; -public interface ElevatorIO { +public interface ExtensionIO { @AutoLog - public static class ElevatorIOInputs { + public static class ExtensionIOInputs { public Distance position = Meters.of(0.0); public LinearVelocity velocity = MetersPerSecond.of(0.0); public LinearAcceleration acceleration = MetersPerSecondPerSecond.of(0.0); @@ -29,28 +29,28 @@ public static class ElevatorIOInputs { } /** - * Updates the inputs for the elevator. + * Updates the inputs for the extension. * * @param inputs The inputs to update. */ - default void updateInputs(ElevatorIOInputs inputs) {} + default void updateInputs(ExtensionIOInputs inputs) {} /** - * Sets the voltage for the elevator. + * Sets the voltage for the extension. * - * @param voltageGoal The voltage of the elevator in volts. + * @param voltageGoal The voltage of the extension in volts. */ default void setVoltageGoal(Voltage voltageGoal) {} /** - * Sets the position goal of the elevator. + * Sets the position goal of the extension. * - * @param positionGoal The position goal of the elevator in meters. + * @param positionGoal The position goal of the extension in meters. */ default void setPositionGoal(Distance positionGoal) {} /** - * Checks if the voltage of the elevator matches the volts argument + * Checks if the voltage of the extension matches the volts argument * * @param voltageReference The voltage to check against * @return True if the voltage matches, false otherwise @@ -70,26 +70,26 @@ default boolean atPositionGoal(Distance positionReference) { } /** - * Sets the position of the elevator. + * Sets the position of the extension. * * @param position The position to set. */ default void setPosition(Distance position) {} /** - * @param gainSlot The CTRE gain slot to set the elevator to. + * @param gainSlot The CTRE gain slot to set the extension to. */ default void setGainSlot(GainSlot gainSlot) {} /** - * Sets the gains for the elevator. + * Sets the gains for the extension. * * @param gains the gains to update */ default void updateGains(Gains gains, GainSlot gainSlot) {} /** - * Sets the constraints for the elevator. + * Sets the constraints for the extension. * * @param constraints the constraints to update */ diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java similarity index 76% rename from lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSim.java rename to lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java index 8cd30d7..902336c 100644 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSim.java +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.wpilib.units.Units.*; @@ -7,38 +7,38 @@ import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; import java.util.Arrays; -import org.wpilib.math.controller.ElevatorFeedforward; +import org.wpilib.math.controller.ExtensionFeedforward; import org.wpilib.math.controller.ProfiledPIDController; import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrapezoidProfile; -import org.wpilib.simulation.ElevatorSim; +import org.wpilib.simulation.ExtensionSim; import org.wpilib.units.measure.*; -public class ElevatorIOSim implements ElevatorIO { - private final ElevatorSim sim; +public class ExtensionIOSim implements ExtensionIO { + private final ExtensionSim sim; private Voltage appliedVolts; private boolean isClosedLoop; private GainSlot gainSlot; private final ProfiledPIDController feedback; - private ElevatorFeedforward feedforward; + private ExtensionFeedforward feedforward; - private final ElevatorConstants constants; + private final ExtensionConstants constants; - public ElevatorIOSim(ElevatorConstants constants) { + public ExtensionIOSim(ExtensionConstants constants) { sim = - new ElevatorSim( - Models.elevatorFromPhysicalConstants( - constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(), - constants.elevatorParameters.CARRIAGE_MASS_KG(), + new ExtensionSim( + Models.extensionFromPhysicalConstants( + constants.extensionParameters.ELEVATOR_MOTOR_CONFIG(), + constants.extensionParameters.CARRIAGE_MASS_KG(), constants.drumRadius, - constants.elevatorGearRatio), - constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(), - constants.elevatorParameters.MIN_HEIGHT().in(Meters), - constants.elevatorParameters.MAX_HEIGHT().in(Meters), + constants.extensionGearRatio), + constants.extensionParameters.ELEVATOR_MOTOR_CONFIG(), + constants.extensionParameters.MIN_HEIGHT().in(Meters), + constants.extensionParameters.MAX_HEIGHT().in(Meters), true, - constants.elevatorParameters.MIN_HEIGHT().in(Meters)); + constants.extensionParameters.MIN_HEIGHT().in(Meters)); appliedVolts = Volts.of(0.0); isClosedLoop = true; @@ -54,7 +54,7 @@ public ElevatorIOSim(ElevatorConstants constants) { constants.constraints.maxAcceleration().get().in(MetersPerSecondPerSecond))); feedforward = - new ElevatorFeedforward( + new ExtensionFeedforward( constants.slot0Gains.kS().get(), constants.slot0Gains.kG().get(), constants.slot0Gains.kV().get(), @@ -64,7 +64,7 @@ public ElevatorIOSim(ElevatorConstants constants) { } @Override - public void updateInputs(ElevatorIOInputs inputs) { + public void updateInputs(ExtensionIOInputs inputs) { if (isClosedLoop) { appliedVolts = Volts.of( @@ -82,10 +82,10 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.acceleration = MetersPerSecondPerSecond.of(-1.0); // TODO: Replace with calculation based on velocity - inputs.appliedVolts = new double[constants.elevatorParameters.NUM_MOTORS()]; - inputs.supplyCurrentAmps = new double[constants.elevatorParameters.NUM_MOTORS()]; - inputs.torqueCurrentAmps = new double[constants.elevatorParameters.NUM_MOTORS()]; - inputs.temperatureCelsius = new double[constants.elevatorParameters.NUM_MOTORS()]; + inputs.appliedVolts = new double[constants.extensionParameters.NUM_MOTORS()]; + inputs.supplyCurrentAmps = new double[constants.extensionParameters.NUM_MOTORS()]; + inputs.torqueCurrentAmps = new double[constants.extensionParameters.NUM_MOTORS()]; + inputs.temperatureCelsius = new double[constants.extensionParameters.NUM_MOTORS()]; Arrays.fill(inputs.appliedVolts, appliedVolts.in(Volts)); Arrays.fill(inputs.supplyCurrentAmps, sim.getCurrentDraw()); @@ -147,7 +147,7 @@ public void setGainSlot(GainSlot gainSlot) { public void updateGains(Gains gains, GainSlot gainSlot) { feedback.setPID(gains.kP().get(), gains.kI().get(), gains.kD().get()); feedforward = - new ElevatorFeedforward( + new ExtensionFeedforward( gains.kS().get(), gains.kG().get(), gains.kV().get(), gains.kA().get()); } diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFX.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFX.java similarity index 92% rename from lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFX.java rename to lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFX.java index 8543d7c..7144ac9 100644 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFX.java +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFX.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.wpilib.units.Units.*; @@ -19,7 +19,7 @@ import java.util.ArrayList; import org.wpilib.units.measure.*; -public class ElevatorIOTalonFX implements ElevatorIO { +public class ExtensionIOTalonFX implements ExtensionIO { // Core hardware components protected final TalonFX talonFX; @@ -27,7 +27,7 @@ public class ElevatorIOTalonFX implements ElevatorIO { // Configuration public final TalonFXConfiguration config; - protected final ElevatorConstants constants; + protected final ExtensionConstants constants; // Sensor inputs private StatusSignal positionRotations; @@ -48,7 +48,7 @@ public class ElevatorIOTalonFX implements ElevatorIO { private MotionMagicVoltage positionVoltageRequest; private VoltageOut voltageRequest; - public ElevatorIOTalonFX(ElevatorConstants constants) { + public ExtensionIOTalonFX(ExtensionConstants constants) { this.constants = constants; @@ -56,7 +56,7 @@ public ElevatorIOTalonFX(ElevatorConstants constants) { talonFX = new TalonFX(constants.leaderCANID, constants.canBus); // Create follower motor array (define length) - followTalonFX = new TalonFX[constants.elevatorParameters.NUM_MOTORS() - 1]; + followTalonFX = new TalonFX[constants.extensionParameters.NUM_MOTORS() - 1]; config = new TalonFXConfiguration(); config.Slot0.withKP(constants.slot0Gains.kP().get()) @@ -65,7 +65,7 @@ public ElevatorIOTalonFX(ElevatorConstants constants) { .withKV(constants.slot0Gains.kV().get()) .withKA(constants.slot0Gains.kA().get()) .withKG(constants.slot0Gains.kG().get()) - .withGravityType(GravityTypeValue.Elevator_Static); + .withGravityType(GravityTypeValue.Extension_Static); config.Slot1.withKP(constants.slot1Gains.kP().get()) .withKD(constants.slot1Gains.kD().get()) @@ -73,7 +73,7 @@ public ElevatorIOTalonFX(ElevatorConstants constants) { .withKV(constants.slot1Gains.kV().get()) .withKA(constants.slot1Gains.kA().get()) .withKG(constants.slot1Gains.kG().get()) - .withGravityType(GravityTypeValue.Elevator_Static); + .withGravityType(GravityTypeValue.Extension_Static); config.Slot2.withKP(constants.slot2Gains.kP().get()) .withKD(constants.slot2Gains.kD().get()) @@ -81,20 +81,20 @@ public ElevatorIOTalonFX(ElevatorConstants constants) { .withKV(constants.slot2Gains.kV().get()) .withKA(constants.slot2Gains.kA().get()) .withKG(constants.slot2Gains.kG().get()) - .withGravityType(GravityTypeValue.Elevator_Static); + .withGravityType(GravityTypeValue.Extension_Static); - config.CurrentLimits.withSupplyCurrentLimit(constants.elevatorSupplyCurrentLimit) + config.CurrentLimits.withSupplyCurrentLimit(constants.extensionSupplyCurrentLimit) .withSupplyCurrentLimitEnable(true) - .withStatorCurrentLimit(constants.elevatorStatorCurrentLimit) + .withStatorCurrentLimit(constants.extensionStatorCurrentLimit) .withStatorCurrentLimitEnable(true); config.Feedback.SensorToMechanismRatio = - constants.elevatorGearRatio / (2 * Math.PI * constants.drumRadius); + constants.extensionGearRatio / (2 * Math.PI * constants.drumRadius); config.SoftwareLimitSwitch.withForwardSoftLimitThreshold( - constants.elevatorParameters.MAX_HEIGHT().in(Meters)) + constants.extensionParameters.MAX_HEIGHT().in(Meters)) .withForwardSoftLimitEnable(true) - .withReverseSoftLimitThreshold(constants.elevatorParameters.MIN_HEIGHT().in(Meters)) + .withReverseSoftLimitThreshold(constants.extensionParameters.MIN_HEIGHT().in(Meters)) .withReverseSoftLimitEnable(true); config.MotionMagic.withMotionMagicAcceleration( @@ -181,7 +181,7 @@ public ElevatorIOTalonFX(ElevatorConstants constants) { } @Override - public void updateInputs(ElevatorIOInputs inputs) { + public void updateInputs(ExtensionIOInputs inputs) { // CTRE status signals are natively in rotations, but setting sensor to mechanism ratio // including the circumference of the drum allows us to transform into meters directly from diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSim.java new file mode 100644 index 0000000..9f2e7b7 --- /dev/null +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSim.java @@ -0,0 +1,61 @@ +package edu.wpi.team190.gompeilib.subsystems.extension; + +import static org.wpilib.units.Units.*; +import static org.wpilib.units.Units.Meters; + +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.team190.gompeilib.core.GompeiLib; +import edu.wpi.team190.gompeilib.core.logging.Trace; +import org.wpilib.math.system.Models; +import org.wpilib.simulation.ExtensionSim; +import org.wpilib.system.RobotController; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; + +public class ExtensionIOTalonFXSim extends ExtensionIOTalonFX { + private final ExtensionSim extensionSim; + + private final TalonFXSimState extensionController; + + public ExtensionIOTalonFXSim(ExtensionConstants constants) { + super(constants); + extensionSim = + new ExtensionSim( + Models.extensionFromPhysicalConstants( + constants.extensionParameters.ELEVATOR_MOTOR_CONFIG(), + constants.extensionParameters.CARRIAGE_MASS_KG(), + constants.drumRadius, + constants.extensionGearRatio), + constants.extensionParameters.ELEVATOR_MOTOR_CONFIG(), + constants.extensionParameters.MIN_HEIGHT().in(Meters), + constants.extensionParameters.MAX_HEIGHT().in(Meters), + true, + constants.extensionParameters.MIN_HEIGHT().in(Meters)); + + extensionController = super.talonFX.getSimState(); + } + + @Override + @Trace + public void updateInputs(ExtensionIOInputs inputs) { + extensionController.setSupplyVoltage(RobotController.getBatteryVoltage()); + double extensionVoltage = extensionController.getMotorVoltage(); + + extensionSim.setInputVoltage(extensionVoltage); + + extensionSim.update(GompeiLib.getLoopPeriod()); + + Angle rotorPosition = + Angle.ofBaseUnits( + extensionSim.getPosition() * constants.extensionGearRatio * constants.drumRadius, + Radians); + AngularVelocity rotorVelocity = + AngularVelocity.ofBaseUnits( + extensionSim.getVelocity() * constants.extensionGearRatio * constants.drumRadius, + RadiansPerSecond); + extensionController.setRawRotorPosition(rotorPosition); + extensionController.setRotorVelocity(rotorVelocity); + + super.updateInputs(inputs); + } +} diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionState.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionState.java new file mode 100644 index 0000000..2f50e20 --- /dev/null +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionState.java @@ -0,0 +1,7 @@ +package edu.wpi.team190.gompeilib.subsystems.extension; + +public enum ExtensionState { + IDLE, + OPEN_LOOP_VOLTAGE_CONTROL, + CLOSED_LOOP_POSITION_CONTROL; +} diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorStateTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorStateTest.java deleted file mode 100644 index 02cbd92..0000000 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorStateTest.java +++ /dev/null @@ -1,19 +0,0 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; - -import static org.junit.jupiter.api.Assertions.*; - -import org.junit.jupiter.api.Test; - -public class ElevatorStateTest { - @Test - public void testEnum() { - assertEquals(3, ElevatorState.values().length); - assertEquals(ElevatorState.IDLE, ElevatorState.valueOf("IDLE")); - assertEquals( - ElevatorState.OPEN_LOOP_VOLTAGE_CONTROL, - ElevatorState.valueOf("OPEN_LOOP_VOLTAGE_CONTROL")); - assertEquals( - ElevatorState.CLOSED_LOOP_POSITION_CONTROL, - ElevatorState.valueOf("CLOSED_LOOP_POSITION_CONTROL")); - } -} diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstantsTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionConstantsTest.java similarity index 71% rename from lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstantsTest.java rename to lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionConstantsTest.java index f77d144..932d255 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstantsTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionConstantsTest.java @@ -1,19 +1,21 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.junit.jupiter.api.Assertions.*; import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; + import org.junit.jupiter.api.Test; import org.wpilib.math.system.DCMotor; import org.wpilib.units.Units; -public class ElevatorConstantsTest { +public class ExtensionConstantsTest { @Test public void testConstantsAndBuilder() { DCMotor motor = DCMotor.getNeo550(1); - ElevatorConstants.ElevatorParameters params = - ElevatorConstants.ElevatorParameters.builder() + ExtensionConstants.ExtensionParameters params = + ExtensionConstants.ExtensionParameters.builder() .withELEVATOR_MOTOR_CONFIG(motor) .withCARRIAGE_MASS_KG(15.0) .withMIN_HEIGHT(Units.Meters.of(0.0)) @@ -30,14 +32,14 @@ public void testConstantsAndBuilder() { .withGoalTolerance(Units.Meters.of(0.01)) .build(); - ElevatorConstants constants = - ElevatorConstants.builder() + ExtensionConstants constants = + ExtensionConstants.builder() .withLeaderCANID(5) - .withElevatorGearRatio(10.0) + .withExtensionGearRatio(10.0) .withDrumRadius(0.02) - .withElevatorSupplyCurrentLimit(40.0) - .withElevatorStatorCurrentLimit(40.0) - .withElevatorParameters(params) + .withExtensionSupplyCurrentLimit(40.0) + .withExtensionStatorCurrentLimit(40.0) + .withExtensionParameters(params) .withSlot0Gains(slot0) .withConstraints(constraints) .withVoltageOffsetStep(Units.Volts.of(0.5)) @@ -46,11 +48,11 @@ public void testConstantsAndBuilder() { assertNotNull(constants); assertEquals(5, constants.leaderCANID); - assertEquals(10.0, constants.elevatorGearRatio); + assertEquals(10.0, constants.extensionGearRatio); assertEquals(0.02, constants.drumRadius); - assertEquals(40.0, constants.elevatorSupplyCurrentLimit); - assertEquals(40.0, constants.elevatorStatorCurrentLimit); - assertEquals(params, constants.elevatorParameters); + assertEquals(40.0, constants.extensionSupplyCurrentLimit); + assertEquals(40.0, constants.extensionStatorCurrentLimit); + assertEquals(params, constants.extensionParameters); assertEquals(slot0, constants.slot0Gains); assertEquals(constraints, constants.constraints); assertEquals(0.5, constants.voltageOffsetStep.in(Units.Volts)); diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSimTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOSimTest.java similarity index 82% rename from lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSimTest.java rename to lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOSimTest.java index 2ffb0e4..f8a26d5 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSimTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOSimTest.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.junit.jupiter.api.Assertions.*; @@ -7,13 +7,17 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIOSim; + import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.wpilib.math.system.DCMotor; import org.wpilib.units.Units; -public class ElevatorIOSimTest { - private ElevatorConstants constants; +public class ExtensionIOSimTest { + private ExtensionConstants constants; @BeforeEach public void setUp() { @@ -25,8 +29,8 @@ public void setUp() { GompeiLib.init(RobotMode.SIM, false, 0.02); DCMotor motor = DCMotor.getNeo550(1); - ElevatorConstants.ElevatorParameters params = - ElevatorConstants.ElevatorParameters.builder() + ExtensionConstants.ExtensionParameters params = + ExtensionConstants.ExtensionParameters.builder() .withELEVATOR_MOTOR_CONFIG(motor) .withCARRIAGE_MASS_KG(15.0) .withMIN_HEIGHT(Units.Meters.of(0.0)) @@ -35,13 +39,13 @@ public void setUp() { .build(); constants = - ElevatorConstants.builder() + ExtensionConstants.builder() .withLeaderCANID(5) - .withElevatorGearRatio(10.0) + .withExtensionGearRatio(10.0) .withDrumRadius(0.02) - .withElevatorSupplyCurrentLimit(40.0) - .withElevatorStatorCurrentLimit(40.0) - .withElevatorParameters(params) + .withExtensionSupplyCurrentLimit(40.0) + .withExtensionStatorCurrentLimit(40.0) + .withExtensionParameters(params) .withSlot0Gains( Gains.fromDoubles() .withPrefix("slot0") @@ -70,9 +74,9 @@ public void setUp() { } @Test - public void testElevatorIOSim() { - ElevatorIOSim sim = new ElevatorIOSim(constants); - ElevatorIO.ElevatorIOInputs inputs = new ElevatorIO.ElevatorIOInputs(); + public void testExtensionIOSim() { + ExtensionIOSim sim = new ExtensionIOSim(constants); + ExtensionIO.ExtensionIOInputs inputs = new ExtensionIO.ExtensionIOInputs(); // Initial state set position sim.setPosition(Units.Meters.of(0.5)); diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSimTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTalonFXSimTest.java similarity index 88% rename from lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSimTest.java rename to lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTalonFXSimTest.java index 421f62b..eb68cd1 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSimTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTalonFXSimTest.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.junit.jupiter.api.Assertions.*; import static org.mockito.Mockito.*; @@ -14,6 +14,10 @@ import edu.wpi.team190.gompeilib.core.robot.RobotMode; import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIOTalonFXSim; + import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.mockito.MockedConstruction; @@ -27,8 +31,8 @@ import org.wpilib.units.measure.Temperature; import org.wpilib.units.measure.Voltage; -public class ElevatorIOTalonFXSimTest { - private ElevatorConstants constants; +public class ExtensionIOTalonFXSimTest { + private ExtensionConstants constants; @BeforeEach public void setUp() { @@ -40,8 +44,8 @@ public void setUp() { GompeiLib.init(RobotMode.SIM, false, 0.02); DCMotor motor = DCMotor.getNeo550(1); - ElevatorConstants.ElevatorParameters params = - ElevatorConstants.ElevatorParameters.builder() + ExtensionConstants.ExtensionParameters params = + ExtensionConstants.ExtensionParameters.builder() .withELEVATOR_MOTOR_CONFIG(motor) .withCARRIAGE_MASS_KG(15.0) .withMIN_HEIGHT(Units.Meters.of(0.0)) @@ -50,13 +54,13 @@ public void setUp() { .build(); constants = - ElevatorConstants.builder() + ExtensionConstants.builder() .withLeaderCANID(5) - .withElevatorGearRatio(10.0) + .withExtensionGearRatio(10.0) .withDrumRadius(0.02) - .withElevatorSupplyCurrentLimit(40.0) - .withElevatorStatorCurrentLimit(40.0) - .withElevatorParameters(params) + .withExtensionSupplyCurrentLimit(40.0) + .withExtensionStatorCurrentLimit(40.0) + .withExtensionParameters(params) .withSlot0Gains( Gains.fromDoubles() .withPrefix("slot0") @@ -85,7 +89,7 @@ public void setUp() { } @Test - public void testElevatorIOTalonFXSim() { + public void testExtensionIOTalonFXSim() { TalonFXConfigurator configurator = mock(TalonFXConfigurator.class); when(configurator.apply(any(TalonFXConfiguration.class))).thenReturn(StatusCode.OK); when(configurator.apply(any(TalonFXConfiguration.class), anyDouble())) @@ -147,8 +151,8 @@ public void testElevatorIOTalonFXSim() { anyDouble(), any(BaseStatusSignal[].class))) .thenReturn(null); - ElevatorIOTalonFXSim sim = new ElevatorIOTalonFXSim(constants); - ElevatorIO.ElevatorIOInputs inputs = new ElevatorIO.ElevatorIOInputs(); + ExtensionIOTalonFXSim sim = new ExtensionIOTalonFXSim(constants); + ExtensionIO.ExtensionIOInputs inputs = new ExtensionIO.ExtensionIOInputs(); sim.updateInputs(inputs); diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTalonFXTest.java similarity index 90% rename from lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXTest.java rename to lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTalonFXTest.java index b2afe6e..048670a 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTalonFXTest.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.junit.jupiter.api.Assertions.*; import static org.mockito.Mockito.*; @@ -14,6 +14,10 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIOTalonFX; + import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.mockito.MockedConstruction; @@ -27,8 +31,8 @@ import org.wpilib.units.measure.Temperature; import org.wpilib.units.measure.Voltage; -public class ElevatorIOTalonFXTest { - private ElevatorConstants constants; +public class ExtensionIOTalonFXTest { + private ExtensionConstants constants; @BeforeEach public void setUp() { @@ -40,8 +44,8 @@ public void setUp() { GompeiLib.init(RobotMode.SIM, false, 0.02); DCMotor motor = DCMotor.getNeo550(1); - ElevatorConstants.ElevatorParameters params = - ElevatorConstants.ElevatorParameters.builder() + ExtensionConstants.ExtensionParameters params = + ExtensionConstants.ExtensionParameters.builder() .withELEVATOR_MOTOR_CONFIG(motor) .withCARRIAGE_MASS_KG(15.0) .withMIN_HEIGHT(Units.Meters.of(0.0)) @@ -50,13 +54,13 @@ public void setUp() { .build(); constants = - ElevatorConstants.builder() + ExtensionConstants.builder() .withLeaderCANID(5) - .withElevatorGearRatio(10.0) + .withExtensionGearRatio(10.0) .withDrumRadius(0.02) - .withElevatorSupplyCurrentLimit(40.0) - .withElevatorStatorCurrentLimit(40.0) - .withElevatorParameters(params) + .withExtensionSupplyCurrentLimit(40.0) + .withExtensionStatorCurrentLimit(40.0) + .withExtensionParameters(params) .withSlot0Gains( Gains.fromDoubles() .withPrefix("slot0") @@ -85,7 +89,7 @@ public void setUp() { } @Test - public void testElevatorIOTalonFX() { + public void testExtensionIOTalonFX() { TalonFXConfigurator configurator = mock(TalonFXConfigurator.class); when(configurator.apply(any(TalonFXConfiguration.class))).thenReturn(StatusCode.OK); when(configurator.apply(any(TalonFXConfiguration.class), anyDouble())) @@ -143,8 +147,8 @@ public void testElevatorIOTalonFX() { anyDouble(), any(BaseStatusSignal[].class))) .thenReturn(null); - ElevatorIOTalonFX io = new ElevatorIOTalonFX(constants); - ElevatorIO.ElevatorIOInputs inputs = new ElevatorIO.ElevatorIOInputs(); + ExtensionIOTalonFX io = new ExtensionIOTalonFX(constants); + ExtensionIO.ExtensionIOInputs inputs = new ExtensionIO.ExtensionIOInputs(); io.updateInputs(inputs); diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTest.java similarity index 84% rename from lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTest.java rename to lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTest.java index 1491224..551b72b 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionIOTest.java @@ -1,18 +1,20 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.junit.jupiter.api.Assertions.*; import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; + import org.junit.jupiter.api.Test; import org.wpilib.units.Units; -public class ElevatorIOTest { +public class ExtensionIOTest { @Test - public void testElevatorIODefaults() { - ElevatorIO io = new ElevatorIO() {}; - ElevatorIO.ElevatorIOInputs inputs = new ElevatorIO.ElevatorIOInputs(); + public void testExtensionIODefaults() { + ExtensionIO io = new ExtensionIO() {}; + ExtensionIO.ExtensionIOInputs inputs = new ExtensionIO.ExtensionIOInputs(); // Call defaults - should not throw io.updateInputs(inputs); diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionStateTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionStateTest.java new file mode 100644 index 0000000..1277f2a --- /dev/null +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionStateTest.java @@ -0,0 +1,21 @@ +package edu.wpi.team190.gompeilib.subsystems.extension; + +import static org.junit.jupiter.api.Assertions.*; + +import org.junit.jupiter.api.Test; + +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionState; + +public class ExtensionStateTest { + @Test + public void testEnum() { + assertEquals(3, ExtensionState.values().length); + assertEquals(ExtensionState.IDLE, ExtensionState.valueOf("IDLE")); + assertEquals( + ExtensionState.OPEN_LOOP_VOLTAGE_CONTROL, + ExtensionState.valueOf("OPEN_LOOP_VOLTAGE_CONTROL")); + assertEquals( + ExtensionState.CLOSED_LOOP_POSITION_CONTROL, + ExtensionState.valueOf("CLOSED_LOOP_POSITION_CONTROL")); + } +} diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionTest.java similarity index 66% rename from lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorTest.java rename to lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionTest.java index fa89726..d300067 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/elevator/ExtensionTest.java @@ -1,4 +1,4 @@ -package edu.wpi.team190.gompeilib.subsystems.elevator; +package edu.wpi.team190.gompeilib.subsystems.extension; import static org.junit.jupiter.api.Assertions.*; import static org.mockito.Mockito.*; @@ -9,6 +9,10 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; +import edu.wpi.team190.gompeilib.subsystems.extension.Extension; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; +import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; + import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.littletonrobotics.junction.Logger; @@ -19,10 +23,10 @@ import org.wpilib.units.Units; import org.wpilib.units.measure.Distance; -public class ElevatorTest { - private ElevatorIO io; +public class ExtensionTest { + private ExtensionIO io; private Subsystem subsystem; - private ElevatorConstants constants; + private ExtensionConstants constants; @BeforeEach public void setUp() { @@ -33,13 +37,13 @@ public void setUp() { } GompeiLib.init(RobotMode.SIM, false, 0.02); - io = mock(ElevatorIO.class); + io = mock(ExtensionIO.class); subsystem = mock(Subsystem.class, CALLS_REAL_METHODS); - when(subsystem.getName()).thenReturn("TestElevatorSubsystem"); + when(subsystem.getName()).thenReturn("TestExtensionSubsystem"); DCMotor motor = DCMotor.getNeo550(1); - ElevatorConstants.ElevatorParameters params = - ElevatorConstants.ElevatorParameters.builder() + ExtensionConstants.ExtensionParameters params = + ExtensionConstants.ExtensionParameters.builder() .withELEVATOR_MOTOR_CONFIG(motor) .withCARRIAGE_MASS_KG(15.0) .withMIN_HEIGHT(Units.Meters.of(0.0)) @@ -48,13 +52,13 @@ public void setUp() { .build(); constants = - ElevatorConstants.builder() + ExtensionConstants.builder() .withLeaderCANID(5) - .withElevatorGearRatio(10.0) + .withExtensionGearRatio(10.0) .withDrumRadius(0.02) - .withElevatorSupplyCurrentLimit(40.0) - .withElevatorStatorCurrentLimit(40.0) - .withElevatorParameters(params) + .withExtensionSupplyCurrentLimit(40.0) + .withExtensionStatorCurrentLimit(40.0) + .withExtensionParameters(params) .withSlot0Gains(Gains.fromDoubles().withPrefix("slot0").build()) .withConstraints( LinearConstraints.fromMeasures() @@ -69,50 +73,50 @@ public void setUp() { } @Test - public void testElevator() { + public void testExtension() { try (MockedStatic mockLogger = mockStatic(Logger.class)) { - Elevator elevator = new Elevator(constants, subsystem, 0, io); + Extension extension = new Extension(constants, subsystem, 0, io); - assertNotNull(elevator); + assertNotNull(extension); // Test periodic in IDLE state - elevator.periodic(); + extension.periodic(); verify(io).updateInputs(any()); // Test open loop control state - elevator.setVoltageGoal(Units.Volts.of(6.0)); - elevator.periodic(); + extension.setVoltageGoal(Units.Volts.of(6.0)); + extension.periodic(); verify(io).setVoltageGoal(Units.Volts.of(6.0)); // Test closed loop control state - elevator.setPositionGoal(Units.Meters.of(1.0)); - elevator.periodic(); + extension.setPositionGoal(Units.Meters.of(1.0)); + extension.periodic(); verify(io).setPositionGoal(any(Distance.class)); // Test setters/getters - elevator.setVoltageGoal( + extension.setVoltageGoal( new Setpoint<>( Units.Volts.of(2.0), Units.Volts.of(0.1), Units.Volts.of(-12), Units.Volts.of(12))); - elevator.setPositionGoal( + extension.setPositionGoal( new Setpoint<>( Units.Meters.of(0.5), Units.Meters.of(0.01), Units.Meters.of(0.0), Units.Meters.of(1.5))); - elevator.getElevatorPosition(); + extension.getExtensionPosition(); // Test delegates - elevator.setPosition(Units.Meters.of(0.4)); + extension.setPosition(Units.Meters.of(0.4)); verify(io).setPosition(Units.Meters.of(0.4)); - elevator.setGainSlot(GainSlot.ONE); + extension.setGainSlot(GainSlot.ONE); verify(io).setGainSlot(GainSlot.ONE); - elevator.updateGains(Gains.fromDoubles().withPrefix("slot0").build(), GainSlot.ZERO); + extension.updateGains(Gains.fromDoubles().withPrefix("slot0").build(), GainSlot.ZERO); verify(io).updateGains(any(), eq(GainSlot.ZERO)); - elevator.updateConstraints( + extension.updateConstraints( LinearConstraints.fromMeasures() .withPrefix("constraints") .withGoalTolerance(Units.Meters.of(0.01)) @@ -123,17 +127,17 @@ public void testElevator() { // test goals when(io.atVoltageGoal(any())).thenReturn(true); - assertTrue(elevator.atVoltageGoal()); - assertTrue(elevator.atVoltageGoal(Units.Volts.of(2.0))); + assertTrue(extension.atVoltageGoal()); + assertTrue(extension.atVoltageGoal(Units.Volts.of(2.0))); when(io.atPositionGoal(any())).thenReturn(true); - assertTrue(elevator.atPositionGoal()); - assertTrue(elevator.atPositionGoal(Units.Meters.of(0.5))); + assertTrue(extension.atPositionGoal()); + assertTrue(extension.atPositionGoal(Units.Meters.of(0.5))); - Command waitCmd = elevator.waitUntilAtGoal(); + Command waitCmd = extension.waitUntilAtGoal(); assertNotNull(waitCmd); - Command sysIdCmd = elevator.runSysIdRoutine(); + Command sysIdCmd = extension.runSysIdRoutine(); assertNotNull(sysIdCmd); } } diff --git a/src/main/java/first/robot/Constants.java b/src/main/java/first/robot/Constants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/FieldConstants.java b/src/main/java/first/robot/FieldConstants.java new file mode 100644 index 0000000..3431761 --- /dev/null +++ b/src/main/java/first/robot/FieldConstants.java @@ -0,0 +1,421 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package first.robot; + +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rectangle2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; +import java.util.List; +import first.robot.util.AllianceFlipUtil; + +/** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ +public class FieldConstants { + + public static AprilTagLayoutType tagLayoutType = AprilTagLayoutType.ANDYMARK; + + // AprilTag related constants + public static final int aprilTagCount = tagLayoutType.getLayout().getTags().size(); + public static final double aprilTagWidth = Units.inchesToMeters(6.5); + + // Field dimensions + public static final double fieldLength = tagLayoutType.getLayout().getFieldLength(); + public static final double fieldWidth = tagLayoutType.getLayout().getFieldWidth(); + + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = tagLayoutType.getLayout().getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + tagLayoutType.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - Units.inchesToMeters(120); + public static final double neutralZoneFar = center + Units.inchesToMeters(120); + public static final double oppHubCenter = + tagLayoutType.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + tagLayoutType.getLayout().getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + + // Dimensions + public static final double width = Units.inchesToMeters(47.0); + public static final double height = + Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = Units.inchesToMeters(41.7); + public static final double innerHeight = Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + tagLayoutType.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + tagLayoutType.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + tagLayoutType.getLayout().getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = tagLayoutType.getLayout().getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = tagLayoutType.getLayout().getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + tagLayoutType.getLayout().getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = tagLayoutType.getLayout().getTagPose(21).get().toPose2d(); + + public static final Rectangle2d FEED_KEEPOUT = + new Rectangle2d( + new Pose2d(fieldLength / 2.0, fieldWidth / 2.0, new Rotation2d()), + Math.abs(farRightCorner.getX() - oppNearLeftCorner.getX()) / 2, + width / 2); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.oppHubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.oppHubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = Hub.nearRightCorner; + public static final Translation2d nearRightCorner = + Hub.nearRightCorner.minus(new Translation2d(0.0, width)); + public static final Translation2d farLeftCorner = Hub.farRightCorner; + public static final Translation2d farRightCorner = + Hub.farRightCorner.minus(new Translation2d(0.0, width)); + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = Hub.oppNearRightCorner; + public static final Translation2d oppNearRightCorner = + Hub.oppNearRightCorner.minus(new Translation2d(0.0, width)); + public static final Translation2d oppFarLeftCorner = Hub.oppFarRightCorner; + public static final Translation2d oppFarRightCorner = + Hub.oppFarRightCorner.minus(new Translation2d(0.0, width)); + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + + public static final Rectangle2d BLUE_TRENCH = + new Rectangle2d( + new Pose2d( + new Translation2d( + LinesVertical.hubCenter, + LinesHorizontal.leftTrenchOpenEnd + (openingWidth / 2)), + new Rotation2d()), + openingWidth / 2, + openingWidth / 2); + public static final Rectangle2d RED_TRENCH = + new Rectangle2d( + new Pose2d( + new Translation2d( + LinesVertical.oppHubCenter, + LinesHorizontal.leftTrenchOpenEnd + (openingWidth / 2)), + new Rotation2d()), + openingWidth / 2, + openingWidth / 2); + } + + public static class RightTrench { + + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + + public static final Rectangle2d BLUE_TRENCH = + new Rectangle2d( + new Pose2d( + new Translation2d( + LinesVertical.hubCenter, + LinesHorizontal.rightTrenchOpenEnd + (openingWidth / 2)), + new Rotation2d()), + openingHeight, + openingHeight); + public static final Rectangle2d RED_TRENCH = + new Rectangle2d( + new Pose2d( + new Translation2d( + LinesVertical.oppHubCenter, + LinesHorizontal.rightTrenchOpenEnd + (openingWidth / 2)), + new Rotation2d()), + openingHeight, + openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + public static final double width = Units.inchesToMeters(49.25); + public static final double depth = Units.inchesToMeters(45.0); + public static final double height = Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = Units.inchesToMeters(32.250); + public static final double frontFaceX = Units.inchesToMeters(43.51); + + public static final double uprightHeight = Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = Units.inchesToMeters(27.0); + public static final double midRungHeight = Units.inchesToMeters(45.0); + public static final double highRungHeight = Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(frontFaceX, tagLayoutType.getLayout().getTagPose(31).get().getY()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (tagLayoutType.getLayout().getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (tagLayoutType.getLayout().getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, tagLayoutType.getLayout().getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (tagLayoutType.getLayout().getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (tagLayoutType.getLayout().getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + public static final Rectangle2d BLUE_TOWER = + new Rectangle2d( + new Pose2d( + new Translation2d(centerPoint.getX() - depth / 2, centerPoint.getY()), + new Rotation2d()), + depth / 2, + depth / 2); + + public static final Rectangle2d RED_TOWER = + new Rectangle2d( + new Pose2d( + new Translation2d(oppCenterPoint.getX() + depth / 2, oppCenterPoint.getY()), + new Rotation2d()), + depth / 2, + depth / 2); + } + + public static class Depot { + // Dimensions + public static final double width = Units.inchesToMeters(42.0); + public static final double depth = Units.inchesToMeters(27.0); + public static final double height = Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + + public static final Translation2d BLUE_FEED_TRANSLATION = new Translation2d(3.0, 5.5); + public static final Translation2d RED_FEED_TRANSLATION = + AllianceFlipUtil.overrideApply(new Translation2d(3.0, 5.5)); + } + + public static class Outpost { + // Dimensions + public static final double width = Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); + public static final double height = Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, tagLayoutType.getLayout().getTagPose(29).get().getY()); + + public static final Translation2d BLUE_FEED_TRANSLATION = new Translation2d(3.0, 2.5); + public static final Translation2d RED_FEED_TRANSLATION = + AllianceFlipUtil.overrideApply(new Translation2d(3.0, 2.5)); + } + + public enum AprilTagLayoutType { + ANDYMARK(AprilTagFields.k2026RebuiltAndymark), + WELDED(AprilTagFields.k2026RebuiltWelded); + + private final AprilTagFields name; + private volatile AprilTagFieldLayout layout; + + AprilTagLayoutType(AprilTagFields name) { + this.name = name; + } + + public AprilTagFieldLayout getLayout() { + if (layout != null) { + return layout; + } + layout = AprilTagFieldLayout.loadField(name); + return layout; + } + } + + public static class AprilTags { + public static final List globalTags = + FieldConstants.tagLayoutType.getLayout().getTags(); + public static final List blueHubTags = + FieldConstants.tagLayoutType.getLayout().getTags().stream() + .filter((AprilTag tag) -> List.of(18, 19, 20, 21, 24, 25, 26, 27).contains(tag.ID)) + .toList(); + public static final List redHubTags = + FieldConstants.tagLayoutType.getLayout().getTags().stream() + .filter((AprilTag tag) -> List.of(2, 3, 4, 5, 8, 9, 10, 11).contains(tag.ID)) + .toList(); + public static final List blueTowerTags = + FieldConstants.tagLayoutType.getLayout().getTags().stream() + .filter((AprilTag tag) -> List.of(31, 32).contains(tag.ID)) + .toList(); + public static final List redTowerTags = + FieldConstants.tagLayoutType.getLayout().getTags().stream() + .filter((AprilTag tag) -> List.of(15, 16).contains(tag.ID)) + .toList(); + } + + public static class Zones { + public static final Rectangle2d[] PROHIBIT_LAUNCH_ZONES = { + Tower.BLUE_TOWER, Tower.RED_TOWER, Hub.FEED_KEEPOUT + }; + public static final Rectangle2d[] HOOD_TUCK_ZONES = { + LeftTrench.BLUE_TRENCH, LeftTrench.RED_TRENCH, RightTrench.BLUE_TRENCH, RightTrench.RED_TRENCH + }; + } +} \ No newline at end of file diff --git a/src/main/java/first/robot/util/AllianceFlipUtil.java b/src/main/java/first/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..745f82f --- /dev/null +++ b/src/main/java/first/robot/util/AllianceFlipUtil.java @@ -0,0 +1,62 @@ +package first.robot.util; + +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.DriverStation; +import first.robot.FieldConstants; + +public class AllianceFlipUtil { + public static double fieldWidth = FieldConstants.fieldWidth; + public static double fieldLength = FieldConstants.fieldLength; + + public static double applyX(double x) { + return shouldFlip() ? fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static double overrideApplyX(double x) { + return fieldLength - x; + } + + public static double overrideApplyY(double y) { + return fieldWidth - y; + } + + public static Translation2d overrideApply(Translation2d translation) { + return new Translation2d( + overrideApplyX(translation.getX()), overrideApplyY(translation.getY())); + } + + public static Rotation2d overrideApply(Rotation2d rotation) { + return rotation.rotateBy(Rotation2d.kPi); + } + + public static Pose2d overrideApply(Pose2d pose) { + return new Pose2d(overrideApply(pose.getTranslation()), overrideApply(pose.getRotation())); + } + + public static boolean shouldFlip() { + return true; + // return DriverStation.getAlliance().isPresent() + // && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } + +//TODO: fix the alliancefliputil +} \ No newline at end of file diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java new file mode 100644 index 0000000..e69de29 From 36ec106a716b021a8e9a244f684c06dac52c8014 Mon Sep 17 00:00:00 2001 From: Ananth Date: Thu, 10 Sep 2026 19:12:52 -0400 Subject: [PATCH 2/3] alliancefliputil --- src/main/java/first/robot/util/AllianceFlipUtil.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/first/robot/util/AllianceFlipUtil.java b/src/main/java/first/robot/util/AllianceFlipUtil.java index 745f82f..d230ddf 100644 --- a/src/main/java/first/robot/util/AllianceFlipUtil.java +++ b/src/main/java/first/robot/util/AllianceFlipUtil.java @@ -4,7 +4,7 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.driverstation.Alliance; -import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.MatchState; import first.robot.FieldConstants; public class AllianceFlipUtil { @@ -53,10 +53,7 @@ public static Pose2d overrideApply(Pose2d pose) { } public static boolean shouldFlip() { - return true; - // return DriverStation.getAlliance().isPresent() - // && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + return MatchState.getAlliance().isPresent() + && MatchState.getAlliance().get() == Alliance.RED; } - -//TODO: fix the alliancefliputil } \ No newline at end of file From 57a64aa84776831bcdfed6f6a50f037325e2067b Mon Sep 17 00:00:00 2001 From: LordVanra Date: Thu, 17 Sep 2026 18:00:54 -0400 Subject: [PATCH 3/3] merge fixes and lint Co-authored-by: SeptimusHeap7 --- .../extension/ElevatorConstants.java | 48 ------------------- .../subsystems/extension/ExtensionIOSim.java | 10 ++-- .../extension/ExtensionConstantsTest.java | 2 - .../extension/ExtensionIOSimTest.java | 4 -- .../extension/ExtensionIOTalonFXSimTest.java | 4 -- .../extension/ExtensionIOTalonFXTest.java | 4 -- .../subsystems/extension/ExtensionIOTest.java | 2 - .../subsystems/extension/ExtensionTest.java | 4 -- src/main/java/first/robot/Constants.java | 1 + src/main/java/first/robot/FieldConstants.java | 12 ++--- .../first/robot/util/AllianceFlipUtil.java | 11 ++--- .../V3_Horse_CV2_OPRCompositeCommands.java | 1 + .../V3_Horse_CV2_OPRAutoLeftTrench.java | 1 + .../subsystems/V3_Horse_CV2_OPRConstants.java | 1 + .../V3_Horse_CV2_OPRMechanism3D.java | 1 + .../V3_Horse_CV2_OPRRobotContainer.java | 1 + .../V3_Horse_CV2_OPRRobotState.java | 1 + .../V3_Horse_CV2_OPRTunerConstants.java | 1 + .../V3_Horse_CV2_TRCompositeCommands.java | 1 + .../V3_Horse_CV2_TRAutoLeftTrench.java | 1 + .../subsystems/V3_Horse_CV2_TRConstants.java | 1 + .../V3_Horse_CV2_TRMechanism3D.java | 1 + .../V3_Horse_CV2_TRRobotContainer.java | 1 + .../subsystems/V3_Horse_CV2_TRRobotState.java | 1 + .../V3_Horse_CV2_TRTunerConstants.java | 1 + .../V3_Horse_CV3_OPRCompositeCommands.java | 1 + .../V3_Horse_CV3_OPRAutoLeftTrench.java | 1 + .../subsystems/V3_Horse_CV3_OPRConstants.java | 1 + .../V3_Horse_CV3_OPRMechanism3D.java | 1 + .../V3_Horse_CV3_OPRRobotContainer.java | 1 + .../V3_Horse_CV3_OPRRobotState.java | 1 + .../V3_Horse_CV3_OPRTunerConstants.java | 1 + .../V3_Horse_CV3_TRCompositeCommands.java | 1 + .../V3_Horse_CV3_TRAutoLeftTrench.java | 1 + .../subsystems/V3_Horse_CV3_TRConstants.java | 1 + .../V3_Horse_CV3_TRMechanism3D.java | 1 + .../V3_Horse_CV3_TRRobotContainer.java | 1 + .../subsystems/V3_Horse_CV3_TRRobotState.java | 1 + .../V3_Horse_CV3_TRTunerConstants.java | 1 + 39 files changed, 45 insertions(+), 85 deletions(-) delete mode 100644 lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java deleted file mode 100644 index 25a99d2..0000000 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ElevatorConstants.java +++ /dev/null @@ -1,48 +0,0 @@ -package edu.wpi.team190.gompeilib.subsystems.extension; - -import com.ctre.phoenix6.CANBus; -import edu.wpi.team190.gompeilib.core.utility.control.Gains; -import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; -import java.util.Set; -import lombok.Builder; -import lombok.NonNull; -import lombok.Singular; -import org.wpilib.math.system.DCMotor; -import org.wpilib.units.measure.Distance; -import org.wpilib.units.measure.Voltage; - -@Builder(setterPrefix = "with") -public class ExtensionConstants { - @NonNull public final Integer leaderCANID; - @NonNull public final CANBus canBus = new CANBus(); - @NonNull public final Double extensionGearRatio; - @NonNull public final Double drumRadius; - - @NonNull public final Double extensionSupplyCurrentLimit; - @NonNull public final Double extensionStatorCurrentLimit; - - @NonNull public final ExtensionParameters extensionParameters; - @NonNull public final Gains slot0Gains; - @Builder.Default public final Gains slot1Gains = Gains.builder().build(); - @Builder.Default public final Gains slot2Gains = Gains.builder().build(); - @NonNull public final LinearConstraints constraints; - - @Singular(value = "alignedFollowerCANID") - @NonNull - public final Set alignedFollowerCANIDs; - - @Singular(value = "opposedFollowerCANID") - @NonNull - public final Set opposedFollowerCANIDs; - - @NonNull public final Voltage voltageOffsetStep; - @NonNull public final Distance heightOffsetStep; - - @Builder(setterPrefix = "with") - public record ExtensionParameters( - @NonNull DCMotor ELEVATOR_MOTOR_CONFIG, - @NonNull Double CARRIAGE_MASS_KG, - @NonNull Distance MIN_HEIGHT, - @NonNull Distance MAX_HEIGHT, - @NonNull Integer NUM_MOTORS) {} -} diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java index 9e89675..603d8a7 100644 --- a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java +++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSim.java @@ -7,11 +7,11 @@ import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; import java.util.Arrays; -import org.wpilib.math.controller.ExtensionFeedforward; +import org.wpilib.math.controller.ElevatorFeedforward; import org.wpilib.math.controller.ProfiledPIDController; import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrapezoidProfile; -import org.wpilib.simulation.ExtensionSim; +import org.wpilib.simulation.ElevatorSim; import org.wpilib.units.measure.*; public class ExtensionIOSim implements ExtensionIO { @@ -22,7 +22,7 @@ public class ExtensionIOSim implements ExtensionIO { private GainSlot gainSlot; private final ProfiledPIDController feedback; - private ExtensionFeedforward feedforward; + private ElevatorFeedforward feedforward; private final ExtensionConstants constants; @@ -54,7 +54,7 @@ public ExtensionIOSim(ExtensionConstants constants) { constants.constraints.maxAcceleration().get().in(MetersPerSecondPerSecond))); feedforward = - new ExtensionFeedforward( + new ElevatorFeedforward( constants.slot0Gains.kS().get(), constants.slot0Gains.kG().get(), constants.slot0Gains.kV().get(), @@ -149,7 +149,7 @@ public void setGainSlot(GainSlot gainSlot) { public void updateGains(Gains gains, GainSlot gainSlot) { feedback.setPID(gains.kP().get(), gains.kI().get(), gains.kD().get()); feedforward = - new ExtensionFeedforward( + new ElevatorFeedforward( gains.kS().get(), gains.kG().get(), gains.kV().get(), gains.kA().get()); } diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstantsTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstantsTest.java index dfcf399..5a0a354 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstantsTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionConstantsTest.java @@ -5,8 +5,6 @@ import edu.wpi.team190.gompeilib.core.GompeiLib; import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; - import org.junit.jupiter.api.Test; import org.wpilib.math.system.DCMotor; import org.wpilib.units.Units; diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSimTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSimTest.java index 5e92db2..1100ba7 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSimTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOSimTest.java @@ -7,10 +7,6 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIOSim; - import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.wpilib.math.system.DCMotor; diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSimTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSimTest.java index 28f2cd2..d5a7f46 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSimTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXSimTest.java @@ -16,10 +16,6 @@ import edu.wpi.team190.gompeilib.core.robot.RobotMode; import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIOTalonFXSim; - import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.mockito.MockedConstruction; diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXTest.java index a5a9f53..812896c 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTalonFXTest.java @@ -16,10 +16,6 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIOTalonFX; - import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.mockito.MockedConstruction; diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTest.java index 551b72b..34f3548 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionIOTest.java @@ -5,8 +5,6 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; - import org.junit.jupiter.api.Test; import org.wpilib.units.Units; diff --git a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionTest.java b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionTest.java index 392fd23..4d3166b 100644 --- a/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionTest.java +++ b/lib/src/test/java/edu/wpi/team190/gompeilib/subsystems/extension/ExtensionTest.java @@ -9,10 +9,6 @@ import edu.wpi.team190.gompeilib.core.utility.control.Gains; import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints; import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot; -import edu.wpi.team190.gompeilib.subsystems.extension.Extension; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionConstants; -import edu.wpi.team190.gompeilib.subsystems.extension.ExtensionIO; - import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/first/robot/Constants.java b/src/main/java/first/robot/Constants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/Constants.java +++ b/src/main/java/first/robot/Constants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/FieldConstants.java b/src/main/java/first/robot/FieldConstants.java index 3431761..724af0a 100644 --- a/src/main/java/first/robot/FieldConstants.java +++ b/src/main/java/first/robot/FieldConstants.java @@ -7,17 +7,17 @@ package first.robot; -import org.wpilib.vision.apriltag.AprilTag; -import org.wpilib.vision.apriltag.AprilTagFieldLayout; -import org.wpilib.vision.apriltag.AprilTagFields; +import first.robot.util.AllianceFlipUtil; +import java.util.List; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rectangle2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Translation3d; import org.wpilib.math.util.Units; -import java.util.List; -import first.robot.util.AllianceFlipUtil; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; /** * Contains information for location of field element and other useful reference points. @@ -418,4 +418,4 @@ public static class Zones { LeftTrench.BLUE_TRENCH, LeftTrench.RED_TRENCH, RightTrench.BLUE_TRENCH, RightTrench.RED_TRENCH }; } -} \ No newline at end of file +} diff --git a/src/main/java/first/robot/util/AllianceFlipUtil.java b/src/main/java/first/robot/util/AllianceFlipUtil.java index d230ddf..999bf55 100644 --- a/src/main/java/first/robot/util/AllianceFlipUtil.java +++ b/src/main/java/first/robot/util/AllianceFlipUtil.java @@ -1,11 +1,11 @@ package first.robot.util; +import first.robot.FieldConstants; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.MatchState; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.driverstation.Alliance; -import org.wpilib.driverstation.MatchState; -import first.robot.FieldConstants; public class AllianceFlipUtil { public static double fieldWidth = FieldConstants.fieldWidth; @@ -53,7 +53,6 @@ public static Pose2d overrideApply(Pose2d pose) { } public static boolean shouldFlip() { - return MatchState.getAlliance().isPresent() - && MatchState.getAlliance().get() == Alliance.RED; + return MatchState.getAlliance().isPresent() && MatchState.getAlliance().get() == Alliance.RED; } -} \ No newline at end of file +} diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/V3_Horse_CV2_OPRCompositeCommands.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/commands/autonomous/V3_Horse_CV2_OPRAutoLeftTrench.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRMechanism3D.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotContainer.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRRobotState.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV2_OPR/subsystems/V3_Horse_CV2_OPRTunerConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/commands/V3_Horse_CV2_TRCompositeCommands.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/commands/autonomous/V3_Horse_CV2_TRAutoLeftTrench.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRMechanism3D.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotContainer.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRRobotState.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV2_TR/subsystems/V3_Horse_CV2_TRTunerConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/V3_Horse_CV3_OPRCompositeCommands.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/commands/autonomous/V3_Horse_CV3_OPRAutoLeftTrench.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRMechanism3D.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotContainer.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRRobotState.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV3_OPR/subsystems/V3_Horse_CV3_OPRTunerConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java b/src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/commands/V3_Horse_CV3_TRCompositeCommands.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java b/src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/commands/autonomous/V3_Horse_CV3_TRAutoLeftTrench.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRConstants.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRMechanism3D.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotContainer.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRRobotState.java @@ -0,0 +1 @@ + diff --git a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java index e69de29..8b13789 100644 --- a/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java +++ b/src/main/java/first/robot/v3_Horse_CV3_TR/subsystems/V3_Horse_CV3_TRTunerConstants.java @@ -0,0 +1 @@ +