diff --git a/.vscode/settings.json b/.vscode/settings.json index fef457d..b6e2b76 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,7 +59,6 @@ ], "java.dependency.enableDependencyCheckup": false, "cSpell.words": [ - "Deadband", "feedforwards", "Holonomic", "Photonvision", diff --git a/README.md b/README.md index 6bcc477..919f6da 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,7 @@ All devices connected to the robot's local network along with each device's assi | ------- | ---------- | | Gateway | 10.56.90.1 | | RoboRio | 10.56.90.2 | +| Vision Coprocessor | 10.56.90.10 | ## Button Bindings diff --git a/cad/camera-holder.FCStd b/cad/camera-holder.FCStd new file mode 100644 index 0000000..7dbfb0e Binary files /dev/null and b/cad/camera-holder.FCStd differ diff --git a/simgui-ds.json b/simgui-ds.json index 2f801a3..f671bb9 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -101,7 +101,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5d42e0..5be2d40 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,17 +5,27 @@ 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.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; @@ -71,7 +81,7 @@ public static final class DriveConstants { public static final int kFrontRightTurningCanId = 5; public static final int kRearRightTurningCanId = 8; - // Auxillary Device Can IDs + // Auxiliary Device Can IDs public static final int kPidgeyCanId = 13; public static final boolean kGyroReversed = false; @@ -137,15 +147,106 @@ public static final class NeoMotorConstants { public static final class VisionConstants { - public static final Transform3d kRobotToCam - = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + public static final String kCameraName1 = "Photonvision"; + public static final String kCameraName2 = "Photonvision2"; + + // Distance to fill pose3d z value assuming robot is on the ground + public static Distance kEncoderZOffset = Inches.of(5.0); + + // Confidence of encoder readings for vision; should be tuned + public static final double kEncoderConfidence = 0.15; + + 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), + 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 Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - public static final AprilTagFieldLayout kTagLayout - = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + 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 class NumericalConstants { public static final double kEpsilon = 1e-6; } + + public static final class TurretConstants { + public static final int kMotorId = 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 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 class ShooterConstants { + public static final int kShooterMotorId = 30; + public static final int kHoodMotorId = 31; + + public static final double kHoodP = 0.1; + public static final double kHoodI = 0.0; + public static final double kHoodD = 0.0; + + public static final double kShooterP = 0.1; + public static final double kShooterI = 0.0; + 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); + + public static final Angle kFeedAngle = Degrees.of(90.0); + } + + public static final class Fixtures { + public static final Pose2d kRedAllianceHub = new Pose2d(); + public static final Pose2d kBlueAllianceHub = new Pose2d(); + + // 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 Distance kFieldYMidpoint = Inches.of(317.69 / 2.0); + + public static final Distance kRedSideNeutralBorder = Inches.of(182.11); + public static final Distance kBlueSideNeutralBorder = Inches.of(651.22 - 182.11); + + public static enum FieldLocations { + AllianceSide, + NeutralLeftSide, + NeutralRightSide, + OpponentSide + } + + // Placeholders + public static final Angle kRedLeftSideFeedHeading = Degrees.of(40); + public static final Angle kRedRightSideFeedHeading = Degrees.of(160); + + // Placeholders + public static final Angle kBlueLeftSideFeedHeading = Degrees.of(-40); + public static final Angle kBlueRightSideFeedHeading = Degrees.of(-160); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..6cd1383 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -15,6 +16,8 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); + addPeriodic(m_robotContainer.pushTurretEncoderReading(), + Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override @@ -23,13 +26,16 @@ public void robotPeriodic() { } @Override - public void disabledInit() {} + public void disabledInit() { + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } @Override - public void disabledExit() {} + public void disabledExit() { + } @Override public void autonomousInit() { @@ -41,10 +47,12 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override - public void autonomousExit() {} + public void autonomousExit() { + } @Override public void teleopInit() { @@ -54,10 +62,12 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override - public void teleopExit() {} + public void teleopExit() { + } @Override public void testInit() { @@ -65,8 +75,10 @@ public void testInit() { } @Override - public void testPeriodic() {} + public void testPeriodic() { + } @Override - public void testExit() {} + public void testExit() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c671577..48d4fd4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,10 +3,14 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import static edu.wpi.first.units.Units.Degrees; +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.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -15,17 +19,23 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.TurretSubsystem; public class RobotContainer { private final DriveSubsystem m_drive = new DriveSubsystem(); - private final CommandXboxController m_driverController = new CommandXboxController(OIConstants.kDriverControllerPort); + 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; + public RobotContainer() { + m_turret = new TurretSubsystem(m_drive::getPose); + m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); @@ -43,11 +53,14 @@ public RobotContainer() { -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), true), m_drive)); + + // TODO: Get rid of this + m_turret.setDefaultCommand(new RunCommand(() -> m_turret.moveToAngle(Radians.of(Math.PI / 4)), m_turret)); } private void configureBindings() { - m_driverController.x().whileTrue(m_drive.enableFacePose(new Pose2d())); - m_driverController.x().whileFalse(m_drive.disableFacePose()); + m_driverController.a().whileTrue(m_drive.faceCardinalHeadingRange(Degrees.of(342), Degrees.of(190))); + m_driverController.a().whileFalse(m_drive.disableFaceHeading()); } public Command getAutonomousCommand() { @@ -57,4 +70,16 @@ public Command getAutonomousCommand() { return new PathPlannerAuto(m_autoSelected); } + + public Runnable pushTurretEncoderReading() { + return () -> { + m_turret.pushCurrentEncoderReading(); + }; + } + + public Command feedPosition(Alliance alliance) { + return new RunCommand(() -> { + + }, m_drive, m_turret); + } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 7d9088e..f7e3d25 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -6,7 +6,6 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,18 +17,23 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.utils.ShuffleboardPid; +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 edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; - import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.sim.Pigeon2SimState; import com.pathplanner.lib.auto.AutoBuilder; @@ -37,16 +41,10 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import edu.wpi.first.units.measure.*; - import static edu.wpi.first.units.Units.*; import java.util.Optional; -import org.photonvision.PhotonCamera; - -import frc.robot.Constants; - public class DriveSubsystem extends SubsystemBase { // Create MAXSwerveModules @@ -84,6 +82,8 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); + private final Vision m_vision = new Vision(Optional.empty(), this::addVisionMeasurement); + // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(pidgey.getYaw().getValue()), @@ -94,7 +94,7 @@ public class DriveSubsystem extends SubsystemBase { m_rearRight.getPosition() }); - SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( + SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( DriveConstants.kDriveKinematics, new Rotation2d(pidgey.getYaw().getValue()), new SwerveModulePosition[] { @@ -108,6 +108,7 @@ public class DriveSubsystem extends SubsystemBase { * Creates a new DriveSubsystem. */ public DriveSubsystem() { + // Usage reporting for MAXSwerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_MaxSwerve); @@ -166,7 +167,22 @@ public Command moveToAngle(Angle angle) { }); } - public Command enableFacePose(Pose2d fixture) { + 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 Command facePose(Pose2d fixture) { return new RunCommand(() -> { Pose2d robotPose = getPose(); @@ -176,16 +192,16 @@ public Command enableFacePose(Pose2d fixture) { double totalDistance = Math.hypot(xFixtureDist, yFixtureDist); // Floating point value correction - if (Math.abs(totalDistance) < Constants.NumericalConstants.kEpsilon) + if (Math.abs(totalDistance) < NumericalConstants.kEpsilon) return; - + m_targetAutoAngle = Radians.of(Math.atan2(yFixtureDist, xFixtureDist)); m_isManualRotate = false; }); } - public Command disableFacePose() { + public Command disableFaceHeading() { return new InstantCommand(() -> { m_isManualRotate = true; }); @@ -220,8 +236,12 @@ public void periodic() { // System.out.println("Current rotation: " + // getPose().getRotation().getRadians()); - poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); - m_field.setRobotPose(poseEstimator.getEstimatedPosition()); + m_poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + + // System.out.println(m_poseEstimator.getEstimatedPosition()); + + m_vision.periodic(); SmartDashboard.putData(m_field); @@ -234,7 +254,7 @@ public void periodic() { * @return The pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return m_poseEstimator.getEstimatedPosition(); } public SwerveModulePosition[] getModulePositions() { @@ -275,13 +295,17 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // 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)); + 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)); + : m_pidController.calculate(getHeading().in(Radians), + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); + + System.out.println("Target " + m_targetAutoAngle + ", Current" + getHeading()); var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative @@ -357,21 +381,64 @@ public Angle getHeading() { return pidgey.getYaw().getValue(); } - public Angle getNonContinuousHeading() { - if (getHeading().in(Radians) > 2.0 * Math.PI) { - return getHeading(); + public void addVisionMeasurement(VisionEstimation estimation) { + System.out.println(estimation.m_pose); + m_poseEstimator.addVisionMeasurement(estimation.m_pose, estimation.m_timestamp, estimation.m_stdDevs); + } + + public Fixtures.FieldLocations getRobotLocation() { + Optional alliance = DriverStation.getAlliance(); + 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)) { + 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)) { + return Fixtures.FieldLocations.AllianceSide; + } else { + return Fixtures.FieldLocations.OpponentSide; + } + } } - double rotations = Math.floor(getHeading().in(Radians) / (2 * Math.PI)); - return Radians.of(getHeading().in(Radians) - rotations); + return null; } - private Angle getOptimalAngle(Angle target, Angle robotHeading) { - // Full robot rotations in radians + private static Angle wrapAngle(Angle heading) { Angle robotRotations = Radians - .of(Math.floor(robotHeading.in(Radians) / (2 * Math.PI)) * 2.0 * Math.PI); + .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); + } - Angle wrappedRobotAngle = robotHeading.minus(robotRotations); + return wrap; + } + + private static Angle getOptimalAngle(Angle target, Angle robotHeading) { + Angle wrappedRobotAngle = wrapAngle(robotHeading); Angle delta = target.minus(wrappedRobotAngle); @@ -385,4 +452,19 @@ private Angle getOptimalAngle(Angle target, Angle robotHeading) { return delta.plus(robotHeading); } + + private static boolean withinRange(Angle min, Angle max, Angle angle) { + angle = 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); + + return t1.minus(angle).abs(Rotations) < t2.minus(angle).abs(Rotations) ? t1 : t2; + } } diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 9988f5e..c92206f 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -38,7 +38,7 @@ public class MAXSwerveModule { private final SparkClosedLoopController m_drivingClosedLoopController; private final SparkClosedLoopController m_turningClosedLoopController; - + private LinearVelocity m_simDriverEncoderVelocity = MetersPerSecond.of(0.0); private Distance m_simDriverEncoderPosition = Meters.of(0.0); private Angle m_simCurrentAngle = Radians.of(0.0); @@ -83,7 +83,8 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular public void updateSimDriverPosition(SwerveModuleState desiredState) { m_simDriverEncoderVelocity = MetersPerSecond.of(desiredState.speedMetersPerSecond); - m_simDriverEncoderPosition = m_simDriverEncoderPosition.plus(m_simDriverEncoderVelocity.times(Constants.DriveConstants.kPeriodicInterval)); + m_simDriverEncoderPosition = m_simDriverEncoderPosition + .plus(m_simDriverEncoderVelocity.times(Constants.DriveConstants.kPeriodicInterval)); } public SwerveModuleState getState() { @@ -94,7 +95,7 @@ public SwerveModuleState getState() { return new SwerveModuleState(m_drivingEncoder.getVelocity(), new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); - return new SwerveModuleState(m_simDriverEncoderVelocity, + return new SwerveModuleState(m_simDriverEncoderVelocity, new Rotation2d(m_simCurrentAngle.in(Radian) - m_chassisAngularOffset)); } @@ -111,7 +112,7 @@ public SwerveModulePosition getPosition() { return new SwerveModulePosition( m_drivingEncoder.getPosition(), new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); - + return new SwerveModulePosition(m_simDriverEncoderPosition, new Rotation2d(m_simCurrentAngle.in(Radian) - m_chassisAngularOffset)); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..5c29608 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; + +public class ShooterSubsystem extends SubsystemBase { + + SparkMax m_shooterMotor = new SparkMax(ShooterConstants.kShooterMotorId, MotorType.kBrushless); + SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, MotorType.kBrushless); + + AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); + + private final SparkClosedLoopController m_shooterClosedLoopController = m_shooterMotor.getClosedLoopController(); + private final SparkClosedLoopController m_hoodClosedLoopController = m_hoodMotor.getClosedLoopController(); + + private final SparkMaxConfig m_shooterConfig = new SparkMaxConfig(); + private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); + + public ShooterSubsystem() { + + m_shooterConfig.closedLoop + .p(ShooterConstants.kShooterP) + .i(ShooterConstants.kShooterI) + .d(ShooterConstants.kShooterD); + + m_hoodConfig.closedLoop + .p(ShooterConstants.kHoodP) + .i(ShooterConstants.kHoodI) + .d(ShooterConstants.kHoodD); + m_hoodConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + + // Inverse since kHoodGearRatio gives encoder -> hood motion, and we need hood + // motion -> encoder + m_hoodConfig.encoder.positionConversionFactor(1.0 / ShooterConstants.kHoodGearRatio); + + m_shooterMotor.configure(m_shooterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_hoodMotor.configure(m_hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public void Aim(Angle angle) { + m_hoodClosedLoopController.setSetpoint(angle.in(Rotations), + ControlType.kPosition); + } + + public void Spin(AngularVelocity shootSpeedVelocity) { + m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); + } + + public void StopShooting() { + Spin(RPM.of(0)); + } + + @Override + public void periodic() { + + } +} diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java new file mode 100644 index 0000000..5ee9929 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -0,0 +1,111 @@ +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.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 edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.TurretConstants; +import frc.robot.utils.PositionBuffer; + +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; + + 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_turretMotor.configure(m_config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void moveToAngle(Angle angle) { + + angle = wrapAngle(angle); + + if (angle.gt(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 + "."); + return; + } + + // System.out.println("Target angle is " + 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 Rotations.of(m_absoluteEncoder.getPosition()); + } + + public Angle getRotationAtTime(double timestamp) { + return m_positionBuffer.getAngleAtTime(timestamp); + } + + @Override + public void periodic() { + } + + // 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)); + } + + @Override + public void simulationPeriodic() { + m_simTurretMotor.setVelocity(1.0); + // System.out.println(m_simAbsoluteEncoder.getPosition()); + } + + 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; + } +} \ 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 new file mode 100644 index 0000000..3315aaf --- /dev/null +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -0,0 +1,108 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Timer; + +public class PositionBuffer { + + private record TimePosition(Angle angle, double timestamp) { + } + + private final RingBuffer m_positions; + + public PositionBuffer(int length) { + m_positions = new RingBuffer<>(length); + } + + public void pushElement(Angle angle, double delay) { + try { + m_positions.push(new TimePosition(angle, Timer.getFPGATimestamp() - delay)); + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + } + } + + public Angle getAngleAtTime(double requestedTime) { + // A fun little binary search algo + int high = m_positions.getLength(); + int low = 0; + + // Avoiding overflow (though isn't really necessary here) + int midpoint = low + (high - low) / 2; + + while (low < high) { + double timeAtMidpoint; + try { + timeAtMidpoint = m_positions.get(midpoint).timestamp; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } + + if (requestedTime > timeAtMidpoint) { + low = midpoint + 1; + } else if (requestedTime < timeAtMidpoint) { + high = midpoint; + } + + midpoint = low + (high - low) / 2; + } + + // Linearly 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; + } catch (Exception e) { + return null; + } + + TimePosition nextTimeStamp; + + // Stampted Time 1 should always be less than stampted time 2 + TimePosition stamptedTime1; + TimePosition stamptedTime2; + + if (dt > 0) { + try { + stamptedTime2 = m_positions.get(midpoint + 1); + stamptedTime1 = closestTimestamp; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } + } else { + try { + stamptedTime1 = m_positions.get(midpoint - 1); + stamptedTime2 = closestTimestamp; + } 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 deltaTheta = stamptedTime2.angle.minus(stamptedTime2.angle); + + return deltaTheta.times(timeSinceStamp1 / deltaTimeStampted).plus(stamptedTime1.angle); + } + + try { + return m_positions.get(midpoint).angle; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } + } +} diff --git a/src/main/java/frc/robot/utils/RingBuffer.java b/src/main/java/frc/robot/utils/RingBuffer.java new file mode 100644 index 0000000..f15f4ac --- /dev/null +++ b/src/main/java/frc/robot/utils/RingBuffer.java @@ -0,0 +1,78 @@ +package frc.robot.utils; + +public class RingBuffer { + private int m_tail; + private int m_head; + private int m_length; + + private Object[] m_elements; + + public RingBuffer(int maxLength) { + + if (maxLength < 1) { + System.out.println("Cannot create ring buffer with a max length of < 1."); + return; + } + + m_elements = new Object[maxLength]; + + m_head = 0; + m_tail = 0; + m_length = maxLength; + } + + public void push(T element) throws Exception { + if (m_head - m_tail == m_length) { + try { + pop(); + } catch (Exception e) { + throw e; + } + } + + synchronized (m_elements) { + m_elements[getTrueIndex(m_head++)] = element; + } + } + + public T pop() throws Exception { + if (m_tail == m_head) { + System.out.println(); + throw new Exception("Cannot pop from empty ring buffer."); + } + + synchronized (m_elements) { + return (T) m_elements[getTrueIndex(m_tail++)]; + } + } + + public T get(int index) throws Exception { + synchronized (m_elements) { + int trueIndex = m_tail + index; + if (trueIndex > m_head) { + System.out.println("Index " + index + " is out of bounds of the ring buffer."); + + throw new Exception("Index " + index + " out of bounds."); + } + + return (T) m_elements[getTrueIndex(trueIndex)]; + } + } + + public int getLength() { + return m_head - m_tail; + } + + @Override + public String toString() { + String str = "{ "; + for (int i = m_tail; i < m_head; i++) { + str += m_elements[getTrueIndex(i)] + " "; + } + return str + "}"; + } + + private int getTrueIndex(int index) { + return index % m_elements.length; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/SubsystemSimulation.java b/src/main/java/frc/robot/utils/SubsystemSimulation.java new file mode 100644 index 0000000..8418112 --- /dev/null +++ b/src/main/java/frc/robot/utils/SubsystemSimulation.java @@ -0,0 +1,29 @@ +package frc.robot.utils; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; + +public class SubsystemSimulation { + private Mechanism2d m_mech; + private MechanismRoot2d m_root; + + // Simulation for elevator + public SubsystemSimulation(String name, Distance width, Distance height, Distance startDistance) { + m_mech = new Mechanism2d(width.in(Meters), height.in(Meters)); + m_root = m_mech.getRoot(name, width.in(Meters) / 2, 0.0); + m_root.append(new MechanismLigament2d(name, startDistance.in(Meters), Math.PI)); + } + + // Simulation for arm + public SubsystemSimulation(String name, Distance width, Distance height, Distance length, Angle startAngle) { + m_mech = new Mechanism2d(width.in(Meters), height.in(Meters)); + m_root = m_mech.getRoot(name, width.in(Meters) / 2, 0.0); + m_root.append(new MechanismLigament2d(name, length.in(Meters), startAngle.in(Radians))); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java new file mode 100644 index 0000000..7ae17a3 --- /dev/null +++ b/src/main/java/frc/robot/utils/Vision.java @@ -0,0 +1,156 @@ +package frc.robot.utils; + +import java.util.List; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Function; +import java.util.function.Supplier; + +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.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 { + + PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); + PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); + + Optional> m_turretAngleSupplier; + + PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(VisionConstants.kTagLayout, + VisionConstants.kRobotToCamOne); + PhotonPoseEstimator m_poseEstimatorTwo = new PhotonPoseEstimator(VisionConstants.kTagLayout, + VisionConstants.kRobotToCamTwo); + + Consumer m_visionConsumer; + private Matrix curStdDevs; + + public Vision(Optional> turretAngleSupplier, Consumer visionConsumer) { + m_turretAngleSupplier = turretAngleSupplier; + m_visionConsumer = visionConsumer; + } + + public void periodic() { + Optional visionEstimationCameraOne = Optional.empty(); + + for (var result : m_camera1.getAllUnreadResults()) { + visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + + if (visionEstimationCameraOne.isEmpty()) { + visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + } + + updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), m_poseEstimatorOne); + + visionEstimationCameraOne.ifPresent(estimation -> { + m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), + estimation.timestampSeconds, getCurrentStdDevs())); + }); + } + + Optional visionEstimationCameraTwo = Optional.empty(); + for (var result : m_camera2.getAllUnreadResults()) { + m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform(result.getTimestampSeconds())); + visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); + + if (visionEstimationCameraTwo.isEmpty()) { + visionEstimationCameraTwo = m_poseEstimatorTwo.estimateLowestAmbiguityPose(result); + } + + updateEstimationStdDevs(visionEstimationCameraTwo, result.getTargets(), m_poseEstimatorTwo); + + visionEstimationCameraTwo.ifPresent(estimation -> { + m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), + estimation.timestampSeconds, getCurrentStdDevs())); + }); + } + } + + private void updateEstimationStdDevs( + Optional estimatedPose, List targets, + PhotonPoseEstimator poseEstimator) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + continue; + numTags++; + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + private Transform3d getTurretCameraTransform(double estimationTime) { + if (m_turretAngleSupplier.isEmpty()) + return VisionConstants.kRobotToCamTwo; + + Angle turretAngle = m_turretAngleSupplier.get().apply(estimationTime); + + 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))); + + Distance cameraX = VisionConstants.kTurretAxisOfRotation.getMeasureX().plus(cameraXOffset); + Distance cameraY = VisionConstants.kTurretAxisOfRotation.getMeasureY().plus(cameraYOffset); + Distance cameraZ = VisionConstants.kTurretAxisOfRotation.getMeasureZ(); + + return new Transform3d(cameraX, cameraY, cameraZ, + new Rotation3d(VisionConstants.kCameraTwoPitch, VisionConstants.kCameraTwoRoll, turretAngle)); + } + + private Matrix getCurrentStdDevs() { + return curStdDevs; + } +} diff --git a/src/main/java/frc/robot/utils/VisionEstimation.java b/src/main/java/frc/robot/utils/VisionEstimation.java new file mode 100644 index 0000000..695e5e4 --- /dev/null +++ b/src/main/java/frc/robot/utils/VisionEstimation.java @@ -0,0 +1,18 @@ +package frc.robot.utils; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public class VisionEstimation { + public Pose2d m_pose; + public double m_timestamp; + public Matrix m_stdDevs; + + public VisionEstimation(Pose2d pose, double timestamp, Matrix stdDevs) { + m_pose = pose; + m_timestamp = timestamp; + m_stdDevs = stdDevs; + } +}