From 5f454ade738b449b37c0ea4563aab90aeaaeac39 Mon Sep 17 00:00:00 2001 From: apurvagyan Date: Thu, 6 Jan 2022 20:53:07 -0800 Subject: [PATCH 1/2] implement PathWeaver trajectories in autos by reading points from outputted csv --- Paths/s-curve.path | 5 ++ Paths/test-part-1.path | 4 ++ Paths/test-part-2.path | 3 + pathweaver.json | 9 +++ src/main/deploy/paths/s-curve.path | 5 ++ src/main/deploy/paths/test-part-1.path | 4 ++ src/main/deploy/paths/test-part-2.path | 4 ++ src/main/java/com/team1678/frc2021/Robot.java | 4 +- .../frc2021/auto/AutoModeSelector.java | 13 ++-- .../team1678/frc2021/auto/WaypointReader.java | 54 +++++++++++++++ .../auto/modes/PathWeaverTestMode.java | 66 +++++++++++++++++++ .../frc2021/auto/modes/TestPathMode.java | 35 ++++++++-- 12 files changed, 196 insertions(+), 10 deletions(-) create mode 100644 Paths/s-curve.path create mode 100644 Paths/test-part-1.path create mode 100644 Paths/test-part-2.path create mode 100644 pathweaver.json create mode 100644 src/main/deploy/paths/s-curve.path create mode 100644 src/main/deploy/paths/test-part-1.path create mode 100644 src/main/deploy/paths/test-part-2.path create mode 100644 src/main/java/com/team1678/frc2021/auto/WaypointReader.java create mode 100644 src/main/java/com/team1678/frc2021/auto/modes/PathWeaverTestMode.java diff --git a/Paths/s-curve.path b/Paths/s-curve.path new file mode 100644 index 0000000..0376dd7 --- /dev/null +++ b/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, diff --git a/Paths/test-part-1.path b/Paths/test-part-1.path new file mode 100644 index 0000000..5dee0f8 --- /dev/null +++ b/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, diff --git a/Paths/test-part-2.path b/Paths/test-part-2.path new file mode 100644 index 0000000..cbf8afe --- /dev/null +++ b/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, diff --git a/pathweaver.json b/pathweaver.json new file mode 100644 index 0000000..bdaa8f4 --- /dev/null +++ b/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": "" +} \ No newline at end of file diff --git a/src/main/deploy/paths/s-curve.path b/src/main/deploy/paths/s-curve.path new file mode 100644 index 0000000..0376dd7 --- /dev/null +++ b/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, diff --git a/src/main/deploy/paths/test-part-1.path b/src/main/deploy/paths/test-part-1.path new file mode 100644 index 0000000..5dee0f8 --- /dev/null +++ b/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, diff --git a/src/main/deploy/paths/test-part-2.path b/src/main/deploy/paths/test-part-2.path new file mode 100644 index 0000000..ac58c8b --- /dev/null +++ b/src/main/deploy/paths/test-part-2.path @@ -0,0 +1,4 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +2.0,-8.21055,1.0,0.0,true,false, +3.0,-8.21055,1.0,0.0,true,false, +4.0,-8.21055,1.0,0.0,true,false, diff --git a/src/main/java/com/team1678/frc2021/Robot.java b/src/main/java/com/team1678/frc2021/Robot.java index 42cf8fd..ce44c64 100644 --- a/src/main/java/com/team1678/frc2021/Robot.java +++ b/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, diff --git a/src/main/java/com/team1678/frc2021/auto/AutoModeSelector.java b/src/main/java/com/team1678/frc2021/auto/AutoModeSelector.java index d4f79f3..519276f 100644 --- a/src/main/java/com/team1678/frc2021/auto/AutoModeSelector.java +++ b/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 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()); diff --git a/src/main/java/com/team1678/frc2021/auto/WaypointReader.java b/src/main/java/com/team1678/frc2021/auto/WaypointReader.java new file mode 100644 index 0000000..ecee0e6 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/com/team1678/frc2021/auto/modes/PathWeaverTestMode.java b/src/main/java/com/team1678/frc2021/auto/modes/PathWeaverTestMode.java new file mode 100644 index 0000000..e616e69 --- /dev/null +++ b/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!"); + } +} diff --git a/src/main/java/com/team1678/frc2021/auto/modes/TestPathMode.java b/src/main/java/com/team1678/frc2021/auto/modes/TestPathMode.java index 8f2648d..75136e4 100644 --- a/src/main/java/com/team1678/frc2021/auto/modes/TestPathMode.java +++ b/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,8 +32,15 @@ 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() { @@ -36,6 +48,21 @@ public TestPathMode() { 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!"); } -} \ No newline at end of file +} From f269df26ad0540eb1bafccd23d23952d9aa950aa Mon Sep 17 00:00:00 2001 From: apurvagyan Date: Sat, 8 Jan 2022 17:38:49 -0800 Subject: [PATCH 2/2] update points and control vectors for traj2 test --- src/main/deploy/paths/test-part-2.path | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/deploy/paths/test-part-2.path b/src/main/deploy/paths/test-part-2.path index ac58c8b..991aa1a 100644 --- a/src/main/deploy/paths/test-part-2.path +++ b/src/main/deploy/paths/test-part-2.path @@ -1,4 +1 @@ -X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name -2.0,-8.21055,1.0,0.0,true,false, -3.0,-8.21055,1.0,0.0,true,false, -4.0,-8.21055,1.0,0.0,true,false, + \ No newline at end of file