diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java new file mode 100644 index 0000000..f4267ec --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -0,0 +1,45 @@ + +package com.stuypulse.robot.constants; + +import com.stuypulse.stuylib.network.SmartBoolean; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; + +/** This interface stores information about each camera. */ +public interface Cameras { + + public Camera[] LimelightCameras = new Camera[] { + //TODO: Set up camera pose + new Camera("limelight-turret", new Pose3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0))), Settings.EnabledSubsystems.LIMELIGHT_TURRET) + }; + + public static class Camera { + private String name; + private Pose3d location; + private SmartBoolean isEnabled; + + public Camera(String name, Pose3d location, SmartBoolean isEnabled) { + this.name = name; + this.location = location; + this.isEnabled = isEnabled; + } + + public String getName() { + return name; + } + + public Pose3d getLocation() { + return location; + } + + public boolean isEnabled() { + return isEnabled.get(); + } + + public void setEnabled(boolean enabled) { + this.isEnabled.set(enabled); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java new file mode 100644 index 0000000..9852f3e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.constants; + +public interface Constants { + public interface Turret { + final public int encoderTeeth18 = 18; + final public int encoderTeeth17 = 17; + final public int bigGearTeeth = 84; + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 46992fc..eaa5888 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -1,4 +1,3 @@ -/************************ PROJECT KITBOT *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ @@ -145,13 +144,12 @@ public static AprilTag getTag(int id) { public final int[] BLUE_TOWER_TAG_IDS = {31, 32}; public final int[] RED_HP_TAG_IDS = {13, 14}; public final int[] BLUE_HP_TAG_IDS = {29, 30}; - + public final Pose2d blueHubCenter = new Pose2d(Units.inchesToMeters(158.60), Units.inchesToMeters(WIDTH / 2.0), new Rotation2d()); public static Pose2d getAllianceHubPose() { return (Robot.isBlue() ? blueHubCenter : transformToOppositeAlliance(blueHubCenter)); } - /* TRANSFORM FUNCTIONS */ public static Pose3d transformToOppositeAlliance(Pose3d pose) { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index da9063b..3fbc262 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -3,7 +3,16 @@ import com.pathplanner.lib.config.PIDConstants; public class Gains { - + public interface Turret { + double kS = 0.0; + double kV = 0.2; + double kA = 0.01; + + double kP = 1.0; + double kI = 0.0; + double kD = 0.05; + } + public interface Shooter { public interface PID { double kP = 2.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 2c46385..4fd1747 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -5,6 +5,7 @@ package com.stuypulse.robot.constants; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -34,6 +35,18 @@ * - The Open Loop Ramp Rate */ public interface Motors { + public interface Turret { + TalonFXConfig MOTOR_CONFIG = new TalonFXConfig() + .withCurrentLimitAmps(40) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withFFConstants(Gains.Turret.kS, Gains.Turret.kV, Gains.Turret.kA, 0) + .withPIDConstants(Gains.Turret.kP, Gains.Turret.kI, Gains.Turret.kD, 0) + .withMotionProfile(Settings.Turret.MAX_VEL.getRotations(), Settings.Turret.MAX_ACCEL.getRotations()) + .withSensorToMechanismRatio(2.80) // seems to be off by a bit + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withContinuousWrap(true); + } public interface Superstructure { TalonFXConfig intakeShooterMotorConfig = new TalonFXConfig() @@ -59,6 +72,7 @@ public static class TalonFXConfig { private final OpenLoopRampsConfigs openLoopRampsConfigs = new OpenLoopRampsConfigs(); private final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); + private final ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs(); private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); public void configure(TalonFX motor) { @@ -220,5 +234,13 @@ public TalonFXConfig withSensorToMechanismRatio(double sensorToMechanismRatio) { return this; } + + public TalonFXConfig withContinuousWrap(boolean enabled) { + closedLoopGeneralConfigs.ContinuousWrap = enabled; + + configuration.withClosedLoopGeneral(closedLoopGeneralConfigs); + + return this; + } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index e7ece41..3adf224 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,7 +12,11 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } - + //TODO: GET PORTS + public interface Turret { + int TURRET_MOTOR = 0; + int ENCODER_18t = 0; + int ENCODER_17t = 0; public interface Swerve { public interface Drive { int FRONT_LEFT = 32; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 85d813f..27e4368 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -8,10 +8,12 @@ import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; -import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; public interface Settings { @@ -22,6 +24,8 @@ public interface Settings { public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); + SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); + SmartBoolean LIMELIGHT_TURRET = new SmartBoolean("Enabled Subsystems/Turret Limelight Is Enabled", true); SmartBoolean SUPERSTRUCTURE = new SmartBoolean("Enabled Subsystems/Superstructure Is Enabled", true); } public interface Superstructure { @@ -39,6 +43,12 @@ public interface Indexer { } } + public interface Turret { + Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); + Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); + double TOLERANCE_DEG = 2.0; + } + public interface Swerve { double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; @@ -84,6 +94,10 @@ public interface Targets { } } } + + public interface Vision { + Vector MIN_STDDEVS = VecBuilder.fill(0.3, 0.3, 5); + } public interface Driver { public interface Drive { diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java new file mode 100644 index 0000000..4d027c9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -0,0 +1,77 @@ +package com.stuypulse.robot.subsystems.turret; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.TurretVisualizer; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public abstract class Turret extends SubsystemBase { + public static Turret instance; + public TurretState state; + + static { + if (Robot.isReal()) { + instance = new TurretImpl(); + } else { + instance = new TurretSim(); + } + } + + public static Turret getInstance() { + return instance; + } + + public Turret() { + state = TurretState.ZERO; + } + + public enum TurretState { + ZERO, + FERRYING, + POINT_AT_HUB; + } + + public Rotation2d getTargetAngle() { + return switch (getTurretState()) { + case ZERO -> new Rotation2d(); + case FERRYING -> getFerryAngle(); + case POINT_AT_HUB -> getPointAtHubAngle(); + }; + } + + public void setTurretState(TurretState targetState) { + state = targetState; + } + + public TurretState getTurretState() { + return state; + } + + public abstract Rotation2d getTurretAngle(); + + public abstract boolean atTargetAngle(); + + public abstract Rotation2d getPointAtHubAngle(); + + public abstract Rotation2d getFerryAngle(); + + public abstract SysIdRoutine getSysIdRoutine(); + + @Override + public void periodic() { + SmartDashboard.putNumber("Turret/Angle (Deg)", getTurretAngle().getDegrees()); + + if (Settings.DEBUG_MODE) { + if (Settings.EnabledSubsystems.TURRET.get()) { + TurretVisualizer.getInstance().updateTurretAngle(getTurretAngle(), atTargetAngle()); + } + else { + TurretVisualizer.getInstance().updateTurretAngle(new Rotation2d(), false); + } + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java new file mode 100644 index 0000000..89bd242 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -0,0 +1,183 @@ +package com.stuypulse.robot.subsystems.turret; + +import java.util.Optional; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.HubUtil.FERRY_TARGET_POSITIONS; +import com.stuypulse.robot.util.HubUtil; +import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class TurretImpl extends Turret { + private TalonFX turretMotor; + + private CANcoder encoder1; + private CANcoder encoder2; + private boolean hasUsedAbsoluteEncoder; + private FERRY_TARGET_POSITIONS targetPosition; + private Optional voltageOverride; + + public TurretImpl() { + turretMotor = new TalonFX(Ports.Turret.TURRET_MOTOR, "swerve"); + + Motors.Turret.MOTOR_CONFIG.configure(turretMotor); + + encoder1 = new CANcoder(Ports.Turret.ENCODER_18t, "swerve"); + encoder1.getConfigurator().apply(new CANcoderConfiguration().withMagnetSensor( + new MagnetSensorConfigs() + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1))); + + encoder2 = new CANcoder(Ports.Turret.ENCODER_17t, "swerve"); + encoder2.getConfigurator().apply(new CANcoderConfiguration().withMagnetSensor( + new MagnetSensorConfigs() + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1))); + + hasUsedAbsoluteEncoder = false; + voltageOverride = Optional.empty(); + targetPosition = FERRY_TARGET_POSITIONS.LEFT_WALL; + // just default to this ? + } + + @Override + public Rotation2d getPointAtHubAngle() { + Vector2D robot = new Vector2D(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()); + Vector2D hub = new Vector2D(HubUtil.getAllianceHubPose().getTranslation()); + Vector2D robotToHub = hub.sub(robot).normalize(); + Vector2D zeroVector = new Vector2D(0.0, 1.0); + + // https://www.youtube.com/watch?v=_VuZZ9_58Wg + double crossProduct = zeroVector.x * robotToHub.y - zeroVector.y * robotToHub.x; + double dotProduct = zeroVector.dot(robotToHub); + + Rotation2d targetAngle = Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct)); + + return targetAngle; + } + + @Override + public Rotation2d getFerryAngle() { + Vector2D robot = new Vector2D(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()); + Vector2D robotToHub = robot + .sub(new Vector2D(targetPosition.getFerryTargetPose().getTranslation())).normalize(); + Vector2D zeroVector = new Vector2D(0.0, 1.0); + // define this as a constant somewhere? + + Rotation2d angle = Rotation2d + .fromDegrees(Math.acos(robotToHub.dot(zeroVector) / robotToHub.magnitude() * zeroVector.magnitude())); + return angle; + } + + // confirm that the angle range is [0, 360) + @Override + public boolean atTargetAngle() { + return Math.abs(getTurretAngle().minus(getTargetAngle()).getDegrees() + 180.0) < Settings.Turret.TOLERANCE_DEG; + } + + @Override + public Rotation2d getTurretAngle() { + return Rotation2d.fromRotations(turretMotor.getPosition().getValueAsDouble()); + } + + private Rotation2d getEncoderPos18t() { + return Rotation2d.fromRotations((encoder1.getAbsolutePosition().getValueAsDouble())); + } + + private Rotation2d getEncoderPos17t() { + return Rotation2d.fromRotations((encoder2.getAbsolutePosition().getValueAsDouble())); + } + + public Rotation2d getAbsoluteTurretAngle() { + long gcdaandb[] = { 0, -1, 1 }; + + Rotation2d encoder17tPosition = getEncoderPos17t(); + double numberOfGearTeethRotated17 = (encoder17tPosition.getRotations() + * (double) Constants.Turret.encoderTeeth17); + + Rotation2d encoder18tPosition = getEncoderPos18t(); + double numberOfGearTeethRotated18 = (encoder18tPosition.getRotations() + * (double) Constants.Turret.encoderTeeth18); + + double crt_Partial17 = numberOfGearTeethRotated17 * gcdaandb[2] * Constants.Turret.encoderTeeth18; + double crt_Partial18 = numberOfGearTeethRotated18 * gcdaandb[1] * Constants.Turret.encoderTeeth17; + + // Java's % operator is not actually the same as the modulo operator + double crt_pos = (crt_Partial17 + crt_Partial18) + % (Constants.Turret.encoderTeeth17 * Constants.Turret.encoderTeeth18); + + crt_pos = (crt_pos < 0) ? (crt_pos + Constants.Turret.encoderTeeth17 * Constants.Turret.encoderTeeth18) + : crt_pos; + + double turretAngle = (crt_pos / (double) Constants.Turret.bigGearTeeth) * 360; + + SmartDashboard.putNumber("Turret/CRT partial17", crt_Partial17); + SmartDashboard.putNumber("Turret/CRT partial18", crt_Partial18); + SmartDashboard.putNumber("Turret/CRT teeth17", numberOfGearTeethRotated17); + SmartDashboard.putNumber("Turret/CRT teeth18", numberOfGearTeethRotated18); + SmartDashboard.putNumber("Turret/CRT crt_pos", crt_pos); + + return Rotation2d.fromDegrees(turretAngle); + } + + @Override + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Turret", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> getTurretAngle().getRotations(), + () -> turretMotor.getVelocity().getValueAsDouble(), + () -> turretMotor.getMotorVoltage().getValueAsDouble(), + getInstance()); + } + + private void setVoltageOverride(Optional volts) { + voltageOverride = volts; + } + + @Override + public void periodic() { + super.periodic(); + + if (!hasUsedAbsoluteEncoder || getTurretAngle().getRotations() > 1.0 || getTurretAngle().getRotations() < 0.0) { + turretMotor.setPosition((getAbsoluteTurretAngle().getDegrees() % 360.0) / 360.0); + hasUsedAbsoluteEncoder = true; + System.out.println("Absolute Encoder Reset triggered"); + } + + if (Settings.EnabledSubsystems.TURRET.get()) { + if (voltageOverride.isPresent()) { + turretMotor.setControl(new MotionMagicVoltage(getTargetAngle().getRotations())); + } else { + turretMotor.setVoltage(voltageOverride.orElse(0.0)); + } + } else { + turretMotor.setVoltage(0); + } + + SmartDashboard.putNumber("Turret/Pos 17t", getEncoderPos17t().getDegrees()); + SmartDashboard.putNumber("Turret/Pos 18t", getEncoderPos18t().getDegrees()); + SmartDashboard.putNumber("Turret/Absolute Angle", getAbsoluteTurretAngle().getDegrees()); + SmartDashboard.putString("Turret/State", getTurretState().toString()); + SmartDashboard.putNumber("Turret/Relative Encoder", getTurretAngle().getDegrees()); + + SmartDashboard.putNumber("Turret/Position", turretMotor.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Turret/Voltage", turretMotor.getMotorVoltage().getValueAsDouble()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java new file mode 100644 index 0000000..c299aa9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java @@ -0,0 +1,158 @@ +package com.stuypulse.robot.subsystems.turret; + +import java.util.Optional; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.HubUtil; +import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.math.Vector2D; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.LinearSystemLoop; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.LinearQuadraticRegulator; +import edu.wpi.first.math.estimator.KalmanFilter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class TurretSim extends Turret { + + private DCMotorSim sim; + private final LinearSystemLoop controller; + + private TrapezoidProfile profile; + private TrapezoidProfile.State setpoint; + private TrapezoidProfile.State goal; + + private double maxAngularVelRadiansPerSecond; + private double maxAngularAccelRadiansPerSecondSquared; + + private Optional voltageOverride; + + public TurretSim() { + LinearSystem linearSystem = LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 1.0, 2.80); + + sim = new DCMotorSim( + linearSystem, + DCMotor.getKrakenX60(1) + ); + + LinearQuadraticRegulator lqr = new LinearQuadraticRegulator( + linearSystem, + VecBuilder.fill(0.00001, 100), + VecBuilder.fill(12), + Settings.DT); + + KalmanFilter kalmanFilter = new KalmanFilter<>( + Nat.N2(), + Nat.N2(), + linearSystem, + VecBuilder.fill(3.0, 3.0), + VecBuilder.fill(0.01, 0.01), + Settings.DT); + + controller = new LinearSystemLoop<>(linearSystem, lqr, kalmanFilter, 12.0, Settings.DT); + + maxAngularVelRadiansPerSecond = Units.degreesToRadians(200.0); + maxAngularAccelRadiansPerSecondSquared = Units.degreesToRadians(400.0); + + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(maxAngularVelRadiansPerSecond, maxAngularAccelRadiansPerSecondSquared); + profile = new TrapezoidProfile(constraints); + goal = new TrapezoidProfile.State(); + setpoint = new TrapezoidProfile.State(); + + voltageOverride = Optional.empty(); + } + + @Override + public Rotation2d getPointAtHubAngle() { + Vector2D robot = new Vector2D(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()); + Vector2D hub = new Vector2D(HubUtil.getAllianceHubPose().getTranslation()); + Vector2D robotToHub = hub.sub(robot).normalize(); + Vector2D zeroVector = new Vector2D(0.0, 1.0); + + // https://www.youtube.com/watch?v=_VuZZ9_58Wg + double crossProduct = zeroVector.x * robotToHub.y - zeroVector.y * robotToHub.x; + double dotProduct = zeroVector.dot(robotToHub); + + Rotation2d targetAngle = Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct)); + + return targetAngle; + } + + @Override + public Rotation2d getFerryAngle() { + return new Rotation2d(); + } + + @Override + public boolean atTargetAngle() { + return Math.abs(getTurretAngle().minus(getTargetAngle()).getDegrees()) < Settings.Turret.TOLERANCE_DEG; + } + + @Override + public Rotation2d getTurretAngle() { + return Rotation2d.fromRadians(sim.getAngularPositionRad()); + } + + private void setVoltageOverride(Optional volts) { + voltageOverride = volts; + } + + @Override + public void periodic() { + super.periodic(); + + goal = new TrapezoidProfile.State(getTargetAngle().getRadians(), 0.0); + setpoint = profile.calculate(Settings.DT, setpoint, goal); + + SmartDashboard.putNumber("Turret/Constraints/Max vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); + SmartDashboard.putNumber("Turret/Constraints/Max accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); + + SmartDashboard.putNumber("Turret/Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); + SmartDashboard.putNumber("Turret/Target (deg)", Units.radiansToDegrees(getTargetAngle().getRadians())); + SmartDashboard.putBoolean("Turret/At Target", atTargetAngle()); + SmartDashboard.putNumber("Turret/Error: abs(turret - target) (deg)", Math.abs(getTurretAngle().minus(getTargetAngle()).getDegrees())); + + controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); // try setpoint.velocity as second arg later + controller.correct(VecBuilder.fill(sim.getAngularPositionRad(), sim.getAngularVelocityRadPerSec())); + controller.predict(Settings.DT); + + if (Settings.EnabledSubsystems.TURRET.get()) { + if (voltageOverride.isPresent()) { + sim.setInputVoltage(voltageOverride.get()); + } else { + sim.setInputVoltage(controller.getU(0)); + } + } else { + sim.setInputVoltage(0); + } + + sim.update(Settings.DT); + } + + + + /* USELESS, DON'T DELETE */ + @Override + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Turret", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> getTurretAngle().getRotations(), + () -> sim.getAngularVelocityRadPerSec(), + () -> sim.getInputVoltage(), + getInstance()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java new file mode 100644 index 0000000..dc1346d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -0,0 +1,121 @@ +package com.stuypulse.robot.subsystems.vision; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Cameras; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; +import com.stuypulse.stuylib.network.SmartBoolean; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LimelightVision extends SubsystemBase{ + + private static final LimelightVision instance; + + static { + instance = new LimelightVision(); + } + + public static LimelightVision getInstance() { + return instance; + } + + private String[] names; + private SmartBoolean enabled; + private SmartBoolean[] camerasEnabled; + + public LimelightVision() { + names = new String[Cameras.LimelightCameras.length]; + for (int i = 0; i < Cameras.LimelightCameras.length; i++) { + names[i] = Cameras.LimelightCameras[i].getName(); + Pose3d robotRelativePose = Cameras.LimelightCameras[i].getLocation(); + LimelightHelpers.setCameraPose_RobotSpace( + names[i], + robotRelativePose.getX(), + robotRelativePose.getY(), + robotRelativePose.getZ(), + robotRelativePose.getRotation().getX(), + robotRelativePose.getRotation().getY(), + robotRelativePose.getRotation().getZ() + ); + } + + camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; + for (int i = 0; i < camerasEnabled.length; i++) { + camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); + LimelightHelpers.SetIMUMode(names[i], 0); + SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + } + + enabled = new SmartBoolean("Vision/Is Enabled", true); + } + + public void setTagWhitelist(int... ids) { + for (String name : names) { + LimelightHelpers.SetFiducialIDFiltersOverride(name, ids); + } + } + + public void enable() { + enabled.set(true); + } + + public void disable() { + enabled.set(false); + } + + public void setCameraEnabled(String name, boolean enabled) { + for (int i = 0; i < names.length; i++) { + if (names[i].equals(name)) { + camerasEnabled[i].set(enabled); + } + } + } + + public void setIMUMode(int mode) { + for (String name : names) { + LimelightHelpers.SetIMUMode(name, mode); + } + } + + @Override + public void periodic() { + if (enabled.get()) { + for (int i = 0; i < names.length; i++) { + if (camerasEnabled[i].get()) { + String limelightName = names[i]; + + LimelightHelpers.SetRobotOrientation( + limelightName, + (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360, + 0, + 0, + 0, + 0, + 0 + ); + + PoseEstimate poseEstimate = Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName) + : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); + + if (poseEstimate != null && poseEstimate.tagCount > 0) { + Pose2d robotPose = poseEstimate.pose; + double timestamp = poseEstimate.timestampSeconds; + CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MIN_STDDEVS.times(1 + poseEstimate.avgTagDist)); + SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", true); + } + else { + SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + } + } + } + } + } +} + \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/HubUtil.java b/src/main/java/com/stuypulse/robot/util/HubUtil.java index 1c27cfc..296aea7 100644 --- a/src/main/java/com/stuypulse/robot/util/HubUtil.java +++ b/src/main/java/com/stuypulse/robot/util/HubUtil.java @@ -1,5 +1,4 @@ - -/************************ PROJECT KITBOT *************************/ +/************************ PROJECT TURRET *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ @@ -8,11 +7,37 @@ package com.stuypulse.robot.util; import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Field.NamedTags; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; public interface HubUtil { + public final Pose2d blueHubCenter = new Pose2d(Units.inchesToMeters(158.60), Units.inchesToMeters(Field.WIDTH / 2.0), new Rotation2d()); + + public static Pose2d getAllianceHubPose() { + return (Robot.isBlue() ? blueHubCenter : Field.transformToOppositeAlliance(blueHubCenter)); + } + + public enum FERRY_TARGET_POSITIONS { + + RIGHT_WALL(new Pose2d(1.0, 1.0, new Rotation2d())), + LEFT_WALL(new Pose2d()); + + private Pose2d targetPosition; + + private FERRY_TARGET_POSITIONS(Pose2d targetPosition) { + this.targetPosition = targetPosition; + } + + public Pose2d getFerryTargetPose() { + return Robot.isBlue() ? targetPosition : Field.transformToOppositeAlliance(targetPosition); + } + + } + public enum HubTag { FRONT_MID(NamedTags.BLUE_HUB_FRONT_SIDE_MID, NamedTags.RED_HUB_FRONT_SIDE_MID), FRONT_LEFT(NamedTags.BLUE_HUB_FRONT_SIDE_LEFT, NamedTags.RED_HUB_FRONT_SIDE_LEFT), diff --git a/src/main/java/com/stuypulse/robot/util/SysId.java b/src/main/java/com/stuypulse/robot/util/SysId.java new file mode 100644 index 0000000..9a741b6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SysId.java @@ -0,0 +1,46 @@ +/************************ PROJECT MARY *************************/ +/* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.util; + +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import com.ctre.phoenix6.SignalLogger; +import java.util.function.Consumer; +import java.util.function.Supplier; + +public class SysId { + public static SysIdRoutine getRoutine( + double rampRate, + double stepVoltage, + String subsystemName, + Consumer voltageSetter, + Supplier positionSupplier, + Supplier velocitySupplier, + Supplier voltageSupplier, + Subsystem subsystemInstance + ) { + return new SysIdRoutine( + new SysIdRoutine.Config( + Units.Volts.of(rampRate).per(Second), + Units.Volts.of(stepVoltage), + null, + state -> SignalLogger.writeString(subsystemName + " SysId-State", state.toString())), + new SysIdRoutine.Mechanism( + output -> voltageSetter.accept(output.in(Volts)), + state -> { + SignalLogger.writeDouble(subsystemName + " Position", positionSupplier.get()); + SignalLogger.writeDouble(subsystemName + " Velocity", velocitySupplier.get()); + SignalLogger.writeDouble(subsystemName + " Voltage", voltageSupplier.get()); + }, + subsystemInstance)); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/TurretVisualizer.java b/src/main/java/com/stuypulse/robot/util/TurretVisualizer.java new file mode 100644 index 0000000..9d2dd5e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/TurretVisualizer.java @@ -0,0 +1,58 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.Robot; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class TurretVisualizer { + public static TurretVisualizer instance; + + static { + instance = new TurretVisualizer(); + } + + public static TurretVisualizer getInstance() { + return instance; + } + + private final Mechanism2d canvas; + private double width, height; + + private final MechanismRoot2d turretPivot; + private final MechanismLigament2d turret; + + private TurretVisualizer() { + width = Units.inchesToMeters(20); + height = Units.inchesToMeters(20); + + canvas = new Mechanism2d(width, height); + + turretPivot = canvas.getRoot("Turret Pivot", width/2, height/2); + + turret = new MechanismLigament2d( + "Turret", + Units.inchesToMeters(5), + Robot.isBlue() ? 0.0 : 180.0, + 4, + new Color8Bit(Color.kBlueViolet) + ); + + turretPivot.append(turret); + } + + public void updateTurretAngle(Rotation2d turretAngle, boolean atTargetAngle) { + turretPivot.setPosition(width/2, height/2); + turret.setAngle(turretAngle); + + turret.setColor(atTargetAngle ? new Color8Bit(Color.kGreen) : new Color8Bit(Color.kRed)); + + SmartDashboard.putData("Visualizers/Turret", canvas); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/vision/LimelightHelpers.java b/src/main/java/com/stuypulse/robot/util/vision/LimelightHelpers.java index b974532..f4afd5f 100644 --- a/src/main/java/com/stuypulse/robot/util/vision/LimelightHelpers.java +++ b/src/main/java/com/stuypulse/robot/util/vision/LimelightHelpers.java @@ -1,3 +1,26 @@ +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + +package com.stuypulse.robot.util.vision; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; /************************ PROJECT KITBOT *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ @@ -19,7 +42,6 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; - import com.fasterxml.jackson.annotation.JsonFormat; import com.fasterxml.jackson.annotation.JsonFormat.Shape; import com.fasterxml.jackson.annotation.JsonProperty;