diff --git a/simgui-ds.json b/simgui-ds.json index f671bb9..37ec5eb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,15 +16,14 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 }, {}, { - "decKey": 79, - "incKey": 80 + "decKey": 81, + "incKey": 69 } ], "axisCount": 5, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5be2d40..db1ddb1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,17 +3,14 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; 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.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -22,6 +19,23 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Seconds; + +import java.lang.reflect.Field; +import java.util.HashMap; + +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Measure; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -29,6 +43,8 @@ import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Velocity; +import frc.robot.utils.ShootingEntry; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -70,16 +86,16 @@ public static final class DriveConstants { public static final double kBackRightChassisAngularOffset = Math.PI / 2; // SPARK MAX CAN IDs Drive Motors - public static final int kFrontLeftDrivingCanId = 2; - public static final int kRearLeftDrivingCanId = 3; + public static final int kFrontLeftDrivingCanId = 10; + public static final int kRearLeftDrivingCanId = 14; public static final int kFrontRightDrivingCanId = 1; - public static final int kRearRightDrivingCanId = 4; + public static final int kRearRightDrivingCanId = 2; // SPARK MAX CAN IDs Turning Motors - public static final int kFrontLeftTurningCanId = 6; - public static final int kRearLeftTurningCanId = 7; - public static final int kFrontRightTurningCanId = 5; - public static final int kRearRightTurningCanId = 8; + public static final int kFrontLeftTurningCanId = 11; + public static final int kRearLeftTurningCanId = 15; + public static final int kFrontRightTurningCanId = 62; + public static final int kRearRightTurningCanId = 3; // Auxiliary Device Can IDs public static final int kPidgeyCanId = 13; @@ -88,9 +104,17 @@ public static final class DriveConstants { public static final Time kPeriodicInterval = Seconds.of(0.02); - public static final double kAutoRotationP = Robot.isReal() ? 0.3 : 3.0; + public static final double kAutoRotationP = Robot.isReal() ? 3.6 : 3.0; public static final double kAutoRotationI = 0.0; public static final double kAutoRotationD = 0.0; + + public static enum RangeType { + Within, + CloseMin, + CloseMax + } + + public static final Angle kTurnToAngleTolerance = Degrees.of(2); } public static final class ModuleConstants { @@ -158,55 +182,95 @@ public static final class VisionConstants { public static final Transform3d kRobotToCamOne = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); - public static final Transform3d kRobotToCamTwo = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + + // These are not final numbers + public static final Transform3d kRobotToCamTwo = new Transform3d( + new Translation3d(Inches.of(8.375), Inches.of(-2.16), Inches.of(-20.668)), new Rotation3d(0, 0, 0)); public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout .loadField(AprilTagFields.kDefaultField); // Placeholder numbers - public static final Pose3d kTurretAxisOfRotation = new Pose3d(Meters.of(0.2), Meters.of(0.3), Meters.of(0.3), - new Rotation3d(0.0, 0.0, 0.0)); public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); - public static final Angle kCameraTwoPitch = Radians.of(0.0); - public static final Angle kCameraTwoRoll = Radians.of(0.0); + public static final Distance kCameraTwoZ = Inches.of(18.0); + + public static final Translation3d kTurretCenterOfRotation = new Translation3d(Meters.of(0.0), Meters.of(0.0), + Inches.of(18)); + + public static final Angle kCameraTwoPitch = Degrees.of(15.0); + public static final Angle kCameraTwoRoll = Degrees.of(0.0); + public static final Angle kCameraTwoYaw = Degrees.of(-21.0); public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); public static final Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); public static final Matrix kVisionStdDevs = VecBuilder.fill(1, 1, 1); + + public static final AngularVelocity kMaxTurretVisionSpeed = RPM.of(30); } public static final class NumericalConstants { public static final double kEpsilon = 1e-6; + public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); + public static final Angle kNoRotation = Radians.of(0.0); + public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); } public static final class TurretConstants { - public static final int kMotorId = 20; + public static final int kMotorId = 18; // Was 20 public static final Angle kMinAngle = Radians.of(0.0); - public static final Angle kMaxAngle = Radians.of((3.0 / 2.0) * Math.PI); + public static final Angle kMaxAngle = Degrees.of(340); public static final int kPositionBufferLength = 4000; public static final Time kEncoderReadingDelay = Seconds.of(0.005); public static final Time kEncoderReadInterval = Seconds.of(0.01); - public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); - public static final Angle kNoRotation = Radians.of(0.0); - public static final double kP = 1.5; public static final double kI = 0.0; public static final double kD = 0.0; public static final int kSmartCurrentLimit = 40; + + public static final Angle kHubMinAngle1 = Rotations.of(0.375); + public static final Angle kHubMaxAngle1 = Rotations.of(0.582); + + public static final Angle kHubMinAngle2 = Rotations.of(0.833); + public static final Angle kHubMaxAngle2 = Rotations.of(0.000); + + public static final Angle kFeedMinAngle = Rotations.of(0.375); + public static final Angle kFeedMaxAngle = Rotations.of(0.582); + + public static final Angle kTurretCameraIdleViewMinAngle = Rotations.of(0.375); + public static final Angle kTurretCameraIdleViewMaxAngle = Rotations.of(0.582); + public static final Angle kTurretCameraMidPoint = kTurretCameraIdleViewMinAngle + .plus(kTurretCameraIdleViewMaxAngle).div(2.0); + + public static final Angle[] kRestrictedAngles = new Angle[] { + kFeedMinAngle, kFeedMaxAngle + }; + + public static final Angle[] kUnrestrictedAngles = new Angle[] { + kHubMinAngle1, kHubMaxAngle1, kHubMinAngle2, kHubMaxAngle2 + }; + + public static final Angle kOvershootAmount = Degrees.of(10.0); + + public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); + + public static final Angle kTurretAngleTolerance = Degrees.of(2.0); + + // TODO: Change to real numbers + public static Angle kNonAimTurretAngle = Degrees.of(25.0); } public static final class ShooterConstants { - public static final int kShooterMotorId = 30; - public static final int kHoodMotorId = 31; + public static final int kShooterMotorId = 70; + public static final int kHoodMotorId = 71; // Was 31 - public static final double kHoodP = 0.1; + public static final double kHoodP = 5.0; // Only use this high P when converion factor is 1. public static final double kHoodI = 0.0; public static final double kHoodD = 0.0; @@ -215,38 +279,101 @@ public static final class ShooterConstants { public static final double kShooterD = 0.0; // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part - public static final double kHoodGearRatio = (62 / 25) * (14 / 218); - + // NOTE: Need to use 14D so the result is a double, otherwise you end up with + // zero. + // 2.48 * 0.0642201834862385 = 0.1592660550458716 + // 16.5 motor rotations to one absolute encoder rotation (roughly) + // NOTE: gear ration commented out for now is it isn't used + // public static final double kHoodGearRatio = (62D / 25) * (14D / 218); + public static final int kHoodSmartCurrentLimit = 20; public static final Angle kFeedAngle = Degrees.of(90.0); + + public static final AngularVelocity kPlaceholderWheelVelocity = RPM.of(2000); + public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); + + public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + + public static final ShootingEntry[] kShootingEntries = { + new ShootingEntry(Meters.of(0.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(1.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(2.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(3.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(4.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(5.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + }; + + public static final Angle kHoodTolerence = Degrees.of(2.0); + + public static final LinearVelocity kMaxStationaryVelocity = MetersPerSecond.of(1e-1); + + public static final double kHoodMinAbsolutePosition = 0.0; + public static final double kHoodMaxAbsolutePosition = 0.55; + + public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; + + // TODO: Change to real numbers + public static AngularVelocity kNonAimShooterVelocity = RPM.of(500); + public static Angle kNonAimHoodAngle = Degrees.of(15.0); + public static AngularVelocity kFeedingWheelVelocity = RPM.of(500); + public static Angle kHoodFeedingPosition = Degrees.of(25.0); + public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); + } + + public static final class StagingConstants { + public static int kFeedIntoHoodMotor = 30; + public static double kFeedIntoHoodSpeed = 0.4; + + public static final int kAgitationMotorId = 31; + public static final double kAgitationSpeed = 0.2; + + public static final int kRollerMotorId = 32; + public static final double kRollerSpeed = 0.3; } public static final class Fixtures { - public static final Pose2d kRedAllianceHub = new Pose2d(); - public static final Pose2d kBlueAllianceHub = new Pose2d(); + public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), Inches.of(154.84)); + public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(651.22 - 182.11), + Inches.of(158.84)); // From a top down perspective of the field with the red alliance on the left // side - public static final Pose2d kTopFeedPose = new Pose2d(); - public static final Pose2d kBottomFeedPose = new Pose2d(); + public static final Translation2d kTopFeedPose = new Translation2d(); + public static final Translation2d kBottomFeedPose = new Translation2d(); - public static final Distance kFieldYMidpoint = Inches.of(317.69 / 2.0); + public static final Distance kFieldYMidpoint = Inches.of(158.84); - public static final Distance kRedSideNeutralBorder = Inches.of(182.11); - public static final Distance kBlueSideNeutralBorder = Inches.of(651.22 - 182.11); + public static final Distance kBlueSideNeutralBorder = Inches.of(182.11); + public static final Distance kRedSideNeutralBorder = Inches.of(651.22 - 182.11); public static enum FieldLocations { AllianceSide, - NeutralLeftSide, - NeutralRightSide, + NeutralSide, OpponentSide } + public static final HashMap kFieldLocationStringMap = new HashMap<>(); + + static { + kFieldLocationStringMap.put(FieldLocations.AllianceSide, "Alliance Side"); + kFieldLocationStringMap.put(FieldLocations.NeutralSide, "Neutral Side"); + kFieldLocationStringMap.put(FieldLocations.OpponentSide, "Opponent Side"); + } + // Placeholders - public static final Angle kRedLeftSideFeedHeading = Degrees.of(40); - public static final Angle kRedRightSideFeedHeading = Degrees.of(160); + public static final Angle kBlueLeftSideFeedHeading = Degrees.of(40); + public static final Angle kBlueRightSideFeedHeading = Degrees.of(160); // Placeholders - public static final Angle kBlueLeftSideFeedHeading = Degrees.of(-40); - public static final Angle kBlueRightSideFeedHeading = Degrees.of(-160); + public static final Angle kRedLeftSideFeedHeading = Degrees.of(-40); + public static final Angle kRedRightSideFeedHeading = Degrees.of(-160); + + public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark) + .getTagPose(3).get().toPose2d(); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6cd1383..b0deed2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,12 +17,13 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); addPeriodic(m_robotContainer.pushTurretEncoderReading(), - Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); + Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + m_robotContainer.periodic(); } @Override @@ -63,6 +64,7 @@ public void teleopInit() { @Override public void teleopPeriodic() { + m_robotContainer.teleopPeriodic(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48d4fd4..edd68e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,41 +4,63 @@ package frc.robot; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.Fixtures; import frc.robot.Constants.OIConstants; +import frc.robot.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.TargetSolution; +import frc.robot.utils.UtilityFunctions; public class RobotContainer { - private final DriveSubsystem m_drive = new DriveSubsystem(); - private final CommandXboxController m_driverController = new CommandXboxController( OIConstants.kDriverControllerPort); private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); - private final TurretSubsystem m_turret; + private final TurretSubsystem m_turret = new TurretSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); - public RobotContainer() { - m_turret = new TurretSubsystem(m_drive::getPose); + AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret, m_shooter); + Field2d m_field; + + // For getting data points for the lookup table + Angle commandedShooterAngle; + AngularVelocity commandedWheelVelocity; + public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); + SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); + SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); + SmartDashboard.putNumber("Turret angle in degrees", 0.0); + // Configure the button bindings configureBindings(); @@ -54,13 +76,21 @@ public RobotContainer() { true), m_drive)); - // TODO: Get rid of this - m_turret.setDefaultCommand(new RunCommand(() -> m_turret.moveToAngle(Radians.of(Math.PI / 4)), m_turret)); + m_field = m_drive.getField(); } private void configureBindings() { - m_driverController.a().whileTrue(m_drive.faceCardinalHeadingRange(Degrees.of(342), Degrees.of(190))); - m_driverController.a().whileFalse(m_drive.disableFaceHeading()); + // m_driverController.a() + // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + + m_driverController.b() + .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)), + Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + + m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); + + System.out.println("Bindings configured"); + m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); } public Command getAutonomousCommand() { @@ -82,4 +112,40 @@ public Command feedPosition(Alliance alliance) { }, m_drive, m_turret); } + + public void teleopPeriodic() { + m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); + + TargetSolution solution = m_aimFactory.GetHubAimSolution(); + + Pose2d robotPose = m_drive.getPose(); + + Distance xDist = Meters.of(solution.distance().in(Meters) + * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureX()); + Distance yDist = Meters.of(solution.distance().in(Meters) + * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureY()); + + Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); + + m_field.getObject("targetPose").setPose(targetPose); + } + + public void periodic() { + commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); + commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + + // System.out.println(m_drive.getRobotLocation()); + } + + private Angle getSmartdashBoardRequestedShooterAngle() { + return Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + } + + private AngularVelocity getSmartdashboardRequestedWheelSpeed() { + return RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); + } + + private Angle getSmartdashBoardRequestedTurretAngle() { + return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); + } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java new file mode 100644 index 0000000..66ef6d3 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -0,0 +1,475 @@ +package frc.robot.commands; + +import java.lang.annotation.Target; +import java.lang.reflect.Array; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +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.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Fixtures; +import frc.robot.Constants.NumericalConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.TurretConstants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.StagingSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.ShootingEntry; +import frc.robot.utils.TargetSolution; +import frc.robot.utils.UtilityFunctions; + +public class AimCommandFactory { + + private DriveSubsystem m_drive; + private TurretSubsystem m_turret; + private ShooterSubsystem m_shooter; + + private boolean m_isAiming = false; + + private AngularVelocity m_wheelVelocity; + + private Translation2d m_lockedTag; + + private StagingSubsystem m_stager = new StagingSubsystem(); + + public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { + m_drive = drive; + m_turret = turret; + m_shooter = shooter; + } + + public Command AimCommand(Supplier isFeedingLeftSide) { + return new RunCommand(() -> { + Aim(isFeedingLeftSide); + m_isAiming = true; + }, m_drive, m_turret).finallyDo(() -> { + m_isAiming = false; + m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; + }); + } + + private void Aim(Supplier isFeedingLeftSide) { + Fixtures.FieldLocations location = m_drive.getRobotLocation(); + + switch (location) { + case AllianceSide: { + TargetSolution solution = GetHubAimSolution(); + + MoveTurretToHeading(solution.hubAngle()); + m_shooter.MoveHoodToPosition(solution.hoodAngle()); + + m_drive.moveByAngle(solution.phi()); + + m_wheelVelocity = solution.wheelSpeed(); + break; + } + case NeutralSide: { + // Heading changes 180 degrees depending on which alliance you are on + Angle offset = DriverStation.getAlliance().get() == Alliance.Blue ? Degrees.of(0) : Degrees.of(180); + Angle absHeading = isFeedingLeftSide.get() ? offset.minus(Degrees.of(50)) + : offset.plus(Degrees.of(50)); + + m_shooter.MoveHoodToPosition(ShooterConstants.kHoodFeedingPosition); + m_wheelVelocity = ShooterConstants.kFeedingWheelVelocity; + MoveTurretToHeading(absHeading); + break; + } + case OpponentSide: { + System.out.println("Why are you here???"); + } + default: + break; + } + } + + public Command AimHoodToPositionCommand(Angle angle) { + return new RunCommand(() -> { + m_shooter.MoveHoodToPosition(angle); + }).until(m_shooter::AtTarget); + } + + public Command AimTurretRelativeToRobot(Angle angle) { + return new RunCommand(() -> { + m_turret.moveToAngle(angle); + }, m_turret).until(m_turret::atTarget); + } + + public Command ShootCommand() { + return new ConditionalCommand(new RunCommand(this::Shoot, m_shooter), + AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) + .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) + .andThen(new RunCommand(this::Shoot, m_shooter)), + () -> m_isAiming).alongWith(new RunCommand(() -> { + m_stager.Agitate(); + m_stager.Feed(); + m_stager.Roll(); + }, m_stager)).finallyDo(m_shooter::Stop); + } + + private void Shoot() { + m_shooter.Spin(m_wheelVelocity); + } + + public TargetSolution GetHubAimSolution() { + Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub + : Fixtures.kRedAllianceHub; + + Translation2d turretTranslation = m_drive.getPose().getTranslation().plus(TurretConstants.kTurretOffset); + + Translation2d translationToHub = hubPosition.minus(turretTranslation); + + Distance turretToHubDistance = Meters + .of(Math.hypot(translationToHub.getMeasureY().in(Meters), translationToHub.getMeasureX().in(Meters))); + Angle turretToHubAngle = Radians + .of(Math.atan2(translationToHub.getMeasureY().in(Meters), translationToHub.getMeasureX().in(Meters))); + + ChassisSpeeds robotSpeeds = m_drive.getChassisSpeeds(); + + return getInterpolatedShootingParameters(turretToHubDistance, + MetersPerSecond.of(robotSpeeds.vxMetersPerSecond), MetersPerSecond.of(robotSpeeds.vyMetersPerSecond), + turretToHubAngle); + } + + public Command MoveTurretToHeadingCommand(Angle heading) { + return new InstantCommand(() -> { + MoveTurretToHeading(heading); + }, m_turret); + } + + public Command MoveHoodToAbsoluteCommand(Angle angle) { + return new InstantCommand(() -> { + System.out.println("Move Hood to position " + angle); + m_shooter.MoveHoodToPosition(angle); + }); + } + + public Command PointAtHub(boolean isRed) { + return new RunCommand(() -> { + Translation2d hubPosition = isRed ? Fixtures.kRedAllianceHub : Fixtures.kBlueAllianceHub; + Translation2d robotPose = m_drive.getPose().getTranslation(); + + double dx = hubPosition.getMeasureX().minus(robotPose.getMeasureX()).in(Meters); + double dy = hubPosition.getMeasureY().minus(robotPose.getMeasureY()).in(Meters); + + Angle angle = Radians.of(Math.atan2(dy, dx)); + + MoveTurretToHeading(angle); + System.out.println(angle); + }).finallyDo(m_drive::disableFaceHeading); + } + + public void MoveTurretToHeading(Angle heading) { + Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); + + Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); + + Angle[] currentRange = getCurrentTurretRange(); + + if (withinAngles(currentRange, robotRelativeTurretAngle)) { + m_turret.moveToAngle(robotRelativeTurretAngle); + } else { + // Gets which ray the robot is closest to + Angle closest = getClosestAngle(robotRelativeTurretAngle, currentRange); + + // The overshoot is negative if the robot has to move in a negative direction; + // same for positive + Angle overshoot = robotRelativeTurretAngle.minus(closest).in(Degrees) < 0.0 + ? TurretConstants.kOvershootAmount + : TurretConstants.kOvershootAmount.times(-1.0); + + closest = closest.plus(overshoot); + + Angle driveTarget = heading.minus(closest); + + System.out.println(); + m_drive.moveToAngle(driveTarget); + m_turret.moveToAngle(closest); + } + } + + public Command PointTurretToFixture(Pose2d fixture) { + return new RunCommand(() -> { + Pose2d robotPose = m_drive.getPose(); + + double dx = fixture.getX() - robotPose.getX(); + double dy = fixture.getY() - robotPose.getY(); + + Angle angle = Radians.of(Math.atan2(dy, dx)).minus(Radians.of(robotPose.getRotation().getRadians())); + + MoveTurretToHeading(angle); + }, m_turret); + } + + // Aims the camera at april tags within range + public Command IdleCameraAim() { + + // TODO: Finish + return new ConditionalCommand(new RunCommand(() -> { + Angle absoluteMinAngle = m_drive.getHeading().plus(TurretConstants.kTurretCameraIdleViewMinAngle); + Angle absoluteMaxAngle = m_drive.getHeading().plus(TurretConstants.kTurretCameraIdleViewMaxAngle); + Pose2d robotPose = m_drive.getPose(); + Angle robotRotation = robotPose.getRotation().getMeasure(); + Translation2d robotTranslation = robotPose.getTranslation(); + + Angle toTagAngle = angleFromTranslation(robotTranslation, m_lockedTag); + + if (!withinRange(robotRotation.plus(absoluteMinAngle), robotRotation.plus(absoluteMaxAngle), toTagAngle)) { + ArrayList aprilTagsInView = aprilTagsWithinRange(absoluteMinAngle, absoluteMaxAngle, + robotTranslation); + + if (aprilTagsInView.isEmpty()) + return; + + var tags = aprilTagsWithinRange(absoluteMinAngle, absoluteMaxAngle, robotTranslation); + Translation2d closestTag = getClosestAngleApriltag(TurretConstants.kTurretCameraMidPoint, + robotTranslation, + (Translation2d[]) tags.toArray()); + + Angle angleToTag = angleFromTranslation(robotTranslation, closestTag); + Angle turretRelativeAngleToTag = UtilityFunctions.WrapAngle(angleToTag.minus(robotRotation)); + + m_lockedTag = closestTag; + m_turret.moveToAngle(turretRelativeAngleToTag); + } + }, m_turret), null, () -> m_turret.getCurrentCommand() == null); + } + + private ArrayList aprilTagsWithinRange(Angle min, Angle max, Translation2d referenceTranslation) { + ArrayList anglesInRange = new ArrayList<>(); + + for (int i = 1; i <= 32; i++) { + Translation2d tag = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark).getTagPose(i).get() + .toPose2d().getTranslation(); + + Angle angleToTag = angleFromTranslation(referenceTranslation, + tag); + + if (angleToTag.gt(min) && angleToTag.lt(max)) { + anglesInRange.add(tag); + } + } + + return anglesInRange; + } + + private static Angle angleFromTranslation(Translation2d reference, Translation2d target) { + double dx = target.minus(reference).getX(); + double dy = target.minus(reference).getY(); + + return Radians.of(Math.atan2(dy, dx)); + } + + private static boolean withinRange(Angle min, Angle max, Angle a) { + a = UtilityFunctions.WrapAngle(a); + min = UtilityFunctions.WrapAngle(min); + max = UtilityFunctions.WrapAngle(max); + + return a.gt(min) && a.lt(max); + } + + private static Angle getClosestAngle(Angle a, Angle... others) { + a = UtilityFunctions.WrapAngle(a); + + // for (Angle as : others) { + // System.out.print(as.in(Degrees) + " "); + // } + // System.out.print(a.in(Degrees) + " is robot heading"); + // System.out.println(); + + Angle closest = UtilityFunctions.WrapAngle(others[0]); + double closestDistance = UtilityFunctions.angleDiff(a, closest).abs(Degrees); + + for (int i = 1; i < others.length; i++) { + Angle candidate = UtilityFunctions.WrapAngle(others[i]); + double dif = UtilityFunctions.angleDiff(a, candidate).abs(Degrees); + + if (dif < closestDistance) { + closest = candidate; + closestDistance = dif; + + // System.out.println(closest + " " + others.length); + } + } + + return closest; + } + + private static Translation2d getClosestAngleApriltag(Angle referenceAngle, Translation2d robot, + Translation2d... tags) { + if (tags.length == 0) + return null; + + referenceAngle = UtilityFunctions.WrapAngle(referenceAngle); + + double closestDistance = 2 * Math.PI; + Translation2d closestPosition = new Translation2d(); + + for (Translation2d tag : tags) { + Angle candidate = UtilityFunctions.WrapAngle(angleFromTranslation(robot, tag)); + double dif = UtilityFunctions.angleDiff(referenceAngle, candidate).abs(Degrees); + + if (dif < closestDistance) { + closestDistance = dif; + closestPosition = tag; + } + } + + return closestPosition; + } + + private static int getFirstEntryIndex(Distance distance) { + int low = 0; + int high = ShooterConstants.kShootingEntries.length; + int mid = 0; + + while (low < high) { + mid = (low + high) / 2; + ShootingEntry midEntry = ShooterConstants.kShootingEntries[mid]; + + if (distance.gt(midEntry.distance())) { + low = mid + 1; + } else { + high = mid; + } + } + + ShootingEntry closestEntry = ShooterConstants.kShootingEntries[mid]; + + int previousEntryIndex; + + if (distance.lt(closestEntry.distance())) { + if (mid == 0) { + previousEntryIndex = mid; + } else { + previousEntryIndex = mid - 1; + } + } else { + if (mid == ShooterConstants.kShootingEntries.length - 1) { + previousEntryIndex = mid - 1; + } else { + previousEntryIndex = mid; + } + } + + return previousEntryIndex; + } + + private static TargetSolution getInterpolatedShootingParameters(Distance distance, LinearVelocity vx, + LinearVelocity vy, Angle turretAngle) { + + LinearVelocity robotVelocity = MetersPerSecond.of(Math.hypot(vx.in(MetersPerSecond), vy.in(MetersPerSecond))); + + int firstEntryIndex = getFirstEntryIndex(distance); + + ShootingEntry firstEntry = ShooterConstants.kShootingEntries[firstEntryIndex]; + ShootingEntry secondEntry = ShooterConstants.kShootingEntries[firstEntryIndex + 1]; + + Angle phi = Radians.of(0.0); + + if (robotVelocity.gt(ShooterConstants.kMaxStationaryVelocity)) { + Time timeOfFlight = Seconds.of(UtilityFunctions.interpolate(firstEntry.distance().in(Meters), + secondEntry.distance().in(Meters), firstEntry.timeOfFlight().in(Seconds), + secondEntry.timeOfFlight().in(Seconds), distance.in(Meters))); + + LinearVelocity radialVelocityTorwardsHub = MetersPerSecond + .of(vy.in(MetersPerSecond) * Math.sin(turretAngle.in(Radians)) + + vx.in(MetersPerSecond) * Math.cos(turretAngle.in(Radians))); + + LinearVelocity tangentialVelocityFromHub = MetersPerSecond + .of(vx.in(MetersPerSecond) * Math.sin(turretAngle.in(Radians)) + + vy.in(MetersPerSecond) * Math.cos(turretAngle.in(Radians))); + + Distance sideDistance = tangentialVelocityFromHub.times(timeOfFlight); + distance = distance.minus(radialVelocityTorwardsHub.times(timeOfFlight)); + + phi = Radians.of(Math.atan(sideDistance.in(Meters) / distance.in(Meters))); + + int transformedFirstEntryIndex = getFirstEntryIndex(distance); + + firstEntry = ShooterConstants.kShootingEntries[transformedFirstEntryIndex]; + secondEntry = ShooterConstants.kShootingEntries[transformedFirstEntryIndex + 1]; + } + + AngularVelocity wheelSpeed = RadiansPerSecond.of(UtilityFunctions.interpolate(firstEntry.distance().in(Meters), + secondEntry.distance().in(Meters), firstEntry.wheelVelocity().in(RadiansPerSecond), + secondEntry.wheelVelocity().in(RadiansPerSecond), distance.in(Meters))); + + Angle hoodAngle = Radians.of(UtilityFunctions.interpolate(firstEntry.distance().in(Meters), + secondEntry.distance().in(Meters), firstEntry.shooterAngle().in(Radians), + secondEntry.shooterAngle().in(Radians), distance.in(Meters))); + + return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); + } + + public Command Aim(Angle turretAngle, Angle hoodAngle) { + return new InstantCommand(() -> { + m_turret.moveToAngle(turretAngle); + m_shooter.MoveHoodToPosition(hoodAngle); + }); + } + + public Command Shoot(AngularVelocity shooterWheelVelocity) { + return new InstantCommand(() -> { + m_shooter.Spin(shooterWheelVelocity); + }); + } + + // TODO: Make this better + + // Our valid shooting ranges are going to change based on the shooter hood + // angle. If the hood angle is too low, then shooting the ball would lead to it + // hitting the side of the robot or other balls currently being held in the + // robot + private Angle[] getCurrentTurretRange() { + if (m_shooter.GetHoodAngle().lt(ShooterConstants.kTurretAngleRestrictiveShooterAngle)) { + return TurretConstants.kRestrictedAngles; + } + + return TurretConstants.kUnrestrictedAngles; + } + + // Must be organized where at every even index it contains the minimum and every + // odd index contains the max angle + private boolean withinAngles(Angle[] angles, Angle candidate) { + if (angles.length % 2 != 0) { + return false; + } + + for (int i = 0; i < angles.length - 1; i += 2) { + Angle min = angles[i]; + Angle max = angles[i + 1]; + if (withinRange(min, max, candidate)) + return true; + } + + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 2bc6a83..9c8832c 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; @@ -24,12 +25,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.utils.ShuffleboardPid; +import frc.robot.utils.TurretPosition; +import frc.robot.utils.UtilityFunctions; import frc.robot.utils.VisionEstimation; import frc.robot.utils.Vision; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; import frc.robot.Constants.NumericalConstants; -import frc.robot.Constants.TurretConstants; +import frc.robot.Constants.OIConstants; +import frc.robot.Constants.DriveConstants.RangeType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -44,6 +48,7 @@ import static edu.wpi.first.units.Units.*; import java.util.Optional; +import java.util.function.Function; public class DriveSubsystem extends SubsystemBase { @@ -82,7 +87,7 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); - private final Vision m_vision = new Vision(Optional.empty(), this::addVisionMeasurement); + private final Vision m_vision; // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, @@ -107,7 +112,10 @@ public class DriveSubsystem extends SubsystemBase { /** * Creates a new DriveSubsystem. */ - public DriveSubsystem() { + public DriveSubsystem(Function turretPositionSupplier) { + + m_vision = new Vision(Optional.of(turretPositionSupplier), this::addVisionMeasurement, + this::getAngularVelocity); // Usage reporting for MAXSwerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_MaxSwerve); @@ -159,27 +167,39 @@ ChassisSpeeds getRobotRelativeSpeeds() { return DriveConstants.kDriveKinematics.toChassisSpeeds(fl, fr, rl, rr); } - public Command moveToAngle(Angle angle) { + public Command moveToAngleCommand(Angle angle) { return new InstantCommand( () -> { - m_isManualRotate = false; - m_targetAutoAngle = angle; + moveToAngle(angle); }); } - public Command faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { - return new InstantCommand(() -> { - Angle robotAngle = getHeading(); - // System.out.println(robotAngle); - - if (withinRange(minAngle, maxAngle, robotAngle)) { - m_isManualRotate = true; - } else { - m_isManualRotate = false; - System.out.println(getClosestAngle(minAngle, maxAngle, robotAngle)); - m_targetAutoAngle = getClosestAngle(minAngle, maxAngle, robotAngle); - } - }, this); + public void moveToAngle(Angle angle) { + m_isManualRotate = false; + m_targetAutoAngle = angle; + + System.out.println("Is Manual Rotate is False in moveToAngle()"); + } + + public void moveByAngle(Angle angle) { + m_isManualRotate = false; + System.out.println("Is Manual Rotate is False in moveByAngle()"); + m_targetAutoAngle = getHeading().plus(angle); + } + + public RangeType faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { + Angle robotAngle = getHeading(); + // System.out.println(robotAngle); + + if (withinRange(minAngle, maxAngle, robotAngle)) { + m_isManualRotate = true; + return RangeType.Within; + } else { + m_isManualRotate = false; + System.out.println("Is Manual Rotate is False in faceCardinalHeadingRange"); + m_targetAutoAngle = getClosestAngle(minAngle, maxAngle, robotAngle); + return m_targetAutoAngle.isEquivalent(minAngle) ? RangeType.CloseMin : RangeType.CloseMax; + } } public Command facePose(Pose2d fixture) { @@ -198,13 +218,12 @@ public Command facePose(Pose2d fixture) { m_targetAutoAngle = Radians.of(Math.atan2(yFixtureDist, xFixtureDist)); m_isManualRotate = false; + System.out.println("Is Manual Rotate is False in facePose()"); }); } - public Command disableFaceHeading() { - return new InstantCommand(() -> { - m_isManualRotate = true; - }); + public void disableFaceHeading() { + m_isManualRotate = true; } @Override @@ -233,10 +252,16 @@ public void periodic() { }); } + if (!m_isManualRotate + && UtilityFunctions.WrapAngle(UtilityFunctions.WrapAngle(getHeading()).minus(m_targetAutoAngle)) + .abs(Degrees) < DriveConstants.kTurnToAngleTolerance.in(Degrees)) { + m_isManualRotate = true; + } + // System.out.println("Current rotation: " + // getPose().getRotation().getRadians()); - m_poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); + m_poseEstimator.update(new Rotation2d(getGyroHeading()), getModulePositions()); m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); // System.out.println(m_poseEstimator.getEstimatedPosition()); @@ -246,6 +271,8 @@ public void periodic() { SmartDashboard.putData(m_field); m_pidController.periodic(); + + SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); } /** @@ -294,24 +321,40 @@ public void resetOdometry(Pose2d pose) { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { // Convert the commanded speeds into the correct units for the drivetrain - if (!m_isManualRotate) - System.out - .println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians) + ", Current: " - + getHeading().in(Radians)); + // if (!m_isManualRotate) + // System.out + // .println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, + // getHeading()).in(Radians) + ", Current: " + // + getHeading().in(Radians)); - double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeed.magnitude(); - double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeed.magnitude(); - double rotDelivered = (m_isManualRotate) ? rot * DriveConstants.kMaxAngularSpeed.magnitude() - : m_pidController.calculate(getHeading().in(Radians), - getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); + final double latestTime = Timer.getFPGATimestamp(); + final double timeElapsed = latestTime - m_latestTime < 0.20 ? latestTime - m_latestTime + : DriveConstants.kPeriodicInterval.in(Seconds); + + m_latestTime = latestTime; - System.out.println("Target " + m_targetAutoAngle + ", Current" + getHeading()); + if (Math.abs(rot) > OIConstants.kDriveDeadband) { + m_isManualRotate = true; + } - var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + final double pidCalculation = m_pidController.calculate(getHeading().in(Radians), + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); + + final double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeed.magnitude(); + final double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeed.magnitude(); + final double rotDelivered = (m_isManualRotate) + ? rot * DriveConstants.kMaxAngularSpeed.magnitude() + : pidCalculation; + + // System.out.println("Target " + m_targetAutoAngle + ", Current" + + // getHeading()); + + final var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, new Rotation2d(getHeading())) - : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); + : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), + timeElapsed)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.kMaxSpeed.magnitude()); @@ -319,12 +362,6 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); m_rearRight.setDesiredState(swerveModuleStates[3]); - - double latestTime = Timer.getFPGATimestamp(); - double timeElapsed = latestTime - m_latestTime < 0.20 ? latestTime - m_latestTime - : DriveConstants.kPeriodicInterval.in(Seconds); - - m_latestTime = latestTime; } public void drive(ChassisSpeeds speeds) { @@ -378,12 +415,32 @@ public void zeroHeading() { * @return the robot's heading in degrees, from -180 to 180 */ public Angle getHeading() { + // return pidgey.getYaw().getValue(); + // TODO: Don't use this code + return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); + } + + public Angle getGyroHeading() { return pidgey.getYaw().getValue(); } public void addVisionMeasurement(VisionEstimation estimation) { - System.out.println(estimation.m_pose); - m_poseEstimator.addVisionMeasurement(estimation.m_pose, estimation.m_timestamp, estimation.m_stdDevs); + // System.out.println("Vision applied."); + m_poseEstimator.addVisionMeasurement(estimation.m_pose, + estimation.m_timestamp, estimation.m_stdDevs); + } + + public ChassisSpeeds getChassisSpeeds() { + + return ChassisSpeeds.fromRobotRelativeSpeeds( + DriveConstants.kDriveKinematics.toChassisSpeeds(m_frontLeft.getState(), m_frontRight.getState(), + m_rearLeft.getState(), m_rearRight.getState()), + new Rotation2d(getHeading())); + + } + + public Field2d getField() { + return m_field; } public Fixtures.FieldLocations getRobotLocation() { @@ -391,29 +448,20 @@ public Fixtures.FieldLocations getRobotLocation() { Pose2d robotPose = getPose(); double x = robotPose.getX(); - double y = robotPose.getY(); if (alliance.isPresent()) { if (alliance.get() == Alliance.Blue) { - if (x < Fixtures.kBlueSideNeutralBorder.in(Meters) && x > Fixtures.kRedSideNeutralBorder.in(Meters)) { - if (y < Fixtures.kFieldYMidpoint.in(Meters)) { - return Fixtures.FieldLocations.NeutralLeftSide; - } else { - return Fixtures.FieldLocations.NeutralRightSide; - } - } else if (x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { + if (x > Fixtures.kBlueSideNeutralBorder.in(Meters) && x < Fixtures.kRedSideNeutralBorder.in(Meters)) { + return Fixtures.FieldLocations.NeutralSide; + } else if (x < Fixtures.kBlueSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.AllianceSide; } else { return Fixtures.FieldLocations.OpponentSide; } } else if (alliance.get() == Alliance.Red) { - if (x > Fixtures.kRedSideNeutralBorder.in(Meters) && x < Fixtures.kBlueSideNeutralBorder.in(Meters)) { - if (y < Fixtures.kFieldYMidpoint.in(Meters)) { - return Fixtures.FieldLocations.NeutralRightSide; - } else { - return Fixtures.FieldLocations.NeutralLeftSide; - } - } else if (x < Fixtures.kRedSideNeutralBorder.in(Meters)) { + if (x < Fixtures.kRedSideNeutralBorder.in(Meters) && x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { + return Fixtures.FieldLocations.NeutralSide; + } else if (x > Fixtures.kRedSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.AllianceSide; } else { return Fixtures.FieldLocations.OpponentSide; @@ -424,21 +472,12 @@ public Fixtures.FieldLocations getRobotLocation() { return null; } - private static Angle wrapAngle(Angle heading) { - Angle robotRotations = Radians - .of(Math.floor(heading.in(Radians) / (2 * Math.PI)) * 2.0 * Math.PI); - - Angle wrap = heading.minus(robotRotations); - - if (wrap.lt(Radians.of(0))) { - wrap = wrap.plus(TurretConstants.kFullRotation); - } - - return wrap; + private AngularVelocity getAngularVelocity() { + return DegreesPerSecond.of(pidgey.getAngularVelocityZDevice().getValueAsDouble()); } private static Angle getOptimalAngle(Angle target, Angle robotHeading) { - Angle wrappedRobotAngle = wrapAngle(robotHeading); + Angle wrappedRobotAngle = UtilityFunctions.WrapAngle(robotHeading); Angle delta = target.minus(wrappedRobotAngle); @@ -454,16 +493,16 @@ private static Angle getOptimalAngle(Angle target, Angle robotHeading) { } private static boolean withinRange(Angle min, Angle max, Angle angle) { - angle = wrapAngle(angle); + angle = UtilityFunctions.WrapAngle(angle); min = getOptimalAngle(angle, min); max = getOptimalAngle(angle, max); return angle.gt(max) && angle.lt(min); } private static Angle getClosestAngle(Angle t1, Angle t2, Angle angle) { - t1 = wrapAngle(t1); - t2 = wrapAngle(t2); - angle = wrapAngle(angle); + t1 = UtilityFunctions.WrapAngle(t1); + t2 = UtilityFunctions.WrapAngle(t2); + angle = UtilityFunctions.WrapAngle(angle); return t1.minus(angle).abs(Rotations) < t2.minus(angle).abs(Rotations) ? t1 : t2; } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5c29608..6a5ee2d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -16,6 +16,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; +import frc.robot.utils.UtilityFunctions; public class ShooterSubsystem extends SubsystemBase { @@ -30,6 +31,8 @@ public class ShooterSubsystem extends SubsystemBase { private final SparkMaxConfig m_shooterConfig = new SparkMaxConfig(); private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); + Angle m_targetAngle = Degrees.of(0.0); + public ShooterSubsystem() { m_shooterConfig.closedLoop @@ -41,29 +44,58 @@ public ShooterSubsystem() { .p(ShooterConstants.kHoodP) .i(ShooterConstants.kHoodI) .d(ShooterConstants.kHoodD); + m_hoodConfig.smartCurrentLimit(ShooterConstants.kHoodSmartCurrentLimit); m_hoodConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_hoodConfig.absoluteEncoder.inverted(true); + m_hoodConfig.inverted(true); - // Inverse since kHoodGearRatio gives encoder -> hood motion, and we need hood - // motion -> encoder - m_hoodConfig.encoder.positionConversionFactor(1.0 / ShooterConstants.kHoodGearRatio); + // .6 rotations = 30 degrees + // 1 rotation = 50 degrees + m_hoodConfig.absoluteEncoder.positionConversionFactor(1); + m_hoodConfig.encoder.positionConversionFactor(1); - m_shooterMotor.configure(m_shooterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - m_hoodMotor.configure(m_hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_shooterMotor.configure(m_shooterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - public void Aim(Angle angle) { - m_hoodClosedLoopController.setSetpoint(angle.in(Rotations), - ControlType.kPosition); + // Position between 0 and .55 + public void MoveHoodToPosition(Angle angle) { + System.out.println("Move hood to position: " + angle); + + // get target absolute encoder position. 0 starts in hood min, hood max is .55 + // (30 degrees of movement) + var targetPosition = angle.in(Degrees) * (ShooterConstants.kHoodDegreeConversionFactor); + if (targetPosition < 0 || targetPosition > ShooterConstants.kHoodMaxAbsolutePosition) { + System.out.println("Hood target position out of bounds. Target: " + targetPosition); + return; + } + var curentPosition = m_absoluteEncoder.getPosition(); + System.out.println("current hood: " + curentPosition); + if (curentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { + System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); + return; + } + + m_hoodClosedLoopController.setSetpoint(targetPosition, ControlType.kPosition); } public void Spin(AngularVelocity shootSpeedVelocity) { m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); } - public void StopShooting() { + public void Stop() { Spin(RPM.of(0)); } + public Angle GetHoodAngle() { + return Degrees.of(m_absoluteEncoder.getPosition() / ShooterConstants.kHoodDegreeConversionFactor); + } + + public boolean AtTarget() { + return UtilityFunctions.angleDiff(GetHoodAngle(), m_targetAngle).abs(Degrees) < ShooterConstants.kHoodTolerence + .in(Degrees); + } + @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java new file mode 100644 index 0000000..8535536 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.StagingConstants; + +public class StagingSubsystem extends SubsystemBase { + SparkMax m_feedIntoHoodMotor = new SparkMax(StagingConstants.kFeedIntoHoodMotor, MotorType.kBrushless); + SparkMax m_agitationMotor = new SparkMax(StagingConstants.kAgitationMotorId, MotorType.kBrushless); + SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, null); + + public void Agitate() { + m_agitationMotor.set(StagingConstants.kAgitationSpeed); + } + + // NOT ACTUALLY FEEDING FUEL TO OTHER SIDE OF FIELD, this feed refers to feedin + // fuel into the hood + public void Feed() { + m_feedIntoHoodMotor.set(StagingConstants.kFeedIntoHoodSpeed); + } + + // Refers to the roller that rolls balls into the feeder + public void Roll() { + m_rollerMotor.set(StagingConstants.kRollerSpeed); + } +} diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 5ee9929..8f610f0 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,111 +1,158 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotations; - -import java.util.function.Supplier; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.sim.SparkMaxSim; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.LinearVelocity; +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.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; +import frc.robot.utils.TurretPosition; +import frc.robot.utils.UtilityFunctions; public class TurretSubsystem extends SubsystemBase { private final SparkMax m_turretMotor = new SparkMax(TurretConstants.kMotorId, MotorType.kBrushless); - private final SparkMaxSim m_simTurretMotor = new SparkMaxSim(m_turretMotor, DCMotor.getNEO(1)); private final AbsoluteEncoder m_absoluteEncoder = m_turretMotor.getAbsoluteEncoder(); private final SparkClosedLoopController m_turretClosedLoopController = m_turretMotor.getClosedLoopController(); private final PositionBuffer m_positionBuffer = new PositionBuffer(TurretConstants.kPositionBufferLength); - private Supplier m_robotPoseSupplier; - private SparkMaxConfig m_config = new SparkMaxConfig(); - public TurretSubsystem(Supplier robotPoseSupplier) { - m_robotPoseSupplier = robotPoseSupplier; + private Mechanism2d m_simMech = new Mechanism2d(4.0, 4.0); + private MechanismRoot2d m_mechRoot = m_simMech.getRoot("Turret Root", 2.0, 2.0); + + private MechanismLigament2d m_simLigament = new MechanismLigament2d("Turret", 2, 0); + + private MechanismLigament2d m_min1 = new MechanismLigament2d("min1", 2, 0); + private MechanismLigament2d m_max1 = new MechanismLigament2d("max1", 2, 0); + private MechanismLigament2d m_min2 = new MechanismLigament2d("min2", 2, 0); + private MechanismLigament2d m_max2 = new MechanismLigament2d("max2", 2, 0); + + private Angle robotRotation; + + private Angle m_targetAngle = Degrees.of(0.0); + private Angle m_simAngle = Degrees.of(0.0); + public TurretSubsystem() { m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); m_config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_config.smartCurrentLimit(TurretConstants.kSmartCurrentLimit); m_config.idleMode(IdleMode.kBrake); + m_config.absoluteEncoder.inverted(true); + m_config.inverted(true); m_turretMotor.configure(m_config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + RPM.of(m_absoluteEncoder.getVelocity()), + TurretConstants.kEncoderReadingDelay.in(Seconds)); + + m_simLigament = m_mechRoot.append(m_simLigament); + + m_min1 = m_mechRoot.append(m_min1); + m_min1.setColor(new Color8Bit("#FF00FF")); + + m_max1 = m_mechRoot.append(m_max1); + m_max1.setColor(new Color8Bit("#FF00FF")); + + m_min2 = m_mechRoot.append(m_min2); + m_min2.setColor(new Color8Bit("#FF0000")); + + m_max2 = m_mechRoot.append(m_max2); + m_max2.setColor(new Color8Bit("#FF0000")); + + robotRotation = Degrees.of(0); } public void moveToAngle(Angle angle) { - angle = wrapAngle(angle); + angle = UtilityFunctions.WrapAngle(angle); if (angle.gt(TurretConstants.kMaxAngle)) { - // System.out - // .println("Angle " + angle + "is bigger than maximum angle " + - // TurretConstants.kMaxAngle + "."); + System.out + .println("Angle " + angle + "is bigger than maximum angle " + + TurretConstants.kMaxAngle + "."); return; } else if (angle.lt(TurretConstants.kMinAngle)) { - // System.out.println( - // "Angle " + angle + "is to smaller than minimum angle " + - // TurretConstants.kMinAngle + "."); + System.out.println( + "Angle " + angle + "is to smaller than minimum angle " + + TurretConstants.kMinAngle + "."); return; } // System.out.println("Target angle is " + angle); + m_targetAngle = angle; m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } - public void pointToHeading(Angle heading) { - // moveToAngle( - // heading.minus(Radians.of(m_robotPoseSupplier.get().getRotation().getRadians() - // - heading.magnitude()))); - moveToAngle(Radians.of(m_robotPoseSupplier.get().getRotation().getRadians() - heading.in(Radians))); + public Angle getRotation() { + return UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())); } - public Angle getRotation() { - return Rotations.of(m_absoluteEncoder.getPosition()); + public void addDriveHeading(Angle angle) { + robotRotation = angle; } - public Angle getRotationAtTime(double timestamp) { + public TurretPosition getRotationAtTime(double timestamp) { return m_positionBuffer.getAngleAtTime(timestamp); + // return new TurretPosition(getRotation(), RotationsPerSecond.of(0.0), + // timestamp); } @Override public void periodic() { + m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + RPM.of(m_absoluteEncoder.getVelocity()), + TurretConstants.kEncoderReadingDelay.in(Seconds)); } // Connected to another periodic loop that runs quicker than 0.02 seconds public void pushCurrentEncoderReading() { - m_positionBuffer.pushElement(Rotations.of(m_absoluteEncoder.getPosition()), - TurretConstants.kEncoderReadingDelay.in(Seconds)); + // m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + // RPM.of(m_absoluteEncoder.getVelocity()), + // TurretConstants.kEncoderReadingDelay.in(Seconds)); + } + + public LinearVelocity getMuzzleVelocityAtHoodAngle() { + return MetersPerSecond.of(5); } @Override public void simulationPeriodic() { - m_simTurretMotor.setVelocity(1.0); - // System.out.println(m_simAbsoluteEncoder.getPosition()); + m_simLigament.setAngle(m_targetAngle.plus(robotRotation).in(Degrees)); + m_min1.setAngle(TurretConstants.kHubMinAngle1.plus(robotRotation).in(Degrees)); + m_max1.setAngle(TurretConstants.kHubMaxAngle1.plus(robotRotation).in(Degrees)); + m_min2.setAngle(TurretConstants.kHubMinAngle2.plus(robotRotation).in(Degrees)); + m_max2.setAngle(TurretConstants.kHubMaxAngle2.plus(robotRotation).in(Degrees)); + SmartDashboard.putData("Turret Rotation", m_simMech); } - private Angle wrapAngle(Angle angle) { - Angle wrap = Radians.of((angle.in(Radians) % TurretConstants.kFullRotation.in(Radians))); - if (wrap.lt(TurretConstants.kNoRotation)) { - wrap = wrap.plus(TurretConstants.kFullRotation); - } - - return wrap; + public boolean atTarget() { + return UtilityFunctions.angleDiff(m_targetAngle, getRotation()) + .abs(Degrees) < TurretConstants.kTurretAngleTolerance.in(Degrees); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index 3315aaf..15e291a 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -1,28 +1,30 @@ package frc.robot.utils; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Timer; public class PositionBuffer { - private record TimePosition(Angle angle, double timestamp) { - } + TurretPosition m_position; - private final RingBuffer m_positions; + private final RingBuffer m_positions; public PositionBuffer(int length) { m_positions = new RingBuffer<>(length); } - public void pushElement(Angle angle, double delay) { + public void pushElement(Angle angle, AngularVelocity velocity, double delay) { try { - m_positions.push(new TimePosition(angle, Timer.getFPGATimestamp() - delay)); + m_positions.push(new TurretPosition(angle, velocity, Timer.getFPGATimestamp() - delay)); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); } } - public Angle getAngleAtTime(double requestedTime) { + public TurretPosition getAngleAtTime(double requestedTime) { // A fun little binary search algo int high = m_positions.getLength(); int low = 0; @@ -30,10 +32,13 @@ public Angle getAngleAtTime(double requestedTime) { // Avoiding overflow (though isn't really necessary here) int midpoint = low + (high - low) / 2; + TurretPosition midpointTurretPosition = null; + double timeAtMidpoint = 0.0; + while (low < high) { - double timeAtMidpoint; try { - timeAtMidpoint = m_positions.get(midpoint).timestamp; + midpointTurretPosition = m_positions.get(midpoint); + timeAtMidpoint = midpointTurretPosition.timestamp(); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; @@ -48,58 +53,54 @@ public Angle getAngleAtTime(double requestedTime) { midpoint = low + (high - low) / 2; } - // Linearly interpolate velocity if we aren't on the first/last timestamp + // Linearly UtilityFunctions.interpolate velocity if we aren't on the first/last + // timestamp if (midpoint != 0 && midpoint != m_positions.getLength() - 1) { - TimePosition closestTimestamp; - - try { - closestTimestamp = m_positions.get(midpoint); - } catch (Exception e) { - return null; - } - double dt; try { - dt = m_positions.get(midpoint).timestamp - requestedTime; + dt = timeAtMidpoint - requestedTime; } catch (Exception e) { return null; } - TimePosition nextTimeStamp; - // Stampted Time 1 should always be less than stampted time 2 - TimePosition stamptedTime1; - TimePosition stamptedTime2; + TurretPosition firstTurretPosition; + TurretPosition secondTurretPosition; if (dt > 0) { try { - stamptedTime2 = m_positions.get(midpoint + 1); - stamptedTime1 = closestTimestamp; + secondTurretPosition = m_positions.get(midpoint + 1); + firstTurretPosition = midpointTurretPosition; } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; } } else { try { - stamptedTime1 = m_positions.get(midpoint - 1); - stamptedTime2 = closestTimestamp; + firstTurretPosition = m_positions.get(midpoint - 1); + secondTurretPosition = midpointTurretPosition; } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; } } - double deltaTimeStampted = stamptedTime2.timestamp - stamptedTime1.timestamp; - double timeSinceStamp1 = requestedTime - stamptedTime1.timestamp; + Angle angle = Radians.of(UtilityFunctions.interpolate(firstTurretPosition.timestamp(), + secondTurretPosition.timestamp(), + firstTurretPosition.angle().in(Radians), secondTurretPosition.angle().in(Radians), requestedTime)); - Angle deltaTheta = stamptedTime2.angle.minus(stamptedTime2.angle); + AngularVelocity velocity = RPM + .of(UtilityFunctions.interpolate(firstTurretPosition.timestamp(), secondTurretPosition.timestamp(), + firstTurretPosition.velocity().in(RPM), secondTurretPosition.velocity().in(RPM), + requestedTime)); - return deltaTheta.times(timeSinceStamp1 / deltaTimeStampted).plus(stamptedTime1.angle); + return new TurretPosition(angle, velocity, requestedTime); } try { - return m_positions.get(midpoint).angle; + // System.out.println(m_positions.get(midpoint) + " is midpoint"); + return m_positions.get(midpoint); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; diff --git a/src/main/java/frc/robot/utils/ShootingEntry.java b/src/main/java/frc/robot/utils/ShootingEntry.java new file mode 100644 index 0000000..0017da2 --- /dev/null +++ b/src/main/java/frc/robot/utils/ShootingEntry.java @@ -0,0 +1,13 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; + +public record ShootingEntry(Distance distance, AngularVelocity wheelVelocity, LinearVelocity muzzleVelocity, + Distance maxHeight, + Time timeOfFlight, Angle shooterAngle) { + +} diff --git a/src/main/java/frc/robot/utils/TargetSolution.java b/src/main/java/frc/robot/utils/TargetSolution.java new file mode 100644 index 0000000..2b6739f --- /dev/null +++ b/src/main/java/frc/robot/utils/TargetSolution.java @@ -0,0 +1,11 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; + +// Phi is how much the aiming heading should be adjusted to properly correct for velocity +public record TargetSolution(Angle hoodAngle, AngularVelocity wheelSpeed, Angle phi, Distance distance, + Angle hubAngle) { + +} diff --git a/src/main/java/frc/robot/utils/TurretPosition.java b/src/main/java/frc/robot/utils/TurretPosition.java new file mode 100644 index 0000000..24a41dd --- /dev/null +++ b/src/main/java/frc/robot/utils/TurretPosition.java @@ -0,0 +1,8 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public record TurretPosition(Angle angle, AngularVelocity velocity, + double timestamp) { +} diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java new file mode 100644 index 0000000..1e8c7eb --- /dev/null +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -0,0 +1,37 @@ +package frc.robot.utils; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; +import edu.wpi.first.units.measure.Angle; +import frc.robot.Constants.NumericalConstants; + +public final class UtilityFunctions { + public static Angle WrapAngle(Angle angle) { + return Radians.of(((angle.in(Radians) % NumericalConstants.kFullRotation.in(Radians)) + + NumericalConstants.kFullRotation.in(Radians)) % NumericalConstants.kFullRotation + .in(Radians)); + } + + public static Angle WrapTo180(Angle angle) { + Angle wrapped = WrapAngle(angle); + + if (wrapped.gt(Degrees.of(180))) { + wrapped = wrapped.minus(NumericalConstants.kFullRotation); + } + + return wrapped; + } + + public static double interpolate(double x1, double x2, double y1, double y2, double x) { + double dsx = x2 - x1; + double dy = y2 - y1; + double dx = x - x1; + + return (dx / Math.abs(dsx) > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; + } + + public static Angle angleDiff(Angle a1, Angle a2) { + Angle diff = a1.minus(a2); + return WrapTo180(diff); + } +} diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 7ae17a3..24ec10b 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -9,24 +9,17 @@ import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonUtils; -import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.Constants.VisionConstants; import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - import static edu.wpi.first.units.Units.*; public class Vision { @@ -34,7 +27,8 @@ public class Vision { PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); - Optional> m_turretAngleSupplier; + Optional> m_turretPositionSupplier; + Supplier m_robotAngularVelocitySupplier; PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(VisionConstants.kTagLayout, VisionConstants.kRobotToCamOne); @@ -44,9 +38,11 @@ public class Vision { Consumer m_visionConsumer; private Matrix curStdDevs; - public Vision(Optional> turretAngleSupplier, Consumer visionConsumer) { - m_turretAngleSupplier = turretAngleSupplier; + public Vision(Optional> turretPositionSupplier, + Consumer visionConsumer, Supplier robotAngularVelocitySupplier) { + m_turretPositionSupplier = turretPositionSupplier; m_visionConsumer = visionConsumer; + m_robotAngularVelocitySupplier = robotAngularVelocitySupplier; } public void periodic() { @@ -69,7 +65,16 @@ public void periodic() { Optional visionEstimationCameraTwo = Optional.empty(); for (var result : m_camera2.getAllUnreadResults()) { - m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform(result.getTimestampSeconds())); + Transform3d cameraTransform; + + cameraTransform = getTurretCameraTransform(result.getTimestampSeconds()); + + if (cameraTransform == null) { + System.out.println("Turret exceeded max velocity valid for reading april tags."); + break; + } + + m_poseEstimatorTwo.setRobotToCameraTransform(cameraTransform); visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); if (visionEstimationCameraTwo.isEmpty()) { @@ -78,7 +83,10 @@ public void periodic() { updateEstimationStdDevs(visionEstimationCameraTwo, result.getTargets(), m_poseEstimatorTwo); + // final var tmp = visionEstimationCameraTwo; + visionEstimationCameraTwo.ifPresent(estimation -> { + // System.out.println(tmp + ", " + cameraTransform); m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), estimation.timestampSeconds, getCurrentStdDevs())); }); @@ -132,22 +140,36 @@ private void updateEstimationStdDevs( } private Transform3d getTurretCameraTransform(double estimationTime) { - if (m_turretAngleSupplier.isEmpty()) + if (m_turretPositionSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; - Angle turretAngle = m_turretAngleSupplier.get().apply(estimationTime); + TurretPosition turretPosition = m_turretPositionSupplier.get().apply(estimationTime); + + // Getting the net velocity of the turret relative to the field + if (turretPosition == null || turretPosition.velocity().plus( + m_robotAngularVelocitySupplier.get()).abs(DegreesPerSecond) > VisionConstants.kMaxTurretVisionSpeed + .in(DegreesPerSecond)) { + return null; + } + + Distance cameraX = VisionConstants.kTurretCameraDistanceToCenter + .times(Math.cos(turretPosition.angle().minus(VisionConstants.kCameraTwoYaw).in(Radians))) + .plus(VisionConstants.kTurretCenterOfRotation.getMeasureX()); + + Distance cameraY = VisionConstants.kTurretCameraDistanceToCenter + .times(Math.sin(turretPosition.angle().minus(VisionConstants.kCameraTwoYaw).in(Radians))) + .plus(VisionConstants.kTurretCenterOfRotation.getMeasureY()); + + Translation3d cameraPosition = new Translation3d(cameraX, cameraY, + VisionConstants.kCameraTwoZ); - Distance cameraXOffset = Meters - .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.cos(turretAngle.in(Radians))); - Distance cameraYOffset = Meters - .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.sin(turretAngle.in(Radians))); + Rotation3d cameraRotation = new Rotation3d(VisionConstants.kCameraTwoRoll, VisionConstants.kCameraTwoPitch, + turretPosition.angle()); - Distance cameraX = VisionConstants.kTurretAxisOfRotation.getMeasureX().plus(cameraXOffset); - Distance cameraY = VisionConstants.kTurretAxisOfRotation.getMeasureY().plus(cameraYOffset); - Distance cameraZ = VisionConstants.kTurretAxisOfRotation.getMeasureZ(); + // System.out.println(cameraX + ", " + cameraY + ", " + + // turretPosition.angle().in(Degrees)); - return new Transform3d(cameraX, cameraY, cameraZ, - new Rotation3d(VisionConstants.kCameraTwoPitch, VisionConstants.kCameraTwoRoll, turretAngle)); + return new Transform3d(cameraPosition, cameraRotation); } private Matrix getCurrentStdDevs() {