From 69952b15d096cccdb060c5451f528ae70ae8436c Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 4 Mar 2024 14:59:19 -0500 Subject: [PATCH] refactor: Remove physics-based pivot simulations. Improve mechanisms. --- simgui.json | 17 +++-- .../java/frc/lib/InterpolatableColor.java | 31 +++++++++ src/main/java/frc/robot/RobotContainer.java | 3 +- src/main/java/frc/robot/RobotMechanisms.java | 66 +++++++++++++++---- src/main/java/frc/robot/arm/Arm.java | 7 ++ .../frc/robot/arm/ShoulderMotorIOSim.java | 66 ++++--------------- .../java/frc/robot/arm/WristMotorIOSim.java | 52 +++------------ src/main/java/frc/robot/intake/Intake.java | 7 +- .../frc/robot/intake/IntakeConstants.java | 3 + .../frc/robot/intake/PivotMotorIOSim.java | 57 +++------------- .../java/frc/robot/odometry/Odometry.java | 11 ++++ src/main/java/frc/robot/shooter/Shooter.java | 4 ++ .../frc/robot/shooter/ShooterConstants.java | 6 ++ 13 files changed, 164 insertions(+), 166 deletions(-) create mode 100644 src/main/java/frc/lib/InterpolatableColor.java diff --git a/simgui.json b/simgui.json index d725fe9..ea41e35 100644 --- a/simgui.json +++ b/simgui.json @@ -11,6 +11,7 @@ "/FMSInfo": "FMSInfo", "/Shuffleboard/Auto/Auto Chooser": "String Chooser", "/Shuffleboard/Auto/SendableChooser[0]": "String Chooser", + "/Shuffleboard/Odometry/Field/Field": "Field2d", "/Shuffleboard/Vision/Field": "Field2d", "/SmartDashboard/Arm Mechanism": "Mechanism2d", "/SmartDashboard/Mechanism": "Mechanism2d", @@ -19,7 +20,7 @@ "/SmartDashboard/arm mech": "Mechanism2d" }, "windows": { - "/Shuffleboard/Auto/Auto Chooser": { + "/SmartDashboard/Mechanism": { "window": { "visible": true } @@ -46,8 +47,7 @@ }, "Velocities": { "open": true - }, - "open": true + } }, "Climber": { "East": { @@ -66,7 +66,8 @@ "Intake": { "Roller": { "open": true - } + }, + "open": true }, "Odometry": { "Position": { @@ -76,6 +77,14 @@ "open": true } }, + "Shooter": { + "Flywheel": { + "open": true + }, + "Serializer": { + "open": true + } + }, "Swerve": { "Constants": { "open": true diff --git a/src/main/java/frc/lib/InterpolatableColor.java b/src/main/java/frc/lib/InterpolatableColor.java new file mode 100644 index 0000000..dafc608 --- /dev/null +++ b/src/main/java/frc/lib/InterpolatableColor.java @@ -0,0 +1,31 @@ +package frc.lib; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.util.Color; + +public class InterpolatableColor { + + private final Color zero, one; + + public InterpolatableColor(Color zero, Color one) { + this.zero = zero; + this.one = one; + } + + 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); + } + + 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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9cf8d7a..86bfe57 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,8 @@ public static RobotContainer getInstance() { /** Initializes subsystem telemetry. */ private void initializeTelemetry() { if (RobotConstants.USE_TELEMETRY) { - Telemetry.initializeShuffleboards(arm, climber, intake, shooter, swerve); + Telemetry.initializeShuffleboards(arm, climber, intake, odometry, shooter, swerve); + SmartDashboard.putData("Mechanism", RobotMechanisms.getInstance().getMechanism()); } SmartDashboard.putData(auto.getAutonomousChooser()); diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index 887a4d6..4534470 100644 --- a/src/main/java/frc/robot/RobotMechanisms.java +++ b/src/main/java/frc/robot/RobotMechanisms.java @@ -9,9 +9,12 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; -import frc.robot.arm.ArmConstants.WristMotorConstants; +import frc.lib.InterpolatableColor; import frc.robot.arm.ArmState; import frc.robot.intake.IntakeConstants.PivotMotorConstants; +import frc.robot.intake.IntakeConstants.RollerMotorConstants; +import frc.robot.shooter.ShooterConstants.FlywheelConstants; +import frc.robot.shooter.ShooterConstants.SerializerConstants; /** Helper class for rendering robot mechanisms. */ public class RobotMechanisms { @@ -20,7 +23,7 @@ public class RobotMechanisms { private final Mechanism2d mechanism; - private MechanismLigament2d shoulder, wrist, intake; + private MechanismLigament2d shoulder, shooter, serializer, intake; private final double WIDTH = 2 @@ -31,6 +34,12 @@ public class RobotMechanisms { private final Translation2d ORIGIN = new Translation2d(WIDTH / 2, 0); + 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 RobotMechanisms() { mechanism = new Mechanism2d(WIDTH, HEIGHT); @@ -54,15 +63,24 @@ private void initializeArmMechanism() { ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(), 0, armThickness, - new Color8Bit(Color.kOrange))); - wrist = + DEFAULT_COLOR)); + shooter = shoulder.append( new MechanismLigament2d( "wrist", - WristMotorConstants.JOINT_CONSTANTS.lengthMeters(), + Units.inchesToMeters(4.321), + 0, + armThickness, + DEFAULT_COLOR)); + + serializer = + shoulder.append( + new MechanismLigament2d( + "serializer", + Units.inchesToMeters(8.771), 0, armThickness, - new Color8Bit(Color.kGreen))); + DEFAULT_COLOR)); } private void initializeFramePerimeterMechanisms() { @@ -79,7 +97,7 @@ private void initializeFramePerimeterMechanisms() { HEIGHT, 90, framePerimeterThickness, - new Color8Bit(Color.kLightGray))); + DEFAULT_COLOR)); mechanism .getRoot( @@ -92,7 +110,7 @@ private void initializeFramePerimeterMechanisms() { HEIGHT, 90, framePerimeterThickness, - new Color8Bit(Color.kLightGray))); + DEFAULT_COLOR)); } private void initializeIntakeMechanism() { @@ -110,7 +128,7 @@ private void initializeIntakeMechanism() { PivotMotorConstants.DISTANCE, 0.0, intakeThickness, - new Color8Bit(Color.kPink))); + DEFAULT_COLOR)); } public static RobotMechanisms getInstance() { @@ -125,15 +143,37 @@ public Mechanism2d getMechanism() { return mechanism; } - public void setArmState(ArmState state) { + public void updateArm(ArmState state) { Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position); Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position); shoulder.setAngle(shoulderRotation); - wrist.setAngle(wristRotation.plus(Rotation2d.fromDegrees(90))); + + // 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)); + + shooter.setAngle(shooterRotation); + serializer.setAngle(serializerRotation); + } + + public void updateIntake(Rotation2d pivotAngle, double rollerVelocityRotationsPerSecond) { + Color color = intakeColor.sample(Math.abs(rollerVelocityRotationsPerSecond), 0, RollerMotorConstants.MAXIMUM_SPEED); + + intake.setAngle(pivotAngle); + intake.setColor(new Color8Bit(color)); } - public void setIntakeAngle(Rotation2d angle) { - intake.setAngle(angle); + public void updateShooter(double velocityMetersPerSecond) { + 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); + + serializer.setColor(new Color8Bit(color)); } } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 2b729cc..20776a7 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.lib.Subsystem; import frc.lib.Telemetry; +import frc.robot.RobotMechanisms; import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues; import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues; import frc.robot.arm.WristMotorIO.WristMotorIOValues; @@ -17,6 +18,8 @@ public class Arm extends Subsystem { /** Instance variable for the arm subsystem singleton. */ private static Arm instance = null; + private final RobotMechanisms mechanism; + /** Limit switch. */ private final LimitSwitchIO limitSwitch; @@ -39,6 +42,8 @@ public class Arm extends Subsystem { /** Creates a new instance of the arm subsystem. */ private Arm() { + mechanism = RobotMechanisms.getInstance(); + limitSwitch = ArmFactory.createLimitSwitch(); shoulderMotor = ArmFactory.createShoulderMotor(); wristMotor = ArmFactory.createWristMotor(); @@ -76,6 +81,8 @@ public void periodic() { shoulderMotor.update(shoulderMotorValues); wristMotor.update(wristMotorValues); + mechanism.updateArm(getPosition()); + setSetpoint(setpoint.nextSetpoint(goal)); } diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java index 9e7c9d7..a838e81 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java @@ -1,39 +1,19 @@ package frc.robot.arm; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.lib.AccelerationCalculator; -import frc.robot.RobotConstants; -import frc.robot.arm.ArmConstants.ShoulderMotorConstants; /** Simulated shoulder motor. */ public class ShoulderMotorIOSim implements ShoulderMotorIO { - private final SingleJointedArmSim singleJointedArmSim; - - private final PIDController feedback; - + private double positionRotations; + private double velocityRotationsPerSecond; private double inputVoltage; private final AccelerationCalculator accelerationCalculator; /** Creates a new simulated shoulder motor. */ public ShoulderMotorIOSim() { - singleJointedArmSim = - new SingleJointedArmSim( - ShoulderMotorConstants.JOINT_CONSTANTS.motor(), - ShoulderMotorConstants.JOINT_CONSTANTS.gearing(), - ShoulderMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(), - ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(), - ShoulderMotorConstants.MINIMUM_ANGLE.getRadians(), - ShoulderMotorConstants.MAXIMUM_ANGLE.getRadians(), - false, - 0.0); - - feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0); - accelerationCalculator = new AccelerationCalculator(); } @@ -42,51 +22,29 @@ public void configure() {} @Override public void update(ShoulderMotorIOValues values) { - singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION); - - values.positionRotations = getPosition(); - values.velocityRotationsPerSecond = getVelocity(); - values.accelerationRotationsPerSecondPerSecond = getAcceleration(); + values.positionRotations = this.positionRotations; + values.velocityRotationsPerSecond = this.velocityRotationsPerSecond; + values.accelerationRotationsPerSecondPerSecond = accelerationCalculator.calculate(velocityRotationsPerSecond); values.inputVoltage = inputVoltage; - values.currentAmps = singleJointedArmSim.getCurrentDrawAmps(); - } - - private double getPosition() { - return Units.radiansToRotations(singleJointedArmSim.getAngleRads()); - } - - private double getVelocity() { - return Units.radiansToRotations(singleJointedArmSim.getVelocityRadPerSec()); - } - - private double getAcceleration() { - return accelerationCalculator.calculate(getVelocity()); } @Override public void setPosition(double positionRotations) { - singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0.0); + this.positionRotations = positionRotations; } @Override public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - double measuredPositionRotations = getPosition(); - - double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); - - double feedforwardVolts = 0.0; - - setVoltage(feedbackVolts + feedforwardVolts); + if (DriverStation.isEnabled()) { + this.positionRotations = positionRotations; + this.velocityRotationsPerSecond = velocityRotationsPerSecond; + } } @Override public void setVoltage(double volts) { - if (DriverStation.isEnabled()) { - inputVoltage = volts; - singleJointedArmSim.setInputVoltage(inputVoltage); - } else { - singleJointedArmSim.setInputVoltage(0.0); - } + // TODO + this.inputVoltage = volts; } } diff --git a/src/main/java/frc/robot/arm/WristMotorIOSim.java b/src/main/java/frc/robot/arm/WristMotorIOSim.java index fe2969a..db4823a 100644 --- a/src/main/java/frc/robot/arm/WristMotorIOSim.java +++ b/src/main/java/frc/robot/arm/WristMotorIOSim.java @@ -1,41 +1,19 @@ package frc.robot.arm; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.lib.AccelerationCalculator; -import frc.robot.RobotConstants; -import frc.robot.arm.ArmConstants.ShoulderMotorConstants; -import frc.robot.arm.ArmConstants.WristMotorConstants; /** Simulated elbow motor. */ public class WristMotorIOSim implements WristMotorIO { - private final SingleJointedArmSim singleJointedArmSim; - - private final PIDController feedback; + private double positionRotations; + private double velocityRotationsPerSecond; + private double inputVoltage; private final AccelerationCalculator accelerationCalculator; - private double inputVoltage; - /** Creates a new simulated elbow motor. */ public WristMotorIOSim() { - // TODO - singleJointedArmSim = - new SingleJointedArmSim( - WristMotorConstants.JOINT_CONSTANTS.motor(), - WristMotorConstants.JOINT_CONSTANTS.gearing(), - ShoulderMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(), - ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters() * 0.5, - WristMotorConstants.MINIMUM_ANGLE.getRadians(), - WristMotorConstants.MAXIMUM_ANGLE.getRadians(), - false, - 0.0); - - feedback = new PIDController(WristMotorConstants.KP, 0, 0); - accelerationCalculator = new AccelerationCalculator(); } @@ -44,36 +22,24 @@ public void configure() {} @Override public void update(WristMotorIOValues values) { - singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION); - - values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads()); - values.velocityRotationsPerSecond = - Units.radiansToRotations(singleJointedArmSim.getVelocityRadPerSec()); + values.positionRotations = this.positionRotations; + values.velocityRotationsPerSecond = this.velocityRotationsPerSecond; values.accelerationRotationsPerSecondPerSecond = - accelerationCalculator.calculate(values.velocityRotationsPerSecond); + accelerationCalculator.calculate(velocityRotationsPerSecond); values.inputVoltage = inputVoltage; - values.currentAmps = singleJointedArmSim.getCurrentDrawAmps(); } @Override public void setPosition(double positionRotations) { - singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0); + this.positionRotations = positionRotations; } @Override public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - double measuredPositionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads()); - - double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); - - double feedforwardVolts = 0.0; - if (DriverStation.isEnabled()) { - inputVoltage = feedbackVolts + feedforwardVolts; - singleJointedArmSim.setInputVoltage(inputVoltage); - } else { - singleJointedArmSim.setInputVoltage(0.0); + this.positionRotations = positionRotations; + this.velocityRotationsPerSecond = velocityRotationsPerSecond; } } } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 90b1070..7a83b7c 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -25,6 +25,8 @@ public class Intake extends Subsystem { /** Instance variable for the intake subsystem singleton. */ private static Intake instance = null; + private final RobotMechanisms mechanism; + /** Pivot motor. */ private final PivotMotorIO pivotMotor; @@ -52,6 +54,8 @@ public class Intake extends Subsystem { /** Creates a new instance of the intake subsystem. */ private Intake() { + mechanism = RobotMechanisms.getInstance(); + pivotMotor = IntakeFactory.createPivotMotor(); rollerMotor = IntakeFactory.createRollerMotor(); @@ -90,8 +94,7 @@ public void periodic() { rollerMotorCurrentFilter.calculate(rollerMotorValues.currentAmps); - RobotMechanisms.getInstance() - .setIntakeAngle(Rotation2d.fromRotations(pivotMotorValues.positionRotations)); + mechanism.updateIntake(Rotation2d.fromRotations(pivotMotorValues.positionRotations), getRollerVelocity()); } @Override diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index e3d2870..fb3d812 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -95,6 +95,9 @@ public static class RollerMotorConstants { /** Duration of the current spike when intaking a note. */ public static final double STALL_DURATION = 0.35; + + /** Maximum speed of the roller in rotations per second. */ + public static final double MAXIMUM_SPEED = 0.31; } /** Constants for intake commands. */ diff --git a/src/main/java/frc/robot/intake/PivotMotorIOSim.java b/src/main/java/frc/robot/intake/PivotMotorIOSim.java index 1e4747b..9ad8c9c 100644 --- a/src/main/java/frc/robot/intake/PivotMotorIOSim.java +++ b/src/main/java/frc/robot/intake/PivotMotorIOSim.java @@ -1,74 +1,33 @@ package frc.robot.intake; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import frc.lib.SingleJointedArmFeedforward; -import frc.robot.RobotConstants; -import frc.robot.intake.IntakeConstants.PivotMotorConstants; /** Simulated pivot motor. */ public class PivotMotorIOSim implements PivotMotorIO { - private final DCMotor motor; - - private final SingleJointedArmSim singleJointedArmSim; - - private final PIDController feedback; - - private final SingleJointedArmFeedforward feedforward; - - public PivotMotorIOSim() { - motor = DCMotor.getVex775Pro(1); - - singleJointedArmSim = - new SingleJointedArmSim( - motor, - PivotMotorConstants.MOTOR_GEARING, - PivotMotorConstants.MOI, - PivotMotorConstants.DISTANCE, - PivotMotorConstants.MINIMUM_ANGLE.getRadians(), - PivotMotorConstants.MAXIMUM_ANGLE.getRadians(), - true, - 0.0); - - feedback = new PIDController(PivotMotorConstants.KP, 0, 0); - - feedforward = new SingleJointedArmFeedforward(0, 0, 0); - } + private double positionRotations; + // private double velocityRotationsPerSecond; @Override public void configure() {} @Override public void update(PivotMotorIOValues values) { - singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION); - - values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads()); + values.positionRotations = this.positionRotations; + // TODO Add velocity values + // values.velocityRotationsPerSecond = this.velocityRotationsPerSecond } @Override public void setPosition(double positionRotations) { - singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0.0); + this.positionRotations = positionRotations; } @Override public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) { - double measuredPositionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads()); - - double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations); - - double feedforwardVolts = - feedforward.calculate( - Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond); - if (DriverStation.isEnabled()) { - singleJointedArmSim.setInputVoltage(feedbackVolts + feedforwardVolts); - } else { - singleJointedArmSim.setInputVoltage(0.0); + this.positionRotations = positionRotations; + // this.velocityRotationsPerSecond = velocityRotationsPerSecond; } } } diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java index 845e0dc..acfbd35 100644 --- a/src/main/java/frc/robot/odometry/Odometry.java +++ b/src/main/java/frc/robot/odometry/Odometry.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; @@ -37,6 +38,8 @@ public class Odometry extends Subsystem { /** Pose estimator using the swerve drive. */ private final SwerveDrivePoseEstimator swervePoseEstimator; + private final Field2d field; + /** Creates a new instance of the odometry subsystem. */ private Odometry() { gyroscope = OdometryFactory.createGyroscope(OdometryConstants.GYROSCOPE_CAN, this); @@ -58,6 +61,8 @@ private Odometry() { initialGyroRotation, swerveModulePositionsSupplier.get(), initialPose); + + field = new Field2d(); } /** @@ -80,6 +85,8 @@ public void periodic() { swervePoseEstimator.update( Rotation2d.fromRotations(gyroscopeValues.yawRotations), swerveModulePositionsSupplier.get()); + + field.setRobotPose(getPosition()); } @Override @@ -95,6 +102,10 @@ public void addToShuffleboard(ShuffleboardTab tab) { velocity.addDouble("X Velocity (mps)", () -> getVelocity().dx); velocity.addDouble("Y Velocity (mps)", () -> getVelocity().dy); velocity.addDouble("Rotation Velocity (dps)", () -> getVelocity().dtheta); + + ShuffleboardLayout field = Telemetry.addColumn(tab, "Field"); + + field.add("Field", this.field); } /** diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 2e88e3f..185a6a5 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; +import frc.robot.RobotMechanisms; import frc.robot.shooter.FlywheelMotorIO.FlywheelMotorIOValues; import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues; import frc.robot.shooter.ShooterConstants.FlywheelConstants; @@ -52,6 +53,9 @@ public static Shooter getInstance() { public void periodic() { serializerMotor.update(serializerMotorValues); flywheelMotor.update(flywheelMotorValues); + + RobotMechanisms.getInstance().updateShooter(getFlywheelTangentialSpeed()); + RobotMechanisms.getInstance().updateSerializer(getSerializerTangentialSpeed()); } @Override diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java index d21e10a..9877c67 100644 --- a/src/main/java/frc/robot/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/shooter/ShooterConstants.java @@ -28,6 +28,9 @@ public static class SerializerConstants { /** Voltage to apply while serializing in volts. */ public static final double SERIALIZE_VOLTAGE = -6.0; + + /** Maximum tengential speed in meters per second. */ + public static final double MAXIMUM_TANGENTIAL_SPEED = 4.75; } /** Constants for the flywheel motor used in the shooter subsystem. */ @@ -49,6 +52,9 @@ public static class FlywheelConstants { /** Voltage to apply while shooting in volts. */ public static final double SHOOT_VOLTAGE = -7.2; // TODO invert voltage to CCW+/CW+? + + /** Maximum tangential speed in meters per second. */ + public static final double MAXIMUM_TANGENTIAL_SPEED = 5.65; } /** Constants for commands used in the shooter subsystem. */