diff --git a/src/main/java/frc/lib/InterpolatableColor.java b/src/main/java/frc/lib/InterpolatableColor.java index dafc608..52522e0 100644 --- a/src/main/java/frc/lib/InterpolatableColor.java +++ b/src/main/java/frc/lib/InterpolatableColor.java @@ -5,27 +5,26 @@ public class InterpolatableColor { - private final Color zero, one; + private final Color zero, one; - public InterpolatableColor(Color zero, Color one) { - this.zero = zero; - this.one = one; - } + public InterpolatableColor(Color zero, Color one) { + this.zero = zero; + this.one = one; + } - public Color sample(double t) { - t = MathUtil.clamp(t, 0, 1); + public Color sample(double t) { + t = MathUtil.clamp(t, 0, 1); - double r = zero.red * (1 - t) + one.red * t; - double g = zero.green * (1 - t) + one.green * t; - double b = zero.blue * (1 - t) + one.blue * t; - - return new Color(r, g, b); - } + double r = zero.red * (1 - t) + one.red * t; + double g = zero.green * (1 - t) + one.green * t; + double b = zero.blue * (1 - t) + one.blue * t; - public Color sample(double x, double min, double max) { - double t = x / (max - min); + return new Color(r, g, b); + } - return sample(t); - } + public Color sample(double x, double min, double max) { + double t = x / (max - min); + return sample(t); + } } diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index 4534470..2307fc7 100644 --- a/src/main/java/frc/robot/RobotMechanisms.java +++ b/src/main/java/frc/robot/RobotMechanisms.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.arm.ArmConstants.ShoulderMotorConstants; import frc.lib.InterpolatableColor; +import frc.robot.arm.ArmConstants.ShoulderMotorConstants; import frc.robot.arm.ArmState; import frc.robot.intake.IntakeConstants.PivotMotorConstants; import frc.robot.intake.IntakeConstants.RollerMotorConstants; @@ -36,9 +36,12 @@ public class RobotMechanisms { private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray); - private final InterpolatableColor shooterColor = new InterpolatableColor(Color.kLightGray, Color.kSalmon); - private final InterpolatableColor serializerColor = new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue); - private final InterpolatableColor intakeColor = new InterpolatableColor(Color.kLightGray, Color.kPaleGreen); + private final InterpolatableColor shooterColor = + new InterpolatableColor(Color.kLightGray, Color.kSalmon); + private final InterpolatableColor serializerColor = + new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue); + private final InterpolatableColor intakeColor = + new InterpolatableColor(Color.kLightGray, Color.kPaleGreen); private RobotMechanisms() { mechanism = new Mechanism2d(WIDTH, HEIGHT); @@ -67,20 +70,12 @@ private void initializeArmMechanism() { shooter = shoulder.append( new MechanismLigament2d( - "wrist", - Units.inchesToMeters(4.321), - 0, - armThickness, - DEFAULT_COLOR)); + "wrist", Units.inchesToMeters(4.321), 0, armThickness, DEFAULT_COLOR)); serializer = shoulder.append( new MechanismLigament2d( - "serializer", - Units.inchesToMeters(8.771), - 0, - armThickness, - DEFAULT_COLOR)); + "serializer", Units.inchesToMeters(8.771), 0, armThickness, DEFAULT_COLOR)); } private void initializeFramePerimeterMechanisms() { @@ -93,11 +88,7 @@ private void initializeFramePerimeterMechanisms() { 0) .append( new MechanismLigament2d( - "framePerimeterLeft_", - HEIGHT, - 90, - framePerimeterThickness, - DEFAULT_COLOR)); + "framePerimeterLeft_", HEIGHT, 90, framePerimeterThickness, DEFAULT_COLOR)); mechanism .getRoot( @@ -106,11 +97,7 @@ private void initializeFramePerimeterMechanisms() { 0) .append( new MechanismLigament2d( - "framePerimeterRight_", - HEIGHT, - 90, - framePerimeterThickness, - DEFAULT_COLOR)); + "framePerimeterRight_", HEIGHT, 90, framePerimeterThickness, DEFAULT_COLOR)); } private void initializeIntakeMechanism() { @@ -124,11 +111,7 @@ private void initializeIntakeMechanism() { ORIGIN.getY() + Units.inchesToMeters(6.283)) .append( new MechanismLigament2d( - "intake_", - PivotMotorConstants.DISTANCE, - 0.0, - intakeThickness, - DEFAULT_COLOR)); + "intake_", PivotMotorConstants.DISTANCE, 0.0, intakeThickness, DEFAULT_COLOR)); } public static RobotMechanisms getInstance() { @@ -149,7 +132,8 @@ public void updateArm(ArmState state) { shoulder.setAngle(shoulderRotation); - // Offset the rendered wrist rotation so that zero degrees is perpendicular to the shoulder, rather than parallel with the shoulder + // Offset the rendered wrist rotation so that zero degrees is perpendicular to the shoulder, + // rather than parallel with the shoulder Rotation2d offsetWristRotation = wristRotation.plus(Rotation2d.fromDegrees(90)); Rotation2d shooterRotation = offsetWristRotation; Rotation2d serializerRotation = offsetWristRotation.plus(Rotation2d.fromDegrees(180)); @@ -159,20 +143,26 @@ public void updateArm(ArmState state) { } public void updateIntake(Rotation2d pivotAngle, double rollerVelocityRotationsPerSecond) { - Color color = intakeColor.sample(Math.abs(rollerVelocityRotationsPerSecond), 0, RollerMotorConstants.MAXIMUM_SPEED); + Color color = + intakeColor.sample( + Math.abs(rollerVelocityRotationsPerSecond), 0, RollerMotorConstants.MAXIMUM_SPEED); intake.setAngle(pivotAngle); intake.setColor(new Color8Bit(color)); } public void updateShooter(double velocityMetersPerSecond) { - Color color = shooterColor.sample(Math.abs(velocityMetersPerSecond), 0, FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED); + Color color = + shooterColor.sample( + Math.abs(velocityMetersPerSecond), 0, FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED); shooter.setColor(new Color8Bit(color)); } public void updateSerializer(double velocityMetersPerSecond) { - Color color = serializerColor.sample(Math.abs(velocityMetersPerSecond), 0, SerializerConstants.MAXIMUM_TANGENTIAL_SPEED); + Color color = + serializerColor.sample( + Math.abs(velocityMetersPerSecond), 0, SerializerConstants.MAXIMUM_TANGENTIAL_SPEED); serializer.setColor(new Color8Bit(color)); } diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java index a838e81..97e393d 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java @@ -24,7 +24,8 @@ public void configure() {} public void update(ShoulderMotorIOValues values) { values.positionRotations = this.positionRotations; values.velocityRotationsPerSecond = this.velocityRotationsPerSecond; - values.accelerationRotationsPerSecondPerSecond = accelerationCalculator.calculate(velocityRotationsPerSecond); + values.accelerationRotationsPerSecondPerSecond = + accelerationCalculator.calculate(velocityRotationsPerSecond); values.inputVoltage = inputVoltage; } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 7a83b7c..5db1bcf 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -94,7 +94,8 @@ public void periodic() { rollerMotorCurrentFilter.calculate(rollerMotorValues.currentAmps); - mechanism.updateIntake(Rotation2d.fromRotations(pivotMotorValues.positionRotations), getRollerVelocity()); + mechanism.updateIntake( + Rotation2d.fromRotations(pivotMotorValues.positionRotations), getRollerVelocity()); } @Override diff --git a/src/main/java/frc/robot/intake/PivotMotorIOSim.java b/src/main/java/frc/robot/intake/PivotMotorIOSim.java index 9ad8c9c..105ced4 100644 --- a/src/main/java/frc/robot/intake/PivotMotorIOSim.java +++ b/src/main/java/frc/robot/intake/PivotMotorIOSim.java @@ -6,6 +6,7 @@ public class PivotMotorIOSim implements PivotMotorIO { private double positionRotations; + // private double velocityRotationsPerSecond; @Override