Incorporation of CSV-Read Points and PathWeaver Trajectory Planning

Paths/s-curve.path

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+1.0,-5.21055,1.0,0.0,false,false,
+2.0,-4.21055,1.0,0.0,true,false,
+3.0,-6.21055,1.0,0.0,true,false,
+4.0,-5.21055,1.0,0.0,true,false,

Paths/test-part-1.path

@@ -0,0 +1,4 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+0.0,-8.21055,1.0,0.0,true,false,
+1.0,-8.21055,1.0,0.0,false,false,
+2.0,-8.21055,1.0,0.0,true,false,

Paths/test-part-2.path

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

pathweaver.json

@@ -0,0 +1,9 @@
+{
+  "lengthUnit": "Meter",
+  "exportUnit": "Always Meters",
+  "maxVelocity": 2.5,
+  "maxAcceleration": 3.0,
+  "trackWidth": 0.6096,
+  "gameName": "Infinite Recharge 2021",
+  "outputDir": ""
+}

src/main/deploy/paths/s-curve.path

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+1.0,-5.21055,1.0,0.0,false,false,
+2.0,-4.21055,1.0,0.0,true,false,
+3.0,-6.21055,1.0,0.0,true,false,
+4.0,-5.21055,1.0,0.0,true,false,

src/main/deploy/paths/test-part-1.path

@@ -0,0 +1,4 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+0.0,-8.21055,1.0,0.0,true,false,
+1.0,-8.21055,1.0,0.0,false,false,
+2.0,-8.21055,1.0,0.0,true,false,

src/main/deploy/paths/test-part-2.path

@@ -0,0 +1 @@
+

src/main/java/com/team1678/frc2021/Robot.java

@@ -113,14 +113,14 @@ public void robotInit() {
                 mSwerve,
                 mRobotStateEstimator,
 				mCanifier,
-				mHood,
+				// mHood,
                 mLimelight, 
                 mIntake, 
                 mIndexer, 
                 mShooter,
                 mTrigger,
                 mSuperstructure,
-                mTurret,
+                // mTurret,
                 mInfrastructure,
                 mSkywalker,
                 mClimber,

src/main/java/com/team1678/frc2021/auto/AutoModeSelector.java

@@ -11,6 +11,7 @@ public class AutoModeSelector {
     enum DesiredMode {
         DO_NOTHING, 
         TEST_PATH_AUTO,
+        PATH_WEAVER_TEST_AUTO,
         FIGURE_EIGHT_AUTO,
         // RIGHT_TEN_BALL_AUTO,
         // RIGHT_FIVE_NEAR_BALL_AUTO,
@@ -35,10 +36,11 @@ enum DesiredMode {
     public AutoModeSelector() {
         mModeChooser = new SendableChooser<>();
         mModeChooser.setDefaultOption("Do Nothing", DesiredMode.DO_NOTHING);
-        mModeChooser.addOption("Test Path", DesiredMode.TEST_PATH_AUTO);
-        mModeChooser.addOption("Figure Eight Path", DesiredMode.FIGURE_EIGHT_AUTO);
-        mModeChooser.addOption("Shot Center Forward", DesiredMode.SHOT_CENTER_FORWARD_AUTO);
-        SmartDashboard.putData("Auto mode", mModeChooser);
+        mModeChooser.addOption("Test Path Mode", DesiredMode.TEST_PATH_AUTO);
+        mModeChooser.addOption("Path Weaver Test Mode", DesiredMode.PATH_WEAVER_TEST_AUTO);
+        mModeChooser.addOption("Figure Eight Path Mode", DesiredMode.FIGURE_EIGHT_AUTO);
+        mModeChooser.addOption("Shot Center Forward Mode", DesiredMode.SHOT_CENTER_FORWARD_AUTO);
+        SmartDashboard.putData("Auto Mode", mModeChooser);
     }
 
     public void updateModeCreator() {
@@ -61,6 +63,9 @@ private Optional<AutoModeBase> getAutoModeForParams(DesiredMode mode) {
         case TEST_PATH_AUTO:
             return Optional.of(new TestPathMode());
 
+        case PATH_WEAVER_TEST_AUTO:
+            return Optional.of(new PathWeaverTestMode());
+
         case FIGURE_EIGHT_AUTO:
             return Optional.of(new FigureEightTestMode());
 

src/main/java/com/team1678/frc2021/auto/WaypointReader.java

@@ -0,0 +1,54 @@
+package com.team1678.frc2021.auto;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.spline.Spline;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+
+import java.io.BufferedReader;
+import java.io.FileReader;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.Arrays;
+
+/* FROM FRC 4277 */
+
+public class WaypointReader {
+    private static final double PATHWEAVER_Y_OFFSET = 8.21055; // See https://www.desmos.com/calculator/0lqfdhxrmj
+
+    /**
+     * Get control vector list from path file
+     * @param pathName Specify the {THIS} in src/main/deploy/waypoints/{THIS}.path
+     * @return control vectors from file
+     */
+    public static TrajectoryGenerator.ControlVectorList getControlVectors(Path path) throws IOException {
+
+        TrajectoryGenerator.ControlVectorList controlVectors = new TrajectoryGenerator.ControlVectorList();
+
+        try (BufferedReader reader = new BufferedReader(new FileReader(path.toFile()))){
+            boolean skippedFirst = false;
+            String line = reader.readLine();
+            while (line != null) {
+                if (!skippedFirst || !line.contains(",")) {
+                    skippedFirst = true;
+                    line = reader.readLine();
+                    continue;
+                }
+                String[] split = line.split(",");
+                controlVectors.add(new Spline.ControlVector(
+                        new double[]{Double.parseDouble(split[0]), Double.parseDouble(split[2]), 0},
+                        new double[]{Double.parseDouble(split[1]) + PATHWEAVER_Y_OFFSET, Double.parseDouble(split[3]), 0}));
+
+                line = reader.readLine();
+            }
+        }
+
+        // Debug print out of arrays in vectors
+        for (Spline.ControlVector vector : controlVectors) {
+            System.out.println(Arrays.toString(vector.x));
+            System.out.println(Arrays.toString(vector.y));
+            System.out.println("===");
+        }
+
+        return controlVectors;
+    }
+}

src/main/java/com/team1678/frc2021/auto/modes/PathWeaverTestMode.java

@@ -0,0 +1,66 @@
+package com.team1678.frc2021.auto.modes;
+
+import com.team1678.frc2021.Constants;
+import com.team1678.frc2021.subsystems.Swerve;
+import com.team1678.frc2021.auto.AutoModeEndedException;
+import com.team1678.frc2021.auto.WaypointReader;
+import com.team1678.frc2021.auto.actions.LambdaAction;
+import com.team1678.frc2021.auto.actions.SwerveTrajectoryAction;
+
+import java.io.IOException;
+import java.nio.file.Path;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
+
+public class PathWeaverTestMode extends AutoModeBase {
+    // Swerve instance 
+    private final Swerve s_Swerve = Swerve.getInstance();
+
+    // required PathWeaver trajectory paths
+    String path = "paths/s-curve.path";
+
+    // trajectories
+    Trajectory samplePathWeaverTrajectory;
+    SwerveTrajectoryAction samplePathWeaverTrajectoryAction;
+
+    public PathWeaverTestMode() {
+
+        try {
+            Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(path);
+            TrajectoryGenerator.ControlVectorList control_vectors = WaypointReader.getControlVectors(trajectoryPath);
+            samplePathWeaverTrajectory = TrajectoryGenerator.generateTrajectory(control_vectors, Constants.AutoConstants.defaultConfig);
+        } catch (IOException ex) {
+            DriverStation.reportError("Unable to open trajectory: " + path, ex.getStackTrace());
+        }
+
+        var thetaController = new ProfiledPIDController(Constants.AutoConstants.kPThetaController, 0, 0,
+                Constants.AutoConstants.kThetaControllerConstraints);
+        thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+        samplePathWeaverTrajectoryAction = new SwerveTrajectoryAction(samplePathWeaverTrajectory,
+            s_Swerve::getPose, Constants.SwerveConstants.swerveKinematics,
+            new PIDController(Constants.AutoConstants.kPXController, 0, 0),
+            new PIDController(Constants.AutoConstants.kPYController, 0, 0), thetaController,
+            s_Swerve::setModuleStates);
+
+    }
+
+    @Override
+    protected void routine() throws AutoModeEndedException {
+        System.out.println("Running test mode auto!");
+
+        // reset odometry at the start of the trajectory
+        runAction(new LambdaAction(() -> s_Swerve.resetOdometry(samplePathWeaverTrajectoryAction.getInitialPose())));
+
+        runAction(samplePathWeaverTrajectoryAction);
+
+        System.out.println("Finished auto!");
+    }
+}

src/main/java/com/team1678/frc2021/auto/modes/TestPathMode.java

@@ -4,15 +4,20 @@
 import com.team1678.frc2021.subsystems.Intake;
 import com.team1678.frc2021.subsystems.Swerve;
 import com.team1678.frc2021.auto.AutoModeEndedException;
+import com.team1678.frc2021.auto.WaypointReader;
 import com.team1678.frc2021.auto.actions.LambdaAction;
 import com.team1678.frc2021.auto.actions.ParallelAction;
 import com.team1678.frc2021.auto.actions.SeriesAction;
 import com.team1678.frc2021.auto.actions.SwerveTrajectoryAction;
 import com.team1678.frc2021.auto.actions.WaitAction;
 
+import java.io.IOException;
+import java.nio.file.Path;
 import java.util.Arrays;
 import java.util.List;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -27,15 +32,37 @@ public class TestPathMode extends AutoModeBase {
     // Swerve instance 
     private final Swerve s_Swerve = Swerve.getInstance();
 
-    // trajectory actions
-    SwerveTrajectoryAction straightTrajectoryAction;
+    // required PathWeaver trajectory paths
+    String path_one = "paths/test-part-1.path";
+    String path_two = "paths/test-part-2.path";
+
+	// trajectories
+	Trajectory straightTrajectory;
+	SwerveTrajectoryAction straightTrajectoryAction;
+
+	Trajectory rotatingTrajectory;
     SwerveTrajectoryAction rotatingTrajectoryAction;
 
     public TestPathMode() {
         TrajectoryConfig config = new TrajectoryConfig(Constants.AutoConstants.kMaxSpeedMetersPerSecond,
                 Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
                         .setKinematics(Constants.SwerveConstants.swerveKinematics);
 
+        try {
+
+          Path traj_path_one = Filesystem.getDeployDirectory().toPath().resolve(path_one);
+          TrajectoryGenerator.ControlVectorList cv_one = WaypointReader.getControlVectors(traj_path_one);
+		  straightTrajectory = TrajectoryGenerator.generateTrajectory(cv_one, Constants.AutoConstants.zeroToSlow);
+
+		  Path traj_path_two = Filesystem.getDeployDirectory().toPath().resolve(path_two);
+          TrajectoryGenerator.ControlVectorList cv_two = WaypointReader.getControlVectors(traj_path_two);
+          rotatingTrajectory = TrajectoryGenerator.generateTrajectory(cv_two, Constants.AutoConstants.slowToZero);
+
+        } catch (IOException ex) {
+		  DriverStation.reportError("Unable to open trajectory: " + path_one, ex.getStackTrace());
+		  DriverStation.reportError("Unable to open trajectory: " + path_two, ex.getStackTrace());
+        }
+        /*
         Trajectory straightTrajectory = TrajectoryGenerator.generateTrajectory(
                 new Pose2d(0, 0, new Rotation2d(0)),
                 List.of(new Translation2d(1.5, 0)),
@@ -45,7 +72,7 @@ public TestPathMode() {
                 new Pose2d(3, 0, new Rotation2d(0)),
                 List.of(new Translation2d(4.5, 0)),
                 new Pose2d(6, 0, new Rotation2d(0)), Constants.AutoConstants.slowToZero);
-
+        */
 
         var thetaController = new ProfiledPIDController(Constants.AutoConstants.kPThetaController, 0, 0,
                 Constants.AutoConstants.kThetaControllerConstraints);
@@ -99,4 +126,4 @@ protected void routine() throws AutoModeEndedException {
 
         System.out.println("Finished auto!");
     }
-}
+}