diff --git a/src/main/java/frc/lib/config/MechanismConfig.java b/src/main/java/frc/lib/config/MechanismConfig.java index e3c1086..423e2a3 100644 --- a/src/main/java/frc/lib/config/MechanismConfig.java +++ b/src/main/java/frc/lib/config/MechanismConfig.java @@ -6,6 +6,7 @@ import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; import frc.lib.config.MotorConfig.MotorConfigBuilder; import java.util.Objects; +import java.util.function.Function; /** Mechanism config. */ public record MechanismConfig( @@ -68,31 +69,32 @@ private MechanismConfigBuilder( } public MechanismConfigBuilder absoluteEncoderConfig( - AbsoluteEncoderConfigBuilder absoluteEncoderConfigBuilder) { - this.absoluteEncoderConfigBuilder = absoluteEncoderConfigBuilder; + Function modifier) { + this.absoluteEncoderConfigBuilder = modifier.apply(absoluteEncoderConfigBuilder); return this; } public MechanismConfigBuilder feedbackControllerConfig( - FeedbackControllerConfigBuilder feedbackControllerConfigBuilder) { - this.feedbackControllerConfigBuilder = feedbackControllerConfigBuilder; + Function modifier) { + this.feedbackControllerConfigBuilder = modifier.apply(feedbackControllerConfigBuilder); return this; } public MechanismConfigBuilder feedforwardControllerConfig( - FeedforwardControllerConfigBuilder feedforwardControllerConfigBuilder) { - this.feedforwardControllerConfigBuilder = feedforwardControllerConfigBuilder; + Function modifier) { + this.feedforwardControllerConfigBuilder = modifier.apply(feedforwardControllerConfigBuilder); return this; } public MechanismConfigBuilder motionProfileConfig( - MotionProfileConfigBuilder motionProfileConfigBuilder) { - this.motionProfileConfigBuilder = motionProfileConfigBuilder; + Function modifier) { + this.motionProfileConfigBuilder = modifier.apply(motionProfileConfigBuilder); return this; } - public MechanismConfigBuilder motorConfig(MotorConfigBuilder motorConfigBuilder) { - this.motorConfigBuilder = motorConfigBuilder; + public MechanismConfigBuilder motorConfig( + Function modifier) { + this.motorConfigBuilder = modifier.apply(motorConfigBuilder); return this; } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index cbc1a3c..0b60dc8 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -7,13 +7,8 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.config.AbsoluteEncoderConfig.AbsoluteEncoderConfigBuilder; -import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; -import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; import frc.lib.config.MechanismConfig.MechanismConfigBuilder; -import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; -import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.PositionControllerIO; import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; @@ -27,21 +22,18 @@ public class Arm extends Subsystem { private final MechanismConfig shoulderConfig = MechanismConfigBuilder.defaults() .absoluteEncoderConfig( - AbsoluteEncoderConfigBuilder.defaults() - .ccwPositive(false) - .offset(Rotation2d.fromDegrees(-173.135))) + absoluteEncoder -> + absoluteEncoder.ccwPositive(false).offset(Rotation2d.fromDegrees(-173.135))) .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(true) - .neutralBrake(true) - .motorToMechanismRatio(39.771428571)) + motor -> + motor.ccwPositive(true).neutralBrake(true).motorToMechanismRatio(39.771428571)) .motionProfileConfig( - MotionProfileConfigBuilder.defaults() - .maximumVelocity(Units.degreesToRotations(240.0)) - .maximumAcceleration(Units.degreesToRotations(240.0))) - .feedforwardControllerConfig( - FeedforwardControllerConfigBuilder.defaults().kS(0.14).kG(0.5125).kV(4.0)) - .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(4.0)) + motionProfile -> + motionProfile + .maximumVelocity(Units.degreesToRotations(240.0)) + .maximumAcceleration(Units.degreesToRotations(240.0))) + .feedforwardControllerConfig(feedforward -> feedforward.kS(0.14).kG(0.5125).kV(4.0)) + .feedbackControllerConfig(feedback -> feedback.kP(4.0)) .build(); /** Shoulder controller. */ diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index cd365e5..afea39a 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -3,12 +3,8 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; -import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; -import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; import frc.lib.config.MechanismConfig.MechanismConfigBuilder; -import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; -import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; @@ -22,28 +18,22 @@ public class Intake extends Subsystem { private final MechanismConfig frontRollerConfig = MechanismConfigBuilder.defaults() .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(false) - .neutralBrake(false) - .motorToMechanismRatio(24.0 / 16.0)) - .motionProfileConfig(MotionProfileConfigBuilder.defaults().maximumVelocity(66)) - .feedforwardControllerConfig( - FeedforwardControllerConfigBuilder.defaults().kS(0.13).kV(0.1683)) - .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.1)) + motor -> + motor.ccwPositive(false).neutralBrake(false).motorToMechanismRatio(24.0 / 16.0)) + .motionProfileConfig(motionProfile -> motionProfile.maximumVelocity(66)) + .feedforwardControllerConfig(feedforward -> feedforward.kS(0.13).kV(0.1683)) + .feedbackControllerConfig(feedback -> feedback.kP(0.1)) .build(); /** Back roller controller config. */ private final MechanismConfig backRollerConfig = MechanismConfigBuilder.defaults() .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(false) - .neutralBrake(false) - .motorToMechanismRatio(24.0 / 16.0)) - .motionProfileConfig(MotionProfileConfigBuilder.defaults().maximumVelocity(66)) - .feedforwardControllerConfig( - FeedforwardControllerConfigBuilder.defaults().kS(0.13).kV(0.1759)) - .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.1)) + motor -> + motor.ccwPositive(false).neutralBrake(false).motorToMechanismRatio(24.0 / 16.0)) + .motionProfileConfig(motionProfile -> motionProfile.maximumVelocity(66)) + .feedforwardControllerConfig(feedforward -> feedforward.kS(0.13).kV(0.1759)) + .feedbackControllerConfig(feedback -> feedback.kP(0.1)) .build(); /** Rollers. */ diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index b9c626a..082d6f1 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -4,12 +4,8 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; -import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; -import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; import frc.lib.config.MechanismConfig.MechanismConfigBuilder; -import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; -import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; @@ -23,15 +19,12 @@ public class Shooter extends Subsystem { private final MechanismConfig flywheelConfig = MechanismConfigBuilder.defaults() .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(false) - .neutralBrake(true) - .motorToMechanismRatio(28.0 / 16.0)) + motor -> + motor.ccwPositive(false).neutralBrake(true).motorToMechanismRatio(28.0 / 16.0)) .motionProfileConfig( - MotionProfileConfigBuilder.defaults().maximumVelocity(60).maximumAcceleration(200)) - .feedforwardControllerConfig( - FeedforwardControllerConfigBuilder.defaults().kS(0.14).kV(0.2)) - .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.14)) + motionProfile -> motionProfile.maximumVelocity(60).maximumAcceleration(200)) + .feedforwardControllerConfig(feedforward -> feedforward.kS(0.14).kV(0.2)) + .feedbackControllerConfig(feedback -> feedback.kP(0.14)) .build(); /** Flywheel controller. */ @@ -47,14 +40,15 @@ public class Shooter extends Subsystem { private final MechanismConfig serializerConfig = MechanismConfigBuilder.defaults() .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(true) - .neutralBrake(false) - .motorToMechanismRatio(36.0 / 16.0)) + motorConfig -> + motorConfig + .ccwPositive(true) + .neutralBrake(false) + .motorToMechanismRatio(36.0 / 16.0)) .motionProfileConfig( - MotionProfileConfigBuilder.defaults().maximumVelocity(45).maximumAcceleration(450)) - .feedforwardControllerConfig( - FeedforwardControllerConfigBuilder.defaults().kS(0.14).kV(0.2617)) + motionProfileConfig -> + motionProfileConfig.maximumVelocity(45).maximumAcceleration(450)) + .feedforwardControllerConfig(feedforwardConfig -> feedforwardConfig.kS(0.14).kV(0.2617)) .build(); /** Serializer controller. */ diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index af4ae55..7649c0e 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -14,13 +14,10 @@ import frc.lib.DriveRequest; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.lib.config.FeedbackControllerConfig.FeedbackControllerConfigBuilder; -import frc.lib.config.FeedforwardControllerConfig.FeedforwardControllerConfigBuilder; import frc.lib.config.MechanismConfig; import frc.lib.config.MechanismConfig.MechanismConfigBuilder; import frc.lib.config.MotionProfileConfig; import frc.lib.config.MotionProfileConfig.MotionProfileConfigBuilder; -import frc.lib.config.MotorConfig.MotorConfigBuilder; import frc.lib.controller.SwerveModuleIO; import frc.robot.RobotConstants; import frc.robot.odometry.Odometry; @@ -42,30 +39,25 @@ public class Swerve extends Subsystem { private final MechanismConfig steerConfig = MechanismConfigBuilder.defaults() .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(false) - .currentLimitAmps(20) - .motorToMechanismRatio(150.0 / 7.0)) - .feedforwardControllerConfig(FeedforwardControllerConfigBuilder.defaults().kS(0.205)) + motor -> + motor.ccwPositive(false).currentLimitAmps(20).motorToMechanismRatio(150.0 / 7.0)) + .feedforwardControllerConfig(feedforward -> feedforward.kS(0.205)) .feedbackControllerConfig( - FeedbackControllerConfigBuilder.defaults() - .continuous(true) - .kP(54.0) - .kD(0.16) - .positionTolerance(Units.degreesToRotations(1.0))) + feedback -> + feedback + .continuous(true) + .kP(54.0) + .kD(0.16) + .positionTolerance(Units.degreesToRotations(1.0))) .build(); /** Drive motor config. */ private final MechanismConfig driveConfig = MechanismConfigBuilder.defaults() .motorConfig( - MotorConfigBuilder.defaults() - .ccwPositive(false) - .currentLimitAmps(40.0) - .motorToMechanismRatio(6.75)) - .feedforwardControllerConfig( - FeedforwardControllerConfigBuilder.defaults().kS(0.14).kV(0.725)) - .feedbackControllerConfig(FeedbackControllerConfigBuilder.defaults().kP(0.75)) + motor -> motor.ccwPositive(false).currentLimitAmps(40.0).motorToMechanismRatio(6.75)) + .feedforwardControllerConfig(feedforward -> feedforward.kS(0.14).kV(0.725)) + .feedbackControllerConfig(feedback -> feedback.kP(0.75)) .build(); /** Wheel circumference. */ diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index a3d72e1..9ea4210 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.lib.CAN; -import frc.lib.config.AbsoluteEncoderConfig.AbsoluteEncoderConfigBuilder; import frc.lib.config.MechanismConfig; import frc.lib.config.MechanismConfig.MechanismConfigBuilder; import frc.lib.controller.PositionControllerIO; @@ -53,8 +52,8 @@ public static SwerveModuleIO createNorthWestModule( new CAN(24, "swerve"), MechanismConfigBuilder.from(steerConfig) .absoluteEncoderConfig( - AbsoluteEncoderConfigBuilder.defaults() - .offset(Rotation2d.fromRotations(-0.084716).unaryMinus())) + absoluteEncoderConfig -> + absoluteEncoderConfig.offset(Rotation2d.fromRotations(-0.084716).unaryMinus())) .build(), driveConfig, wheelCircumference); @@ -82,8 +81,8 @@ public static SwerveModuleIO createNorthEastModule( new CAN(30, "swerve"), MechanismConfigBuilder.from(steerConfig) .absoluteEncoderConfig( - AbsoluteEncoderConfigBuilder.defaults() - .offset(Rotation2d.fromRotations(0.196777).unaryMinus())) + absoluteEncoderConfig -> + absoluteEncoderConfig.offset(Rotation2d.fromRotations(0.196777).unaryMinus())) .build(), driveConfig, wheelCircumference); @@ -111,8 +110,8 @@ public static SwerveModuleIO createSouthEastModule( new CAN(26, "swerve"), MechanismConfigBuilder.from(steerConfig) .absoluteEncoderConfig( - AbsoluteEncoderConfigBuilder.defaults() - .offset(Rotation2d.fromRotations(0.276611).unaryMinus())) + absoluteEncoderConfig -> + absoluteEncoderConfig.offset(Rotation2d.fromRotations(0.276611).unaryMinus())) .build(), driveConfig, wheelCircumference); @@ -140,8 +139,8 @@ public static SwerveModuleIO createSouthWestModule( new CAN(28, "swerve"), MechanismConfigBuilder.from(steerConfig) .absoluteEncoderConfig( - AbsoluteEncoderConfigBuilder.defaults() - .offset(Rotation2d.fromRotations(0.223145).unaryMinus())) + absoluteEncoderConfig -> + absoluteEncoderConfig.offset(Rotation2d.fromRotations(0.223145).unaryMinus())) .build(), driveConfig, wheelCircumference);