diff --git a/yams/java/yams/mechanisms/config/ArmConfig.java b/yams/java/yams/mechanisms/config/ArmConfig.java index 7311b913..246b8139 100644 --- a/yams/java/yams/mechanisms/config/ArmConfig.java +++ b/yams/java/yams/mechanisms/config/ArmConfig.java @@ -1,5 +1,6 @@ package yams.mechanisms.config; +import edu.wpi.first.math.Pair; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; @@ -23,47 +24,63 @@ public class ArmConfig /** * {@link SmartMotorController} for the {@link yams.mechanisms.positional.Arm} */ - private final SmartMotorController motor; + private Optional motor; /** * The network root of the mechanism (Optional). */ - protected Optional networkTableName = Optional.empty(); + protected Optional networkTableName = Optional.empty(); /** * Telemetry name. */ - private Optional telemetryName = Optional.empty(); + private Optional telemetryName = Optional.empty(); /** * Telemetry verbosity */ - private Optional telemetryVerbosity = Optional.empty(); + private Optional telemetryVerbosity = Optional.empty(); /** * Lower Hard Limit for the {@link yams.mechanisms.positional.Arm} to be representing in simulation. */ - private Optional lowerHardLimit = Optional.empty(); + private Optional lowerHardLimit = Optional.empty(); /** * Upper hard limit for the {@link yams.mechanisms.positional.Arm} representing in simulation. */ - private Optional upperHardLimit = Optional.empty(); + private Optional upperHardLimit = Optional.empty(); /** * {@link yams.mechanisms.positional.Arm} length for simulation. */ - private Optional length = Optional.empty(); + private Optional length = Optional.empty(); /** * {@link yams.mechanisms.positional.Arm} mass for simulation. */ - private Optional weight = Optional.empty(); + private Optional weight = Optional.empty(); /** * {@link yams.mechanisms.positional.Arm} MOI from CAD software. If not given estimated with length and weight. */ - private OptionalDouble moi = OptionalDouble.empty(); + private OptionalDouble moi = OptionalDouble.empty(); /** * Sim color value */ - private Color8Bit simColor = new Color8Bit(Color.kOrange); + private Color8Bit simColor = new Color8Bit(Color.kOrange); /** * Mechanism position configuration for the {@link yams.mechanisms.positional.Pivot} (Optional). */ - private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); + private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); + /** + * Horizontal zero of the arm's absolute encoder. + */ + private Optional horizantalZero = Optional.empty(); + /** + * Starting position of the arms motor controller encoder. + */ + private Optional startingPosition = Optional.empty(); + /** + * Lower and upper soft limits of the arms closed loop controller. (LowerLimit, UpperLimit) + */ + private Optional> softLimits = Optional.empty(); + /** + * Wrapping angles for the Arm applied to the motor controllers closed loop controller. (Min, Max) + */ + private Optional> continuousWrapping = Optional.empty(); /** * Arm Configuration class @@ -72,9 +89,73 @@ public class ArmConfig */ public ArmConfig(SmartMotorController motorController) { - motor = motorController; + motor = Optional.of(motorController); } + /** + * Arm configuration class. Required + */ + public ArmConfig() + {} + + /** + * Copy constructor. + * + * @param cfg Configuration to copy from. + */ + private ArmConfig(ArmConfig cfg) + { + motor = cfg.motor; + networkTableName = cfg.networkTableName; + telemetryName = cfg.telemetryName; + telemetryVerbosity = cfg.telemetryVerbosity; + lowerHardLimit = cfg.lowerHardLimit; + upperHardLimit = cfg.upperHardLimit; + length = cfg.length; + weight = cfg.weight; + moi = cfg.moi; + simColor = cfg.simColor; + mechanismPositionConfig = cfg.mechanismPositionConfig; + startingPosition = cfg.startingPosition; + softLimits = cfg.softLimits; + continuousWrapping = cfg.continuousWrapping; + horizantalZero = cfg.horizantalZero; + } + + @Override + public ArmConfig clone() + { + return new ArmConfig(this); + } + + /** + * Configure the {@link SmartMotorController} for the {@link yams.mechanisms.positional.Arm} + * + * @param motorController Primary {@link SmartMotorController} for the {@link yams.mechanisms.positional.Arm} + * @return {@link ArmConfig} for chaining. + */ + public ArmConfig withSmartMotorController(SmartMotorController motorController) + { + if (motor.isPresent()) + { + throw new ArmConfigurationException("Arm already has a SmartMotorController!", + "Cannot set a new one!", + ".withSmartMotorController(SmartMotorController)"); + } + motor = Optional.of(motorController); + moi.ifPresent(moi -> motorController.getConfig().withMomentOfInertia(moi)); + if (length.isPresent() && weight.isPresent() && moi.isEmpty()) + { + motorController.getConfig().withMomentOfInertia(length.get(), weight.get()); + } + horizantalZero.ifPresent(this::withHorizontalZero); + startingPosition.ifPresent(this::withStartingPosition); + softLimits.ifPresent(limits -> withSoftLimits(limits.getFirst(), limits.getSecond())); + continuousWrapping.ifPresent(wrapping -> withWrapping(wrapping.getFirst(), wrapping.getSecond())); + return this; + } + + /** * Publish the color in sim as this. * @@ -96,7 +177,7 @@ public ArmConfig withSimColor(final Color8Bit simColor) */ public ArmConfig withMOI(double MOI) { - motor.getConfig().withMomentOfInertia(MOI); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(MOI)); this.moi = OptionalDouble.of(MOI); return this; } @@ -111,7 +192,7 @@ public ArmConfig withLength(Distance distance) { if (weight.isPresent() && distance != null) { - motor.getConfig().withMomentOfInertia(distance, weight.get()); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(distance, weight.get())); } this.length = Optional.ofNullable(distance); return this; @@ -127,7 +208,7 @@ public ArmConfig withMass(Mass mass) { if (length.isPresent() && mass != null) { - motor.getConfig().withMomentOfInertia(length.get(), mass); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(length.get(), mass)); } this.weight = Optional.ofNullable(mass); return this; @@ -165,7 +246,7 @@ public ArmConfig withTelemetry(String networkRoot, String telemetryName, Telemet } /** - * Set the elevator mechnism position configuration. + * Set the elevator mechanism position configuration. * * @param mechanismPositionConfig {@link MechanismPositionConfig} for the {@link yams.mechanisms.positional.Elevator} * @return {@link PivotConfig} for chaining @@ -176,27 +257,30 @@ public ArmConfig withMechanismPositionConfig(MechanismPositionConfig mechanismPo return this; } + /** - * Set the horizontal zero of the arm. + * Set the horizontal zero of the arms absolute encoder. * * @param horizontalZero Offset of the arm that will make the arm read 0 when horizontal. * @return {@link ArmConfig} for chaining. */ public ArmConfig withHorizontalZero(Angle horizontalZero) { - motor.getConfig().withExternalEncoderZeroOffset(horizontalZero); + this.horizantalZero = Optional.ofNullable(horizontalZero); + motor.ifPresent(motor -> motor.getConfig().withExternalEncoderZeroOffset(horizontalZero)); return this; } /** - * Set the arm starting position. + * Set the arm starting angle of the motor controller encoder. * * @param startingPosition Starting position of the arm. * @return {@link ArmConfig} for chaining */ public ArmConfig withStartingPosition(Angle startingPosition) { - motor.getConfig().withStartingPosition(startingPosition); + this.startingPosition = Optional.ofNullable(startingPosition); + motor.ifPresent(motor -> motor.getConfig().withStartingPosition(startingPosition)); return this; } @@ -209,7 +293,8 @@ public ArmConfig withStartingPosition(Angle startingPosition) */ public ArmConfig withSoftLimits(Angle lowerLimit, Angle upperLimit) { - motor.getConfig().withSoftLimit(lowerLimit, upperLimit); + softLimits = Optional.of(Pair.of(lowerLimit, upperLimit)); + motor.ifPresent(motor -> motor.getConfig().withSoftLimit(lowerLimit, upperLimit)); return this; } @@ -223,7 +308,8 @@ public ArmConfig withSoftLimits(Angle lowerLimit, Angle upperLimit) */ public ArmConfig withWrapping(Angle min, Angle max) { - motor.getConfig().withContinuousWrapping(min, max); + continuousWrapping = Optional.of(Pair.of(min, max)); + motor.ifPresent(motor -> motor.getConfig().withContinuousWrapping(min, max)); return this; } @@ -248,7 +334,7 @@ public ArmConfig withHardLimit(Angle min, Angle max) */ public boolean applyConfig() { - return motor.applyConfig(motor.getConfig()); + return motor.orElseThrow().applyConfig(motor.orElseThrow().getConfig()); } /** @@ -328,7 +414,7 @@ public Optional getTelemetryName() */ public Optional getStartingAngle() { - return motor.getConfig().getStartingPosition(); + return startingPosition; } /** @@ -338,7 +424,7 @@ public Optional getStartingAngle() */ public SmartMotorController getMotor() { - return motor; + return motor.orElseThrow(); } /** diff --git a/yams/java/yams/mechanisms/config/DifferentialMechanismConfig.java b/yams/java/yams/mechanisms/config/DifferentialMechanismConfig.java index dd3fce38..222c8c83 100644 --- a/yams/java/yams/mechanisms/config/DifferentialMechanismConfig.java +++ b/yams/java/yams/mechanisms/config/DifferentialMechanismConfig.java @@ -30,11 +30,11 @@ public class DifferentialMechanismConfig /** * {@link SmartMotorController} for the {@link DifferentialMechanism} */ - private final SmartMotorController leftMotorController; + private Optional leftMotorController = Optional.empty(); /** * {@link SmartMotorController} for the {@link DifferentialMechanism} */ - private final SmartMotorController rightMotorController; + private Optional rightMotorController = Optional.empty(); /** * The network root of the mechanism (Optional). */ @@ -97,11 +97,68 @@ public class DifferentialMechanismConfig */ public DifferentialMechanismConfig(SmartMotorController left, SmartMotorController right) { - leftMotorController = left; - rightMotorController = right; + leftMotorController = Optional.ofNullable(left); + rightMotorController = Optional.ofNullable(right); mechanismPositionConfig.withMovementPlane(Plane.XY); } + /** + * Differential Mechanism configuration class. Required call to + * {@link #withSmartMotorControllers(SmartMotorController, SmartMotorController)} before usage. + */ + public DifferentialMechanismConfig() + { + mechanismPositionConfig.withMovementPlane(Plane.XY); + } + + /** + * Copy constructor. + * + * @param cfg Configuration to copy. + */ + private DifferentialMechanismConfig(DifferentialMechanismConfig cfg) + { + this.leftMotorController = cfg.leftMotorController; + this.rightMotorController = cfg.rightMotorController; + this.telemetryName = cfg.telemetryName; + this.telemetryVerbosity = cfg.telemetryVerbosity; + this.twistGearing = cfg.twistGearing; + this.twistAngle = cfg.twistAngle; + this.tiltAngle = cfg.tiltAngle; + this.startingTwistAngle = cfg.startingTwistAngle; + this.startingTiltAngle = cfg.startingTiltAngle; + this.MOI = cfg.MOI; + this.length = cfg.length; + this.simColor = cfg.simColor; + this.mechanismPositionConfig = cfg.mechanismPositionConfig; + this.networkRoot = cfg.networkRoot; + } + + @Override + public DifferentialMechanismConfig clone() + { + return new DifferentialMechanismConfig(this); + } + + /** + * Add the smart motor controllers to the configuration. + * + * @param left Left {@link SmartMotorController} + * @param right Right {@link SmartMotorController} + * @return {@link DifferentialMechanismConfig} for chaining. + */ + public DifferentialMechanismConfig withSmartMotorControllers(SmartMotorController left, SmartMotorController right) + { + if(leftMotorController.isPresent()) + throw new DifferentialMechanismConfigurationException("Left motor controller already defined.","Cannot redefine left motor controller!", ".withSmartMotorControllers"); + if(rightMotorController.isPresent()) + throw new DifferentialMechanismConfigurationException("Right motor controller already defined.","Cannot redefine right motor controller!", ".withSmartMotorControllers"); + leftMotorController = Optional.ofNullable(left); + rightMotorController = Optional.ofNullable(right); + setStartingEncoderPositions(); + return this; + } + /** * Get the twist angle of the {@link DifferentialMechanism} with any additional twist gearing. * @@ -161,8 +218,8 @@ private void setStartingEncoderPositions() var tilt = startingTiltAngle.get(); var left = getLeftMechanismPosition(tilt, twist); var right = getRightMechanismPosition(tilt, twist); - leftMotorController.getConfig().withStartingPosition(left); - rightMotorController.getConfig().withStartingPosition(right); + leftMotorController.ifPresent(leftMotorController ->leftMotorController.getConfig().withStartingPosition(left)); + rightMotorController.ifPresent(rightMotorController -> rightMotorController.getConfig().withStartingPosition(right)); } } @@ -319,8 +376,8 @@ public DifferentialMechanismConfig withTiltStartingPosition(Angle startingPositi */ public boolean applyConfig() { - return leftMotorController.applyConfig(leftMotorController.getConfig()) && rightMotorController.applyConfig( - rightMotorController.getConfig()); + return leftMotorController.orElseThrow().applyConfig(leftMotorController.orElseThrow().getConfig()) && + rightMotorController.orElseThrow().applyConfig(rightMotorController.orElseThrow().getConfig()); } @@ -448,7 +505,7 @@ public Optional> getTiltAngleSupplier() */ public SmartMotorController getLeftMotorController() { - return leftMotorController; + return leftMotorController.orElseThrow(); } /** @@ -458,7 +515,7 @@ public SmartMotorController getLeftMotorController() */ public SmartMotorController getRightMotorController() { - return rightMotorController; + return rightMotorController.orElseThrow(); } /** diff --git a/yams/java/yams/mechanisms/config/ElevatorConfig.java b/yams/java/yams/mechanisms/config/ElevatorConfig.java index cdfcaa26..83052fbd 100644 --- a/yams/java/yams/mechanisms/config/ElevatorConfig.java +++ b/yams/java/yams/mechanisms/config/ElevatorConfig.java @@ -2,12 +2,14 @@ import static edu.wpi.first.units.Units.Degrees; +import edu.wpi.first.math.Pair; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Mass; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import java.util.Optional; +import java.util.OptionalInt; import yams.exceptions.SmartMotorControllerConfigurationException; import yams.motorcontrollers.SmartMotorController; import yams.motorcontrollers.SmartMotorControllerConfig; @@ -22,7 +24,7 @@ public class ElevatorConfig /** * {@link SmartMotorController} for the {@link yams.mechanisms.positional.Elevator} */ - private final SmartMotorController motor; + private Optional motor = Optional.empty(); /** * The network root of the mechanism (Optional). */ @@ -59,8 +61,23 @@ public class ElevatorConfig /** * Mechanism position configuration for the {@link yams.mechanisms.positional.Pivot} (Optional). */ - private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); - + private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); + /** + * Drum radius of the elevator spool, or the sprocket pitch * teeth. + */ + private Optional drumCircumference = Optional.empty(); + /** + * Elevator stages, applied to the motor controller config gearing by dividing it by the number of stages given. + */ + private OptionalInt stages = OptionalInt.empty(); + /** + * Starting height to set the motor's encoder to. + */ + private Optional startingHeight = Optional.empty(); + /** + * Soft limits of the {@link SmartMotorController} closed loop controller. Can be exceeded. (LowerLimit, UpperLimit) + */ + private Optional> softLimits = Optional.empty(); /** * Elevator Configuration class @@ -69,7 +86,60 @@ public class ElevatorConfig */ public ElevatorConfig(SmartMotorController motorController) { - motor = motorController; + motor = Optional.ofNullable(motorController); + } + + /** + * Elevator Configuration class + * + * @implNote You are REQUIRED to call {@link #withSmartMotorController(SmartMotorController)} before this is used with + * an {@link yams.mechanisms.positional.Elevator} + */ + public ElevatorConfig() {} + + /** + * Copy constructor. + * + * @param cfg Configuration to copy from. + */ + private ElevatorConfig(ElevatorConfig cfg) + { + this.motor = cfg.motor; + this.drumCircumference = cfg.drumCircumference; + this.stages = cfg.stages; + this.startingHeight = cfg.startingHeight; + this.softLimits = cfg.softLimits; + this.simColor = cfg.simColor; + this.angle = cfg.angle; + this.carriageWeight = cfg.carriageWeight; + this.telemetryName = cfg.telemetryName; + this.telemetryVerbosity = cfg.telemetryVerbosity; + this.networkRoot = cfg.networkRoot; + this.mechanismPositionConfig = cfg.mechanismPositionConfig; + this.lowerHardLimit = cfg.lowerHardLimit; + this.upperHardLimit = cfg.upperHardLimit; + } + + @Override + public ElevatorConfig clone() + { + return new ElevatorConfig(this); + } + + /** + * Set the {@link SmartMotorController} for the {@link yams.mechanisms.positional.Elevator} + * + * @param motorController Primary {@link SmartMotorController} for the {@link yams.mechanisms.positional.Elevator} + * @return {@link ElevatorConfig} for chaining. + */ + public ElevatorConfig withSmartMotorController(SmartMotorController motorController) + { + motor = Optional.of(motorController); + drumCircumference.ifPresent(drumRadius->motor.get().getConfig().withMechanismCircumference(drumRadius)); + stages.ifPresent(this::withCascadingElevatorStages); + startingHeight.ifPresent(this::withStartingHeight); + softLimits.ifPresent(softLimits->withSoftLimits(softLimits.getFirst(), softLimits.getSecond())); + return this; } /** @@ -80,7 +150,23 @@ public ElevatorConfig(SmartMotorController motorController) */ public ElevatorConfig withDrumRadius(Distance drumRadius) { - motor.getConfig().withMechanismCircumference(drumRadius.times(2 * Math.PI)); + this.drumCircumference = Optional.ofNullable(drumRadius.times(2 * Math.PI)); + motor.ifPresent(motor -> motor.getConfig().withMechanismCircumference(drumRadius.times(2 * Math.PI))); + return this; + } + + /** + * Set the {@link yams.mechanisms.positional.Elevator} drum radius via the chain pitch (.25in or .35in) and teeth + * count. + * + * @param chainPitch Chain pitch. + * @param teeth Sprocket teeth count. + * @return {@link ElevatorConfig} for chaining. + */ + public ElevatorConfig withDrumRadius(Distance chainPitch, int teeth) + { + this.drumCircumference = Optional.ofNullable(chainPitch.times(teeth)); + motor.ifPresent(motor -> motor.getConfig().withMechanismCircumference(chainPitch.times(teeth))); return this; } @@ -160,8 +246,8 @@ public ElevatorConfig withTelemetry(String networkRoot, String telemetryName, Te */ public ElevatorConfig withCascadingElevatorStages(int stages) { - var smcConfig = motor.getConfig(); - smcConfig.withGearing(smcConfig.getGearing().div(2)); + this.stages = OptionalInt.of(stages); + motor.ifPresent(motor -> motor.getConfig().withGearing(motor.getConfig().getGearing().div(stages))); return this; } @@ -185,10 +271,13 @@ public ElevatorConfig withMechanismPositionConfig(MechanismPositionConfig mechan */ public ElevatorConfig withStartingHeight(Distance startingPosition) { - motor.getConfig().withStartingPosition(startingPosition); + startingHeight = Optional.ofNullable(startingPosition); + motor.ifPresent(motor->motor.getConfig().withStartingPosition(startingPosition)); return this; } + + /** * Set the elevator soft limits. When exceeded the power will be set to 0. * @@ -198,7 +287,8 @@ public ElevatorConfig withStartingHeight(Distance startingPosition) */ public ElevatorConfig withSoftLimits(Distance lowerLimit, Distance upperLimit) { - motor.getConfig().withSoftLimit(lowerLimit, upperLimit); + softLimits = Optional.of(Pair.of(lowerLimit,upperLimit)); + motor.ifPresent(motor->motor.getConfig().withSoftLimit(lowerLimit, upperLimit)); return this; } @@ -223,7 +313,7 @@ public ElevatorConfig withHardLimits(Distance min, Distance max) */ public boolean applyConfig() { - return motor.applyConfig(motor.getConfig()); + return motor.orElseThrow().applyConfig(motor.orElseThrow().getConfig()); } /** @@ -283,8 +373,8 @@ public Optional getTelemetryName() */ public Optional getStartingHeight() { - return motor.getConfig().getStartingPosition().isPresent() ? Optional.of(motor.getConfig() - .convertFromMechanism(motor.getConfig() + return motor.orElseThrow().getConfig().getStartingPosition().isPresent() ? Optional.of(motor.orElseThrow().getConfig() + .convertFromMechanism(motor.orElseThrow().getConfig() .getStartingPosition() .get())) : Optional.empty(); @@ -297,7 +387,7 @@ public Optional getStartingHeight() */ public SmartMotorController getMotor() { - return motor; + return motor.orElseThrow(); } /** @@ -317,13 +407,13 @@ public Color8Bit getSimColor() */ public Distance getDrumRadius() { - if (motor.getConfig().getMechanismCircumference().isEmpty()) + if (motor.orElseThrow().getConfig().getMechanismCircumference().isEmpty()) { throw new SmartMotorControllerConfigurationException("Mechanism circumference is undefined", "Drum radius cannot be fetched.", "withMechanismCircumference(Distance)"); } - return motor.getConfig().getMechanismCircumference().get().div(2 * Math.PI); + return motor.orElseThrow().getConfig().getMechanismCircumference().get().div(2 * Math.PI); } /** diff --git a/yams/java/yams/mechanisms/config/FlyWheelConfig.java b/yams/java/yams/mechanisms/config/FlyWheelConfig.java index 06d49c44..16cd00e1 100644 --- a/yams/java/yams/mechanisms/config/FlyWheelConfig.java +++ b/yams/java/yams/mechanisms/config/FlyWheelConfig.java @@ -28,55 +28,55 @@ public class FlyWheelConfig /** * {@link SmartMotorController} for the {@link FlyWheel} */ - private final SmartMotorController motor; + private Optional motor; /** * The network root of the mechanism (Optional). */ - protected Optional networkTableName = Optional.empty(); + protected Optional networkTableName = Optional.empty(); /** * Minimum velocity of the shooter. */ - private Optional minVelocity = Optional.empty(); + private Optional minVelocity = Optional.empty(); /** * Maximum velocity of the shooter. */ - private Optional maxVelocity = Optional.empty(); + private Optional maxVelocity = Optional.empty(); /** * Telemetry name. */ - private Optional telemetryName = Optional.empty(); + private Optional telemetryName = Optional.empty(); /** * Telemetry verbosity */ - private Optional telemetryVerbosity = Optional.empty(); + private Optional telemetryVerbosity = Optional.empty(); /** * {@link FlyWheel} length for simulation. */ - private Optional length = Optional.empty(); + private Optional length = Optional.empty(); /** * {@link FlyWheel} mass for simulation. */ - private Optional weight = Optional.empty(); + private Optional weight = Optional.empty(); /** * {@link FlyWheel} MOI from CAD software. If not given estimated with length and weight. */ - private OptionalDouble moi = OptionalDouble.empty(); + private OptionalDouble moi = OptionalDouble.empty(); /** * Sim color value */ - private Color8Bit simColor = new Color8Bit(Color.kOrange); + private Color8Bit simColor = new Color8Bit(Color.kOrange); /** * Mechanism position configuration for the {@link yams.mechanisms.positional.Pivot} (Optional). */ - private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); - /* + private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); + /** * Use speedometer simulation for the shooter. */ - private boolean useSpeedometer = false; - /* + private boolean useSpeedometer = false; + /** * Max velocity of the speedometer simulation (Optional). */ - private Optional speedometerMaxVelocity = Optional.empty(); + private Optional speedometerMaxVelocity = Optional.empty(); /** * Arm Configuration class @@ -85,7 +85,57 @@ public class FlyWheelConfig */ public FlyWheelConfig(SmartMotorController motorController) { - motor = motorController; + motor = Optional.ofNullable(motorController); + } + + /** + * FlyWheel configuration class. + * + * @implNote Required to call {@link #withSmartMotorController(SmartMotorController)} before this is used with an + * {@link FlyWheel} + */ + public FlyWheelConfig() {} + + private FlyWheelConfig(FlyWheelConfig cfg) + { + this.motor = cfg.motor; + this.networkTableName = cfg.networkTableName; + this.minVelocity = cfg.minVelocity; + this.maxVelocity = cfg.maxVelocity; + this.telemetryName = cfg.telemetryName; + this.telemetryVerbosity = cfg.telemetryVerbosity; + this.length = cfg.length; + this.weight = cfg.weight; + this.moi = cfg.moi; + this.simColor = cfg.simColor; + this.mechanismPositionConfig = cfg.mechanismPositionConfig; + this.useSpeedometer = cfg.useSpeedometer; + this.speedometerMaxVelocity = cfg.speedometerMaxVelocity; + } + + @Override + public FlyWheelConfig clone() + { + return new FlyWheelConfig(this); + } + + /** + * Set the {@link SmartMotorController} for the {@link FlyWheel} + * + * @param motorController Primary {@link SmartMotorController} for the {@link FlyWheel} + * @return {@link FlyWheelConfig} for chaining. + */ + public FlyWheelConfig withSmartMotorController(SmartMotorController motorController) + { + if (motor.isPresent()) + { + throw new FlyWheelConfigurationException("FlyWheel SmartMotorController already set!", + "Flywheel cannot be set", + "withSmartMotorController(SmartMotorController)"); + } + motor = Optional.of(motorController); + moi.ifPresent(this::withMOI); + return this; } /** @@ -235,22 +285,22 @@ public FlyWheelConfig withSimColor(final Color8Bit simColor) } /** - * Configure the MOI directly instead of estimating it with the length and mass of the - * {@link FlyWheel} for simulation. + * Configure the MOI directly instead of estimating it with the length and mass of the {@link FlyWheel} for + * simulation. * * @param MOI Moment of Inertia of the {@link FlyWheel} * @return {@link FlyWheelConfig} for chaining. */ public FlyWheelConfig withMOI(double MOI) { - motor.getConfig().withMomentOfInertia(MOI); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(MOI)); this.moi = OptionalDouble.of(MOI); return this; } /** - * Configure the MOI directly instead of estimating it with the length and mass of the - * {@link FlyWheel} for simulation. + * Configure the MOI directly instead of estimating it with the length and mass of the {@link FlyWheel} for + * simulation. * * @param length Length of the {@link FlyWheel}. * @param weight Weight of the {@link FlyWheel} @@ -258,7 +308,7 @@ public FlyWheelConfig withMOI(double MOI) */ public FlyWheelConfig withMOI(Distance length, Mass weight) { - motor.getConfig().withMomentOfInertia(length, weight); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(length, weight)); this.moi = OptionalDouble.of(SingleJointedArmSim.estimateMOI(length.in(Meters), weight.in(Kilograms))); return this; } @@ -338,7 +388,7 @@ public FlyWheelConfig withTelemetry(String networkRoot, String telemetryName, Te */ public boolean applyConfig() { - return motor.applyConfig(motor.getConfig()); + return motor.orElseThrow().applyConfig(motor.orElseThrow().getConfig()); } /** @@ -419,7 +469,7 @@ public Optional getTelemetryName() */ public SmartMotorController getMotor() { - return motor; + return motor.orElseThrow(); } /** diff --git a/yams/java/yams/mechanisms/config/PivotConfig.java b/yams/java/yams/mechanisms/config/PivotConfig.java index bf8560ad..7f7c1b97 100644 --- a/yams/java/yams/mechanisms/config/PivotConfig.java +++ b/yams/java/yams/mechanisms/config/PivotConfig.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.Pair; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Mass; @@ -26,40 +27,52 @@ public class PivotConfig /** * {@link SmartMotorController} for the {@link yams.mechanisms.positional.Pivot} */ - private final SmartMotorController motor; + private Optional motor; /** * The network root of the mechanism (Optional). */ @Deprecated - protected Optional networkRoot = Optional.empty(); + protected Optional networkRoot = Optional.empty(); /** * Telemetry name. */ - private Optional telemetryName = Optional.empty(); + private Optional telemetryName = Optional.empty(); /** * Telemetry verbosity */ - private Optional telemetryVerbosity = Optional.empty(); + private Optional telemetryVerbosity = Optional.empty(); /** * Lower Hard Limit for the {@link yams.mechanisms.positional.Pivot} to be representing in simulation. */ - private Optional lowerHardLimit = Optional.empty(); + private Optional lowerHardLimit = Optional.empty(); /** * Upper hard limit for the {@link yams.mechanisms.positional.Pivot} representing in simulation. */ - private Optional upperHardLimit = Optional.empty(); + private Optional upperHardLimit = Optional.empty(); /** * {@link yams.mechanisms.positional.Pivot} MOI from CAD software. If not given estimated with length and weight. */ - private OptionalDouble moi = OptionalDouble.empty(); + private OptionalDouble moi = OptionalDouble.empty(); /** * Sim color value */ - private Color8Bit simColor = new Color8Bit(Color.kOrange); + private Color8Bit simColor = new Color8Bit(Color.kOrange); /** * Mechanism position configuration for the {@link yams.mechanisms.positional.Pivot} */ - private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); + private MechanismPositionConfig mechanismPositionConfig = new MechanismPositionConfig(); + /** + * Starting position of the pivot {@link SmartMotorController} motor encoder. + */ + private Optional startingPosition = Optional.empty(); + /** + * Soft limits of the pivot motor {@link SmartMotorController} closed loop controller. (LowerLimit, UpperLimit) + */ + private Optional> softLimits = Optional.empty(); + /** + * Continuous wrapping of the pivot {@link SmartMotorController} closed loop controller. (Min, Max) + */ + private Optional> continuousWrapping = Optional.empty(); /** * Pivot Configuration class @@ -68,10 +81,69 @@ public class PivotConfig */ public PivotConfig(SmartMotorController motorController) { - motor = motorController; + motor = Optional.ofNullable(motorController); mechanismPositionConfig.withMovementPlane(Plane.XY); } + /** + * Pivot Configuration class + * + * @implNote Required call to {@link #withSmartMotorController(SmartMotorController)} before usage. + */ + public PivotConfig() + { + mechanismPositionConfig.withMovementPlane(Plane.XY); + } + + /** + * Copy constructor. + * + * @param cfg Config to copy. + */ + public PivotConfig(PivotConfig cfg) + { + this.motor = cfg.motor; + this.networkRoot = cfg.networkRoot; + this.telemetryName = cfg.telemetryName; + this.telemetryVerbosity = cfg.telemetryVerbosity; + this.lowerHardLimit = cfg.lowerHardLimit; + this.upperHardLimit = cfg.upperHardLimit; + this.moi = cfg.moi; + this.simColor = cfg.simColor; + this.mechanismPositionConfig = cfg.mechanismPositionConfig; + this.startingPosition = cfg.startingPosition; + this.softLimits = cfg.softLimits; + this.continuousWrapping = cfg.continuousWrapping; + } + + @Override + public PivotConfig clone() + { + return new PivotConfig(this); + } + + /** + * Configure the {@link SmartMotorController} for the {@link yams.mechanisms.positional.Pivot} + * + * @param motorController {@link SmartMotorController} for the {@link yams.mechanisms.positional.Pivot}. + * @return {@link PivotConfig} for chaining. + */ + public PivotConfig withSmartMotorController(SmartMotorController motorController) + { + if (motor.isPresent()) + { + throw new PivotConfigurationException("Pivot SmartMotorController already set!", + "Pivot motor cannot be set", + "withSmartMotorController(SmartMotorController)"); + } + motor = Optional.of(motorController); + startingPosition.ifPresent(this::withStartingPosition); + softLimits.ifPresent(softLimits -> withSoftLimits(softLimits.getFirst(), softLimits.getSecond())); + continuousWrapping.ifPresent(continuousWrapping -> withWrapping(continuousWrapping.getFirst(), + continuousWrapping.getSecond())); + return this; + } + /** * Publish the color in sim as this. * @@ -93,7 +165,7 @@ public PivotConfig withSimColor(final Color8Bit simColor) */ public PivotConfig withMOI(double MOI) { - motor.getConfig().withMomentOfInertia(MOI); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(MOI)); this.moi = OptionalDouble.of(MOI); return this; } @@ -108,7 +180,7 @@ public PivotConfig withMOI(double MOI) */ public PivotConfig withMOI(Distance length, Mass weight) { - motor.getConfig().withMomentOfInertia(length, weight); + motor.ifPresent(motor -> motor.getConfig().withMomentOfInertia(length, weight)); this.moi = OptionalDouble.of(SingleJointedArmSim.estimateMOI(length.in(Meters), weight.in(Kilograms))); return this; } @@ -145,7 +217,7 @@ public PivotConfig withTelemetry(String networkRoot, String telemetryName, Telem } /** - * Set the elevator mechnism position configuration. + * Set the elevator mechanism position configuration. * * @param mechanismPositionConfig {@link MechanismPositionConfig} for the {@link yams.mechanisms.positional.Elevator} * @return {@link PivotConfig} for chaining @@ -164,7 +236,8 @@ public PivotConfig withMechanismPositionConfig(MechanismPositionConfig mechanism */ public PivotConfig withStartingPosition(Angle startingPosition) { - motor.getConfig().withStartingPosition(startingPosition); + this.startingPosition = Optional.ofNullable(startingPosition); + motor.ifPresent(motor -> motor.getConfig().withStartingPosition(startingPosition)); return this; } @@ -177,7 +250,8 @@ public PivotConfig withStartingPosition(Angle startingPosition) */ public PivotConfig withSoftLimits(Angle lowerLimit, Angle upperLimit) { - motor.getConfig().withSoftLimit(lowerLimit, upperLimit); + softLimits = Optional.of(Pair.of(lowerLimit, upperLimit)); + motor.ifPresent(motor -> motor.getConfig().withSoftLimit(lowerLimit, upperLimit)); return this; } @@ -191,7 +265,8 @@ public PivotConfig withSoftLimits(Angle lowerLimit, Angle upperLimit) */ public PivotConfig withWrapping(Angle min, Angle max) { - motor.getConfig().withContinuousWrapping(min, max); + continuousWrapping = Optional.of(Pair.of(min, max)); + motor.ifPresent(motor -> motor.getConfig().withContinuousWrapping(min, max)); return this; } @@ -216,7 +291,7 @@ public PivotConfig withHardLimit(Angle min, Angle max) */ public boolean applyConfig() { - return motor.applyConfig(motor.getConfig()); + return motor.orElseThrow().applyConfig(motor.orElseThrow().getConfig()); } @@ -283,7 +358,7 @@ public Optional getTelemetryName() */ public Optional getStartingAngle() { - return motor.getConfig().getStartingPosition(); + return motor.orElseThrow().getConfig().getStartingPosition(); } /** @@ -293,7 +368,7 @@ public Optional getStartingAngle() */ public SmartMotorController getMotor() { - return motor; + return motor.orElseThrow(); } /** diff --git a/yams/java/yams/mechanisms/config/SwerveModuleConfig.java b/yams/java/yams/mechanisms/config/SwerveModuleConfig.java index 062f9105..33a48e1e 100644 --- a/yams/java/yams/mechanisms/config/SwerveModuleConfig.java +++ b/yams/java/yams/mechanisms/config/SwerveModuleConfig.java @@ -28,53 +28,60 @@ public class SwerveModuleConfig /** * {@link SmartMotorController} for the {@link yams.mechanisms.swerve.SwerveModule} */ - private final SmartMotorController driveMotor; + private Optional driveMotor; /** * {@link SmartMotorController} for the {@link yams.mechanisms.swerve.SwerveModule} */ - private final SmartMotorController azimuthMotor; + private Optional azimuthMotor; /** * Telemetry name. */ - private Optional telemetryName = Optional.empty(); + private Optional telemetryName = Optional.empty(); /** * Telemetry verbosity */ - private Optional telemetryVerbosity = Optional.empty(); + private Optional telemetryVerbosity = Optional.empty(); /** * Absolute encoder supplier for the azimuth {@link SmartMotorController}. */ - private Optional> absoluteEncoderSupplier = Optional.empty(); + private Optional> absoluteEncoderSupplier = Optional.empty(); /** * Absolute encoder offset for the azimuth {@link SmartMotorController} to 0 with the bevel facing left. */ - private Optional absoluteEncoderOffset = Optional.empty(); + private Optional absoluteEncoderOffset = Optional.empty(); /** * Gearbox for the absolute encoder. */ - private GearBox absoluteEncoderGearbox = new GearBox(new double[]{1}); + private GearBox absoluteEncoderGearbox = new GearBox(new double[]{1}); /** * Swerve module state optimization using * {@link edu.wpi.first.math.kinematics.SwerveModuleState#optimize(Rotation2d)}. */ - private boolean swerveModuleStateOptimization = true; + private boolean swerveModuleStateOptimization = true; /** * Swerve module cosine compensation. */ - private boolean cosineCompensation = false; + private boolean cosineCompensation = false; /** * Coupling ratio for the {@link yams.mechanisms.swerve.SwerveModule}. */ - private GearBox couplingRatio; + private GearBox couplingRatio; /** * Swerve module minimum velocity. */ - private Optional minimumVelocity = Optional.empty(); + private Optional minimumVelocity = Optional.empty(); /** * Distance from the center of rotation for the {@link yams.mechanisms.swerve.SwerveModule}. */ - private Optional distanceFromCenterOfRotation = Optional.empty(); - + private Optional distanceFromCenterOfRotation = Optional.empty(); + /** + * Absolute encoder for the azimuth {@link SmartMotorController}, must be of the same vendor as the azimuth motor. + */ + private Optional absoluteEncoder = Optional.empty(); + /** + * Wheel circumference for the drive {@link SmartMotorController}. + */ + private Optional wheelCircumference = Optional.empty(); /** * Create the {@link SwerveModuleConfig} for the {@link yams.mechanisms.swerve.SwerveModule} * @@ -83,8 +90,65 @@ public class SwerveModuleConfig */ public SwerveModuleConfig(SmartMotorController drive, SmartMotorController azimuth) { - this.driveMotor = drive; - this.azimuthMotor = azimuth; + this.driveMotor = Optional.ofNullable(drive); + this.azimuthMotor = Optional.ofNullable(azimuth); + } + + /** + * Create the {@link SwerveModuleConfig} for the {@link yams.mechanisms.swerve.SwerveModule} + * + * @implNote Required to use {@link #withSmartMotorController(SmartMotorController, SmartMotorController)} BEFORE + * passed into {@link yams.mechanisms.swerve.SwerveModule} + */ + public SwerveModuleConfig() {} + + /** + * Copy constructor. + * + * @param cfg Config to copy. + */ + private SwerveModuleConfig(SwerveModuleConfig cfg) + { + this.driveMotor = cfg.driveMotor; + this.azimuthMotor = cfg.azimuthMotor; + this.telemetryName = cfg.telemetryName; + this.telemetryVerbosity = cfg.telemetryVerbosity; + this.absoluteEncoderSupplier = cfg.absoluteEncoderSupplier; + this.absoluteEncoderOffset = cfg.absoluteEncoderOffset; + this.absoluteEncoderGearbox = cfg.absoluteEncoderGearbox; + this.swerveModuleStateOptimization = cfg.swerveModuleStateOptimization; + this.cosineCompensation = cfg.cosineCompensation; + this.couplingRatio = cfg.couplingRatio; + this.minimumVelocity = cfg.minimumVelocity; + this.distanceFromCenterOfRotation = cfg.distanceFromCenterOfRotation; + this.absoluteEncoder = cfg.absoluteEncoder; + this.wheelCircumference = cfg.wheelCircumference; + } + + @Override + public SwerveModuleConfig clone() + { + return new SwerveModuleConfig(this); + } + + /** + * Set the {@link SmartMotorController} for the {@link yams.mechanisms.swerve.SwerveModule}. + * + * @param driveMotor {@link SmartMotorController} for the drive motor. + * @param azimuthMotor {@link SmartMotorController} for the azimuth motor. + * @return {@link SwerveModuleConfig} for chaining. + */ + public SwerveModuleConfig withSmartMotorController(SmartMotorController driveMotor, SmartMotorController azimuthMotor) + { + if (this.driveMotor.isPresent()) + {throw new IllegalStateException("Drive motor controller already set.");} + if (this.azimuthMotor.isPresent()) + {throw new IllegalStateException("Azimuth motor controller already set.");} + this.driveMotor = Optional.of(driveMotor); + this.azimuthMotor = Optional.of(azimuthMotor); + absoluteEncoder.ifPresent(this::withAbsoluteEncoder); + wheelCircumference.ifPresent(circumference -> driveMotor.getConfig().withMechanismCircumference(circumference)); + return this; } /** @@ -109,7 +173,8 @@ public SwerveModuleConfig withCosineCompensation(boolean compensate) */ public SwerveModuleConfig withAbsoluteEncoder(Object absoluteEncoder) { - azimuthMotor.getConfig().withExternalEncoder(absoluteEncoder); + this.absoluteEncoder = Optional.ofNullable(absoluteEncoder); + azimuthMotor.ifPresent(azimuthMotor -> azimuthMotor.getConfig().withExternalEncoder(absoluteEncoder)); return this; } @@ -198,9 +263,12 @@ public SwerveModuleConfig withAbsoluteEncoder(DoubleSupplier rotationSupplier) public SwerveModuleConfig withAbsoluteEncoderGearing(GearBox gearing) { absoluteEncoderGearbox = gearing; - SmartMotorControllerConfig azimuthConfig = azimuthMotor.getConfig(); - if (azimuthConfig.getExternalEncoder().isPresent()) - {azimuthConfig.withExternalEncoderGearing(new MechanismGearing(gearing));} + if (azimuthMotor.isPresent()) + { + SmartMotorControllerConfig azimuthConfig = azimuthMotor.orElseThrow().getConfig(); + if (azimuthConfig.getExternalEncoder().isPresent()) + {azimuthConfig.withExternalEncoderGearing(new MechanismGearing(gearing));} + } return this; } @@ -215,16 +283,17 @@ public SwerveModuleConfig withAbsoluteEncoderOffset(Angle offset) { absoluteEncoderOffset = Optional.ofNullable(offset); - SmartMotorControllerConfig azimuthConfig = azimuthMotor.getConfig(); - if (azimuthConfig.getExternalEncoder().isPresent()) + if (azimuthMotor.isPresent()) { - azimuthConfig.withExternalEncoderZeroOffset(offset); + SmartMotorControllerConfig azimuthConfig = azimuthMotor.orElseThrow().getConfig(); + if (azimuthConfig.getExternalEncoder().isPresent()) + {azimuthConfig.withExternalEncoderZeroOffset(offset);} } return this; } /** - * Set the wheel radius for the {@link SmartMotorController}, ideally should be set inside of the drive motor + * Set the wheel radius for the {@link SmartMotorController}, ideally should be set inside the drive motor * {@link SmartMotorControllerConfig} * * @param radius Radius of the wheel. @@ -232,13 +301,14 @@ public SwerveModuleConfig withAbsoluteEncoderOffset(Angle offset) */ public SwerveModuleConfig withWheelRadius(Distance radius) { - SmartMotorControllerConfig driveConfig = driveMotor.getConfig(); - driveConfig.withMechanismCircumference(radius.times(2)); + wheelCircumference = Optional.ofNullable(radius.times(2.0).times(Math.PI)); + driveMotor.ifPresent(driveMotor -> driveMotor.getConfig() + .withMechanismCircumference(radius.times(2.0).times(Math.PI))); return this; } /** - * Set the wheel diameter for the {@link SmartMotorController}, ideally should be set inside of the drive motor + * Set the wheel diameter for the {@link SmartMotorController}, ideally should be set inside the drive motor * {@link SmartMotorControllerConfig}. * * @param diameter Diameter of the wheel. @@ -246,8 +316,9 @@ public SwerveModuleConfig withWheelRadius(Distance radius) */ public SwerveModuleConfig withWheelDiameter(Distance diameter) { - SmartMotorControllerConfig driveConfig = driveMotor.getConfig(); - driveConfig.withMechanismCircumference(diameter); + wheelCircumference = Optional.ofNullable(diameter.times(Math.PI)); + driveMotor.ifPresent(driveMotor -> driveMotor.getConfig() + .withMechanismCircumference(diameter.times(Math.PI))); return this; } @@ -299,7 +370,7 @@ public Angle getAbsoluteEncoderAngle() return absoluteEncoderSupplier.map(angleSupplier -> angleSupplier.get() .times(absoluteEncoderGearbox.getInputToOutputConversionFactor()) .minus(absoluteEncoderOffset.orElse(Rotations.of(0)))) - .orElse(azimuthMotor.getMechanismPosition()); + .orElse(azimuthMotor.orElseThrow().getMechanismPosition()); } /** @@ -319,7 +390,7 @@ public boolean getStateOptimization() */ public SmartMotorController getDriveMotor() { - return driveMotor; + return driveMotor.orElseThrow(); } /** @@ -329,7 +400,7 @@ public SmartMotorController getDriveMotor() */ public SmartMotorController getAzimuthMotor() { - return azimuthMotor; + return azimuthMotor.orElseThrow(); } /**