Merge pull request #149 from frc5024/wpi2021

.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "currentLanguage": "java",
+    "enableCppIntellisense": false,
+    "projectYear": "2021",
+    "teamNumber": 5024
+}

build.gradle

@@ -10,7 +10,7 @@ buildscript {
     dependencies {
       // Update this version number to update GradleRIO
       // Also update the number below
-      classpath "edu.wpi.first:GradleRIO:2020.+"
+      classpath "edu.wpi.first:GradleRIO:2021.+"
     }
 }
 

gradle5k/gradle5k.gradle

@@ -16,7 +16,8 @@ apply plugin: "edu.wpi.first.GradleRIO"
 def versions = [
 
     // Core FRC libraries
-    wpilib:     "2020.3.+" // See: https://github.com/wpilibsuite/allwpilib/releases
+    // Using a "+" here will allow you to use beta versions of libraries
+    wpilib:     "2021.1.1-alpha-1" // See: https://github.com/wpilibsuite/allwpilib/releases
     // ctre       "5.+" // See: http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/wpiapi-java/
     // navx       "3.+" // See: https://www.kauailabs.com/dist/frc/2020/navx_frc.json (Check ["javaDependencies"][0]["version"])
     // rev_color  "1.+" // See: https://github.com/REVrobotics/Color-Sensor-v3/releases

lib5k/src/main/java/io/github/frc5024/lib5k/bases/drivetrain/AbstractDriveTrain.java

@@ -3,18 +3,21 @@
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.simulation.Field2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import io.github.frc5024.common_drive.gearing.Gear;
 import io.github.frc5024.lib5k.bases.drivetrain.commands.PathFollowerCommand;
 import io.github.frc5024.lib5k.bases.drivetrain.commands.TurnToCommand;
 import io.github.frc5024.lib5k.hardware.common.drivebase.IDifferentialDrivebase;
 import io.github.frc5024.lib5k.logging.RobotLogger;
+import io.github.frc5024.lib5k.utils.FRCFieldConstants;
 import io.github.frc5024.lib5k.utils.interfaces.SafeSystem;
 import io.github.frc5024.libkontrol.statemachines.StateMachine;
 import io.github.frc5024.libkontrol.statemachines.StateMetadata;
 import io.github.frc5024.purepursuit.pathgen.Path;
 
-public abstract class AbstractDriveTrain extends SubsystemBase implements IDifferentialDrivebase, SafeSystem, AutoCloseable {
+public abstract class AbstractDriveTrain extends SubsystemBase
+        implements IDifferentialDrivebase, SafeSystem, AutoCloseable {
 
     // Logging
     protected RobotLogger logger = RobotLogger.getInstance();
@@ -35,6 +38,9 @@ protected enum State {
     private Translation2d goalPoseEpsilon = null;
     private boolean waitingForNextLoop = false;
 
+    // Simulation
+    private Field2d simField = new Field2d();
+
     /**
      * Create an AbstractDriveTrain
      */
@@ -205,6 +211,9 @@ public void periodic() {
         // Run state machine
         stateMachine.update();
         waitingForNextLoop = false;
+
+        // Save the field location
+        simField.setRobotPose(getPose().plus(FRCFieldConstants.LIB5K_TO_WPILIB_COORDINATE_TRANSFORM));
     }
 
     /**
@@ -271,7 +280,7 @@ public void reset() {
 
     @Override
     public void close() throws Exception {
-        
+
     }
 
 }

lib5k/src/main/java/io/github/frc5024/lib5k/utils/FRCFieldConstants.java

@@ -0,0 +1,23 @@
+package io.github.frc5024.lib5k.utils;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.util.Units;
+
+/**
+ * Standard FRC field measurements. Anything year-specific should be added as
+ * needed
+ */
+public class FRCFieldConstants {
+
+    public static final Translation2d FIELD_SIZE = new Translation2d(Units.inchesToMeters(629.25),
+            Units.inchesToMeters(323.25));
+    public static final Translation2d HALF_FIELD_SIZE = FIELD_SIZE.div(2);
+    public static final Translation2d HALF_FIELD_WIDTH = new Translation2d(0, HALF_FIELD_SIZE.getY());
+    public static final Translation2d HALF_FIELD_LENGTH = new Translation2d(HALF_FIELD_SIZE.getX(), 0);
+
+    // Transformation that will convert a Lib5K coordinate to a WPILib coordinate
+    public static final Transform2d LIB5K_TO_WPILIB_COORDINATE_TRANSFORM = new Transform2d(HALF_FIELD_WIDTH, new Rotation2d());
+
+}