Feature/turret vision

.vscode/settings.json

@@ -59,7 +59,6 @@
   ],
   "java.dependency.enableDependencyCheckup": false,
   "cSpell.words": [
-    "Deadband",
     "feedforwards",
     "Holonomic",
     "Photonvision",

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
 

simgui-ds.json

@@ -101,7 +101,8 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

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<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
 
-        public static final AprilTagFieldLayout kTagLayout
-                = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+        public static final Matrix<N3, N1> kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+        public static final Matrix<N3, N1> 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);
+    }
 }

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,19 +62,23 @@ public void teleopInit() {
   }
 
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
-  public void teleopExit() {}
+  public void teleopExit() {
+  }
 
   @Override
   public void testInit() {
     CommandScheduler.getInstance().cancelAll();
   }
 
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+  }
 
   @Override
-  public void testExit() {}
+  public void testExit() {
+  }
 }

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<String> 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);
+    }
 }