Path feature

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2021.1.2"
+    id "edu.wpi.first.GradleRIO" version "2021.2.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+0.0,0.0,10.0,0.0,true,false,
+6.0,0.0,0.0,0.0,true,false,

src/main/PathWeaverProject/PathWeaver/pathweaver.json

@@ -0,0 +1,9 @@
+{
+  "lengthUnit": "Meter",
+  "exportUnit": "Same as Project",
+  "maxVelocity": 3.6576,
+  "maxAcceleration": 3.048,
+  "wheelBase": 0.6604,
+  "gameName": "Infinite Recharge",
+  "outputDir": "../../deploy"
+}

src/main/deploy/output/SixForward.wpilib.json

@@ -0,0 +1 @@
+[{"time":0.0,"velocity":0.0,"acceleration":9.999999999999998,"pose":{"translation":{"x":0.0,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.12499997033737249,"velocity":1.2499997033737247,"acceleration":10.000000000000002,"pose":{"translation":{"x":0.07812496292172,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1767763612825469,"velocity":1.7677636128254688,"acceleration":10.0,"pose":{"translation":{"x":0.1562494095414877,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.21650497682420286,"velocity":2.1650497682420284,"acceleration":10.0,"pose":{"translation":{"x":0.23437202494824305,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24999625680028503,"velocity":2.49996256800285,"acceleration":10.000000000000005,"pose":{"translation":{"x":0.31249064207077026,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.27950036230118047,"velocity":2.7950036230118047,"acceleration":9.999999999999995,"pose":{"translation":{"x":0.3906022626324557,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.30617089283798543,"velocity":3.061708928379854,"acceleration":10.0,"pose":{"translation":{"x":0.4687030781060457,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3306927548853784,"velocity":3.306927548853784,"acceleration":10.0,"pose":{"translation":{"x":0.5467884906684048,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3535118482187757,"velocity":3.5351184821877566,"acceleration":10.00000000000001,"pose":{"translation":{"x":0.6248531341552734,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3749375668070688,"velocity":3.749375668070688,"acceleration":10.0,"pose":{"translation":{"x":0.7028908950160258,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.39519487174517537,"velocity":3.9519487174517534,"acceleration":9.99999999999999,"pose":{"translation":{"x":0.7808949332684278,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4144533033897536,"velocity":4.1445330338975355,"acceleration":10.0,"pose":{"translation":{"x":0.8588577034533955,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43284430817321656,"velocity":4.328443081732165,"acceleration":10.000000000000023,"pose":{"translation":{"x":0.9367709755897522,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45047216476248275,"velocity":4.5047216476248275,"acceleration":10.000000000000023,"pose":{"translation":{"x":1.014625856128987,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.46742118242758585,"velocity":4.674211824275859,"acceleration":10.0,"pose":{"translation":{"x":1.0924128089100122,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.48376061768480544,"velocity":4.8376061768480545,"acceleration":10.0,"pose":{"translation":{"x":1.1701216761139221,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.49954813566237044,"velocity":4.995481356623705,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.24774169921875,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5148323105544613,"velocity":5.1483231055446135,"acceleration":10.0,"pose":{"translation":{"x":1.3252615399542265,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5296544725113793,"velocity":5.296544725113794,"acceleration":10.0,"pose":{"translation":{"x":1.4026693012565374,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5440500984694483,"velocity":5.440500984694483,"acceleration":10.0,"pose":{"translation":{"x":1.479952548223082,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5580498775319693,"velocity":5.580498775319694,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.5570983290672302,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5716805394751655,"velocity":5.716805394751655,"acceleration":10.0,"pose":{"translation":{"x":1.6340931960730813,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5849655077951557,"velocity":5.849655077951557,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.7109232265502214,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5979254207321313,"velocity":5.979254207321314,"acceleration":10.0,"pose":{"translation":{"x":1.7875740437884815,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6105785515415186,"velocity":6.105785515415188,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.8640308380126953,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6229411508862546,"velocity":6.2294115088625475,"acceleration":10.0,"pose":{"translation":{"x":1.9402783873374574,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6350277283271779,"velocity":6.350277283271781,"acceleration":10.0,"pose":{"translation":{"x":2.016301078721881,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6468512856792286,"velocity":6.468512856792288,"acceleration":10.000000000000046,"pose":{"translation":{"x":2.0920829289243557,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.658423511952194,"velocity":6.584235119521943,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.167607605457306,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6697549473564114,"velocity":6.697549473564117,"acceleration":10.0,"pose":{"translation":{"x":2.2428584475419484,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6808551221901835,"velocity":6.808551221901839,"acceleration":10.0,"pose":{"translation":{"x":2.3178184870630503,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6917326751749819,"velocity":6.9173267517498225,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.392470469523687,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7023954548543148,"velocity":7.02395454854315,"acceleration":10.0,"pose":{"translation":{"x":2.466796875,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7128506069431314,"velocity":7.1285060694313165,"acceleration":10.0,"pose":{"translation":{"x":2.5407799390959553,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7231046499502128,"velocity":7.231046499502131,"acceleration":10.000000000000048,"pose":{"translation":{"x":2.614401673898101,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7331635409552664,"velocity":7.331635409552667,"acceleration":10.0,"pose":{"translation":{"x":2.6876438889303245,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7430327330755504,"velocity":7.430327330755507,"acceleration":9.99999999999995,"pose":{"translation":{"x":2.760488212108612,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7527172258817788,"velocity":7.5271722588177905,"acceleration":10.00000000000005,"pose":{"translation":{"x":2.8329161106958054,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7622216098033903,"velocity":7.622216098033905,"acceleration":10.0,"pose":{"translation":{"x":2.90490891225636,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7715501053866951,"velocity":7.715501053866954,"acceleration":-3.3717588560186713,"pose":{"translation":{"x":2.9764478256111033,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7807795440416799,"velocity":7.684381612345928,"acceleration":-10.0,"pose":{"translation":{"x":3.047513961791992,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7900192272742247,"velocity":7.59198478002048,"acceleration":-10.0,"pose":{"translation":{"x":3.118088354996871,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7993046414541025,"velocity":7.499130638221702,"acceleration":-10.0,"pose":{"translation":{"x":3.1881519835442305,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8086349335117604,"velocity":7.405827717645123,"acceleration":-10.0,"pose":{"translation":{"x":3.2576857908279635,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8180092132949669,"velocity":7.312084919813058,"acceleration":-10.0,"pose":{"translation":{"x":3.3266707062721252,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8274265535274705,"velocity":7.217911517488022,"acceleration":-10.000000000000052,"pose":{"translation":{"x":3.39508766628569,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.836885989741622,"velocity":7.123317155346507,"acceleration":-10.0,"pose":{"translation":{"x":3.462917635217309,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8463865201841579,"velocity":7.028311850921148,"acceleration":-9.999999999999947,"pose":{"translation":{"x":3.530141626310069,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8559271056942862,"velocity":6.9329059958198656,"acceleration":-10.0,"pose":{"translation":{"x":3.59674072265625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8655066695531509,"velocity":6.837110357231218,"acceleration":-10.0,"pose":{"translation":{"x":3.6626960981520824,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8751240973036861,"velocity":6.7409360797258655,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.727989038452506,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8847782365397934,"velocity":6.644394687364793,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.7926009619259275,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944678966636967,"velocity":6.547498086125758,"acceleration":-10.0,"pose":{"translation":{"x":3.8565134406089783,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9139488245368117,"velocity":6.352688807394609,"acceleration":-10.0,"pose":{"translation":{"x":3.9821672458201647,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9335565809593984,"velocity":6.156611243168741,"acceleration":-10.0,"pose":{"translation":{"x":4.104806900024414,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9532802335489408,"velocity":5.959374717273318,"acceleration":-10.00000000000003,"pose":{"translation":{"x":4.22429264895618,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9731082033060778,"velocity":5.761095019701946,"acceleration":-10.000000000000048,"pose":{"translation":{"x":4.340489208698273,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9930282476692652,"velocity":5.561894576070071,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.4532664362341166,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.013027440538092,"velocity":5.361902647381805,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.5625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0330921489311773,"velocity":5.16125556345095,"acceleration":-10.000000000000018,"pose":{"translation":{"x":4.668072050437331,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.053208005884192,"velocity":4.960096993920802,"acceleration":-9.999999999999982,"pose":{"translation":{"x":4.769871890544891,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0733598791204104,"velocity":4.758578261558617,"acceleration":-10.0,"pose":{"translation":{"x":4.8677966464310884,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0935318349371121,"velocity":4.556858703391601,"acceleration":-10.0,"pose":{"translation":{"x":4.961751937866211,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1137070966419202,"velocity":4.35510608634352,"acceleration":-10.000000000000021,"pose":{"translation":{"x":5.0516525488346815,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.133867996738349,"velocity":4.153497085379233,"acceleration":-9.999999999999979,"pose":{"translation":{"x":5.137423098087311,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1539959218921958,"velocity":3.952217833840764,"acceleration":-10.0,"pose":{"translation":{"x":5.218998709693551,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1740712495003311,"velocity":3.7514645577594097,"acceleration":-10.000000000000012,"pose":{"translation":{"x":5.29632568359375,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.194073274417897,"velocity":3.5514443085837506,"acceleration":-10.0,"pose":{"translation":{"x":5.369362166151404,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.213980124061222,"velocity":3.3523758121505,"acceleration":-10.000000000000007,"pose":{"translation":{"x":5.438078820705414,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2534143609235189,"velocity":2.958033443527531,"acceleration":-10.000000000000004,"pose":{"translation":{"x":5.562501907348633,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2921714334605936,"velocity":2.5704627181567834,"acceleration":-9.999999999999995,"pose":{"translation":{"x":5.669636070728302,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3300217313168947,"velocity":2.1919597395937727,"acceleration":-10.0,"pose":{"translation":{"x":5.759765625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3667016944400214,"velocity":1.8251601083625066,"acceleration":-10.0,"pose":{"translation":{"x":5.833439528942108,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.401903648667902,"velocity":1.4731405660837007,"acceleration":-10.0,"pose":{"translation":{"x":5.89149284362793,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4663168090582577,"velocity":0.8290089621801443,"acceleration":-9.999999999999998,"pose":{"translation":{"x":5.96563720703125,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5492177052762721,"velocity":0.0,"acceleration":-9.999999999999998,"pose":{"translation":{"x":6.0,"y":8.21055},"rotation":{"radians":0.0}},"curvature":"NaN"}]

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

@@ -77,6 +77,14 @@ public final class Constants {
 
     public static final double kHopperSpinPercentage = 1.0; //Todo: set limit
 
+    public static final int kNeoEncoderResolution = 42; // Neo encoder has 42 counts per revolution
+    public static final double kDriveBaseWheelDiameter = 6.0; // in inches
+    public static final double kGearboxRatio = 9.7; // 9.7 motor rotations : 1 wheel rotation Reduction; slows the rotation down
+    public static final double kInchesToMeters = 0.0254;
+    public static final double kPositionConversionFactor = kDriveBaseWheelDiameter * Math.PI / kGearboxRatio * kInchesToMeters; // in meters
+
+    public static final double kWheelBaseTrackWidthInches = 26;
+    public static final double kWheelBaseTrackWidthMeters = kWheelBaseTrackWidthInches * kInchesToMeters;
     public static final int kHopperCurrentLimit = 40; //Todo: may change
     public static final int kIntakeDoubleSolenoidForwardChannel = 0;
     public static final int kIntakeDoubleSolenoidReverseChannel = 2;

src/main/java/frc/robot/MotorController.java

@@ -37,20 +37,24 @@ public MotorController(String name, int deviceID) {
         mSparkMax.restoreFactoryDefaults();
     }
 
-    //Todo: we must add constants for PID values once we have found the right values
-    public MotorController(String name, int deviceID, int smartCurrentLimit, boolean... enablePid) {
-        this(name, deviceID); //calls the constructor for correct arguements
+    public MotorController(String name, int deviceID, int smartCurrentLimit) {
+        this(name, deviceID);
         //Current limiting is required to prevent brown outs
         mSparkMax.setSmartCurrentLimit(smartCurrentLimit);
-        //If enablePid has any number of booleans greater than 0 we are enabling pid
-        if (enablePid.length > 0)
-        {
-            mP = SmartDashboard.getNumber(mName + " P Value", 1.0);
-            mI = SmartDashboard.getNumber(mName + " I Value", 0.0);
-            mD = SmartDashboard.getNumber(mName + " D Value", 0.0);
-            mPIDController = mSparkMax.getPIDController();
-            setPID();
-        }
+    }
+
+    public MotorController(String name, int deviceID, int smartCurrentLimit, boolean enablePid) {
+        this(name, deviceID, smartCurrentLimit, SmartDashboard.getNumber(name + " P Value", 1.0), 
+            SmartDashboard.getNumber(name + " I Value", 0.0), SmartDashboard.getNumber(name + " D Value", 0.0)); //calls the constructor for correct arguments
+    }
+
+    public MotorController(String name, int deviceID, int smartCurrentLimit, double P, double I, double D) {
+        this(name, deviceID, smartCurrentLimit); //calls the constructor for correct arguements
+        mP = P;
+        mI = I;
+        mD = D;
+        mPIDController = mSparkMax.getPIDController();
+        setPID();
         mSparkMax.setOpenLoopRampRate(.1);
     }
 

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

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.commands.ShootCommandGroup;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.utilities.path;
 import frc.robot.commands.DriveBaseTeleopCommand;
 import frc.robot.commands.IntakeSpinMotorBackwardCommand;
 import frc.robot.commands.IntakeSpinMotorForwardCommand;
@@ -51,6 +52,9 @@ public class RobotContainer {
   private final ShootCommandGroup mShootCommandGroup = new ShootCommandGroup(mShooterSubsystem, mDriveBaseSubsystem, mHopperSubsystem);
   private final StartEndCommand mIntakeExtendCommand = new StartEndCommand(mIntakeSubsystem::setIntakeExtended, mIntakeSubsystem::setIntakeRetracted);
 
+  //Paths
+  private final path mSixFeetForward = new path("output/SixForward.wpilib.json", mDriveBaseSubsystem);
+
   public RobotContainer() {
     // Configure the button bindings
     for (int i = 1; i < mButtons.length; i++) {
@@ -76,6 +80,7 @@ private void configureButtonBindings() {
     mButtons[Constants.kAButton].whenPressed(mSwitchDriveModeCommand);
     //The buttons below are just for testing functionality
     mButtons[Constants.kRightTriggerButton].whileHeld(mCycleHopperCommand);
+    mButtons[Constants.kLeftTriggerButton].whileHeld(mSixFeetForward.getPathCommand());
   }