From bd89c5044097ace3b63581ab5bab5c46314f9237 Mon Sep 17 00:00:00 2001 From: Billy Pierce Date: Mon, 18 Feb 2019 11:06:12 -0800 Subject: [PATCH 1/2] added basic pathfinder code --- .../robot/autonly/PathfinderBasic.java | 69 +++++++++++++++ vendordeps/PathfinderOLD.json | 85 +++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java create mode 100644 vendordeps/PathfinderOLD.json diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java b/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java new file mode 100644 index 00000000..e834ee06 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java @@ -0,0 +1,69 @@ +package org.usfirst.frc4904.robot.autonly; + + +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Command; +import jaci.pathfinder.Pathfinder; +import jaci.pathfinder.Trajectory; +import jaci.pathfinder.Waypoint; +import jaci.pathfinder.followers.DistanceFollower; +import jaci.pathfinder.modifiers.TankModifier; + +public class PathfinderBasic extends Command { + double leftDistanceOffset = RobotMap.Component.leftWheelEncoder.getDistance(); + double rightDistanceOffset = RobotMap.Component.rightWheelEncoder.getDistance(); + Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, + 0.05, 1.7, 2.0, 60.0); + Waypoint[] points = new Waypoint[] { // TODO: these are just random waypoints. + new Waypoint(-4, -1, Pathfinder.d2r(-45)), + new Waypoint(-2, -2, 0), + new Waypoint(0, 0, 0) + }; + Trajectory trajectory = Pathfinder.generate(points, config); + // Wheelbase Width = 0.5m + TankModifier modifier = new TankModifier(trajectory).modify(0.5); // TODO: Modify + // Do something with the new Trajectories... + Trajectory left = modifier.getLeftTrajectory(); + Trajectory right = modifier.getRightTrajectory(); + DistanceFollower leftTrajectory = new DistanceFollower(left); + DistanceFollower rightTrajectory = new DistanceFollower(right); + + public PathfinderBasic() { + // Use requires() here to declare subsystem dependencies + requires(RobotMap.Component.leftWheelA); + requires(RobotMap.Component.leftWheelB); + requires(RobotMap.Component.rightWheelA); + requires(RobotMap.Component.rightWheelB); + leftTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0); + rightTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double leftOutput = leftTrajectory.calculate(RobotMap.Component.leftWheelEncoder.getDistance() - leftDistanceOffset); + double rightOutput = rightTrajectory + .calculate(RobotMap.Component.rightWheelEncoder.getDistance() - rightDistanceOffset); + RobotMap.Component.leftWheelA.set(leftOutput); + RobotMap.Component.leftWheelB.set(leftOutput); + RobotMap.Component.rightWheelA.set(rightOutput); + RobotMap.Component.rightWheelB.set(rightOutput); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return leftTrajectory.isFinished() && rightTrajectory.isFinished(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/vendordeps/PathfinderOLD.json b/vendordeps/PathfinderOLD.json new file mode 100644 index 00000000..7dc8a1f2 --- /dev/null +++ b/vendordeps/PathfinderOLD.json @@ -0,0 +1,85 @@ +{ + "fileName": "PathfinderOLD.json", + "name": "PathfinderOld", + "version": "2019.2.19", + "uuid": "7194a2d4-2860-4bcc-86c0-97879737d875", + "mavenUrls": [ + "https://dev.imjac.in/maven" + ], + "jsonUrl": "https://dev.imjac.in/maven/jaci/pathfinder/PathfinderOLD-latest.json", + "cppDependencies": [ + { + "groupId": "jaci.pathfinder", + "artifactId": "Pathfinder-Core", + "version": "2019.2.19", + "libName": "pathfinder", + "configuration": "native_pathfinder_old", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxx86-64", + "windowsx86-64", + "osxx86-64", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "jaci.pathfinder", + "artifactId": "Pathfinder-FRCSupport", + "version": "2019.2.19", + "libName": "pathfinder_frc", + "configuration": "native_pathfinder_old", + "headerClassifier": "headers", + "binaryPlatforms": [] + } + ], + "javaDependencies": [ + { + "groupId": "jaci.pathfinder", + "artifactId": "Pathfinder-Java", + "version": "2019.2.19" + }, + { + "groupId": "jaci.jniloader", + "artifactId": "JNILoader", + "version": "1.0.1" + }, + { + "groupId": "jaci.pathfinder", + "artifactId": "Pathfinder-FRCSupport", + "version": "2019.2.19" + } + ], + "jniDependencies": [ + { + "groupId": "jaci.pathfinder", + "artifactId": "Pathfinder-JNI", + "version": "2019.2.19", + "isJar": true, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxx86-64", + "windowsx86-64", + "osxx86-64", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "jaci.pathfinder", + "artifactId": "Pathfinder-CoreJNI", + "version": "2019.2.19", + "isJar": true, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxx86-64", + "windowsx86-64", + "osxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file From 5b10df6cebeb4d9d5a318e4fa2bf8771df7fbcf3 Mon Sep 17 00:00:00 2001 From: Billy Pierce Date: Mon, 18 Feb 2019 11:51:12 -0800 Subject: [PATCH 2/2] generalized PathfinderBasic class --- .../org/usfirst/frc4904/robot/RobotMap.java | 2 ++ .../robot/autonly/PathfinderBasic.java | 32 +++++++++---------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 319c187b..535cc60a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -54,6 +54,8 @@ public static class Wheel { public static final double INCHES_PER_TICK = Metrics.Wheel.CIRCUMFERENCE_INCHES / Metrics.Wheel.TICKS_PER_REVOLUTION; } + public static final double ROBOT_WIDTH_INCHES = -1; // TODO: set + public static final double ROBOT_WIDTH_METERS = ROBOT_WIDTH_INCHES * 0.0254; // TODO: set } public static class PID { diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java b/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java index e834ee06..af7de514 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java @@ -14,30 +14,30 @@ public class PathfinderBasic extends Command { double rightDistanceOffset = RobotMap.Component.rightWheelEncoder.getDistance(); Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0); - Waypoint[] points = new Waypoint[] { // TODO: these are just random waypoints. - new Waypoint(-4, -1, Pathfinder.d2r(-45)), - new Waypoint(-2, -2, 0), - new Waypoint(0, 0, 0) - }; - Trajectory trajectory = Pathfinder.generate(points, config); - // Wheelbase Width = 0.5m - TankModifier modifier = new TankModifier(trajectory).modify(0.5); // TODO: Modify - // Do something with the new Trajectories... - Trajectory left = modifier.getLeftTrajectory(); - Trajectory right = modifier.getRightTrajectory(); - DistanceFollower leftTrajectory = new DistanceFollower(left); - DistanceFollower rightTrajectory = new DistanceFollower(right); + Trajectory trajectory; + TankModifier modifier; + DistanceFollower leftTrajectory; + DistanceFollower rightTrajectory; - public PathfinderBasic() { - // Use requires() here to declare subsystem dependencies + public PathfinderBasic(Waypoint... waypoints) { requires(RobotMap.Component.leftWheelA); requires(RobotMap.Component.leftWheelB); requires(RobotMap.Component.rightWheelA); requires(RobotMap.Component.rightWheelB); - leftTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0); + trajectory = Pathfinder.generate(waypoints, config); + modifier = new TankModifier(trajectory).modify(RobotMap.Metrics.ROBOT_WIDTH_METERS); + leftTrajectory = new DistanceFollower(modifier.getLeftTrajectory()); + rightTrajectory = new DistanceFollower(modifier.getRightTrajectory()); + leftTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0); // TODO: set these PIDVA values rightTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0); } + @Override + protected void initialize() { + leftDistanceOffset = RobotMap.Component.leftWheelEncoder.getDistance(); + rightDistanceOffset = RobotMap.Component.rightWheelEncoder.getDistance(); + } + // Called repeatedly when this Command is scheduled to run @Override protected void execute() {