diff --git a/simgui-ds.json b/simgui-ds.json index acf3b7e..8ddf38c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -101,7 +101,8 @@ "useGamepad": true }, { - "guid": "Keyboard0" + "guid": "030000006d0400001dc2000014400000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index 26ecd0f..e59080e 100644 --- a/src/main/java/frc/robot/RobotMechanisms.java +++ b/src/main/java/frc/robot/RobotMechanisms.java @@ -8,10 +8,7 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.lib.InterpolatableColor; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; import frc.robot.superstructure.SuperstructureState; /** Helper class for rendering robot mechanisms. */ @@ -21,7 +18,7 @@ public class RobotMechanisms { private final Mechanism2d mechanism; - private MechanismLigament2d shoulder, flywheel, serializer; + private MechanismLigament2d shoulder; private final double WIDTH = 2 @@ -32,15 +29,15 @@ public class RobotMechanisms { private final Translation2d ORIGIN = new Translation2d(WIDTH / 2, 0); - public static final Translation2d SHOULDER_TO_ORIGIN = - new Translation2d(Units.inchesToMeters(-11.361), Units.inchesToMeters(7.721)); + private final Translation2d ORIGIN_TO_SHOULDER_BASE = new Translation2d(Units.inchesToMeters(-7.5), Units.inchesToMeters(3.75)); - private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray); + private final double SHOULDER_BASE_HEIGHT = Units.inchesToMeters(18); + + private final double SHOULDER_BASE_TOP_TO_SHOULDER = Units.inchesToMeters(1.5); + + private final Translation2d SHOULDER_BASE_TO_SHOULDER = new Translation2d(0, SHOULDER_BASE_HEIGHT - SHOULDER_BASE_TOP_TO_SHOULDER); - private final InterpolatableColor flywheelColor = - new InterpolatableColor(Color.kLightGray, Color.kSalmon); - private final InterpolatableColor serializerColor = - new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue); + private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray); private RobotMechanisms() { mechanism = new Mechanism2d(WIDTH, HEIGHT); @@ -51,30 +48,23 @@ private RobotMechanisms() { } private void initializeArmMechanism() { - Translation2d armRootTranslation = ORIGIN.plus(SHOULDER_TO_ORIGIN); + Translation2d armRootTranslation = ORIGIN.plus(ORIGIN_TO_SHOULDER_BASE); double armThickness = Units.inchesToMeters(2) * 100; MechanismRoot2d armRoot = mechanism.getRoot("arm", armRootTranslation.getX(), armRootTranslation.getY()); + MechanismLigament2d shoulderBase = armRoot.append(new MechanismLigament2d("base", SHOULDER_BASE_TO_SHOULDER.getY(), 90, armThickness, DEFAULT_COLOR)); + shoulder = - armRoot.append( + shoulderBase.append( new MechanismLigament2d( "shoulder", ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(), 0, armThickness, DEFAULT_COLOR)); - flywheel = - shoulder.append( - new MechanismLigament2d( - "wrist", Units.inchesToMeters(4.321), 0, armThickness, DEFAULT_COLOR)); - - serializer = - shoulder.append( - new MechanismLigament2d( - "serializer", Units.inchesToMeters(8.771), 0, armThickness, DEFAULT_COLOR)); } private void initializeFramePerimeterMechanisms() { @@ -114,20 +104,8 @@ public Mechanism2d getMechanism() { public void updateSuperstructure(SuperstructureState state) { Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position); - shoulder.setAngle(shoulderRotation); + Rotation2d offsetShoulderRotation = shoulderRotation.minus(Rotation2d.fromDegrees(90)); - flywheel.setColor( - new Color8Bit( - flywheelColor.sample( - Math.abs(state.flywheelVelocityRotationsPerSecond()), - 0, - FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED))); - - serializer.setColor( - new Color8Bit( - serializerColor.sample( - Math.abs(state.serializerVelocityRotationsPerSecond()), - 0, - SerializerConstants.MAXIMUM_TANGENTIAL_SPEED))); + shoulder.setAngle(offsetShoulderRotation); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index 9bfc97b..d0ba631 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -7,9 +7,9 @@ public class SuperstructureConstants { public static class ShoulderAngleConstants { - public static final Rotation2d INITIAL = Rotation2d.fromDegrees(52.5); + public static final Rotation2d INITIAL = Rotation2d.fromDegrees(0); - public static final Rotation2d STOW = Rotation2d.fromDegrees(29.5); + public static final Rotation2d STOW = Rotation2d.fromDegrees(-19.379); public static final Rotation2d AMP = Rotation2d.fromDegrees(90);