April tag align

build.gradle

@@ -68,7 +68,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+    //testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 }
 
 test {

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,18 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/java/frc/robot/Constants.java

@@ -0,0 +1,161 @@
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide
+ * numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants
+ * should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+
+  /**
+   * Input/Output constants
+   */
+  public static class IOConstants {
+    public static final int kDriverControllerPort = 0;
+
+    public static final double kControllerDeadband = 0.2;
+    public static final double kSlowModeScalar = 0.8;
+  }
+
+  public static final class DriveConstants {
+    // TODO: set motor and encoder constants
+    public static final int kFrontLeftDriveMotorPort = 1;
+    public static final int kRearLeftDriveMotorPort = 3;
+    public static final int kFrontRightDriveMotorPort = 5;
+    public static final int kRearRightDriveMotorPort = 7;
+
+    public static final int kFrontLeftTurningMotorPort = 2;
+    public static final int kRearLeftTurningMotorPort = 4;
+    public static final int kFrontRightTurningMotorPort = 6;
+    public static final int kRearRightTurningMotorPort = 8;
+
+    public static final int kFrontLeftTurningEncoderPort = 9;
+    public static final int kRearLeftTurningEncoderPort = 10;
+    public static final int kFrontRightTurningEncoderPort = 11;
+    public static final int kRearRightTurningEncoderPort = 12;
+
+    public static final double kFrontLeftTurningEncoderOffset = 0;
+    public static final double kRearLeftTurningEncoderOffset = 0;
+    public static final double kFrontRightTurningEncoderOffset = 0;
+    public static final double kRearRightTurningEncoderOffset = 0;
+
+    // TODO: Test motor orientations before driving on an actual robot
+    public static final boolean kFrontLeftDriveMotorReversed = false;
+    public static final boolean kRearLeftDriveMotorReversed = false;
+    public static final boolean kFrontRightDriveMotorReversed = true;
+    public static final boolean kRearRightDriveMotorReversed = true;
+
+    /** Distance between centers of right and left wheels on robot (in meters). */
+    public static final double kTrackWidth = 0.5;
+
+    /** Distance between front and back wheels on robot (in meters). */
+    public static final double kWheelBase = 0.5;
+
+    /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
+    public static final double kWheelDiameterMeters = 0.1;
+
+    /** Gear ratio between the motor and the wheel. */
+    public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 configuration
+
+    // TODO: Tune this PID before running on a robot on the ground
+    public static final double kPModuleTurningController = 0.3;
+
+    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+    /** For a a SDS Mk4i L1 swerve base with Neos */
+    public static final double kMaxSpeedMetersPerSecond = 3.6576;
+    /** For a a SDS Mk4i L1 swerve base with Neos */
+    public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3;
+
+    /** Heading Correction */
+    public static final double kHeadingCorrectionTurningStopTime = 0.2;
+    // TODO: Tune this PID before running on a robot on the ground
+    public static final double kPHeadingCorrectionController = 5;
+  }
+
+  public final static class TagAlignConstants {
+		public final static double kTagPIDkPxy = 0; // xy PID constants
+		public final static double kTagPIDkIxy = 0;
+		public final static double kTagPIDkDxy = 0;
+
+		public final static double kTagPIDkPomega = 0; // omega PID constants
+		public final static double kTagPIDkIomega = 0;
+		public final static double kTagPIDkDomega = 0;
+
+		public static final Pose2d[] kTargetPoses = new Pose2d[] {
+				new Pose2d(new Translation2d(585.75, 313), new Rotation2d(Math.toRadians(270))), //Amp //Change y based on length of robot using 28in long, Red side
+
+				//Asumed that these are positions for a single source, so that you have 3 positions for the source on the left
+				//Left , center, right, all on same obj //TODO: Make sure these work with robot demensions/bumpers
+				new Pose2d(new Translation2d(590, 9), new Rotation2d(Math.toRadians(120))), //SourceLeft
+				new Pose2d(new Translation2d(631.125, 20.625), new Rotation2d(Math.toRadians(120))), //etc.
+				new Pose2d(new Translation2d(637, 34), new Rotation2d(Math.toRadians(120))),
+
+				new Pose2d(new Translation2d(598.125, 222.75), new Rotation2d(0)), //TODO: Check this (Red speaker)
+				new Pose2d(new Translation2d(464.5, 117.315), new Rotation2d(Math.toRadians(300))),
+				new Pose2d(new Translation2d(421.115, 161.62), new Rotation2d(Math.toRadians(180))),
+				new Pose2d(new Translation2d(459.75, 197.725), new Rotation2d(Math.toRadians(60))),
+				new Pose2d(new Translation2d(186.855, 195.662), new Rotation2d(Math.toRadians(120))),
+				new Pose2d(new Translation2d(182.245, 161.62), new Rotation2d(0)),
+				new Pose2d(new Translation2d(193.875, 123.75), new Rotation2d(Math.toRadians(240))),
+				new Pose2d(new Translation2d(78.375, 165), new Rotation2d(0)),
+				new Pose2d(new Translation2d(330, 165), new Rotation2d(0)), //Switch angle by 1pi if you want opposite rotation
+				new Pose2d(new Translation2d(585.75, 165), new Rotation2d(0)) //StageCenterRight
+		}; // Poses are in the same order as the enumerator.
+		// TODO: populate poses
+
+		//TODO: tune all these parameters
+		public static final double kTolxy = 0.1; // tolerance in meters for x and y
+		public static final double kTolomega = 0.1; // tolerance in radians for omega
+		public static final double kConsectol = 10; // onsecutive cycles of being within tolerance needed to end command
+		public static final double kMinProx = 1; // minimum proximity radius in meters.
+		// To avoid accidents, the robot must be
+		// within the minimum proximity for the tag alignment command to run.
+		// If the robot exceeds the minimum proximity the command will finish
+
+		public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new items to the end
+			Amp,
+			SourceLeft,
+			SourceCenter,
+			SourceRight,
+			Speaker,
+			StageLeftLeft, // The left side of the left stage
+			StageLeftCenter, // the center side of the left stage
+			StageLeftRight, // etc.
+			StageRightLeft,
+			StageRightCenter,
+			StageRightRight,
+			StageCenterLeft,
+			StageCenterCenter,
+			StageCenterRight // the right side of the center stage
+		} //stage left, stage right, and stage center are the locations written in the game manual pg. 28
+
+		public static final double kFieldWidth = 651.25 * 0.0254; // Width of the field in meters
+
+		public static final NetworkTableInstance kNTInstance = NetworkTableInstance.create(); // Ideally all network table
+																																													// instances should be put in
+																																													// a wrapper and only the
+																																													// default instance should be
+																																													// used.
+		// TODO: write unit tests that write to NT limelight and verify PID output
+		// direction is correct. Needs to be implemented after drive since it uses pose
+		// estimator.
+	}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -4,15 +4,70 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.IOConstants;
+import frc.robot.subsystems.DriveSubsystem;
+
 
 public class RobotContainer {
+  // The robot's subsystems and commands are defined here
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+  private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
+
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
+    // Configure the trigger bindings
     configureBindings();
+
+    m_robotDrive.setDefaultCommand(
+        new RunCommand(
+            () -> m_robotDrive.drive(
+                MathUtil.applyDeadband(
+                    -m_driverController.getLeftY(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxSpeedMetersPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    * 0.8,
+                MathUtil.applyDeadband(
+                    -m_driverController.getLeftX(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxSpeedMetersPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    * 0.8,
+                MathUtil.applyDeadband(
+                    m_driverController.getRightX(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxAngularSpeedRadiansPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    / 2,
+                !m_driverController.getRightBumper()),
+            m_robotDrive));
   }
 
-  private void configureBindings() {}
+  /**
+   * Use this method to define your button->command mappings.
+   */
+  private void configureBindings() {
+    new JoystickButton(m_driverController, Button.kStart.value)
+        .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
+  }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");