diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/PathBuilder.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/PathBuilder.java new file mode 100644 index 0000000..98d8bda --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/PathBuilder.java @@ -0,0 +1,155 @@ +package org.usfirst.frc4388.paths; + +import java.util.List; + +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.control.PathSegment; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +/** + * Class used to convert a list of Waypoints into a Path object consisting of arc and line PathSegments + * + * @see Waypoint + * @see Path + * @see PathSegment + */ +public class PathBuilder { + private static final double kEpsilon = 1E-9; + private static final double kReallyBigNumber = 1E9; + + public static Path buildPathFromWaypoints(List w) { + Path p = new Path(); + if (w.size() < 2) + throw new Error("Path must contain at least 2 waypoints"); + int i = 0; + if (w.size() > 2) { + do { + new Arc(getPoint(w, i), getPoint(w, i + 1), getPoint(w, i + 2)).addToPath(p); + i++; + } while (i < w.size() - 2); + } + new Line(w.get(w.size() - 2), w.get(w.size() - 1)).addToPath(p, 0); + p.extrapolateLast(); + p.verifySpeeds(); + // System.out.println(p); + return p; + } + + private static Waypoint getPoint(List w, int i) { + if (i > w.size()) + return w.get(w.size() - 1); + return w.get(i); + } + + /** + * A waypoint along a path. Contains a position, radius (for creating curved paths), and speed. The information from + * these waypoints is used by the PathBuilder class to generate Paths. Waypoints also contain an optional marker + * that is used by the WaitForPathMarkerAction. + * + * @see PathBuilder + * @see WaitForPathMarkerAction + */ + public static class Waypoint { + Translation2d position; + double radius; + double speed; + String marker; + + public Waypoint(Waypoint other) { + this(other.position.x(), other.position.y(), other.radius, other.speed, other.marker); + } + + public Waypoint(double x, double y, double r, double s) { + position = new Translation2d(x, y); + radius = r; + speed = s; + } + + public Waypoint(Translation2d pos, double r, double s) { + position = pos; + radius = r; + speed = s; + } + + public Waypoint(double x, double y, double r, double s, String m) { + position = new Translation2d(x, y); + radius = r; + speed = s; + marker = m; + } + } + + /** + * A Line object is formed by two Waypoints. Contains a start and end position, slope, and speed. + */ + static class Line { + Waypoint a; + Waypoint b; + Translation2d start; + Translation2d end; + Translation2d slope; + double speed; + + public Line(Waypoint a, Waypoint b) { + this.a = a; + this.b = b; + slope = new Translation2d(a.position, b.position); + speed = b.speed; + start = a.position.translateBy(slope.scale(a.radius / slope.norm())); + end = b.position.translateBy(slope.scale(-b.radius / slope.norm())); + } + + private void addToPath(Path p, double endSpeed) { + double pathLength = new Translation2d(end, start).norm(); + if (pathLength > kEpsilon) { + if (b.marker != null) { + p.addSegment(new PathSegment(start.x(), start.y(), end.x(), end.y(), b.speed, + p.getLastMotionState(), endSpeed, b.marker)); + } else { + p.addSegment(new PathSegment(start.x(), start.y(), end.x(), end.y(), b.speed, + p.getLastMotionState(), endSpeed)); + } + } + + } + } + + /** + * An Arc object is formed by two Lines that share a common Waypoint. Contains a center position, radius, and speed. + */ + static class Arc { + Line a; + Line b; + Translation2d center; + double radius; + double speed; + + public Arc(Waypoint a, Waypoint b, Waypoint c) { + this(new Line(a, b), new Line(b, c)); + } + + public Arc(Line a, Line b) { + this.a = a; + this.b = b; + this.speed = (a.speed + b.speed) / 2; + this.center = intersect(a, b); + this.radius = new Translation2d(center, a.end).norm(); + } + + private void addToPath(Path p) { + a.addToPath(p, speed); + if (radius > kEpsilon && radius < kReallyBigNumber) { + p.addSegment(new PathSegment(a.end.x(), a.end.y(), b.start.x(), b.start.y(), center.x(), center.y(), + speed, p.getLastMotionState(), b.speed)); + } + } + + private static Translation2d intersect(Line l1, Line l2) { + final RigidTransform2d lineA = new RigidTransform2d(l1.end, new Rotation2d(l1.slope, true).normal()); + final RigidTransform2d lineB = new RigidTransform2d(l2.start, new Rotation2d(l2.slope, true).normal()); + return lineA.intersection(lineB); + } + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/PathContainer.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/PathContainer.java new file mode 100644 index 0000000..857ae50 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/PathContainer.java @@ -0,0 +1,16 @@ +package org.usfirst.frc4388.paths; + +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; + +/** + * Interface containing all information necessary for a path including the Path itself, the Path's starting pose, and + * whether or not the robot should drive in reverse along the path. + */ +public interface PathContainer { + Path buildPath(); + + RigidTransform2d getStartPose(); + + boolean isReversed(); +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5.java new file mode 100644 index 0000000..292f1fe --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class Backup5 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(218,236,0,60)); + sWaypoints.add(new Waypoint(223,236,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(218, 236), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5Right.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5Right.java new file mode 100644 index 0000000..a23aac2 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5Right.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class Backup5Right implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(218,90,0,60)); + sWaypoints.add(new Waypoint(223,89,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(218, 90), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleLeft.java new file mode 100644 index 0000000..721d967 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleLeft.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class CenterStartToScaleLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(19,157,0,0)); + sWaypoints.add(new Waypoint(40,157,20,75)); + sWaypoints.add(new Waypoint(115,282,60,75)); + sWaypoints.add(new Waypoint(210,239,20,75, "raiseElevator")); + sWaypoints.add(new Waypoint(250,213,20,75)); + sWaypoints.add(new Waypoint(272,210,0,75)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleRight.java new file mode 100644 index 0000000..f76de5e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleRight.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class CenterStartToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(19,157,0,0)); + sWaypoints.add(new Waypoint(40,157,20,75)); + sWaypoints.add(new Waypoint(115,46,60,75)); + sWaypoints.add(new Waypoint(210,81,0,75, "raiseElevator")); + sWaypoints.add(new Waypoint(250,106,20,75)); + sWaypoints.add(new Waypoint(275,116,0,75)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeft.java new file mode 100644 index 0000000..c830f6b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeft.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class CenterStartToSwitchLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(19,157,0,80)); + sWaypoints.add(new Waypoint(40,157,20,80)); + sWaypoints.add(new Waypoint(100,209,15,80)); + sWaypoints.add(new Waypoint(116,214,0,80)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeftV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeftV2.java new file mode 100644 index 0000000..b4d31fb --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeftV2.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class CenterStartToSwitchLeftV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(19,157,0,80)); + sWaypoints.add(new Waypoint(40,157,20,80)); + sWaypoints.add(new Waypoint(100,204,15,80)); + sWaypoints.add(new Waypoint(116,209,0,80)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchRight.java new file mode 100644 index 0000000..b453483 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchRight.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class CenterStartToSwitchRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(19,157,0,80)); + sWaypoints.add(new Waypoint(40,157,20,80)); + sWaypoints.add(new Waypoint(100,114,15,80)); + sWaypoints.add(new Waypoint(121,113,0,80)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5.java new file mode 100644 index 0000000..12e2e47 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class Forward5 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(230,236,0,60)); + sWaypoints.add(new Waypoint(218,234,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(230, 236), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5Right.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5Right.java new file mode 100644 index 0000000..136f2ce --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5Right.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class Forward5Right implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(230,89,0,60)); + sWaypoints.add(new Waypoint(218,91,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(230, 89), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/GoStraight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/GoStraight.java new file mode 100644 index 0000000..9f4e65d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/GoStraight.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class GoStraight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,70,0,0)); + sWaypoints.add(new Waypoint(150,70,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 70), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToCenterStart.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToCenterStart.java new file mode 100644 index 0000000..0858685 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToCenterStart.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToCenterStart implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,40)); + sWaypoints.add(new Waypoint(39,274,20,40)); + sWaypoints.add(new Waypoint(60,250,0,40)); + sWaypoints.add(new Waypoint(80,170,20,40)); + sWaypoints.add(new Waypoint(45,160,0,40)); + sWaypoints.add(new Waypoint(20,160,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeft.java new file mode 100644 index 0000000..7f0df65 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeft.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,100)); + sWaypoints.add(new Waypoint(180,274,30,100, "raiseElevator")); + sWaypoints.add(new Waypoint(240,239,30,100)); + sWaypoints.add(new Waypoint(273,241,0,100)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeftV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeftV2.java new file mode 100644 index 0000000..35799b2 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeftV2.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleLeftV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,130)); + sWaypoints.add(new Waypoint(40,274,0,130, "shiftHi")); + sWaypoints.add(new Waypoint(145,273,55,130, "raiseElevator")); + sWaypoints.add(new Waypoint(270,232,0,130, "startEject")); + sWaypoints.add(new Waypoint(287,225,0,130)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRight.java new file mode 100644 index 0000000..bcfe073 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRight.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,110)); + sWaypoints.add(new Waypoint(159,274,0,110)); + sWaypoints.add(new Waypoint(225,274,40,110)); + sWaypoints.add(new Waypoint(220,87,30,110)); + sWaypoints.add(new Waypoint(250,87,0,50, "raiseElevator")); + sWaypoints.add(new Waypoint(275,90,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPR.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPR.java new file mode 100644 index 0000000..7c7ef4f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPR.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleRightAPR implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,40)); + sWaypoints.add(new Waypoint(55,274,20,40)); + sWaypoints.add(new Waypoint(55,65,80,80)); + sWaypoints.add(new Waypoint(260,55,0,80)); + sWaypoints.add(new Waypoint(310,55,20,60)); + sWaypoints.add(new Waypoint(320,20,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV2.java new file mode 100644 index 0000000..15fe63b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV2.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleRightAPRV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,40)); + sWaypoints.add(new Waypoint(55,274,20,40)); + sWaypoints.add(new Waypoint(55,250,0,110, "shiftHi")); + sWaypoints.add(new Waypoint(55,210,0,110)); + sWaypoints.add(new Waypoint(55,105,80,110)); + sWaypoints.add(new Waypoint(140,100,0,80)); + sWaypoints.add(new Waypoint(270,115,0,80, "shiftLow")); + sWaypoints.add(new Waypoint(300,120,0,80, "raiseElevator")); + sWaypoints.add(new Waypoint(330,120,20,80)); + sWaypoints.add(new Waypoint(330,100,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV3.java new file mode 100644 index 0000000..77ad6ea --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV3.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleRightAPRV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,40)); + sWaypoints.add(new Waypoint(55,274,20,40)); + sWaypoints.add(new Waypoint(55,35,80,100)); + sWaypoints.add(new Waypoint(313,25,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightSave.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightSave.java new file mode 100644 index 0000000..cce7223 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightSave.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToScaleRightSave implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,110)); + sWaypoints.add(new Waypoint(159,274,0,110)); + sWaypoints.add(new Waypoint(220,274,40,110)); + sWaypoints.add(new Waypoint(214,210,0,60)); + sWaypoints.add(new Waypoint(210,180,0,100)); + sWaypoints.add(new Waypoint(209,150,0,80)); + sWaypoints.add(new Waypoint(209,120,0,70)); + sWaypoints.add(new Waypoint(214,87,30,50)); + sWaypoints.add(new Waypoint(250,87,0,50, "raiseElevator")); + sWaypoints.add(new Waypoint(270,90,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchLeftV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchLeftV3.java new file mode 100644 index 0000000..1f9ed87 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchLeftV3.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToSwitchLeftV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(65,270,0,60)); + sWaypoints.add(new Waypoint(65,231,30,60, "raiseElevator")); + sWaypoints.add(new Waypoint(110,231,0,60)); + sWaypoints.add(new Waypoint(126,231,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(65, 270), Rotation2d.fromDegrees(-90.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRight.java new file mode 100644 index 0000000..0cb0650 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRight.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToSwitchRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,274,0,40)); + sWaypoints.add(new Waypoint(39,274,20,40)); + sWaypoints.add(new Waypoint(60,200,0,40)); + sWaypoints.add(new Waypoint(80,129,20,40)); + sWaypoints.add(new Waypoint(106,124,0,40)); + sWaypoints.add(new Waypoint(126,124,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRightV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRightV3.java new file mode 100644 index 0000000..e4ab19a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRightV3.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftStartToSwitchRightV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(65,270,0,80)); + sWaypoints.add(new Waypoint(65,134,40,80, "raiseElevator")); + sWaypoints.add(new Waypoint(110,134,0,80)); + sWaypoints.add(new Waypoint(126,134,0,80)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(65, 270), Rotation2d.fromDegrees(-90.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCube.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCube.java new file mode 100644 index 0000000..c95fe6d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCube.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftSwitch2ndCube implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(230,236,0,30)); + sWaypoints.add(new Waypoint(195,215,0,30)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(230, 236), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCubeV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCubeV2.java new file mode 100644 index 0000000..90bdaa1 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCubeV2.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class LeftSwitch2ndCubeV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(230,236,0,60)); + sWaypoints.add(new Waypoint(210,220,0,40)); + sWaypoints.add(new Waypoint(201,205,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(230, 236), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToScaleRight.java new file mode 100644 index 0000000..cb59bf9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToScaleRight.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class PyramidToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(100,165,0,60)); + sWaypoints.add(new Waypoint(70,165,20,60)); + sWaypoints.add(new Waypoint(80,72,45,60, "shiftHi")); + sWaypoints.add(new Waypoint(230,50,0,130, "raiseElevator")); + sWaypoints.add(new Waypoint(400,40,0,130)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(100, 165), Rotation2d.fromDegrees(0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeft.java new file mode 100644 index 0000000..29cb553 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeft.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class PyramidToSwitchLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(46,179,0,60)); + sWaypoints.add(new Waypoint(66,179,15,60)); + sWaypoints.add(new Waypoint(104,231,10,60)); + sWaypoints.add(new Waypoint(116,236,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(46, 179), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeftV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeftV2.java new file mode 100644 index 0000000..945c940 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeftV2.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class PyramidToSwitchLeftV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(46,179,0,60)); + sWaypoints.add(new Waypoint(66,184,15,60)); + sWaypoints.add(new Waypoint(94,211,10,60)); + sWaypoints.add(new Waypoint(120,216,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(46, 179), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRight.java new file mode 100644 index 0000000..8ef8bc4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRight.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class PyramidToSwitchRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(46,145,0,60)); + sWaypoints.add(new Waypoint(66,145,15,60)); + sWaypoints.add(new Waypoint(110,93,10,60)); + sWaypoints.add(new Waypoint(122,88,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(46, 145), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRightV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRightV2.java new file mode 100644 index 0000000..777abad --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRightV2.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class PyramidToSwitchRightV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(46,145,0,80)); + sWaypoints.add(new Waypoint(66,145,15,80)); + sWaypoints.add(new Waypoint(100,93,15,80)); + sWaypoints.add(new Waypoint(118,88,0,50)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(46, 145), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeft.java new file mode 100644 index 0000000..cd4dc14 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeft.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,50,0,0)); + sWaypoints.add(new Waypoint(220,50,40,60)); + sWaypoints.add(new Waypoint(214,114,0,60, "raiseElevator")); + sWaypoints.add(new Waypoint(204,174,0,60)); + sWaypoints.add(new Waypoint(204,232,30,60)); + sWaypoints.add(new Waypoint(275,237,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 50), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPR.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPR.java new file mode 100644 index 0000000..91a5531 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPR.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleLeftAPR implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,40)); + sWaypoints.add(new Waypoint(55,51,20,40)); + sWaypoints.add(new Waypoint(55,260,80,100)); + sWaypoints.add(new Waypoint(260,270,0,100)); + sWaypoints.add(new Waypoint(310,270,20,60)); + sWaypoints.add(new Waypoint(320,295,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV2.java new file mode 100644 index 0000000..d542aa9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV2.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleLeftAPRV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,40)); + sWaypoints.add(new Waypoint(55,51,20,40)); + sWaypoints.add(new Waypoint(55,75,0,110, "shiftHi")); + sWaypoints.add(new Waypoint(55,130,0,110)); + sWaypoints.add(new Waypoint(55,230,80,110)); + sWaypoints.add(new Waypoint(140,235,0,80)); + sWaypoints.add(new Waypoint(240,220,0,80, "shiftLow")); + sWaypoints.add(new Waypoint(270,215,0,80, "raiseElevator")); + sWaypoints.add(new Waypoint(310,215,20,80)); + sWaypoints.add(new Waypoint(310,235,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV3.java new file mode 100644 index 0000000..6ea050d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV3.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleLeftAPRV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,40)); + sWaypoints.add(new Waypoint(55,51,20,40)); + sWaypoints.add(new Waypoint(55,290,80,100)); + sWaypoints.add(new Waypoint(313,300,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSave.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSave.java new file mode 100644 index 0000000..141b83d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSave.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleLeftSave implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,110)); + sWaypoints.add(new Waypoint(159,51,0,110)); + sWaypoints.add(new Waypoint(220,51,40,110)); + sWaypoints.add(new Waypoint(218,115,0,60)); + sWaypoints.add(new Waypoint(218,145,0,100)); + sWaypoints.add(new Waypoint(218,175,0,100)); + sWaypoints.add(new Waypoint(218,205,0,100)); + sWaypoints.add(new Waypoint(218,234,25,50)); + sWaypoints.add(new Waypoint(250,228,0,50, "raiseElevator")); + sWaypoints.add(new Waypoint(271,226,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSaveV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSaveV2.java new file mode 100644 index 0000000..1969e00 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSaveV2.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleLeftSaveV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,110)); + sWaypoints.add(new Waypoint(60,51,0,130, "shiftHi")); + sWaypoints.add(new Waypoint(159,51,0,130)); + sWaypoints.add(new Waypoint(220,51,30,110)); + sWaypoints.add(new Waypoint(218,115,0,60)); + sWaypoints.add(new Waypoint(218,145,0,130)); + sWaypoints.add(new Waypoint(218,175,0,130)); + sWaypoints.add(new Waypoint(218,205,0,130)); + sWaypoints.add(new Waypoint(218,234,25,50)); + sWaypoints.add(new Waypoint(250,234,0,50, "raiseElevator")); + sWaypoints.add(new Waypoint(270,231,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRight.java new file mode 100644 index 0000000..0cdadbf --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRight.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,100)); + sWaypoints.add(new Waypoint(180,51,30,100, "raiseElevator")); + sWaypoints.add(new Waypoint(240,86,30,100)); + sWaypoints.add(new Waypoint(273,84,0,100)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRightV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRightV2.java new file mode 100644 index 0000000..9754554 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRightV2.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToScaleRightV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(18,51,0,130)); + sWaypoints.add(new Waypoint(40,51,0,130, "shiftHi")); + sWaypoints.add(new Waypoint(145,52,55,130, "raiseElevator")); + sWaypoints.add(new Waypoint(257,93,0,130, "startEject")); + sWaypoints.add(new Waypoint(282,100,0,130)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchLeftV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchLeftV3.java new file mode 100644 index 0000000..0441ea7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchLeftV3.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToSwitchLeftV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(78,55,0,100)); + sWaypoints.add(new Waypoint(78,181,20,100, "raiseElevator")); + sWaypoints.add(new Waypoint(105,190,0,70)); + sWaypoints.add(new Waypoint(134,194,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(78, 55), Rotation2d.fromDegrees(90.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchRightV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchRightV3.java new file mode 100644 index 0000000..d770939 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchRightV3.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightStartToSwitchRightV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(65,55,0,60)); + sWaypoints.add(new Waypoint(65,94,30,60, "raiseElevator")); + sWaypoints.add(new Waypoint(110,94,0,60)); + sWaypoints.add(new Waypoint(126,94,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(65, 55), Rotation2d.fromDegrees(90.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightSwitch2ndCubeV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightSwitch2ndCubeV2.java new file mode 100644 index 0000000..5a33759 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightSwitch2ndCubeV2.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class RightSwitch2ndCubeV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(230,89,0,60)); + sWaypoints.add(new Waypoint(210,105,0,40)); + sWaypoints.add(new Waypoint(201,120,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(230, 89), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToOuttaHere.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToOuttaHere.java new file mode 100644 index 0000000..aa96ec7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToOuttaHere.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToOuttaHere implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(287,240,0,80)); + sWaypoints.add(new Waypoint(287,285,20,80)); + sWaypoints.add(new Waypoint(314,285,0,80)); + sWaypoints.add(new Waypoint(329,276,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(287, 240), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft.java new file mode 100644 index 0000000..1e6c001 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToSwitchLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(291,247,0,60)); + sWaypoints.add(new Waypoint(230,238,0,60)); + sWaypoints.add(new Waypoint(217,236,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(291, 247), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2.java new file mode 100644 index 0000000..afe4778 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToSwitchLeft2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(291,241,0,60)); + sWaypoints.add(new Waypoint(248,241,30,60)); + sWaypoints.add(new Waypoint(230,215,0,40)); + sWaypoints.add(new Waypoint(218,200,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(291, 241), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2V2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2V2.java new file mode 100644 index 0000000..3ad8bdc --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2V2.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToSwitchLeft2V2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(291,241,0,60)); + sWaypoints.add(new Waypoint(248,241,15,60)); + sWaypoints.add(new Waypoint(228,225,0,40)); + sWaypoints.add(new Waypoint(220,215,10,20)); + sWaypoints.add(new Waypoint(218,200,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(291, 241), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftNoIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftNoIntake.java new file mode 100644 index 0000000..cc81514 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftNoIntake.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToSwitchLeftNoIntake implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(276,247,0,60)); + sWaypoints.add(new Waypoint(232,237,0,60)); + sWaypoints.add(new Waypoint(225,236,0,30)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(276, 247), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV2.java new file mode 100644 index 0000000..893bf02 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV2.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToSwitchLeftV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(267,240,0,60)); + sWaypoints.add(new Waypoint(224,233,0,60)); + sWaypoints.add(new Waypoint(214,231,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(267, 240), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV3.java new file mode 100644 index 0000000..f28b345 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV3.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleLeftToSwitchLeftV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(281,247,0,60)); + sWaypoints.add(new Waypoint(230,238,0,60)); + sWaypoints.add(new Waypoint(218,236,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(281, 247), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightSideToSwitchRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightSideToSwitchRight.java new file mode 100644 index 0000000..dcaf79c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightSideToSwitchRight.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightSideToSwitchRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(330,40,0,80)); + sWaypoints.add(new Waypoint(254,73,0,80)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(330, 40), Rotation2d.fromDegrees(155)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToOuttaHere.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToOuttaHere.java new file mode 100644 index 0000000..ac6bdd3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToOuttaHere.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToOuttaHere implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(287,85,0,80)); + sWaypoints.add(new Waypoint(287,40,20,80)); + sWaypoints.add(new Waypoint(314,40,0,80)); + sWaypoints.add(new Waypoint(335,50,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(287, 85), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToPyramid.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToPyramid.java new file mode 100644 index 0000000..089d477 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToPyramid.java @@ -0,0 +1,39 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToPyramid implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(282,80,0,130)); + sWaypoints.add(new Waypoint(230,65,30,130, "shiftHi")); + sWaypoints.add(new Waypoint(105,62,0,130, "shiftLo")); + sWaypoints.add(new Waypoint(60,62,40,60)); + sWaypoints.add(new Waypoint(60,146,20,60, "intakeOn")); + sWaypoints.add(new Waypoint(80,160,0,60)); + sWaypoints.add(new Waypoint(150,160,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(282, 80), Rotation2d.fromDegrees(180)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight.java new file mode 100644 index 0000000..36afc6e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(287,85,0,40)); + sWaypoints.add(new Waypoint(234,91,0,40)); + sWaypoints.add(new Waypoint(214,94,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(287, 85), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight2.java new file mode 100644 index 0000000..1d230f4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight2.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRight2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(297,85,0,0)); + sWaypoints.add(new Waypoint(254,84,20,60)); + sWaypoints.add(new Waypoint(219,111,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(297, 85), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight23Cube.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight23Cube.java new file mode 100644 index 0000000..380f296 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight23Cube.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRight23Cube implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(291,84,0,60)); + sWaypoints.add(new Waypoint(248,84,30,60)); + sWaypoints.add(new Waypoint(230,110,0,40)); + sWaypoints.add(new Waypoint(218,125,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(291, 84), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight3Cube.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight3Cube.java new file mode 100644 index 0000000..d29d309 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight3Cube.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRight3Cube implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(291,78,0,60)); + sWaypoints.add(new Waypoint(230,87,0,60)); + sWaypoints.add(new Waypoint(218,89,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(291, 78), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightNoIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightNoIntake.java new file mode 100644 index 0000000..d840fb7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightNoIntake.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRightNoIntake implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(276,78,0,60)); + sWaypoints.add(new Waypoint(232,88,0,60)); + sWaypoints.add(new Waypoint(225,89,0,30)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(276, 78), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV2.java new file mode 100644 index 0000000..31644be --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV2.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRightV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(267,85,0,60)); + sWaypoints.add(new Waypoint(224,92,0,60)); + sWaypoints.add(new Waypoint(214,94,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(267, 85), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV3.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV3.java new file mode 100644 index 0000000..bc8fccc --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV3.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class ScaleRightToSwitchRightV3 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(281,78,0,60)); + sWaypoints.add(new Waypoint(230,87,0,60)); + sWaypoints.add(new Waypoint(218,89,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(281, 78), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeft2ToScaleLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeft2ToScaleLeft.java new file mode 100644 index 0000000..06e1992 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeft2ToScaleLeft.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeft2ToScaleLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(219,213,0,60)); + sWaypoints.add(new Waypoint(248,235,20,60, "raiseElevator")); + sWaypoints.add(new Waypoint(291,221,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(219, 213), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStart.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStart.java new file mode 100644 index 0000000..db8ff0f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStart.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeftToCenterStart implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(116,220,0,40)); + sWaypoints.add(new Waypoint(100,215,15,40)); + sWaypoints.add(new Waypoint(74,184,15,40)); + sWaypoints.add(new Waypoint(54,184,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(116, 220), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStartV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStartV2.java new file mode 100644 index 0000000..0812e8a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStartV2.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeftToCenterStartV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(116,220,0,60)); + sWaypoints.add(new Waypoint(100,215,15,60)); + sWaypoints.add(new Waypoint(64,172,0,60)); + sWaypoints.add(new Waypoint(54,164,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(116, 220), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToNullZone.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToNullZone.java new file mode 100644 index 0000000..a79e7de --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToNullZone.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeftToNullZone implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(72,246,0,40)); + sWaypoints.add(new Waypoint(97,250,10,40)); + sWaypoints.add(new Waypoint(132,280,20,60)); + sWaypoints.add(new Waypoint(242,270,10,60)); + sWaypoints.add(new Waypoint(242,250,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(72, 246), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleLeft.java new file mode 100644 index 0000000..066156a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleLeft.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeftToScaleLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,234,0,60)); + sWaypoints.add(new Waypoint(231,234,0,60, "raiseElevator")); + sWaypoints.add(new Waypoint(291,237,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 234), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRight.java new file mode 100644 index 0000000..1a81872 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRight.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeftToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,234,0,60)); + sWaypoints.add(new Waypoint(236,234,10,80)); + sWaypoints.add(new Waypoint(232,154,0,80)); + sWaypoints.add(new Waypoint(232,95,25,80)); + sWaypoints.add(new Waypoint(271,98,0,50, "raiseElevator")); + sWaypoints.add(new Waypoint(301,107,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 234), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRightV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRightV2.java new file mode 100644 index 0000000..8e2b42e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRightV2.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchLeftToScaleRightV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,234,0,60)); + sWaypoints.add(new Waypoint(236,234,10,60)); + sWaypoints.add(new Waypoint(231,154,0,60, "raiseElevator")); + sWaypoints.add(new Waypoint(231,89,25,50)); + sWaypoints.add(new Waypoint(277,94,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 234), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight.java new file mode 100644 index 0000000..da647f4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRight2ToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(219,111,0,0)); + sWaypoints.add(new Waypoint(254,84,20,60)); + sWaypoints.add(new Waypoint(297,85,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(219, 111), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight3Cube.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight3Cube.java new file mode 100644 index 0000000..f0400e6 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight3Cube.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRight2ToScaleRight3Cube implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(219,112,0,60)); + sWaypoints.add(new Waypoint(248,90,20,60, "raiseElevator")); + sWaypoints.add(new Waypoint(291,104,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(219, 112), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStart.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStart.java new file mode 100644 index 0000000..265c95d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStart.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRightToCenterStart implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(116,104,0,50)); + sWaypoints.add(new Waypoint(100,109,15,50)); + sWaypoints.add(new Waypoint(74,145,15,50)); + sWaypoints.add(new Waypoint(54,145,0,50)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(116, 104), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStartV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStartV2.java new file mode 100644 index 0000000..b6842a7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStartV2.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRightToCenterStartV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(116,104,0,80)); + sWaypoints.add(new Waypoint(100,109,15,80)); + sWaypoints.add(new Waypoint(74,140,15,80)); + sWaypoints.add(new Waypoint(54,140,0,50)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(116, 104), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeft.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeft.java new file mode 100644 index 0000000..5c07d98 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeft.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRightToScaleLeft implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,91,0,60)); + sWaypoints.add(new Waypoint(236,91,10,80)); + sWaypoints.add(new Waypoint(232,171,0,80)); + sWaypoints.add(new Waypoint(232,230,25,80)); + sWaypoints.add(new Waypoint(271,227,0,50, "raiseElevator")); + sWaypoints.add(new Waypoint(301,218,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 91), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeftV2.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeftV2.java new file mode 100644 index 0000000..feb42e7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeftV2.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRightToScaleLeftV2 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,91,0,60)); + sWaypoints.add(new Waypoint(236,91,10,60)); + sWaypoints.add(new Waypoint(231,171,0,60, "raiseElevator")); + sWaypoints.add(new Waypoint(231,236,25,50)); + sWaypoints.add(new Waypoint(277,231,0,40)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 91), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight.java new file mode 100644 index 0000000..5f546b4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class SwitchRightToScaleRight implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,95,0,0)); + sWaypoints.add(new Waypoint(231,95,0,60, "raiseElevator")); + sWaypoints.add(new Waypoint(261,88,0,60)); + sWaypoints.add(new Waypoint(291,88,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 95), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight3Cube.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight3Cube.java new file mode 100644 index 0000000..e805cf5 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight3Cube.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4388.paths.auton; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + + +public class SwitchRightToScaleRight3Cube implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(224,91,0,60)); + sWaypoints.add(new Waypoint(231,91,0,60, "raiseElevator")); + sWaypoints.add(new Waypoint(291,88,0,60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(224, 91), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/CompBot.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/CompBot.java new file mode 100644 index 0000000..c7cf486 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/CompBot.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4388.paths.profiles; + +/** + * Contains the corrective values for Comp bot + */ +public class CompBot implements RobotProfile { + + @Override + public double getRedBoilerGearXCorrection() { + return 3.0; + } + + @Override + public double getRedBoilerGearYCorrection() { + return 4.0; + } + + @Override + public double getRedHopperXOffset() { + return 0.0; + } + + @Override + public double getRedHopperYOffset() { + return -3.0; + } + + @Override + public double getBlueBoilerGearXCorrection() { + return 0.5; + } + + @Override + public double getBlueBoilerGearYCorrection() { + return 1.0; + } + + @Override + public double getBlueHopperXOffset() { + return -5.5; + } + + @Override + public double getBlueHopperYOffset() { + return 0.0; + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/DalyField.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/DalyField.java new file mode 100644 index 0000000..c9cd050 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/DalyField.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4388.paths.profiles; + +/** + * Contains the measurements for the Daly field at St. Louis champs + */ +public class DalyField implements FieldProfile { + + @Override + public double getRedCenterToBoiler() { + return 125.44; + } + + @Override + public double getRedWallToAirship() { + return 114.5; + } + + @Override + public double getRedCenterToHopper() { + return 162; + } + + @Override + public double getRedWallToHopper() { // TODO: verify this + return 110.5; + } + + @Override + public double getBlueCenterToBoiler() { + return 126.76; + } + + @Override + public double getBlueWallToAirship() { + return 113.75; + } + + @Override + public double getBlueCenterToHopper() { + return 162.25; + } + + @Override + public double getBlueWallToHopper() { + return 109.625; + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/FieldProfile.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/FieldProfile.java new file mode 100644 index 0000000..ec49f7a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/FieldProfile.java @@ -0,0 +1,26 @@ +package org.usfirst.frc4388.paths.profiles; + +/** + * Interface that holds all the field measurements required by the PathAdapter + * + * @see PathAdapter + */ +public interface FieldProfile { + + public double getRedCenterToBoiler(); + + public double getRedWallToAirship(); + + public double getRedCenterToHopper(); + + public double getRedWallToHopper(); + + public double getBlueCenterToBoiler(); + + public double getBlueWallToAirship(); + + public double getBlueCenterToHopper(); + + public double getBlueWallToHopper(); + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PathAdapter.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PathAdapter.java new file mode 100644 index 0000000..db4aa9a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PathAdapter.java @@ -0,0 +1,261 @@ +package org.usfirst.frc4388.paths.profiles; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +/** + * Uses a field and robot profile to calculate Waypoints for the paths used by the GearThenHopperShoot auto modes. + * + * @see RobotProfile + * @see FieldProfile + */ +public class PathAdapter { + + static final RobotProfile kRobotProfile = new CompBot(); + static final FieldProfile kFieldProfile = new PracticeField(); + + // Path Variables + static final double kLargeRadius = 45; + static final double kModerateRadius = 30; + static final double kNominalRadius = 20; + static final double kSmallRadius = 10; + static final double kSpeed = 80; + + // Don't mess with these + static final double kPegOffsetX = 17.77; // center of airship to boiler peg + static final double kPegOffsetY = 30.66; // front of airship to boiler + // pegkRobotProfile.getBlueBoilerGearXCorrection() + static final Rotation2d kRedPegHeading = Rotation2d.fromDegrees(240); + static final Rotation2d kBluePegHeading = Rotation2d.fromDegrees(125); + static final Rotation2d kRedHopperHeading = Rotation2d.fromDegrees(45); // angle to hit the red hopper at + static final Rotation2d kBlueHopperHeading = Rotation2d.fromDegrees(315); // angle to hit the blue hopper at + static final Rotation2d kStartHeading = Rotation2d.fromDegrees(180); // start angle (backwards) + static final double kGearPlacementDist = Constants.kCenterToRearBumperDistance + 10; // distance away from the + // airship wall to place the + // gear at + static final double kHopperOffsetX = 3.0; // How far from the closest edge of the hopper to aim + static final double kHopperSkew = 6.0; // How far into the wall to place the final point (to ensure we keep nudging + // into the wall) + static final double kFrontDist = Constants.kCenterToIntakeDistance; + static final double kSideDist = Constants.kCenterToSideBumperDistance; + static final double kHopperTurnDistance = 40; // how long the third segment in the hopper path should be + static final double kGearTurnDistance = 24; // how long the first segment in the hopper path should be + static final double kEndHopperPathX = 84.5; // X position we want the hopper path to end at + static final double kFieldHeight = 324; // total height of the field in inches (doesn't really have to be accurate, + // everything is relative) + + public static Translation2d getRedHopperPosition() { + Translation2d contactPoint = new Translation2d( + kFieldProfile.getRedWallToHopper() + kHopperOffsetX + kRobotProfile.getRedHopperXOffset(), + kFieldHeight / 2 - kFieldProfile.getRedCenterToHopper() - kRobotProfile.getRedHopperYOffset()); + Translation2d robotOffset = new Translation2d(kFrontDist, kSideDist); + robotOffset = robotOffset.direction().rotateBy(kRedHopperHeading).toTranslation().scale(robotOffset.norm()); + return contactPoint.translateBy(robotOffset); + } + + // third point in the hopper path + public static Translation2d getRedHopperTurnPosition() { + Translation2d hopperPosition = getRedHopperPosition(); + Translation2d turnOffset = new Translation2d(kRedHopperHeading.cos() * kHopperTurnDistance, + kRedHopperHeading.sin() * kHopperTurnDistance); + return hopperPosition.translateBy(turnOffset); + } + + // second point in the hopper path + public static Translation2d getRedGearTurnPosition() { + Translation2d gearPosition = getRedGearPosition(); + Translation2d turnOffset = new Translation2d(kRedPegHeading.cos() * kGearTurnDistance, + kRedPegHeading.sin() * kGearTurnDistance); + return gearPosition.translateBy(turnOffset); + } + + public static Translation2d getRedGearCorrection() { + return RigidTransform2d.fromRotation(kRedPegHeading) + .transformBy(RigidTransform2d + .fromTranslation((new Translation2d(-kRobotProfile.getRedBoilerGearXCorrection(), + -kRobotProfile.getRedBoilerGearYCorrection())))) + .getTranslation(); + } + + // final position in the gear path, first position in the hopper path + public static Translation2d getRedGearPosition() { + Translation2d pegPosition = new Translation2d(kFieldProfile.getRedWallToAirship() + kPegOffsetX, + kFieldHeight / 2 - kPegOffsetY); + Translation2d robotOffset = new Translation2d(kRedPegHeading.cos() * kGearPlacementDist, + kRedPegHeading.sin() * kGearPlacementDist); + return pegPosition.translateBy(robotOffset); + } + + private static Translation2d getRedGearPositionCorrected() { + return getRedGearPosition().translateBy(getRedGearCorrection()); + } + + // first position in the gear path + public static RigidTransform2d getRedStartPose() { + return new RigidTransform2d(new Translation2d(Constants.kCenterToFrontBumperDistance, + kFieldHeight / 2 - kFieldProfile.getRedCenterToBoiler() + Constants.kCenterToSideBumperDistance), + kStartHeading); + } + + // second position in the gear path + private static Translation2d getRedCenterPosition() { + RigidTransform2d end = new RigidTransform2d(getRedGearPositionCorrected(), kRedPegHeading); + return getRedStartPose().intersection(end); + } + + private static Path sRedGearPath = null; + + public static Path getRedGearPath() { + if (sRedGearPath == null) { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(getRedStartPose().getTranslation(), 0, kSpeed)); + sWaypoints.add(new Waypoint(getRedCenterPosition(), kLargeRadius, kSpeed)); + sWaypoints.add(new Waypoint(getRedGearPositionCorrected(), 0, kSpeed)); + + sRedGearPath = PathBuilder.buildPathFromWaypoints(sWaypoints); + } + return sRedGearPath; + } + + private static Path sRedHopperPath = null; + + public static Path getRedHopperPath() { + if (sRedHopperPath == null) { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(getRedGearPosition(), 0, kSpeed)); + sWaypoints.add(new Waypoint(getRedGearTurnPosition(), kSmallRadius, kSpeed)); + sWaypoints.add(new Waypoint(getRedHopperTurnPosition(), kModerateRadius, kSpeed)); + sWaypoints.add(new Waypoint(getRedHopperPosition(), kSmallRadius, kSpeed)); + + Translation2d redHopperEndPosition = new Translation2d(getRedHopperPosition()); + redHopperEndPosition.setX(kEndHopperPathX); // move X position to desired place + redHopperEndPosition.setY(redHopperEndPosition.y() - kHopperSkew); // TODO make constant + sWaypoints.add(new Waypoint(redHopperEndPosition, 0, kSpeed)); + sRedHopperPath = PathBuilder.buildPathFromWaypoints(sWaypoints); + } + return sRedHopperPath; + + } + + public static Translation2d getBlueHopperPosition() { + Translation2d contactPoint = new Translation2d( + kFieldProfile.getBlueWallToHopper() + kHopperOffsetX + kRobotProfile.getBlueHopperXOffset(), + kFieldHeight / 2 + kFieldProfile.getBlueCenterToHopper() + kRobotProfile.getBlueHopperYOffset()); + Translation2d robotOffset = new Translation2d(kFrontDist, -kSideDist); + robotOffset = robotOffset.direction().rotateBy(kBlueHopperHeading).toTranslation().scale(robotOffset.norm()); + return contactPoint.translateBy(robotOffset); + } + + public static Translation2d getBlueHopperTurnPosition() { + Translation2d hopperPosition = getBlueHopperPosition(); + Translation2d turnOffset = new Translation2d(kBlueHopperHeading.cos() * kHopperTurnDistance, + kBlueHopperHeading.sin() * kHopperTurnDistance); + return hopperPosition.translateBy(turnOffset); + } + + public static Translation2d getBlueGearTurnPosition() { + Translation2d gearPosition = getBlueGearPosition(); + Translation2d turnOffset = new Translation2d(kBluePegHeading.cos() * kGearTurnDistance, + kBluePegHeading.sin() * kGearTurnDistance); + return gearPosition.translateBy(turnOffset); + } + + public static Translation2d getBlueGearCorrection() { + return RigidTransform2d.fromRotation(kBluePegHeading) + .transformBy(RigidTransform2d + .fromTranslation((new Translation2d(-kRobotProfile.getBlueBoilerGearXCorrection(), + -kRobotProfile.getBlueBoilerGearYCorrection())))) + .getTranslation(); + } + + private static Translation2d getBlueGearPosition() { + Translation2d pegPosition = new Translation2d(kFieldProfile.getBlueWallToAirship() + kPegOffsetX, + kFieldHeight / 2 + kPegOffsetY); + Translation2d robotOffset = new Translation2d(kBluePegHeading.cos() * kGearPlacementDist, + kBluePegHeading.sin() * kGearPlacementDist); + return pegPosition.translateBy(robotOffset); + } + + private static Translation2d getBlueGearPositionCorrected() { + return getBlueGearPosition().translateBy(getBlueGearCorrection()); + } + + public static RigidTransform2d getBlueStartPose() { + return new RigidTransform2d(new Translation2d(Constants.kCenterToFrontBumperDistance, + kFieldHeight / 2 + kFieldProfile.getBlueCenterToBoiler() - Constants.kCenterToSideBumperDistance), + kStartHeading); + } + + private static Translation2d getBlueCenterPosition() { + RigidTransform2d end = new RigidTransform2d(getBlueGearPositionCorrected(), kBluePegHeading); + return getBlueStartPose().intersection(end); + } + + private static Path sBlueGearPath = null; + + public static Path getBlueGearPath() { + if (sBlueGearPath == null) { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(getBlueStartPose().getTranslation(), 0, kSpeed)); + sWaypoints.add(new Waypoint(getBlueCenterPosition(), kLargeRadius, kSpeed)); + sWaypoints.add(new Waypoint(getBlueGearPositionCorrected(), 0, kSpeed)); + + sBlueGearPath = PathBuilder.buildPathFromWaypoints(sWaypoints); + } + return sBlueGearPath; + } + + private static Path sBlueHopperPath = null; + + public static Path getBlueHopperPath() { + if (sBlueHopperPath == null) { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(getBlueGearPosition(), 0, 0)); + sWaypoints.add(new Waypoint(getBlueGearTurnPosition(), kSmallRadius, kSpeed)); + sWaypoints.add(new Waypoint(getBlueHopperTurnPosition(), kModerateRadius, kSpeed)); + sWaypoints.add(new Waypoint(getBlueHopperPosition(), kSmallRadius, kSpeed)); + + Translation2d blueHopperEndPosition = new Translation2d(getBlueHopperPosition()); + blueHopperEndPosition.setX(kEndHopperPathX); // move x position to desired place + blueHopperEndPosition.setY(blueHopperEndPosition.y() + kHopperSkew); + sWaypoints.add(new Waypoint(blueHopperEndPosition, 0, kSpeed)); + + sBlueHopperPath = PathBuilder.buildPathFromWaypoints(sWaypoints); + } + return sBlueHopperPath; + } + + public static void calculatePaths() { + getBlueHopperPath(); + getRedHopperPath(); + getBlueGearPath(); + getRedGearPath(); + } + + public static void main(String[] args) { + System.out.println("Red:\n" + getRedStartPose().getTranslation()); + System.out.println("Center: " + getRedCenterPosition()); + System.out.println("Gear: " + getRedGearPositionCorrected()); + System.out.println("Gear turn: " + getRedGearTurnPosition()); + System.out.println("Hopper turn: " + getRedHopperTurnPosition()); + System.out.println("Hopper: " + getRedHopperPosition()); + System.out.println("Start to boiler gear path:\n" + getRedGearPath()); + System.out.println("Boiler gear to hopper path:\n" + getRedHopperPath()); + System.out.println("\nBlue:\n" + getBlueStartPose().getTranslation()); + System.out.println("Center: " + getBlueCenterPosition()); + System.out.println("Gear: " + getBlueGearPositionCorrected()); + System.out.println("Gear turn: " + getBlueGearTurnPosition()); + System.out.println("Hopper turn: " + getBlueHopperTurnPosition()); + System.out.println("Hopper: " + getBlueHopperPosition()); + System.out.println("Start to boiler gear path:\n" + getBlueGearPath()); + System.out.println("Boiler gear to hopper path:\n" + getBlueHopperPath()); + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeBot.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeBot.java new file mode 100644 index 0000000..9ce98b7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeBot.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4388.paths.profiles; + +/** + * Contains the corrective values for Practice bot + */ +public class PracticeBot implements RobotProfile { + + @Override + public double getRedBoilerGearXCorrection() { + return 2.5; + } + + @Override + public double getRedBoilerGearYCorrection() { + return 7.0; + } + + @Override + public double getRedHopperXOffset() { + return 0.0; + } + + @Override + public double getRedHopperYOffset() { + return -6.0; + } + + @Override + public double getBlueBoilerGearXCorrection() { + return 2.5; + } + + @Override + public double getBlueBoilerGearYCorrection() { + return -1.0; + } + + @Override + public double getBlueHopperXOffset() { + return 0.0; + } + + @Override + public double getBlueHopperYOffset() { + return -4.0; + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeField.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeField.java new file mode 100644 index 0000000..64a8a20 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeField.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4388.paths.profiles; + +/** + * Contains the measurements for the practice field at the 254 lab + */ +public class PracticeField implements FieldProfile { + + @Override + public double getRedCenterToBoiler() { + return 127.5; + } + + @Override + public double getRedWallToAirship() { + return 116.5; + } + + @Override + public double getRedCenterToHopper() { + return 160.66; + } + + @Override + public double getRedWallToHopper() { + return 108.0; + } + + @Override + public double getBlueCenterToBoiler() { + return 125.5; + } + + @Override + public double getBlueWallToAirship() { + return 114.0; + } + + @Override + public double getBlueCenterToHopper() { + return 161.0; + } + + @Override + public double getBlueWallToHopper() { + return 110.0; + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/RobotProfile.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/RobotProfile.java new file mode 100644 index 0000000..6a434ee --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/RobotProfile.java @@ -0,0 +1,28 @@ +package org.usfirst.frc4388.paths.profiles; + +/** + * Interface that holds all the corrective values for how each robot actually drives. + * + * @see PathAdapter + */ +public interface RobotProfile { + + // red + public double getRedBoilerGearXCorrection(); + + public double getRedBoilerGearYCorrection(); + + public double getRedHopperXOffset(); + + public double getRedHopperYOffset(); + + // blue + public double getBlueBoilerGearXCorrection(); + + public double getBlueBoilerGearYCorrection(); + + public double getBlueHopperXOffset(); + + public double getBlueHopperYOffset(); + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/CenterTest.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/CenterTest.java new file mode 100644 index 0000000..56819b3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/CenterTest.java @@ -0,0 +1,36 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class CenterTest implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(0, 0, 0, 0)); + sWaypoints.add(new Waypoint(-20, 0, 0, 60)); + sWaypoints.add(new Waypoint(-40, 0, 20, 60)); + sWaypoints.add(new Waypoint(-60, 20, 20, 60)); + sWaypoints.add(new Waypoint(-120, 20, 0, 60)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(0, 0), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurn.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurn.java new file mode 100644 index 0000000..3bc9387 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurn.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class LeftTurn implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(20, 60, 0, 0)); + sWaypoints.add(new Waypoint(60, 60, 0, 20)); + sWaypoints.add(new Waypoint(60, 100, 0, 20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } + // WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":0,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}] + // IS_REVERSED: false + // FILE_NAME: LeftTurn +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadius.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadius.java new file mode 100644 index 0000000..904944d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadius.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class LeftTurnRadius implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(20, 60, 0, 0)); + sWaypoints.add(new Waypoint(60, 60, 30, 20)); + sWaypoints.add(new Waypoint(60, 100, 0, 20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(0.0)); + } + + @Override + public boolean isReversed() { + return false; + } + // WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":30,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}] + // IS_REVERSED: false + // FILE_NAME: LeftTurnRadius +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusReversed.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusReversed.java new file mode 100644 index 0000000..0624acd --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusReversed.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class LeftTurnRadiusReversed implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(20, 60, 0, 0)); + sWaypoints.add(new Waypoint(60, 60, 30, 20)); + sWaypoints.add(new Waypoint(60, 100, 0, 20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } + // WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":30,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}] + // IS_REVERSED: true + // FILE_NAME: LeftTurnRadiusReversed +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusStart90.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusStart90.java new file mode 100644 index 0000000..aeb459d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusStart90.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class LeftTurnRadiusStart90 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(60, 100, 0, 0)); + sWaypoints.add(new Waypoint(60, 60, 30, 20)); + sWaypoints.add(new Waypoint(100, 60, 0, 20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(60, 100), Rotation2d.fromDegrees(-90.0)); + } + + @Override + public boolean isReversed() { + return false; + } + // WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":30,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}] + // IS_REVERSED: false + // FILE_NAME: LeftTurnRadius +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnReversed.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnReversed.java new file mode 100644 index 0000000..8d12959 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnReversed.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class LeftTurnReversed implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(20, 60, 0, 0)); + sWaypoints.add(new Waypoint(60, 60, 0, 20)); + sWaypoints.add(new Waypoint(60, 100, 0, 20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } + // WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":0,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}] + // IS_REVERSED: true + // FILE_NAME: UntitledPath +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/RightTurnRadiusStart90.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/RightTurnRadiusStart90.java new file mode 100644 index 0000000..1fa3372 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/RightTurnRadiusStart90.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class RightTurnRadiusStart90 implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint(60,100,0,0)); + sWaypoints.add(new Waypoint(60,60,30,20)); + sWaypoints.add(new Waypoint(20,60,0,20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(60, 100), Rotation2d.fromDegrees(-90.0)); + } + + @Override + public boolean isReversed() { + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/paths/test/SCurveReversed.java b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/SCurveReversed.java new file mode 100644 index 0000000..cf2235f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/paths/test/SCurveReversed.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.paths.test; + +import java.util.ArrayList; + +import org.usfirst.frc4388.paths.PathBuilder; +import org.usfirst.frc4388.paths.PathBuilder.Waypoint; +import org.usfirst.frc4388.paths.PathContainer; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; + +public class SCurveReversed implements PathContainer { + + @Override + public Path buildPath() { + ArrayList sWaypoints = new ArrayList(); + sWaypoints.add(new Waypoint( 20, 0, 0, 0)); + sWaypoints.add(new Waypoint( 60, 0, 20, 20)); + sWaypoints.add(new Waypoint(100, 15, 20, 20)); + sWaypoints.add(new Waypoint(130, 15, 0, 20)); + + return PathBuilder.buildPathFromWaypoints(sWaypoints); + } + + @Override + public RigidTransform2d getStartPose() { + return new RigidTransform2d(new Translation2d(20, 0), Rotation2d.fromDegrees(180.0)); + } + + @Override + public boolean isReversed() { + return true; + } + // WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":20,"radius":20,"comment":""},{"position":{"x":100,"y":75},"speed":20,"radius":20,"comment":""},{"position":{"x":130,"y":75},"speed":20,"radius":0,"comment":""}] + // IS_REVERSED: true + // FILE_NAME: SCurveReversed +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java index 6319a55..97e3217 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -43,6 +43,136 @@ public class Constants { public static int kWristEncoderticksPerRev = 4096; public static double kWristDegreesOfTravel = 360; public static double kWristEncoderTicksPerDegree = 11.38; + + + + + + + + + + + + + + + + + /* ROBOT PHYSICAL CONSTANTS */ + + // Wheels + + // Drive constants + + // Geometry + public static double kCenterToFrontBumperDistance = 16.33; + public static double kCenterToIntakeDistance = 23.11; + public static double kCenterToRearBumperDistance = 16.99; + public static double kCenterToSideBumperDistance = 17.225; + + /* CONTROL LOOP GAINS */ + + // PID gains for drive velocity loop (HIGH GEAR) + // Units: setpoint, error, and output are in inches per second. + public static double kDriveHighGearVelocityKp = 0.2; //1.0; + public static double kDriveHighGearVelocityKi = 0.0; + public static double kDriveHighGearVelocityKd = 0.04;//0.04; + public static double kDriveHighGearVelocityKf = 0.07;//0.07; + public static int kDriveHighGearVelocityIZone = 200; + public static double kDriveHighGearVelocityRampRate = 0.05; // 0.02 + public static double kDriveHighGearNominalOutput = 0.5/12.0; + public static double kDriveHighGearMaxSetpoint = 17.0 * 12.0; // 17 fps + + // PID gains for drive velocity loop (LOW GEAR) + // Units: setpoint, error, and output are in inches per second. + public static double kDriveLowGearVelocityKp = 0.1; //.1 + public static double kDriveLowGearVelocityKi = 0.0; + public static double kDriveLowGearVelocityKd = 0.04; // .04 + public static double kDriveLowGearVelocityKf = 0.03; // .07 + public static int kDriveLowGearVelocityIZone = 200; + public static double kDriveLowGearVelocityRampRate = 0.02; + public static double kDriveLowGearNominalOutput = 0.1/12.0; + public static double kDriveLowGearMaxSetpoint = 10.0 * 12.0; // 8 fps + + // PID gains for drive velocity loop (LOW GEAR) + // Units: setpoint, error, and output are in inches per second. + public static double kDriveLowGearPositionKp = 1.0; // 1.0 + public static double kDriveLowGearPositionKi = 0.002; //0.002 + public static double kDriveLowGearPositionKd = 0.04; + public static double kDriveLowGearPositionKf = .45; // 0.45 + public static int kDriveLowGearPositionIZone = 700; + public static double kDriveLowGearPositionRampRate = 0.05; // V/s + public static double kDriveLowGearMaxVelocity = 8.0 * 12.0 * 60.0 / (Math.PI * kDriveWheelDiameterInches); // 8 fps + // in RPM + public static double kDriveLowGearMaxAccel = 18.0 * 12.0 * 60.0 / (Math.PI * kDriveWheelDiameterInches); // 18 fps/s + // in RPM/s + + public static double kDriveVoltageCompensationRampRate = 0.0; + + // Turn to heading gains + public static double kDriveTurnKp = 3.0; + public static double kDriveTurnKi = 1.5; + public static double kDriveTurnKv = 0.0; + public static double kDriveTurnKffv = 1.0; + public static double kDriveTurnKffa = 0.0; + public static double kDriveTurnMaxVel = 360.0; + public static double kDriveTurnMaxAcc = 720.0; + + // Path following constants + public static double kMinLookAhead = 12.0; // inches + public static double kMinLookAheadSpeed = 9.0; // inches per second + public static double kMaxLookAhead = 24.0; // inches + public static double kMaxLookAheadSpeed = 120.0; // inches per second + public static double kDeltaLookAhead = kMaxLookAhead - kMinLookAhead; + public static double kDeltaLookAheadSpeed = kMaxLookAheadSpeed - kMinLookAheadSpeed; + + public static double kInertiaSteeringGain = 0.0; // angular velocity command is multiplied by this gain * + // our speed + // in inches per sec +// public static double kSegmentCompletionTolerance = 0.1; // inches +// public static double kPathFollowingMaxAccel = 120.0; // inches per second^2 +// public static double kPathFollowingMaxVel = 120.0; // inches per second +// public static double kPathFollowingProfileKp = 0.05; //5.0 +// public static double kPathFollowingProfileKi = 0.03; +// public static double kPathFollowingProfileKv = 0.02; +// public static double kPathFollowingProfileKffv = 1.0; +// public static double kPathFollowingProfileKffa = 0.05; +// public static double kPathFollowingGoalPosTolerance = 0.75; +// public static double kPathFollowingGoalVelTolerance = 12.0; +// public static double kPathStopSteeringDistance = 9.0; + + public static double kSegmentCompletionTolerance = 0.1; // inches + public static double kPathFollowingMaxAccel = 90.0; // inches per second^2 + public static double kPathFollowingMaxVel = 150.0; // inches per second + public static double kPathFollowingProfileKp = 5.0; //5.0 + public static double kPathFollowingProfileKi = 0.03; // 0.03 + public static double kPathFollowingProfileKv = 0.02; //0.02 + public static double kPathFollowingProfileKffv = 1.2; //1.2 + public static double kPathFollowingProfileKffa = 0.05; //0.05 + public static double kPathFollowingGoalPosTolerance = 0.75; + public static double kPathFollowingGoalVelTolerance = 12.0; + public static double kPathStopSteeringDistance = 9.0; + + + + + + + + + + + + + + + + + + + + // CONTROL LOOP GAINS @@ -75,7 +205,6 @@ public class Constants { // Path following constants public static double kPathFollowingLookahead = 24.0; // inches - public static double kPathFollowingMaxVel = 120.0; // inches/sec - public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2 + } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 3d01ef0..348c90a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -51,11 +51,11 @@ public class OI climbUp.whenReleased(new InitiateClimber(false)); JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); - shiftUp.whenPressed(new DriveSpeedShift(true)); + //shiftUp.whenPressed(new DriveSpeedShift()); // shiftUp.whenPressed(new LEDIndicators(true)); JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - shiftDown.whenPressed(new DriveSpeedShift(false)); + // shiftDown.whenPressed(new DriveSpeedShift(false)); // shiftDown.whenPressed(new LEDIndicators(false)); //Wrist @@ -63,8 +63,7 @@ public class OI wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); //Arm - JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON); - ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID)); + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 8ed83e7..e681d58 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -1,115 +1,151 @@ package org.usfirst.frc4388.robot; -import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.CameraServer; +import java.util.HashMap; +//import org.usfirst.frc4388.controller.Robot.OperationMode; +import org.usfirst.frc4388.robot.commands.ArmAutoZero;// +import org.usfirst.frc4388.robot.Robot.OperationMode; +import org.usfirst.frc4388.robot.subsystems.Arm; +import org.usfirst.frc4388.robot.subsystems.Climber; +import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.robot.subsystems.Pnumatics; +import org.usfirst.frc4388.robot.subsystems.Wrist; +//import org.usfirst.frc4388.utility.ControlLooper; +import org.usfirst.frc4388.utility.Looper; +import org.usfirst.frc4388.utility.control.RobotStateEstimator; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.control.RobotState; + import edu.wpi.first.wpilibj.DriverStation; //import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;; -//import org.usfirst.frc4388.controller.Robot.OperationMode; -import org.usfirst.frc4388.robot.commands.*; - -import org.usfirst.frc4388.robot.OI; -import org.usfirst.frc4388.robot.subsystems.*; -import org.usfirst.frc4388.utility.ControlLooper; -import org.usfirst.frc4388.robot.subsystems.Drive; -import org.usfirst.frc4388.robot.subsystems.Arm; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;; - -public class Robot extends IterativeRobot +public class Robot extends TimedRobot { public static OI oi; + + public static final Drive drive = Drive.getInstance(); + public static final Arm arm = Arm.getInstance(); - public static final Drive drive = new Drive(); - public static final Arm arm = new Arm(); - public static final Climber climber = new Climber(); + + public static final Climber climber = new Climber(); public static final Pnumatics pnumatics = new Pnumatics(); public static final Wrist wrist = new Wrist(); public static final long periodMS = 10; - public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS); - - public static enum OperationMode { TEST, COMPETITION }; - public static OperationMode operationMode = OperationMode.TEST; + // public static final ControlLooper controlLoop = new ControlLooper("Main + // control loop", periodMS); + public static final Looper controlLoop = new Looper(); private SendableChooser operationModeChooser; + + private Command autonomousCommand; + public static enum OperationMode { TEST, PRACTICE, COMPETITION }; + public static OperationMode operationMode = OperationMode.COMPETITION; + + + private RobotState robotState =/* org.usfirst.frc4388.utility.control.*/RobotState.getInstance(); + + public void zeroAllSensors() { + //drive.zeroSensors(); + robotState.reset(Timer.getFPGATimestamp(), new RigidTransform2d()); + //drive.zeroSensors(); + } + + + + + + @Override public void robotInit() { - //Printing game data to riolog - String gameData = DriverStation.getInstance().getGameSpecificMessage(); - System.out.println(gameData); - CameraServer.getInstance().startAutomaticCapture(); - CameraServer.getInstance().putVideo("res", 300, 220); - try { + setPeriod(Constants.kLooperDt * 2); + System.out.println("Main loop period = " + getPeriod()); oi = OI.getInstance(); - - controlLoop.addLoopable(drive); - //controlLoop.addLoopable(arm); - - - operationModeChooser = new SendableChooser(); - operationModeChooser.addDefault("Test", OperationMode.TEST); - operationModeChooser.addObject("Competition", OperationMode.COMPETITION); + controlLoop.register(drive); + controlLoop.register(arm); + controlLoop.register(RobotStateEstimator.getInstance()); + + operationModeChooser = new SendableChooser(); + operationModeChooser.addDefault("Competition", OperationMode.COMPETITION); + operationModeChooser.addObject("Test", OperationMode.TEST); SmartDashboard.putData("Operation Mode", operationModeChooser); - - - - //ledLights.setAllLightsOn(false); - } - catch (Exception e) - { - System.err.println("An error occurred in robotInit()"); - } - } - - public void disabledInit(){ + LiveWindow.setEnabled(false); + zeroAllSensors(); } + private void setPeriod(double d) { + } + + public void robotPeriodic() { + updateStatus(); + } + + @Override + public void disabledInit() { + drive.setLimeLED(false); + } + @Override public void disabledPeriodic() { Scheduler.getInstance().run(); updateStatus(); } - - public void autonomousInit() { + @Override + public void autonomousInit() { + controlLoop.start(); + drive.setIsRed(getAlliance().equals(Alliance.Red)); + arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES); updateChoosers(); - controlLoop.start(); drive.endGyroCalibration(); drive.resetEncoders(); drive.resetGyro(); - drive.setIsRed(getAlliance().equals(Alliance.Red)); + drive.setIsRed(getAlliance().equals(Alliance.Red)); + if (autonomousCommand != null) { + autonomousCommand.start(); + } } - + @Override public void autonomousPeriodic() { Scheduler.getInstance().run(); updateStatus(); } - + @Override public void teleopInit() { - drive.setToBrakeOnNeutral(false); - updateChoosers(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + + operationMode = operationModeChooser.getSelected(); + controlLoop.start(); - drive.resetEncoders(); - drive.endGyroCalibration(); + //drive.setShiftState(DriveSpeedShiftState.LO); + drive.endGyroCalibration(); + zeroAllSensors(); + + if (operationMode != OperationMode.COMPETITION) { + Scheduler.getInstance().add(new ArmAutoZero(false)); + } + else { + arm.setPositionPID(arm.getPositionInches()); + } + } + //Arm.setArmControlMode(Arm.ArmControlMode.JOYSTICK_MANUAL); - updateStatus(); - } - - + @Override public void teleopPeriodic() { Scheduler.getInstance().run(); @@ -129,11 +165,16 @@ public class Robot extends IterativeRobot { return m_ds.getAlliance(); } - + public double getMatchTime() { + return m_ds.getMatchTime(); + } + public void updateStatus() { drive.updateStatus(operationMode); - arm.updateStatus(operationMode); + arm.updateStatus(operationMode); + robotState.updateStatus(operationMode); + } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index d13fe44..efa00b7 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -7,6 +7,8 @@ public class RobotMap { public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; + public static final int pilot = 0; + public static final int copilot = 1; // MOTORS @@ -24,8 +26,8 @@ public class RobotMap { //Arm Motors public static final int ARM_MOTOR1_ID = 8; public static final int ARM_MOTOR2_ID = 9; - public static final int CLIMBER_CAN_ID = 10; - public static final int FLIP_OUT_CLIMBER = 11; + public static final int CLIMBER_LEGS_CAN_ID = 10; + public static final int FLIPPY_BOIS_CAN_ID = 11; public static final int CLIMBER_LEFT = 12; public static final int CLIMBER_RIGHT = 13; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java new file mode 100644 index 0000000..4e29397 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java @@ -0,0 +1,64 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm; + +import edu.wpi.first.wpilibj.command.Command; + +public class ArmAutoZero extends Command +{ + private double MIN_ELEVATOR_POSITION_CHANGE = 0.05; + private double lastElevatorPosition; + private int encoderCount; + + public ArmAutoZero(boolean interrutible) { + + requires(Robot.arm); + setInterruptible(interrutible); + } + + @Override + protected void initialize() { + lastElevatorPosition = Arm.MAX_POSITION_INCHES; + Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED); + encoderCount = 0; +// System.out.println("Auto zero initialize"); + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED); + double currentElevatorPosition = Robot.arm.getPositionInches(); + double elevatorPositionChange = lastElevatorPosition - currentElevatorPosition; + lastElevatorPosition = currentElevatorPosition; + boolean test = encoderCount > 2 && Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE && Robot.arm.getAverageMotorCurrent() > Arm.AUTO_ZERO_MOTOR_CURRENT; + System.out.println("encoderCount = " + encoderCount + ", test = " + test + ", elevator change = " + elevatorPositionChange + ", current = " + Robot.arm.getAverageMotorCurrent()); + + if (Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) { + encoderCount++; + } + else { + encoderCount = 0; + } + + return test; + } + + @Override + protected void end() { + Robot.arm.setSpeed(0); + Robot.arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES); + Robot.arm.setPositionPID(Arm.MIN_POSITION_INCHES); +// System.out.println("Elevator Zeroed"); + } + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java index b8bf31a..6b1c02e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -1,13 +1,14 @@ + + package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; -import java.lang.Math; import edu.wpi.first.wpilibj.command.Command; /** - * Description + * */ public class ArmSetMode extends Command { @@ -20,11 +21,14 @@ public class ArmSetMode extends Command { // Called just before this Command runs the first time protected void initialize() { - if (controlMode == ArmControlMode.PID) { - Robot.wrist.setPositionPID(Robot.arm.getYPositionInches()); + if (controlMode == ArmControlMode.JOYSTICK_PID) { + Robot.arm.setPositionPID(Robot.arm.getPositionInches()); } else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { - Robot.wrist.setSpeedJoystick(0); + Robot.arm.setSpeedJoystick(0); + } + else { + Robot.arm.setSpeed(0.0); } } @@ -45,4 +49,4 @@ public class ArmSetMode extends Command { // subsystems is scheduled to run protected void interrupted() { } -} +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java index 5c33de3..e1d4fba 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java @@ -28,7 +28,7 @@ public class ArmSetPositionPID extends Command { // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return Math.abs(Robot.arm.getYPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES; + return Math.abs(Robot.arm.getPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES; } // Called once after isFinished returns true diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java index 21e328a..060669f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java @@ -2,26 +2,23 @@ package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.*; - import edu.wpi.first.wpilibj.command.Command; /** * */ public class ArmSetSpeed extends Command { - - private double RaiseSpeed; - // Constructor with speed - public ArmSetSpeed(double RaiseSpeed) { - this.RaiseSpeed = RaiseSpeed; - // requires(Robot.elevatorAuton); + private double speed; + + public ArmSetSpeed(double speed) { + this.speed = speed; + requires(Robot.arm); } // Called just before this Command runs the first time protected void initialize() { - //Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed); + Robot.arm.setSpeed(speed); } // Called repeatedly when this Command is scheduled to run diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java deleted file mode 100644 index 5c25540..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveAbsoluteTurnMP extends Command -{ - private double absoluteTurnAngleDeg, maxTurnRateDegPerSec; - private MPSoftwareTurnType turnType; - - public DriveAbsoluteTurnMP(double absoluteTurnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { - requires(Robot.drive); - this.absoluteTurnAngleDeg = absoluteTurnAngleDeg; - this.maxTurnRateDegPerSec = maxTurnRateDegPerSec; - this.turnType = turnType; - } - - protected void initialize() { -// if (Robot.drive.isRed() == false) { -// absoluteTurnAngleDeg = absoluteTurnAngleDeg * -1; -// } - Robot.drive.setAbsoluteTurnMP(absoluteTurnAngleDeg, maxTurnRateDegPerSec, turnType); - } - - protected void execute() { - } - - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java deleted file mode 100644 index 9d2d3e4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveGyroReset extends Command -{ - public DriveGyroReset() { - requires(Robot.drive); - } - - @Override - protected void initialize() { - Robot.drive.resetGyro(); - Robot.drive.resetEncoders(); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java deleted file mode 100644 index 0bc270b..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveRelativeTurnPID extends Command -{ - private double relativeTurnAngleDeg; - private MPSoftwareTurnType turnType; - - public DriveRelativeTurnPID(double relativeTurnAngleDeg, MPSoftwareTurnType turnType) { - requires(Robot.drive); - this.relativeTurnAngleDeg = relativeTurnAngleDeg; - this.turnType = turnType; - } - - protected void initialize() { - Robot.drive.setRelativeTurnPID(relativeTurnAngleDeg, 0.3, 0.1, turnType); - } - - protected void execute() { - } - - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java deleted file mode 100644 index d3e8293..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveSpeedShift extends Command -{ - public boolean speed; - - public DriveSpeedShift(boolean speed) { - this.speed=speed; - requires(Robot.pnumatics); - } - - @Override - protected void initialize() { - Robot.pnumatics.setShiftState(speed); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java deleted file mode 100644 index a477dc7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java +++ /dev/null @@ -1,103 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class DriveStraightBasic extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; - private double m_speed; - private boolean m_goingBackwards; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - - public DriveStraightBasic(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - m_goingBackwards = (m_targetInches < 0.0); - } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.drive.resetGyro(); - Robot.drive.resetEncoders(); - Robot.drive.setControlMode(DriveControlMode.RAW); - // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); - m_commandInitTimestamp = Timer.getFPGATimestamp(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - if (m_useGyroLock) { - steer = - Robot.drive.getGyroAngleDeg() / 25.0; //TODO: tune - } - Robot.drive.rawMoveSteer(m_speed, steer); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); - if (remaining < 0.0) { - finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } - if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); - } else { - SmartDashboard.putNumber("DSB finDist", position); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.drive.rawMoveSteer(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java deleted file mode 100644 index 21fede7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java +++ /dev/null @@ -1,115 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class DriveStraightBasicAbs extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; - private double m_speed; - private boolean m_goingBackwards; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - private double m_targetGyroAngleDeg; // what we want the gyro to read as we're driving straight - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - - public DriveStraightBasicAbs(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - m_goingBackwards = (m_targetInches < 0.0); - } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } - - // Called just before this Command runs the first time - protected void initialize() { - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - if (m_useAbsolute) { - m_targetGyroAngleDeg = m_desiredAbsoluteAngle; - } else { - m_targetGyroAngleDeg = currentAngleDeg; - } - Robot.drive.resetEncoders(); - Robot.drive.setControlMode(DriveControlMode.RAW); - // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); - m_commandInitTimestamp = Timer.getFPGATimestamp(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - if (m_useGyroLock) { - double yawError = Robot.drive.getGyroAngleDeg() - m_targetGyroAngleDeg; - steer = -yawError / Constants.kDriveStraightBasicYawErrorDivisor; - if (steer < -Constants.kDriveStraightBasicMaxSteerMagnitude) { - steer = -Constants.kDriveStraightBasicMaxSteerMagnitude; - } else if (Constants.kDriveStraightBasicMaxSteerMagnitude < steer) { - steer = Constants.kDriveStraightBasicMaxSteerMagnitude; - } - } - Robot.drive.rawMoveSteer(m_speed, steer); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); - if (remaining < 0.0) { - finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } - if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); - } else { - SmartDashboard.putNumber("DSB finDist", position); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.drive.rawMoveSteer(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java deleted file mode 100644 index e205818..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java +++ /dev/null @@ -1,60 +0,0 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// Java from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - - -public class DriveStraightMP extends Command { - private double m_distanceInches; - private double m_maxVelocityInchesPerSec; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - - public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_distanceInches = distanceInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - // Called once after isFinished returns true - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java deleted file mode 100644 index 82223d9..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java +++ /dev/null @@ -1,151 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.BHRMathUtils; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveTurnBasic extends Command -{ - private boolean m_useAbsolute; // if true, we don't reset the gyro before starting the turn - private double m_turnAngleDeg; // either absolute or relative to current heading, depending on useAbsolute - private double m_maxTurnRateDegPerSec; - private MPSoftwareTurnType m_turnType; - private boolean m_turningLeft; - private double m_targetGyroAngleDeg; // what we want the gyro to read when we're done - - public DriveTurnBasic(boolean useAbsolute, double turnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { - requires(Robot.drive); - m_useAbsolute = useAbsolute; - m_turnAngleDeg = turnAngleDeg; - m_maxTurnRateDegPerSec = maxTurnRateDegPerSec; - m_turnType = turnType; - } - - protected void initialize() { - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - if (m_useAbsolute) { - m_targetGyroAngleDeg = m_turnAngleDeg; - } else { - m_targetGyroAngleDeg = currentAngleDeg + m_turnAngleDeg; - } - if ((m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.RIGHT_ARC)) { - m_turningLeft = true; // gyro angle will be DEcreasing, so make sure we aim for an angle in that direction - while (m_targetGyroAngleDeg >= currentAngleDeg) { - m_targetGyroAngleDeg = m_targetGyroAngleDeg - 360.0; - } - } else if ((m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.LEFT_ARC)) { - m_turningLeft = false; // gyro angle will be INcreasing, so make sure we aim for an angle in that direction - while (m_targetGyroAngleDeg <= currentAngleDeg) { - m_targetGyroAngleDeg = m_targetGyroAngleDeg + 360.0; - } - } else { // MPSoftwareTurnType.TANK -- need to determine based on values passed - if (m_useAbsolute) { // this is the only case where we have to DECIDE which direction to turn - m_turnAngleDeg = BHRMathUtils.normalizeAngle0To360(m_turnAngleDeg); - m_targetGyroAngleDeg = BHRMathUtils.adjustAccumAngleToDesired(currentAngleDeg, m_turnAngleDeg); - m_turningLeft = (m_targetGyroAngleDeg < currentAngleDeg); - } else { - m_turningLeft = (m_turnAngleDeg < 0); - } - } - System.out.println("Turning " + (m_turningLeft?"left":"right") + " from " + currentAngleDeg + " to " + m_targetGyroAngleDeg + " degrees"); - Robot.drive.setControlMode(DriveControlMode.RAW); - Robot.drive.resetEncoders(); - } - - protected void execute() { - double output = Constants.kDriveTurnBasicSingleMotorOutput; - - if (m_turnType == MPSoftwareTurnType.TANK) { - output = Constants.kDriveTurnBasicTankMotorOutput; - if (m_turningLeft) { - Robot.drive.rawDriveLeftRight(-output, -output); // left backward, right forward - } else { - Robot.drive.rawDriveLeftRight(output, output); // left forward, right backward - } -// for (CANTalonEncoder motorController : motorControllers) { -// //motorController.set(output); -// motorController.set(ControlMode.PercentOutput, output); -// } - } - else if (m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - Robot.drive.rawDriveLeftRight(2.0 * output, 0.0); // left forward double speed -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(0); -// motorController.set(ControlMode.PercentOutput, 0); -// } -// else { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - Robot.drive.rawDriveLeftRight(0.0, -2.0 * output); // right forward double speed -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// else { -// //motorController.set(0); -// motorController.set(ControlMode.PercentOutput, 0); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.LEFT_ARC) { - Robot.drive.rawDriveLeftRight(2.0 * output, -output); // left fwd 2x, right fwd 1x -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(1.0 * output); -// motorController.set(ControlMode.PercentOutput, 1.0 * output); -// } -// else { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.RIGHT_ARC) { - Robot.drive.rawDriveLeftRight(output, -2.0 * output); // left fwd 1x, right fwd 2x -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// else { -// //motorController.set(1.0 * output); -// motorController.set(ControlMode.PercentOutput, 1.0 * output); -// } -// } - } - } - - protected boolean isFinished() { - boolean finished; - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - - if (m_turningLeft) { - finished = currentAngleDeg <= m_targetGyroAngleDeg; - } else { - finished = currentAngleDeg >= m_targetGyroAngleDeg; - } - return finished; - } - - protected void end() { - Robot.drive.rawDriveLeftRight(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index a8bedac..4b992c4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -1,27 +1,20 @@ package org.usfirst.frc4388.robot.subsystems; - import java.util.ArrayList; -import java.lang.Math; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.utility.CANTalonEncoder; import org.usfirst.frc4388.utility.Loop; import org.usfirst.frc4388.utility.MPTalonPIDController; import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.TalonSRXEncoder; +import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SensorCollection; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -29,37 +22,46 @@ public class Arm extends Subsystem implements Loop { private static Arm instance; - public static enum ArmControlMode {PID, JOYSTICK_MANUAL }; - public static final double DEGREE_START_OFFSET = 70; + public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; - public static final double RADIUS_OF_ARM = 43.3; - public static final double OFF_SET_BELOW = 49.3; - - // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks - public static final double ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3))-(DEGREE_START_OFFSET); - public static final double X_POSITION_MATH = ((RADIUS_OF_ARM)*(Math.cos(ENCODER_TICKS_TO_DEGREES)))+(OFF_SET_BELOW); - public static final double Y_POSITION_MATH = (RADIUS_OF_ARM)*(Math.sin(ENCODER_TICKS_TO_DEGREES)); + // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks + public static final double ENCODER_TICKS_TO_INCHES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI); - public double ARM_ANGLE_DEGREES = 0; - // Defined speeds + public static final double CLIMB_SPEED = -1.0; + public static final double TEST_SPEED_UP = 0.3; + public static final double TEST_SPEED_DOWN = -0.3; + public static final double AUTO_ZERO_SPEED = -0.3; public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; + public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; // Defined positions + public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; + public static final double ZERO_POSITION_INCHES = -0.25; + public static final double NEAR_ZERO_POSITION_INCHES = 0.0; public static final double MIN_POSITION_INCHES = 0.0; public static final double MAX_POSITION_INCHES = 83.4; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; + public static final double SWITCH_POSITION_INCHES = 24.0; + public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR + public static final double SCALE_LOW_POSITION_INCHES = 56.0; + public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0 + public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0; + public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES; + public static final double CLIMB_BAR_POSITION_INCHES = 70.0; + public static final double CLIMB_HIGH_POSITION_INCHES = 10.0; + public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0; - LimitSwitchSource limitSwitchSource; // Motion profile max velocities and accel times public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; + public static final double MP_T1 = 400; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 // Motor controllers private ArrayList motorControllers = new ArrayList(); - private CANTalonEncoder motor1; + private TalonSRXEncoder motor1; private TalonSRX motor2; // PID controller and params @@ -69,33 +71,45 @@ public class Arm extends Subsystem implements Loop public static int MP_SLOT = 1; private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); + private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); public static final double KF_UP = 0.005; public static final double KF_DOWN = 0.0; public static final double PID_ERROR_INCHES = 1.0; private long periodMs = (long)(Constants.kLooperDt * 1000.0); + // Pneumatics + private Solenoid speedShift; // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; + private ArmControlMode elevatorControlMode = ArmControlMode.JOYSTICK_MANUAL; private double targetPositionInchesPID = 0; private boolean firstMpPoint; + private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; - - public Arm() { + private Arm() { try { - motor1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); - //motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID); - + motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); + motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID); + + + motor1.setInverted(true); + motor2.setInverted(true); + +// if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { +// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); +// } + + motorControllers.add(motor1); + - motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); } catch (Exception e) { System.err.println("An error occurred in the DriveTrain constructor"); } } + @Override public void initDefaultCommand() { } @@ -104,35 +118,44 @@ public class Arm extends Subsystem implements Loop mpController.resetZeroPosition(position); } - private synchronized void setArmControlMode(ArmControlMode controlMode) { - this.armControlMode = controlMode; + private synchronized void setElevatorControlMode(ArmControlMode controlMode) { + this.elevatorControlMode = controlMode; } - private synchronized ArmControlMode getArmControlMode() { - return this.armControlMode; + private synchronized ArmControlMode getElevatorControlMode() { + return this.elevatorControlMode; } - - + public void setSpeed(double speed) { + motor1.set(ControlMode.PercentOutput, speed); + setElevatorControlMode(ArmControlMode.MANUAL); + } public void setSpeedJoystick(double speed) { motor1.set(ControlMode.PercentOutput, speed); - setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + setElevatorControlMode(ArmControlMode.JOYSTICK_MANUAL); } public void setPositionPID(double targetPositionInches) { mpController.setPIDSlot(PID_SLOT); updatePositionPID(targetPositionInches); - setArmControlMode(ArmControlMode.PID); + setElevatorControlMode(ArmControlMode.JOYSTICK_PID); setFinished(false); } public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = motor1.getPositionWorld()*Y_POSITION_MATH; + double startPositionInches = motor1.getPositionWorld(); mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); } - + + public void setPositionMP(double targetPositionInches) { + double startPositionInches = motor1.getPositionWorld(); + mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); + setFinished(false); + firstMpPoint = true; + setElevatorControlMode(ArmControlMode.MOTION_PROFILE); + } private double limitPosition(double targetPosition) { if (targetPosition < MIN_POSITION_INCHES) { @@ -147,7 +170,11 @@ public class Arm extends Subsystem implements Loop @Override public void onStart(double timestamp) { + mpController = new MPTalonPIDController(periodMs, motorControllers); + mpController.setPID(mpPIDParams, MP_SLOT); + mpController.setPID(pidPIDParamsHiGear, PID_SLOT); mpController.setPIDSlot(PID_SLOT); + mpController.setPID(pidPIDParamsHiGear, PID_SLOT); mpController.setPIDSlot(PID_SLOT); } @@ -160,43 +187,63 @@ public class Arm extends Subsystem implements Loop @Override public void onLoop(double timestamp) { synchronized (Arm.this) { - switch( getArmControlMode() ) { - case PID: + switch( getElevatorControlMode() ) { + case JOYSTICK_PID: controlPidWithJoystick(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); break; + case MOTION_PROFILE: + if (!isFinished()) { + if (firstMpPoint) { + mpController.setPIDSlot(MP_SLOT); + firstMpPoint = false; + } + setFinished(mpController.controlLoopUpdate()); + if (isFinished()) { + mpController.setPIDSlot(PID_SLOT); + } + } + break; default: break; - - } } } - - - private void controlPidWithJoystick() { - //double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - //double deltaPosition = joystickPosition *.5; - targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition; + double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick(joyStickSpeed); } - - - public double getYPositionInches() { - return motor1.getPositionWorld()*Y_POSITION_MATH; - + /* + public void setShiftState(ElevatorSpeedShiftState state) { + shiftState = state; + if(state == ElevatorSpeedShiftState.HI) { + joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI; + speedShift.set(true); + mpController.setPID(pidPIDParamsHiGear, PID_SLOT); + } + else if(state == ElevatorSpeedShiftState.LO) { + joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; + speedShift.set(false); + mpController.setPID(pidPIDParamsLoGear, PID_SLOT); + } } - public double getXPositionInches() { - return motor1.getPositionWorld()*X_POSITION_MATH; + + public ElevatorSpeedShiftState getShiftState() { + return shiftState; + } +*/ + public double getPositionInches() { + return motor1.getPositionWorld(); } // public double getAverageMotorCurrent() { @@ -204,7 +251,7 @@ public class Arm extends Subsystem implements Loop // } public double getAverageMotorCurrent() { - return (motor1.getOutputCurrent() + motor2.getOutputCurrent() / 2); + return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2; } public synchronized boolean isFinished() { @@ -223,7 +270,12 @@ public class Arm extends Subsystem implements Loop if (operationMode == Robot.OperationMode.TEST) { try { SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld()); + SmartDashboard.putNumber("Elevator Motor 1 Amps", motor1.getOutputCurrent()); + SmartDashboard.putNumber("Elevator Motor 2 Amps", motor2.getOutputCurrent()); SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); +// SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID)); +// SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID)); +// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); } catch (Exception e) { @@ -237,4 +289,4 @@ public class Arm extends Subsystem implements Loop } return instance; } -} +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java index 3cacf76..ab07ab6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -28,7 +28,7 @@ public class Climber extends Subsystem{ try{ - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + Climber = new WPI_TalonSRX(RobotMap.CLIMBER_LEGS_CAN_ID); } catch (Exception e) { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 3d29c64..058869a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -2,87 +2,75 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.ArrayList; -import java.util.Set; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +//import com.ctre.phoenix.sensors.PigeonIMU; +//import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; +import com.kauailabs.navx.frc.AHRS; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.OI; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.utility.AdaptivePurePursuitController; +import org.usfirst.frc4388.utility.BHRDifferentialDrive; import org.usfirst.frc4388.utility.BHRMathUtils; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.ControlLoopable; -import org.usfirst.frc4388.utility.Kinematics; -import org.usfirst.frc4388.utility.MMTalonPIDController; +import org.usfirst.frc4388.utility.Loop; import org.usfirst.frc4388.utility.MPSoftwarePIDController; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.utility.MPTalonPIDController; -import org.usfirst.frc4388.utility.MPTalonPIDPathController; -import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController; -//import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import org.usfirst.frc4388.utility.MotionProfilePoint; -//import org.usfirst.frc4388.utility.MotionProfileBoxCar; import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.Path; -import org.usfirst.frc4388.utility.PathGenerator; -import org.usfirst.frc4388.utility.RigidTransform2d; -import org.usfirst.frc4388.utility.Rotation2d; import org.usfirst.frc4388.utility.SoftwarePIDController; -import org.usfirst.frc4388.utility.Translation2d; +import org.usfirst.frc4388.utility.TalonSRXEncoder; +import org.usfirst.frc4388.utility.TalonSRXFactory; +import org.usfirst.frc4388.utility.control.Kinematics; +import org.usfirst.frc4388.utility.control.Lookahead; +import org.usfirst.frc4388.utility.control.Path; +import org.usfirst.frc4388.utility.control.PathFollower; +import org.usfirst.frc4388.utility.control.RobotState; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Twist2d; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -//import com.ctre.PigeonImu; -//import com.ctre.PigeonImu.CalibrationMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; - -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -//import edu.wpi.first.wpilibj.DigitalInput; -//import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -//import edu.wpi.first.wpilibj.Solenoid; -//import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.SPI; +/* +public class Drive extends Subsystem implements Loop { + private static Drive instance; + public static enum DriveControlMode { + JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, ADAPTIVE_PURSUIT, VELOCITY_SETPOINT, CAMERA_TRACK + }; -/** - * - */ -public class Drive extends Subsystem implements ControlLoopable -{ - public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW}; - public static enum SpeedShiftState { HI, LO }; - public static enum ClimberState { DEPLOYED, RETRACTED }; + public static enum DriveSpeedShiftState { HI, LO}; - public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches; - public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch; - - public static final double CLIMB_SPEED = 0.45; - - public static final double VOLTAGE_RAMP_RATE = 150; // Volts per second - public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full - public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full + public static enum ClimberState { + DEPLOYED, RETRACTED + }; + + // One revolution of the wheel = Pi * D inches = 60/24 revs due to gears * 36/12 + // revs due mag encoder gear on ball shifter * 4096 ticks + public static final double ENCODER_TICKS_TO_INCHES = (36.0 / 12.0) * (60.0 / 24.0) * 4096.0 / (5.8 * Math.PI); + public static final double TRACK_WIDTH_INCHES = 24.56; // 26.937; // Motion profile max velocities and accel times public static final double MAX_TURN_RATE_DEG_PER_SEC = 320; - public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 120; //72; - public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC = 200; - public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320; - public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400; - public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270; - public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC = 400; - public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25; - public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80; - - - - + public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 140; // 72; + public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320; + public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400; + public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270; + public static final double MP_SLOW_VELOCITY_INCHES_PER_SEC = 25; + public static final double MP_SLOW_MEDIUM_VELOCITY_INCHES_PER_SEC = 50; + public static final double MP_MEDIUM_VELOCITY_INCHES_PER_SEC = 80; + public static final double MP_FAST_VELOCITY_INCHES_PER_SEC = 100; + public static final double MP_STRAIGHT_T1 = 600; public static final double MP_STRAIGHT_T2 = 300; public static final double MP_TURN_T1 = 600; @@ -90,38 +78,32 @@ public class Drive extends Subsystem implements ControlLoopable public static final double MP_MAX_TURN_T1 = 400; public static final double MP_MAX_TURN_T2 = 200; + public static final double OPEN_LOOP_VOLTAGE_RAMP_HI = 0.0; + public static final double OPEN_LOOP_VOLTAGE_RAMP_LO = 0.1; + // Motor controllers - private ArrayList motorControllers = new ArrayList(); + private ArrayList motorControllers = new ArrayList(); - private CANTalonEncoder leftDrive1; - private WPI_TalonSRX leftDrive2; -// private WPI_TalonSRX leftDrive3; - - private CANTalonEncoder rightDrive1; - private WPI_TalonSRX rightDrive2; -// private WPI_TalonSRX rightDrive3; + private TalonSRXEncoder leftDrive1; + private TalonSRX leftDrive2; + private TalonSRX leftDrive3; - private DifferentialDrive m_drive; - - //PID encoder and motor - private CANTalonEncoder elevatorRight; - private WPI_TalonSRX elevatorLeft; + private TalonSRXEncoder rightDrive1; + private TalonSRX rightDrive2; + private TalonSRX rightDrive3; - //private DigitalInput hopperSensorRed; - //private DigitalInput hopperSensorBlue; - - - - private double climbSpeed; + private BHRDifferentialDrive m_drive; private boolean isRed = true; + private boolean mIsBrakeMode; - private double periodMs; - private double lastControlLoopUpdatePeriod = 0.0; - private double lastControlLoopUpdateTimestamp = 0.0; + private long periodMs = (long)(Constants.kLooperDt * 1000.0); + + protected Rotation2d mAngleAdjustment = Rotation2d.identity(); // Pneumatics - //private Solenoid speedShift; + private Solenoid speedShift; + private DriveSpeedShiftState shiftState = DriveSpeedShiftState.HI; // Input devices public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0; @@ -136,6 +118,14 @@ public class Drive extends Subsystem implements ControlLoopable public static final double MOVE_NON_LINEARITY = 1.0; public static final double STICK_DEADBAND = 0.02; + +// public static final double PITCH_THRESHOLD_1 = 20; + public static final double PITCH_THRESHOLD_2 = 25; + + private int pitchWindowSize = 5; + private int windowIndex = 0; + private double pitchSum = 0; + private double[] pitchAverageWindow = new double[pitchWindowSize]; private int m_moveNonLinear = 0; private int m_steerNonLinear = -3; @@ -153,156 +143,160 @@ public class Drive extends Subsystem implements ControlLoopable private double m_steerTrim = 0.0; private boolean isFinished; - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; + private DriveControlMode driveControlMode = DriveControlMode.JOYSTICK; - private MPTalonPIDController mpStraightController; - private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons -// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni + private static final int kLowGearPositionControlSlot = 0; + private static final int kHighGearVelocityControlSlot = 1; + private static final int kLowGearVelocityControlSlot = 2; + + private MPTalonPIDController mpStraightController; +// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.15); // 4 colsons + private PIDParams mpStraightPIDParams = new PIDParams(0.05, 0, 0, 0.0008, 0.004, 0.03); // 4 omni private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); - private MMTalonPIDController mmStraightController; - private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24); - private MPSoftwarePIDController mpTurnController; // p i d a v g izone - private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels -// private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni +// private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels + private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni private SoftwarePIDController pidTurnController; - //private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008 - private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008 - private double targetPIDAngle; + private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008 - private MPTalonPIDPathController mpPathController; - private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100); // 4 omni g=.3 - - //PID target - private double targetPIDPosition; - - private MPTalonPIDPathVelocityController mpPathVelocityController; - private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); - - private AdaptivePurePursuitController adaptivePursuitController; - private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); +// private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); + private PathFollower mPathFollower; + private Path mCurrentPath = null; + private RobotState mRobotState = RobotState.getInstance(); - private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); - private RigidTransform2d currentPose = zeroPose; - private RigidTransform2d lastPose = zeroPose; - private double left_encoder_prev_distance_ = 0; - private double right_encoder_prev_distance_ = 0; - - //private PigeonImu gyroPigeon; + //private PigeonIMU gyroPigeon; //private double[] yprPigeon = new double[3]; - private AHRS gyroNavX; + //private boolean useGyroLock; + //private double gyroLockAngleDeg; + // double kPGyro = 0.04; + //private boolean isCalibrating = false; + //private double gyroOffsetDeg = 0; + + private AHRS gyroNavX; private boolean useGyroLock; private double gyroLockAngleDeg; //private double kPGyro = 0.04; private double kPGyro = 0.0625; private boolean isCalibrating = false; private double gyroOffsetDeg = 0; + + private double mLastValidGyroAngle; + private double mCameraVelocity; + private double kCamera = 0.8; + + private double limeArea; + private double limeX; + private double limeY; + private double limeSkew; + private boolean isLimeValid; + private double LEDMode; + private double camMode; + /* + /** + * Check if the drive talons are configured for velocity control + //////////////////////////////////////// + protected static boolean usesTalonVelocityControl(DriveControlMode state) { + if (state == DriveControlMode.VELOCITY_SETPOINT || state == DriveControlMode.ADAPTIVE_PURSUIT || state == DriveControlMode.CAMERA_TRACK) { + return true; + } + return false; + } - public Drive() { + /** + * Check if the drive talons are configured for position control + ///////////////////////////////////////////////////// + protected static boolean usesTalonPositionControl(DriveControlMode state) { + if (state == DriveControlMode.MP_STRAIGHT || + state == DriveControlMode.MP_TURN || + state == DriveControlMode.HOLD) { + return true; + } + return false; + } + + private Drive() { try { - leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); - leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID); -// leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID); + leftDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); + leftDrive2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID); - rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); - rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); -// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); - - //gyroPigeon = new PigeonImu(leftDrive2); - gyroNavX = new AHRS(SPI.Port.kMXP); - //hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID); - //hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID); - - //leftDrive1.clearStickyFaults(); - //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); - //leftDrive1.setNominalClosedLoopVoltage(12.0); - leftDrive1.clearStickyFaults(0); - leftDrive1.setInverted(false);//false on comp 2108, false on microbot - leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot + rightDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); + rightDrive2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID); + + leftDrive1.setSafetyEnabled(false); - //leftDrive1.setCurrentLimit(15); - //leftDrive1.enableCurrentLimit(true); - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); - leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); - leftDrive1.configNominalOutputForward(+1.0f, 0); - leftDrive1.configNominalOutputReverse(-1.0f, 0); - + leftDrive1.setSensorPhase(false); -// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// Driver.reportError("Could not detect left drive encoder encoder!", false); -// } + leftDrive1.setInverted(true); + leftDrive2.setInverted(true); + leftDrive3.setInverted(true); - - leftDrive2.setInverted(false);//false on comp 2108, false on microbot - leftDrive2.setSafetyEnabled(false); - leftDrive2.setNeutralMode(NeutralMode.Brake); - leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); - - - - //rightDrive1.clearStickyFaults(); - //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); - //rightDrive1.setNominalClosedLoopVoltage(12.0); - rightDrive1.clearStickyFaults(0); - rightDrive1.setInverted(true);//true on comp 2108, false on microbot - rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot rightDrive1.setSafetyEnabled(false); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); - rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); - rightDrive1.configNominalOutputForward(+1.0f, 0); - rightDrive1.configNominalOutputReverse(-1.0f, 0); -// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// DriverStation.reportError("Could not detect right drive encoder encoder!", false); -// } + rightDrive1.setSensorPhase(false); - - rightDrive2.setInverted(true);//True on comp 2108, false on microbot - rightDrive2.setSafetyEnabled(false); - rightDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); - - - + rightDrive1.setInverted(false); + rightDrive2.setInverted(false); + rightDrive3.setInverted(false); motorControllers.add(leftDrive1); motorControllers.add(rightDrive1); - - //m_drive = new RobotDrive(leftDrive1, rightDrive1); - //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); - //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true); - //m_drive.setSafetyEnabled(false); - m_drive = new DifferentialDrive(leftDrive1, rightDrive1); - //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true); //TODO URGENT: verify - //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify - m_drive.setSafetyEnabled(false); - //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); - } + m_drive = new BHRDifferentialDrive(leftDrive1, rightDrive1); + m_drive.setSafetyEnabled(false); + + + System.out.println("this should be first"); + gyroNavX = new AHRS(SPI.Port.kMXP); +// gyroPigeon.clearStickyFaults(10); + + speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); + + loadGains(); + setBrakeMode(true); + } catch (Exception e) { System.err.println("An error occurred in the DriveTrain constructor"); } } + + private void setOpenLoopVoltageRamp(double timeTo12VSec) { + leftDrive1.configOpenloopRamp(timeTo12VSec, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configOpenloopRamp(timeTo12VSec, TalonSRXEncoder.TIMEOUT_MS); + } - public void setToBrakeOnNeutral(boolean brakeVsCoast) { - if (brakeVsCoast) { - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive2.setNeutralMode(NeutralMode.Brake); - } else { - leftDrive1.setNeutralMode(NeutralMode.Coast); - leftDrive2.setNeutralMode(NeutralMode.Coast); - rightDrive1.setNeutralMode(NeutralMode.Coast); - rightDrive2.setNeutralMode(NeutralMode.Coast); - } - } - - @Override + public synchronized void loadGains() { + leftDrive1.setPIDFIZone(kLowGearVelocityControlSlot, + Constants.kDriveLowGearVelocityKp, + Constants.kDriveLowGearVelocityKi, + Constants.kDriveLowGearVelocityKd, + Constants.kDriveLowGearVelocityKf, + Constants.kDriveLowGearVelocityIZone); + + rightDrive1.setPIDFIZone(kLowGearVelocityControlSlot, + Constants.kDriveLowGearVelocityKp, + Constants.kDriveLowGearVelocityKi, + Constants.kDriveLowGearVelocityKd, + Constants.kDriveLowGearVelocityKf, + Constants.kDriveLowGearVelocityIZone); + + leftDrive1.setPIDFIZone(kHighGearVelocityControlSlot, + Constants.kDriveHighGearVelocityKp, + Constants.kDriveHighGearVelocityKi, + Constants.kDriveHighGearVelocityKd, + Constants.kDriveHighGearVelocityKf, + Constants.kDriveHighGearVelocityIZone); + + rightDrive1.setPIDFIZone(kHighGearVelocityControlSlot, + Constants.kDriveHighGearVelocityKp, + Constants.kDriveHighGearVelocityKi, + Constants.kDriveHighGearVelocityKd, + Constants.kDriveHighGearVelocityKf, + Constants.kDriveHighGearVelocityIZone); + } + + @Override public void initDefaultCommand() { } @@ -311,26 +305,55 @@ public class Drive extends Subsystem implements ControlLoopable return getGyroNavXAngleDeg(); } - //public double getGyroPigeonAngleDeg() { - // gyroPigeon.GetYawPitchRoll(yprPigeon); - // return -yprPigeon[0] + gyroOffsetDeg; - //} - public double getGyroNavXAngleDeg() { return gyroNavX.getAngle() + gyroOffsetDeg; } + /* + public synchronized double getGyroPitchAngle() { + gyroPigeon.getYawPitchRoll(yprPigeon); + return yprPigeon[2]; + } - public void resetGyro() { - //gyroPigeon.SetYaw(0); - gyroNavX.zeroYaw(); + + +///////////////////////////////////////////////// /* + public boolean checkPitchAngle() { + double pitchAngle = Math.abs(getGyroPitchAngle()); + if(pitchAngle > 10) { + return true; + } + return false; + } + + ///////////////////////////////////////////////////////////// * / + + public synchronized void resetGyro() { + //gyroPigeon.SetYaw(0); + System.out.println("IF this works the screw the thing bellow"); + gyroNavX.zeroYaw(); } - public void resetEncoders() { - mpStraightController.resetZeroPosition(); + public synchronized Rotation2d getGyroAngle() { + return mAngleAdjustment.rotateBy(Rotation2d.fromDegrees(-getGyroAngleDeg())); + } + + public synchronized void setGyroAngle(Rotation2d adjustment) { + resetGyro(); + mAngleAdjustment = adjustment; + } + + public synchronized void resetEncoders() { + rightDrive1.setPosition(0); + leftDrive1.setPosition(0); } - public void calibrateGyro() { - //gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature); + public void zeroSensors() { + resetEncoders(); + resetGyro(); + } + + public void calibrateGyro() { + //gyroPigeon.enterCalibrationMode(CalibrationMode.Temperature, TalonSRXEncoder.TIMEOUT_MS); } public void endGyroCalibration() { @@ -342,55 +365,20 @@ public class Drive extends Subsystem implements ControlLoopable public void setGyroOffset(double offsetDeg) { gyroOffsetDeg = offsetDeg; } - - //public boolean isHopperSensorRedOn() { - // return hopperSensorRed.get(); - //} - - //public boolean isHopperSensorBlueOn() { - // return hopperSensorBlue.get(); - //} - - //public boolean isHopperSensorOn() { - // if (isRed() == true) { - // return isHopperSensorRedOn(); - // } - // else { - // return isHopperSensorBlueOn(); - // } - //} - - public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - mmStraightController.setPID(mmStraightPIDParams); - mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); - setControlMode(DriveControlMode.MOTION_MAGIC); - } - + public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - mpStraightController.setPID(mpStraightPIDParams); + mpStraightController.setPID(mpStraightPIDParams, kLowGearPositionControlSlot); + mpStraightController.setPIDSlot(kLowGearPositionControlSlot); mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); setControlMode(DriveControlMode.MP_STRAIGHT); } - - //public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - // double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - // mpStraightController.setPID(mpStraightPIDParams); - // mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); - // setControlMode(DriveControlMode.MP_STRAIGHT); - //} - + public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); setControlMode(DriveControlMode.MP_TURN); } - - //public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) { - // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); - // setControlMode(DriveControlMode.MP_TURN); - //} - + public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES); setControlMode(DriveControlMode.MP_TURN); @@ -401,39 +389,6 @@ public class Drive extends Subsystem implements ControlLoopable setControlMode(DriveControlMode.MP_TURN); } - //public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) { - // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); - // setControlMode(DriveControlMode.MP_TURN); - //} - - public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { - this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg(); - pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); - setControlMode(DriveControlMode.PID_TURN); - } - - public void setPathMP(PathGenerator path) { - mpPathController.setPID(mpPathPIDParams); - mpPathController.setMPPathTarget(path); - setControlMode(DriveControlMode.MP_PATH); - } - - public void setPathVelocityMP(PathGenerator path) { - mpPathVelocityController.setPID(mpPathPIDParams); - mpPathVelocityController.setMPPathTarget(path); - setControlMode(DriveControlMode.MP_PATH_VELOCITY); - } - - public void setPathAdaptivePursuit(Path path, boolean reversed) { - currentPose = zeroPose; - lastPose = zeroPose; - left_encoder_prev_distance_ = 0; - right_encoder_prev_distance_ = 0; - adaptivePursuitController.setPID(adaptivePursuitPIDParams); - adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); - setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); - } - public void setDriveHold(boolean status) { if (status) { setControlMode(DriveControlMode.HOLD); @@ -443,131 +398,88 @@ public class Drive extends Subsystem implements ControlLoopable } } - public void updatePose() { - double left_distance = leftDrive1.getPositionWorld(); - double right_distance = rightDrive1.getPositionWorld(); - Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); - lastPose = currentPose; - currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); - left_encoder_prev_distance_ = left_distance; - right_encoder_prev_distance_ = right_distance; - } - - public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) { - return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle); - } - - /** - * Path Markers are an optional functionality that name the various - * Waypoints in a Path with a String. This can make defining set locations - * much easier. - * - * @return Set of Strings with Path Markers that the robot has crossed. - */ - public synchronized Set getPathMarkersCrossed() { - if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) { - return null; - } else { - return adaptivePursuitController.getMarkersCrossed(); - } - } - public synchronized void setControlMode(DriveControlMode controlMode) { - this.controlMode = controlMode; - if (controlMode == DriveControlMode.JOYSTICK) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.MANUAL) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.CLIMB) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.HOLD) { - mpStraightController.setPID(mpHoldPIDParams); - //leftDrive1.changeControlMode(TalonControlMode.Position); - //leftDrive1.setPosition(0); - //leftDrive1.set(0); - //rightDrive1.changeControlMode(TalonControlMode.Position); - //rightDrive1.setPosition(0); - //rightDrive1.set(0); - leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + this.driveControlMode = controlMode; + if (controlMode == DriveControlMode.HOLD) { + mpStraightController.setPID(mpHoldPIDParams, kLowGearPositionControlSlot); + leftDrive1.setPosition(0); leftDrive1.set(ControlMode.Position, 0); - rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + rightDrive1.setPosition(0); rightDrive1.set(ControlMode.Position, 0); } - isFinished = false; + setFinished(false); } + + public synchronized DriveControlMode getControlMode() { + return driveControlMode; + } - public synchronized void controlLoopUpdate() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time - lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; - } - lastControlLoopUpdateTimestamp = currentTimestamp; + @Override + public void onStart(double timestamp) { + synchronized (Drive.this) { + mpStraightController = new MPTalonPIDController(periodMs, motorControllers); + mpStraightController.setPID(mpStraightPIDParams, kLowGearPositionControlSlot); + mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); + pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); + } + } + + @Override + public void onStop(double timestamp) { + // TODO Auto-generated method stub - // Do the update - if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { - driveWithJoystick(); - } - else if (!isFinished) { - if (controlMode == DriveControlMode.MP_STRAIGHT) { - isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); + } + + @Override + public void onLoop(double timestamp) { + synchronized (Drive.this) { + DriveControlMode currentControlMode = getControlMode(); + + if (currentControlMode == DriveControlMode.JOYSTICK) { + driveWithJoystick(); } - else if (controlMode == DriveControlMode.MP_TURN) { - isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); + else if (!isFinished()) { + switch (currentControlMode) { + case MP_STRAIGHT : + setFinished(mpStraightController.controlLoopUpdate(getGyroAngleDeg())); + break; + case MP_TURN: + setFinished(mpTurnController.controlLoopUpdate(getGyroAngleDeg())); + break; + case PID_TURN: + setFinished(pidTurnController.controlLoopUpdate(getGyroAngleDeg())); + break; + case ADAPTIVE_PURSUIT: + if (mPathFollower != null) { + updatePathFollower(timestamp); + } + return; + case CAMERA_TRACK: + updateCameraTrack(); + return; + default: + System.out.println("Unknown drive control mode: " + currentControlMode); + break; + } } - else if (controlMode == DriveControlMode.PID_TURN) { - isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_PATH) { - isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MOTION_MAGIC) { - isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) { - updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + else { + // hold in current state } } } - public void setSpeed(double speed) { + public synchronized void setSpeed(double speed) { if (speed == 0) { setControlMode(DriveControlMode.JOYSTICK); } else { setControlMode(DriveControlMode.MANUAL); - rightDrive1.set(-speed); - leftDrive1.set(speed); + rightDrive1.set(ControlMode.PercentOutput, speed); + leftDrive1.set(ControlMode.PercentOutput, speed); } } - public void setClimbSpeed(double climbSpeed) { - this.climbSpeed = climbSpeed; - if (climbSpeed == 0) { - setControlMode(DriveControlMode.JOYSTICK); - } - else { - setControlMode(DriveControlMode.CLIMB); - } - } - - public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) { + public synchronized void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) { if (snapToAbsolute0or180) { gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg()); } @@ -577,129 +489,249 @@ public class Drive extends Subsystem implements ControlLoopable this.useGyroLock = useGyroLock; } - public void driveWithJoystick() { + /** + * Called periodically when the robot is in cmaera track mode. + //////////////////////////////////////////////////////////* / + private void updateCameraTrack() { + updateLimelight(); + double deltaVelocity = 0; + if (isLimeValid) { + deltaVelocity = limeX * kCamera; + mLastValidGyroAngle = getGyroAngleDeg(); + System.out.println("Valid lime angle = " + limeX); + } else { + deltaVelocity = (getGyroAngleDeg() - mLastValidGyroAngle) * kCamera; + System.out.println("In Valid lime angle = " + limeX); + } + updateVelocitySetpoint(mCameraVelocity + deltaVelocity, mCameraVelocity - deltaVelocity); + } + + /** + * Configures the drivebase to drive a path. Used for autonomous driving + * + * @see Path + //////////////////////////////////////////////// * / + public synchronized void setCameraTrack(double straightVelocity) { + if (driveControlMode != DriveControlMode.CAMERA_TRACK) { + setFinished(false); + configureTalonsForSpeedControl(); + driveControlMode = DriveControlMode.CAMERA_TRACK; + mLastValidGyroAngle = getGyroAngleDeg(); + mCameraVelocity = straightVelocity; + } else { + setVelocitySetpoint(0, 0); + System.out.println("Oh NOOOO in velocity set point for camera track"); + } + } + + /** + * Called periodically when the robot is in path following mode. Updates the path follower with the robots latest + * pose, distance driven, and velocity, the updates the wheel velocity setpoints. + ///////////////////////////////////////////////////////// * / + private void updatePathFollower(double timestamp) { + RigidTransform2d robot_pose = mRobotState.getLatestFieldToVehicle().getValue(); + Twist2d command = mPathFollower.update(timestamp, robot_pose, + RobotState.getInstance().getDistanceDriven(), RobotState.getInstance().getPredictedVelocity().dx); + if (!mPathFollower.isFinished()) { + Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command); + updateVelocitySetpoint(setpoint.left, setpoint.right); + } else { + updateVelocitySetpoint(0, 0); + } + } + + /** + * Configures the drivebase to drive a path. Used for autonomous driving + * + * @see Path + //////////////////////////////////////////////////////////////* / + public synchronized void setWantDrivePath(Path path, boolean reversed) { + if (mCurrentPath != path || driveControlMode != DriveControlMode.ADAPTIVE_PURSUIT) { + configureTalonsForSpeedControl(); + RobotState.getInstance().resetDistanceDriven(); + mPathFollower = new PathFollower(path, reversed, + new PathFollower.Parameters( + new Lookahead(Constants.kMinLookAhead, Constants.kMaxLookAhead, + Constants.kMinLookAheadSpeed, Constants.kMaxLookAheadSpeed), + Constants.kInertiaSteeringGain, Constants.kPathFollowingProfileKp, + Constants.kPathFollowingProfileKi, Constants.kPathFollowingProfileKv, + Constants.kPathFollowingProfileKffv, Constants.kPathFollowingProfileKffa, + Constants.kPathFollowingMaxVel, Constants.kPathFollowingMaxAccel, + Constants.kPathFollowingGoalPosTolerance, Constants.kPathFollowingGoalVelTolerance, + Constants.kPathStopSteeringDistance)); + + driveControlMode = DriveControlMode.ADAPTIVE_PURSUIT; + mCurrentPath = path; + } else { + setVelocitySetpoint(0, 0); + System.out.println("Oh NOOOO in velocity set point"); + } + } + + /** + * Start up velocity mode. This sets the drive train in high gear as well. + * + * @param left_inches_per_sec + * @param right_inches_per_sec + //////////////////////////////////////////////////* / + public synchronized void setVelocitySetpoint(double left_inches_per_sec, double right_inches_per_sec) { + configureTalonsForSpeedControl(); + driveControlMode = DriveControlMode.VELOCITY_SETPOINT; + updateVelocitySetpoint(left_inches_per_sec, right_inches_per_sec); + } + + /** + * Adjust Velocity setpoint (if already in velocity mode) + * + * @param left_inches_per_sec + * @param right_inches_per_sec + ////////////////////////////////////////////////////////////////* / + private synchronized void updateVelocitySetpoint(double left_inches_per_sec, double right_inches_per_sec) { + if (usesTalonVelocityControl(driveControlMode)) { + final double max_desired = Math.max(Math.abs(left_inches_per_sec), Math.abs(right_inches_per_sec)); + final double maxSetpoint = getShiftState() == DriveSpeedShiftState.HI ? Constants.kDriveHighGearMaxSetpoint : Constants.kDriveLowGearMaxSetpoint; + final double scale = max_desired > maxSetpoint ? maxSetpoint / max_desired : 1.0; + + leftDrive1.setVelocityWorld(left_inches_per_sec * scale); + rightDrive1.setVelocityWorld(right_inches_per_sec * scale); +// double command = leftDrive1.convertEncoderWorldToTicks(left_inches_per_sec * scale) * 0.1; +// System.out.println("vel Com u/s = " + command + ", vel com in/sec= " + left_inches_per_sec * scale + ", scale = " + scale + ", left pos in = " + getLeftPositionInches() + ", right pos in = " + getRightPositionInches() + ", left vel in/sec = " + getLeftVelocityInchesPerSec() + ", left vel u/s = " + leftDrive1.getSelectedSensorVelocity(0)); + } else { + System.out.println("Hit a bad velocity control state"); + leftDrive1.set(ControlMode.Velocity, 0); + rightDrive1.set(ControlMode.Velocity, 0); + } + } + + /** + * Configures talons for velocity control + //////////////////////////////////////////////////////////////* / + public void configureTalonsForSpeedControl() { + if (!usesTalonVelocityControl(driveControlMode)) { + leftDrive1.enableVoltageCompensation(true); + leftDrive1.configVoltageCompSaturation(12.0, TalonSRXEncoder.TIMEOUT_MS); + leftDrive1.configPeakOutputForward(+1.0f, TalonSRXEncoder.TIMEOUT_MS); + leftDrive1.configPeakOutputReverse(-1.0f, TalonSRXEncoder.TIMEOUT_MS); + + rightDrive1.enableVoltageCompensation(true); + rightDrive1.configVoltageCompSaturation(12.0, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configPeakOutputForward(+1.0f, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configPeakOutputReverse(-1.0f, TalonSRXEncoder.TIMEOUT_MS); + + if (getShiftState() == DriveSpeedShiftState.HI) { + System.out.println("configureTalonsForSpeedControl HI"); + leftDrive1.selectProfileSlot(kHighGearVelocityControlSlot, TalonSRXEncoder.PID_IDX); + leftDrive1.configNominalOutputForward(Constants.kDriveHighGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + leftDrive1.configNominalOutputReverse(-Constants.kDriveHighGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + leftDrive1.configClosedloopRamp(Constants.kDriveHighGearVelocityRampRate, TalonSRXEncoder.TIMEOUT_MS); + + rightDrive1.selectProfileSlot(kHighGearVelocityControlSlot, TalonSRXEncoder.PID_IDX); + rightDrive1.configNominalOutputForward(Constants.kDriveHighGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configNominalOutputReverse(-Constants.kDriveHighGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configClosedloopRamp(Constants.kDriveHighGearVelocityRampRate, TalonSRXEncoder.TIMEOUT_MS); + } + else { + System.out.println("configureTalonsForSpeedControl LO"); + leftDrive1.selectProfileSlot(kLowGearVelocityControlSlot, TalonSRXEncoder.PID_IDX); + leftDrive1.configNominalOutputForward(Constants.kDriveLowGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + leftDrive1.configNominalOutputReverse(-Constants.kDriveLowGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + leftDrive1.configClosedloopRamp(Constants.kDriveLowGearVelocityRampRate, TalonSRXEncoder.TIMEOUT_MS); + + rightDrive1.selectProfileSlot(kLowGearVelocityControlSlot, TalonSRXEncoder.PID_IDX); + rightDrive1.configNominalOutputForward(Constants.kDriveLowGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configNominalOutputReverse(-Constants.kDriveLowGearNominalOutput, TalonSRXEncoder.TIMEOUT_MS); + rightDrive1.configClosedloopRamp(Constants.kDriveLowGearVelocityRampRate, TalonSRXEncoder.TIMEOUT_MS); + } + } + } + + public synchronized boolean isDoneWithPath() { + if (driveControlMode == DriveControlMode.ADAPTIVE_PURSUIT && mPathFollower != null) { + return mPathFollower.isFinished(); + } else { + System.out.println("Robot is not in path following mode 1"); + return true; + } + } + + public synchronized void forceDoneWithPath() { + if (driveControlMode == DriveControlMode.ADAPTIVE_PURSUIT && mPathFollower != null) { + mPathFollower.forceFinish(); + } else { + System.out.println("Robot is not in path following mode 2, control mode = " + driveControlMode); + } + } + + public synchronized boolean hasPassedMarker(String marker) { + if (driveControlMode == DriveControlMode.ADAPTIVE_PURSUIT && mPathFollower != null) { + return mPathFollower.hasPassedMarker(marker); + } else { + System.out.println("Robot is not in path following mode 3. Control mode = " + driveControlMode); + return false; + } + } + + public boolean isBrakeMode() { + return mIsBrakeMode; + } + + public synchronized void setBrakeMode(boolean on) { + if (mIsBrakeMode != on) { + mIsBrakeMode = on; + rightDrive1.setNeutralMode(NeutralMode.Brake); + rightDrive2.setNeutralMode(NeutralMode.Brake); + rightDrive3.setNeutralMode(NeutralMode.Brake); + leftDrive1.setNeutralMode(NeutralMode.Brake); + leftDrive2.setNeutralMode(NeutralMode.Brake); + leftDrive3.setNeutralMode(NeutralMode.Brake); + } + } + + public synchronized void driveWithJoystick() { if(m_drive == null) return; - // switch(m_controllerMode) { - // case CONTROLLER_JOYSTICK_ARCADE: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick1().getX(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_JOYSTICK_TANK: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick2().getY(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_drive.tankDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_JOYSTICK_CHEESY: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick2().getX(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_XBOX_CHEESY: - // boolean turbo = OI.getInstance().getDriveTrainController() - // .getLeftJoystickButton(); - // boolean slow = OI.getInstance().getDriveTrainController() - // .getRightJoystickButton(); - // double speedToUseMove, speedToUseSteer; - // if (turbo && !slow) { - // speedToUseMove = m_moveScaleTurbo; - // speedToUseSteer = m_steerScaleTurbo; - // } else if (!turbo && slow) { - // speedToUseMove = m_moveScaleSlow; - // speedToUseSteer = m_steerScaleSlow; - // } else { - // speedToUseMove = m_moveScale; - // speedToUseSteer = m_steerScale; - // } - // m_moveInput = - // OI.getInstance().getDriveTrainController().getLeftYAxis(); - // m_steerInput = - // OI.getInstance().getDriveTrainController().getRightXAxis(); - m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis(); - m_steerInput = OI.getInstance().getDriverController().getRightXAxis(); - - if (controlMode == DriveControlMode.JOYSTICK) { - m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + m_moveInput = OI.getInstance().getDriverController().getLeftYAxis(); + m_steerInput = -OI.getInstance().getDriverController().getRightXAxis(); + + m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - } - else if (controlMode == DriveControlMode.CLIMB) { - m_moveOutput = climbSpeed; - } - - if (useGyroLock) { // If currently in gyro lock mode, - if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning, - setGyroLock(false, false); // turn off gyro lock mode - } - } else { // If not yet in gyro lock mode, - if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning, - setGyroLock(true, false); // gyro lock to current heading - } - } - + m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + if (useGyroLock) { double yawError = gyroLockAngleDeg - getGyroAngleDeg(); m_steerOutput = kPGyro * yawError; - } else { - m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); } + /* + double pitchAngle = updatePitchWindow(); + if(Math.abs(pitchAngle) > PITCH_THRESHOLD_2) { + m_moveOutput = Math.signum(pitchAngle) * -1.0; + m_steerOutput = 0; + System.out.println("Pitch Treshhold 2 angle = " + pitchAngle); + } +/////////////////////////////////////////////////* / + m_drive.arcadeDrive(-m_moveOutput, -m_steerOutput); + } + + + + + /* + private double updatePitchWindow() { + double lastPitchAngle = pitchAverageWindow[windowIndex]; + double currentPitchAngle = getGyroPitchAngle(); + pitchAverageWindow[windowIndex] = currentPitchAngle; + pitchSum = pitchSum - lastPitchAngle + currentPitchAngle; + + windowIndex++; + if (windowIndex == pitchWindowSize) { + windowIndex = 0; + } - m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75); - // break; - // case CONTROLLER_XBOX_ARCADE_RIGHT: - // m_moveInput = - // OI.getInstance().getDrivetrainController().getRightYAxis(); - // m_steerInput = - // OI.getInstance().getDrivetrainController().getRightXAxis(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_XBOX_ARCADE_LEFT: - // m_moveInput = - // OI.getInstance().getDrivetrainController().getLeftYAxis(); - // m_steerInput = - // OI.getInstance().getDrivetrainController().getLeftXAxis(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // } + return pitchSum/pitchWindowSize; } - - public void rawMoveSteer(double move, double steer) { - m_drive.arcadeDrive(move, steer, false); - } - - public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { - - if (elevatorRight.getSelectedSensorPosition(0) >= 3550) { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5); - } - else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); - } - } - + //////////////////////////////////////////////////////* / + private boolean inDeadZone(double input) { boolean inDeadZone; if (Math.abs(input) < STICK_DEADBAND) { @@ -751,14 +783,22 @@ public class Drive extends Subsystem implements ControlLoopable / Math.asin(steerNonLinearity); } - //public void setShiftState(SpeedShiftState state) { - // if(state == SpeedShiftState.HI) { - // speedShift.set(true); - // } - // else if(state == SpeedShiftState.LO) { - // speedShift.set(false); - // } - //} + public void setShiftState(DriveSpeedShiftState state) { + shiftState = state; + + System.out.println("shift state = " + state); + setOpenLoopVoltageRamp(state == DriveSpeedShiftState.HI ? OPEN_LOOP_VOLTAGE_RAMP_HI : OPEN_LOOP_VOLTAGE_RAMP_LO); + if(state == DriveSpeedShiftState.HI) { + speedShift.set(false); + } + else if(state == DriveSpeedShiftState.LO) { + speedShift.set(true); + } + } + + public DriveSpeedShiftState getShiftState() { + return shiftState; + } public synchronized boolean isFinished() { return isFinished; @@ -768,19 +808,6 @@ public class Drive extends Subsystem implements ControlLoopable this.isFinished = isFinished; } - @Override - public void setPeriodMs(long periodMs) { - //mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers); - mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers); - mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); - pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); - mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers); - mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers); - adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers); - - this.periodMs = periodMs; - } - public double getPeriodMs() { return periodMs; } @@ -808,55 +835,98 @@ public class Drive extends Subsystem implements ControlLoopable public static double inchesPerSecondToRpm(double inches_per_second) { return inchesToRotations(inches_per_second) * 60; } + + public double getRightPositionInches() { + return rightDrive1.getPositionWorld(); + } - public double getLeftPositionWorld() { + public double getLeftPositionInches() { return leftDrive1.getPositionWorld(); } - public double getRightPositionWorld() { - return rightDrive1.getPositionWorld(); + public double getRightVelocityInchesPerSec() { + return rightDrive1.getVelocityWorld(); } + public double getLeftVelocityInchesPerSec() { + return leftDrive1.getVelocityWorld(); + } + + public double getAverageLeftCurrent() { + return (leftDrive1.getOutputCurrent() + leftDrive2.getOutputCurrent() + leftDrive3.getOutputCurrent()) / 3; + } + + public double getAverageRightCurrent() { + return (rightDrive1.getOutputCurrent() + rightDrive2.getOutputCurrent() + rightDrive3.getOutputCurrent()) / 3; + } + + public NetworkTable getLimetable() { + return NetworkTableInstance.getDefault().getTable("limelight"); + } + + private void updateLimelight() { + NetworkTable limeTable = getLimetable(); + + double valid = limeTable.getEntry("tv").getDouble(0); + if (valid == 0) { + isLimeValid = false; + } + else if (valid == 1) { + isLimeValid = true; + } + + limeX = limeTable.getEntry("tx").getDouble(0); + limeY = limeTable.getEntry("ty").getDouble(0); + limeArea = limeTable.getEntry("ta").getDouble(0); + limeSkew = limeTable.getEntry("ts").getDouble(0); + } + + //Set the LED mode of the limelight + public void setLimeLED(boolean isOn) { + getLimetable().getEntry("ledMode").setDouble(isOn ? 0 : 1); + } + + //Set the camera mode + public void setLimeCameraMode(boolean isOn) { + getLimetable().getEntry("camMode").setDouble(isOn ? 1 : 0); + } + public void updateStatus(Robot.OperationMode operationMode) { if (operationMode == Robot.OperationMode.TEST) { try { - SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); - SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld()); - SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld()); - SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12); - SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12); - //SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID)); - //SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID)); -// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID)); - //SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID)); - //SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID)); -// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID)); - //SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn()); - //SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn()); - SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); - //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); + SmartDashboard.putNumber("Drive Right Position Inches", rightDrive1.getPositionWorld()); + SmartDashboard.putNumber("Drive Left Position Inches", leftDrive1.getPositionWorld()); + SmartDashboard.putNumber("Drive Right Velocity InPerSec", rightDrive1.getVelocityWorld()); + SmartDashboard.putNumber("Drive Left Velocity InPerSec", leftDrive1.getVelocityWorld()); + SmartDashboard.putNumber("Drive Left 1 Amps", leftDrive1.getOutputCurrent()); + SmartDashboard.putNumber("Drive Left 2 Amps", leftDrive2.getOutputCurrent()); + SmartDashboard.putNumber("Drive Left 3 Amps", leftDrive3.getOutputCurrent()); + SmartDashboard.putNumber("Drive Left Average Amps", getAverageLeftCurrent()); + SmartDashboard.putNumber("Drive Right 1 Amps", rightDrive1.getOutputCurrent()); + SmartDashboard.putNumber("Drive Right 2 Amps", rightDrive2.getOutputCurrent()); + SmartDashboard.putNumber("Drive Right 3 Amps", rightDrive3.getOutputCurrent()); + SmartDashboard.putNumber("Drive Right Average Amps", getAverageRightCurrent()); SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg()); - MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); - double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; - SmartDashboard.putNumber("Gyro Delta", delta); - //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); - SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); - SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); - SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); - SmartDashboard.putNumber("Steer Output", m_steerOutput); - SmartDashboard.putNumber("Move Output", m_moveOutput); - SmartDashboard.putNumber("Steer Input", m_steerInput); - SmartDashboard.putNumber("Move Input", m_moveInput); + SmartDashboard.putData("Diff Drive", m_drive); + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTableEntry tx = table.getEntry("tx"); + NetworkTableEntry ty = table.getEntry("ty"); + NetworkTableEntry ta = table.getEntry("ta"); + SmartDashboard.putNumber("Limelight Valid", table.getEntry("tv").getDouble(0)); + SmartDashboard.putNumber("Limelight X", table.getEntry("tx").getDouble(0)); + SmartDashboard.putNumber("Limelight Y", table.getEntry("ty").getDouble(0)); + SmartDashboard.putNumber("Limelight Area", table.getEntry("ta").getDouble(0)); } catch (Exception e) { - System.err.println("Drivetrain update status error"); } } - else { - - } } + public static Drive getInstance() { + if(instance == null) { + instance = new Drive(); + } + return instance; + } + } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 4ee0897..c936377 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -70,7 +70,7 @@ public class Wrist extends Subsystem public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; - public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES; + public double armAngleDegrees = 90; public static final double targetAngleDegreesBallIn = -45; ///Change values public static final double targetAngleDegreesBallOut = 360; @@ -175,7 +175,7 @@ public class Wrist extends Subsystem public void onStart(double timestamp) { //mpController.setPID(mpPIDParams); - mpController.setPID(pidPIDParamsLevel); + // mpController.setPID(pidPIDParamsLevel); mpController.setPIDSlot(PID_SLOT); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java deleted file mode 100644 index c217c81..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java +++ /dev/null @@ -1,228 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; -import java.util.Optional; -import java.util.Set; - -import org.usfirst.frc4388.robot.Constants; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.Timer; - -/** - * Implements an adaptive pure pursuit controller. See: - * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 - * .pdf - * - * Basically, we find a spot on the path we'd like to follow and calculate the - * wheel speeds necessary to make us land on that spot. The target spot is a - * specified distance ahead of us, and we look further ahead the greater our - * tracking error. - */ -public class AdaptivePurePursuitController { - private static final double kEpsilon = 1E-9; - - double mFixedLookahead; - Path mPath; - RigidTransform2d.Delta mLastCommand; - double mLastTime; - double mMaxAccel; - double mDt; - boolean mReversed; - double mPathCompletionTolerance; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - //motorController.configNominalOutputVoltage(+0.0f, -0.0f); - //motorController.configPeakOutputVoltage(+12.0f, -12.0f); - //motorController.setProfile(0); - motorController.config_kP(0, pidParams.kP, 0); - motorController.config_kI(0, pidParams.kI, 0); - motorController.config_kD(0, pidParams.kD, 0); - motorController.config_kF(0, pidParams.kF, 0); - motorController.configNominalOutputForward(+0.0f, 0); - motorController.configNominalOutputReverse(-0.0f, 0); - motorController.configPeakOutputForward(+1.0f, 0); - motorController.configPeakOutputReverse(-1.0f, 0); - motorController.selectProfileSlot(0, 0); - } - } - - public void setPath(double fixed_lookahead, double max_accel, Path path, - boolean reversed, double path_completion_tolerance) { - mFixedLookahead = fixed_lookahead; - mMaxAccel = max_accel; - mPath = path; - mDt = periodMs; - mLastCommand = null; - mReversed = reversed; - mPathCompletionTolerance = path_completion_tolerance; - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Speed); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Velocity, 0); - } - } - - public boolean isDone() { - double remainingLength = mPath.getRemainingLength(); - return remainingLength <= mPathCompletionTolerance; - } - - public boolean controlLoopUpdate(RigidTransform2d robot_pose) { - RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp()); - Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command); - - // Scale the command to respect the max velocity limits - double max_vel = 0.0; - max_vel = Math.max(max_vel, Math.abs(setpoint.left)); - max_vel = Math.max(max_vel, Math.abs(setpoint.right)); - if (max_vel > Constants.kPathFollowingMaxVel) { - double scaling = Constants.kPathFollowingMaxVel / max_vel; - setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling); - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - motorController.setVelocityWorld(-setpoint.right); - } - else { - motorController.setVelocityWorld(-setpoint.left); - } - } - - return isDone(); - } - - public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) { - RigidTransform2d pose = robot_pose; - if (mReversed) { - pose = new RigidTransform2d(robot_pose.getTranslation(), - robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI))); - } - - double distance_from_path = mPath.update(robot_pose.getTranslation()); - if (this.isDone()) { - return new RigidTransform2d.Delta(0, 0, 0); - } - - PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(), - distance_from_path + mFixedLookahead); - Optional circle = joinPath(pose, lookahead_point.translation); - - double speed = lookahead_point.speed; - if (mReversed) { - speed *= -1; - } - // Ensure we don't accelerate too fast from the previous command - double dt = now - mLastTime; - if (mLastCommand == null) { - mLastCommand = new RigidTransform2d.Delta(0, 0, 0); - dt = mDt; - } - double accel = (speed - mLastCommand.dx) / dt; - if (accel < -mMaxAccel) { - speed = mLastCommand.dx - mMaxAccel * dt; - } else if (accel > mMaxAccel) { - speed = mLastCommand.dx + mMaxAccel * dt; - } - - // Ensure we slow down in time to stop - // vf^2 = v^2 + 2*a*d - // 0 = v^2 + 2*a*d - double remaining_distance = mPath.getRemainingLength(); - double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance); - if (Math.abs(speed) > max_allowed_speed) { - speed = max_allowed_speed * Math.signum(speed); - } - final double kMinSpeed = 4.0; - if (Math.abs(speed) < kMinSpeed) { - // Hack for dealing with problems tracking very low speeds with - // Talons - speed = kMinSpeed * Math.signum(speed); - } - - RigidTransform2d.Delta rv; - if (circle.isPresent()) { - rv = new RigidTransform2d.Delta(speed, 0, - (circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius); - } else { - rv = new RigidTransform2d.Delta(speed, 0, 0); - } - mLastTime = now; - mLastCommand = rv; - return rv; - } - - public Set getMarkersCrossed() { - return mPath.getMarkersCrossed(); - } - - public static class Circle { - public final Translation2d center; - public final double radius; - public final boolean turn_right; - - public Circle(Translation2d center, double radius, boolean turn_right) { - this.center = center; - this.radius = radius; - this.turn_right = turn_right; - } - } - - public static Optional joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) { - double x1 = robot_pose.getTranslation().getX(); - double y1 = robot_pose.getTranslation().getY(); - double x2 = lookahead_point.getX(); - double y2 = lookahead_point.getY(); - - Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point); - double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin() - - pose_to_lookahead.getY() * robot_pose.getRotation().cos(); - if (Math.abs(cross_product) < kEpsilon) { - return Optional.empty(); - } - - double dx = x1 - x2; - double dy = y1 - y2; - double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos(); - double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin(); - - double cross_term = mx * dx + my * dy; - - if (Math.abs(cross_term) < kEpsilon) { - // Points are colinear - return Optional.empty(); - } - - return Optional.of(new Circle( - new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term), - (-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)), - .5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0)); - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRDifferentialDrive.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRDifferentialDrive.java new file mode 100644 index 0000000..a4037e0 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRDifferentialDrive.java @@ -0,0 +1,382 @@ +package org.usfirst.frc4388.utility; + +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.RobotDriveBase; +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + +/** + * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive + * base, "tank drive", or West Coast Drive. + * + *

These drive bases typically have drop-center / skid-steer with two or more wheels per side + * (e.g., 6WD or 8WD). This class takes a SpeedController per side. For four and + * six motor drivetrains, construct and pass in {@link edu.wpi.first.wpilibj.SpeedControllerGroup} + * instances as follows. + * + *

Four motor drivetrain: + * 


+ * public class Robot {
+ *   Spark m_frontLeft = new Spark(1);
+ *   Spark m_rearLeft = new Spark(2);
+ *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
+ *
+ *   Spark m_frontRight = new Spark(3);
+ *   Spark m_rearRight = new Spark(4);
+ *   SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
+ *
+ *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ *
+ * + *

Six motor drivetrain: + * 


+ * public class Robot {
+ *   Spark m_frontLeft = new Spark(1);
+ *   Spark m_midLeft = new Spark(2);
+ *   Spark m_rearLeft = new Spark(3);
+ *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
+ *
+ *   Spark m_frontRight = new Spark(4);
+ *   Spark m_midRight = new Spark(5);
+ *   Spark m_rearRight = new Spark(6);
+ *   SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
+ *
+ *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ *
+ * + *

A differential drive robot has left and right wheels separated by an arbitrary width. + * + *

Drive base diagram: + * 

+ * |_______|
+ * | |   | |
+ *   |   |
+ * |_|___|_|
+ * |       |
+ *
+ * + *

Each drive() function provides different inverse kinematic relations for a differential drive + * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is + * usually unnecessary. + * + *

This library uses the NED axes convention (North-East-Down as external reference in the world + * frame): http://www.nuclearprojects.com/ins/images/axis_big.png. + * + *

The positive X axis points ahead, the positive Y axis points right, and the positive Z axis + * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is + * positive. + * + *

Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will + * be set to 0, and larger values will be scaled so that the full range is still used. This + * deadband value can be changed with {@link #setDeadband}. + * + *

RobotDrive porting guide: + *
{@link #tankDrive(double, double)} is equivalent to + * {@link edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used. + *
{@link #arcadeDrive(double, double)} is equivalent to + * {@link edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used + * and the the rotation input is inverted eg arcadeDrive(y, -rotation) + *
{@link #curvatureDrive(double, double, boolean)} is similar in concept to + * {@link edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn + * mode. However, it is not designed to give exactly the same response. + */ +public class BHRDifferentialDrive extends RobotDriveBase { + public static final double kDefaultQuickStopThreshold = 0.2; + public static final double kDefaultQuickStopAlpha = 0.1; + + private static int instances = 0; + + private SpeedController m_leftMotor; + private SpeedController m_rightMotor; + + private double m_quickStopThreshold = kDefaultQuickStopThreshold; + private double m_quickStopAlpha = kDefaultQuickStopAlpha; + private double m_quickStopAccumulator = 0.0; + private boolean m_reported = false; + + /** + * Construct a DifferentialDrive. + * + *

To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be + * inverted, do so before passing it in. + */ + public BHRDifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) { + m_leftMotor = leftMotor; + m_rightMotor = rightMotor; + addChild(m_leftMotor); + addChild(m_rightMotor); + instances++; + setName("DifferentialDrive", instances); + } + + /** + * Arcade drive method for differential drive platform. + * The calculated values will be squared to decrease sensitivity at low speeds. + * + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + */ + @SuppressWarnings("ParameterName") + public void arcadeDrive(double xSpeed, double zRotation) { + arcadeDrive(xSpeed, zRotation, true); + } + + /** + * Arcade drive method for differential drive platform. + * + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + * @param squaredInputs If set, decreases the input sensitivity at low speeds. + */ + @SuppressWarnings("ParameterName") + public void arcadeDrive(double xSpeed, double zRotation, boolean squaredInputs) { + if (!m_reported) { + HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_ArcadeStandard); + m_reported = true; + } + + xSpeed = limit(xSpeed); + xSpeed = applyDeadband(xSpeed, m_deadband); + + zRotation = limit(zRotation); + zRotation = applyDeadband(zRotation, m_deadband); + + // Square the inputs (while preserving the sign) to increase fine control + // while permitting full power. + if (squaredInputs) { + xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); + zRotation = Math.copySign(zRotation * zRotation, zRotation); + } + + double leftMotorOutput; + double rightMotorOutput; + + double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed); + + if (xSpeed >= 0.0) { + // First quadrant, else second quadrant + if (zRotation >= 0.0) { + leftMotorOutput = maxInput; + rightMotorOutput = xSpeed - zRotation; + } else { + leftMotorOutput = xSpeed + zRotation; + rightMotorOutput = maxInput; + } + } else { + // Third quadrant, else fourth quadrant + if (zRotation >= 0.0) { + leftMotorOutput = xSpeed + zRotation; + rightMotorOutput = maxInput; + } else { + leftMotorOutput = maxInput; + rightMotorOutput = xSpeed - zRotation; + } + } + + m_leftMotor.set(limit(leftMotorOutput) * m_maxOutput); + m_rightMotor.set(limit(rightMotorOutput) * m_maxOutput); + + // m_safetyHelper.feed(); + } + + /** + * Curvature drive method for differential drive platform. + * + *

The rotation argument controls the curvature of the robot's path rather than its rate of + * heading change. This makes the robot more controllable at high speeds. Also handles the + * robot's quick turn functionality - "quick turn" overrides constant-curvature turning for + * turn-in-place maneuvers. + * + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + * @param isQuickTurn If set, overrides constant-curvature turning for + * turn-in-place maneuvers. + */ + @SuppressWarnings("ParameterName") + public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) { + if (!m_reported) { + // HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_Curvature); + m_reported = true; + } + + xSpeed = limit(xSpeed); + xSpeed = applyDeadband(xSpeed, m_deadband); + + zRotation = limit(zRotation); + zRotation = applyDeadband(zRotation, m_deadband); + + double angularPower; + boolean overPower; + + if (isQuickTurn) { + if (Math.abs(xSpeed) < m_quickStopThreshold) { + m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator + + m_quickStopAlpha * limit(zRotation) * 2; + } + overPower = true; + angularPower = zRotation; + } else { + overPower = false; + angularPower = Math.abs(xSpeed) * zRotation - m_quickStopAccumulator; + + if (m_quickStopAccumulator > 1) { + m_quickStopAccumulator -= 1; + } else if (m_quickStopAccumulator < -1) { + m_quickStopAccumulator += 1; + } else { + m_quickStopAccumulator = 0.0; + } + } + + double leftMotorOutput = xSpeed + angularPower; + double rightMotorOutput = xSpeed - angularPower; + + // If rotation is overpowered, reduce both outputs to within acceptable range + if (overPower) { + if (leftMotorOutput > 1.0) { + rightMotorOutput -= leftMotorOutput - 1.0; + leftMotorOutput = 1.0; + } else if (rightMotorOutput > 1.0) { + leftMotorOutput -= rightMotorOutput - 1.0; + rightMotorOutput = 1.0; + } else if (leftMotorOutput < -1.0) { + rightMotorOutput -= leftMotorOutput + 1.0; + leftMotorOutput = -1.0; + } else if (rightMotorOutput < -1.0) { + leftMotorOutput -= rightMotorOutput + 1.0; + rightMotorOutput = -1.0; + } + } + + // Normalize the wheel speeds + double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput)); + if (maxMagnitude > 1.0) { + leftMotorOutput /= maxMagnitude; + rightMotorOutput /= maxMagnitude; + } + + m_leftMotor.set(leftMotorOutput * m_maxOutput); + m_rightMotor.set(rightMotorOutput * m_maxOutput); + + // m_safetyHelper.feed(); + } + + /** + * Tank drive method for differential drive platform. + * The calculated values will be squared to decrease sensitivity at low speeds. + * + * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is + * positive. + * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is + * positive. + */ + public void tankDrive(double leftSpeed, double rightSpeed) { + tankDrive(leftSpeed, rightSpeed, true); + } + + /** + * Tank drive method for differential drive platform. + * + * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is + * positive. + * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is + * positive. + * @param squaredInputs If set, decreases the input sensitivity at low speeds. + */ + public void tankDrive(double leftSpeed, double rightSpeed, boolean squaredInputs) { + if (!m_reported) { + HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_Tank); + m_reported = true; + } + + leftSpeed = limit(leftSpeed); + leftSpeed = applyDeadband(leftSpeed, m_deadband); + + rightSpeed = limit(rightSpeed); + rightSpeed = applyDeadband(rightSpeed, m_deadband); + + // Square the inputs (while preserving the sign) to increase fine control + // while permitting full power. + if (squaredInputs) { + leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed); + rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed); + } + + m_leftMotor.set(leftSpeed * m_maxOutput); + m_rightMotor.set(rightSpeed * m_maxOutput); + + // m_safetyHelper.feed(); + } + + /** + * Sets the QuickStop speed threshold in curvature drive. + * + *

QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn. + * + *

While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value + * outputted by the low-pass filter when the robot's speed along the X axis is below the + * threshold. When QuickTurn is disabled, the accumulator's value is applied against the computed + * angular power request to slow the robot's rotation. + * + * @param threshold X speed below which quick stop accumulator will receive rotation rate values + * [0..1.0]. + */ + public void setQuickStopThreshold(double threshold) { + m_quickStopThreshold = threshold; + } + + /** + * Sets the low-pass filter gain for QuickStop in curvature drive. + * + *

The low-pass filter filters incoming rotation rate commands to smooth out high frequency + * changes. + * + * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes. + * Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and + * above 2.0 are unstable. + */ + public void setQuickStopAlpha(double alpha) { + m_quickStopAlpha = alpha; + } + + @Override + public void stopMotor() { + m_leftMotor.stopMotor(); + m_rightMotor.stopMotor(); + // m_safetyHelper.feed(); + } + + @Override + public String getDescription() { + return "DifferentialDrive"; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("DifferentialDrive"); +// builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set); + builder.addDoubleProperty( + "Left Motor Speed", + () -> m_leftMotor.get(), + x -> m_leftMotor.set(x)); + builder.addDoubleProperty( + "Right Motor Speed", + () -> m_rightMotor.get(), + x -> m_rightMotor.set(x)); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java index 135bb97..65dfc5f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java @@ -6,6 +6,8 @@ import java.io.FileWriter; import java.io.IOException; import java.lang.reflect.Field; import java.math.BigDecimal; +import java.nio.file.Files; +import java.nio.file.StandardOpenOption; import java.util.ArrayList; import java.util.Collection; import java.util.HashMap; @@ -19,8 +21,8 @@ import org.json.simple.parser.ParseException; /** * ConstantsBase * - * Base class for storing robot constants. Anything stored as a public static - * field will be reflected and be able to set externally + * Base class for storing robot constants. Anything stored as a public static field will be reflected and be able to set + * externally */ public abstract class ConstantsBase { HashMap modifiedKeys = new HashMap(); @@ -54,6 +56,18 @@ public abstract class ConstantsBase { return new File(filePath); } + public boolean truncateUserConstants() { + try { + Files.write(getFile().toPath(), new byte[0], StandardOpenOption.TRUNCATE_EXISTING); + loadFromFile(); + return true; + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + return false; + } + } + public boolean setConstant(String name, Double value) { return setConstantRaw(name, value); } @@ -77,6 +91,9 @@ public abstract class ConstantsBase { success = true; if (!value.equals(current)) { modifiedKeys.put(name, true); + System.out.println("Constant Modified:" + field.getName()); + } else { + System.out.println("Constant Not Modified:" + field.getName()); } } catch (IllegalArgumentException | IllegalAccessException e) { System.out.println("Could not set field: " + name); @@ -116,7 +133,7 @@ public abstract class ConstantsBase { public Collection getConstants() { List constants = (List) getAllConstants(); - int stop = constants.size() - 1; + int stop = constants.size(); for (int i = 0; i < constants.size(); ++i) { Constant c = constants.get(i); if ("kEndEditableArea".equals(c.name)) { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java index ac7be21..7f8d7ec 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java @@ -4,6 +4,9 @@ import java.util.Timer; import java.util.TimerTask; import java.util.Vector; +import org.usfirst.frc4388.robot.subsystems.Arm; +import org.usfirst.frc4388.robot.subsystems.Drive; + /** * ControlLooper.java *

@@ -15,7 +18,7 @@ import java.util.Vector; * * @author Tom Bottiglieri */ - +/* public class ControlLooper { private Timer controlLoopTimer; @@ -72,8 +75,12 @@ public class ControlLooper } } - public void addLoopable(ControlLoopable c) { - loopables.addElement(c); - c.setPeriodMs(periodMs); + public void addLoopable(Drive drive) { + loopables.addElement(drive); + drive.setPeriodMs(periodMs); } -} \ No newline at end of file + + + public void register(Arm arm) { + } +}*/ \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java new file mode 100644 index 0000000..0e797d4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java @@ -0,0 +1,67 @@ +package org.usfirst.frc4388.utility; + +import java.io.FileWriter; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.Date; +import java.util.UUID; + +/** + * Tracks start-up and caught crash events, logging them to a file which dosn't roll over + */ +public class CrashTracker { + + private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID(); + + public static void logRobotStartup() { + logMarker("robot startup"); + } + + public static void logRobotConstruction() { + logMarker("robot startup"); + } + + public static void logRobotInit() { + logMarker("robot init"); + } + + public static void logTeleopInit() { + logMarker("teleop init"); + } + + public static void logAutoInit() { + logMarker("auto init"); + } + + public static void logDisabledInit() { + logMarker("disabled init"); + } + + public static void logThrowableCrash(Throwable throwable) { + logMarker("Exception", throwable); + } + + private static void logMarker(String mark) { + logMarker(mark, null); + } + + private static void logMarker(String mark, Throwable nullableException) { + + try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) { + writer.print(RUN_INSTANCE_UUID.toString()); + writer.print(", "); + writer.print(mark); + writer.print(", "); + writer.print(new Date().toString()); + + if (nullableException != null) { + writer.print(", "); + nullableException.printStackTrace(writer); + } + + writer.println(); + } catch (IOException e) { + e.printStackTrace(); + } + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java new file mode 100644 index 0000000..8f9b6e6 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java @@ -0,0 +1,19 @@ +package org.usfirst.frc4388.utility; + +/** + * Runnable class with reports all uncaught throws to CrashTracker + */ +public abstract class CrashTrackingRunnable implements Runnable { + + @Override + public final void run() { + try { + runCrashTracked(); + } catch (Throwable t) { + CrashTracker.logThrowableCrash(t); + throw t; + } + } + + public abstract void runCrashTracked(); +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java deleted file mode 100644 index e3c3910..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.usfirst.frc4388.utility; - -import org.usfirst.frc4388.robot.Constants; - -/** - * Provides forward and inverse kinematics equations for the robot modeling the - * wheelbase as a differential drive (with a corrective factor to account for - * the inherent skidding of the center 4 wheels quasi-kinematically). - */ - -public class Kinematics { - private static final double kEpsilon = 1E-9; - - /** - * Forward kinematics using only encoders, rotation is implicit (less - * accurate than below, but useful for predicting motion) - */ - public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta) { - double linear_velocity = (left_wheel_delta + right_wheel_delta) / 2; - double delta_v = (right_wheel_delta - left_wheel_delta) / 2; - double delta_rotation = delta_v * 2 * Constants.kTrackScrubFactor / Constants.kTrackEffectiveDiameter; - return new RigidTransform2d.Delta(linear_velocity, 0, delta_rotation); - } - - /** - * Forward kinematics using encoders and explicitly measured rotation (ex. - * from gyro) - */ - public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta, - double delta_rotation_rads) { - return new RigidTransform2d.Delta((left_wheel_delta + right_wheel_delta) / 2, 0, delta_rotation_rads); - } - - /** Append the result of forward kinematics to a previous pose. */ - public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta, - double right_wheel_delta, Rotation2d current_heading) { - RigidTransform2d.Delta with_gyro = forwardKinematics(left_wheel_delta, right_wheel_delta, - current_pose.getRotation().inverse().rotateBy(current_heading).getRadians()); - return current_pose.transformBy(RigidTransform2d.fromVelocity(with_gyro)); - } - - public static class DriveVelocity { - public final double left; - public final double right; - - public DriveVelocity(double left, double right) { - this.left = left; - this.right = right; - } - } - - public static DriveVelocity inverseKinematics(RigidTransform2d.Delta velocity) { - if (Math.abs(velocity.dtheta) < kEpsilon) { - return new DriveVelocity(velocity.dx, velocity.dx); - } - double delta_v = Constants.kTrackEffectiveDiameter * velocity.dtheta / (2 * Constants.kTrackScrubFactor); - return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java index 4e88da6..792bd07 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java @@ -11,4 +11,4 @@ public interface Loop { public void onLoop(double timestamp); public void onStop(double timestamp); -} \ No newline at end of file +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java new file mode 100644 index 0000000..823131f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java @@ -0,0 +1,89 @@ +package org.usfirst.frc4388.utility; + +import java.util.ArrayList; +import java.util.List; + +import org.usfirst.frc4388.robot.Constants; + +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This code runs all of the robot's loops. Loop objects are stored in a List object. They are started when the robot + * powers up and stopped after the match. + */ +public class Looper { + public final double kPeriod = Constants.kLooperDt; + + private boolean running_; + + private final Notifier notifier_; + private final List loops_; + private final Object taskRunningLock_ = new Object(); + private double timestamp_ = 0; + private double dt_ = 0; + + private final CrashTrackingRunnable runnable_ = new CrashTrackingRunnable() { + @Override + public void runCrashTracked() { + synchronized (taskRunningLock_) { + if (running_) { + double now = Timer.getFPGATimestamp(); + + for (Loop loop : loops_) { + loop.onLoop(now); + } + + dt_ = now - timestamp_; + timestamp_ = now; + } + } + } + }; + + public Looper() { + notifier_ = new Notifier(runnable_); + running_ = false; + loops_ = new ArrayList<>(); + } + + public synchronized void register(Loop loop) { + synchronized (taskRunningLock_) { + loops_.add(loop); + } + } + + public synchronized void start() { + if (!running_) { + System.out.println("Starting loops"); + synchronized (taskRunningLock_) { + timestamp_ = Timer.getFPGATimestamp(); + for (Loop loop : loops_) { + loop.onStart(timestamp_); + } + running_ = true; + } + notifier_.startPeriodic(kPeriod); + } + } + + public synchronized void stop() { + if (running_) { + System.out.println("Stopping loops"); + notifier_.stop(); + synchronized (taskRunningLock_) { + running_ = false; + timestamp_ = Timer.getFPGATimestamp(); + for (Loop loop : loops_) { + System.out.println("Stopping " + loop); + loop.onStop(timestamp_); + } + } + } + } + + public void outputToSmartDashboard() { + SmartDashboard.putNumber("looper_dt", dt_); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java index 28dced9..c3be5a6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java @@ -2,9 +2,9 @@ package org.usfirst.frc4388.utility; import java.util.ArrayList; -import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.subsystems.Drive; +//import com.ctre.CANTalon.TalonControlMode; import com.ctre.phoenix.motorcontrol.ControlMode; public class MMTalonPIDController @@ -12,7 +12,7 @@ public class MMTalonPIDController protected static enum MMControlMode { STRAIGHT, TURN }; public static enum MMTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY }; - protected ArrayList motorControllers; + protected ArrayList motorControllers; protected long periodMs; protected PIDParams pidParams; protected boolean useGyroLock; @@ -23,73 +23,54 @@ public class MMTalonPIDController protected MMTalonTurnType turnType; protected double targetValue; - public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) + public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) { this.motorControllers = motorControllers; this.periodMs = periodMs; setPID(pidParams); } - - private int convertInchesPerSecToTicksPer100ms(double inchesPerSec) { - return (int)Math.round(Constants.kDriveEncoderTicksPerInch * inchesPerSec / 10); - } public void setPID(PIDParams pidParams) { this.pidParams = pidParams; - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kF(0, pidParams.kF, 0); //TODO: verify want parameter slot 0, with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF); } } - public void setMMStraightTarget(double startValue, double targetValue, double maxVelocityInchesPerSec, double maxAccelerationInchesPerSecPerSec, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { + public void setMMStraightTarget(double startValue, double targetValue, double maxVelocity, double maxAcceleration, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { controlMode = MMControlMode.STRAIGHT; this.startGyroAngle = desiredAngle; this.useGyroLock = useGyroLock; this.targetValue = targetValue; // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (resetEncoder) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + motorController.setPosition(0); } - //motorController.setMotionMagicCruiseVelocity(maxVelocity); - //motorController.setMotionMagicAcceleration(maxAcceleration); - //motorController.set(targetValue); - //motorController.changeControlMode(TalonControlMode.MotionMagic); - motorController.configMotionCruiseVelocity(convertInchesPerSecToTicksPer100ms(maxVelocityInchesPerSec), 0); - motorController.configMotionAcceleration(convertInchesPerSecToTicksPer100ms(maxAccelerationInchesPerSecPerSec), 0); + motorController.configMotionCruiseVelocity((int)maxVelocity, TalonSRXEncoder.TIMEOUT_MS); + motorController.configMotionAcceleration((int)maxAcceleration, TalonSRXEncoder.TIMEOUT_MS); motorController.set(ControlMode.MotionMagic, targetValue); } } public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); + motorController.set(ControlMode.MotionMagic, targetValue); } } public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); } } public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPositionWorld(angle); } } @@ -120,14 +101,12 @@ public class MMTalonPIDController leftTarget = targetValue - deltaDistance; // Update the controllers with updated set points. - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(rightTarget); - motorController.set(motorController.getControlMode(), rightTarget); //TODO: change to explicit mode set? + motorController.set(ControlMode.MotionMagic, rightTarget); } else { - //motorController.set(leftTarget); - motorController.set(motorController.getControlMode(), leftTarget); //TODO: change to explicit mode set? + motorController.set(ControlMode.MotionMagic, leftTarget); } } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java index b91fc31..ca093b5 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java @@ -2,13 +2,14 @@ package org.usfirst.frc4388.utility; import java.util.ArrayList; +//import com.ctre.CANTalon.TalonControlMode; import com.ctre.phoenix.motorcontrol.ControlMode; public class MPSoftwarePIDController { public static enum MPSoftwareTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY, LEFT_ARC, RIGHT_ARC }; - protected ArrayList motorControllers; + protected ArrayList motorControllers; protected long periodMs; protected PIDParams pidParams; protected MotionProfileBoxCar mp; @@ -21,7 +22,7 @@ public class MPSoftwarePIDController protected double prevError = 0.0; // the prior error (used to compute velocity) protected double totalError = 0.0; // the sum of the errors for use in the integral calc - public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) + public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) { this.motorControllers = motorControllers; this.periodMs = periodMs; @@ -39,32 +40,23 @@ public class MPSoftwarePIDController this.useGyroLock = true; // Set up the motion profile - mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2); - // NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - + mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2); prevError = 0.0; totalError = 0.0; } - //public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) { - // this.turnType = turnType; - // this.useGyroLock = true; - // - // // Set up the motion profile - // mp = MotionProfileCache.getInstance().getMP(key); - // this.startGyroAngle = mp.getStartDistance(); - // this.targetGyroAngle = mp.getTargetDistance(); - // - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - // - // prevError = 0.0; - // totalError = 0.0; - //} + public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) { + this.turnType = turnType; + this.useGyroLock = true; + + // Set up the motion profile + mp = MotionProfileCache.getInstance().getMP(key); + this.startGyroAngle = mp.getStartDistance(); + this.targetGyroAngle = mp.getTargetDistance(); + + prevError = 0.0; + totalError = 0.0; + } public boolean controlLoopUpdate() { return controlLoopUpdate(0); @@ -93,55 +85,51 @@ public class MPSoftwarePIDController // Update the controllers set point. if (turnType == MPSoftwareTurnType.TANK) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.set(output); - motorController.set(ControlMode.PercentOutput, output); + for (TalonSRXEncoder motorController : motorControllers) { + if (motorController.isRight()) { + motorController.set(ControlMode.PercentOutput, -output); + } + else { + motorController.set(ControlMode.PercentOutput, output); + } } } else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(0); motorController.set(ControlMode.PercentOutput, 0); } else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); + motorController.set(ControlMode.PercentOutput, 2.0 * output); } } } else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(2.0 * output); motorController.set(ControlMode.PercentOutput, 2.0 * output); } else { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); + motorController.set(ControlMode.PercentOutput, 0); } } } else if (turnType == MPSoftwareTurnType.LEFT_ARC) { - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(1.0 * output); motorController.set(ControlMode.PercentOutput, 1.0 * output); } else { - //motorController.set(2.0 * output); motorController.set(ControlMode.PercentOutput, 2.0 * output); } } } else if (turnType == MPSoftwareTurnType.RIGHT_ARC) { - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(2.0 * output); motorController.set(ControlMode.PercentOutput, 2.0 * output); } else { - //motorController.set(1.0 * output); motorController.set(ControlMode.PercentOutput, 1.0 * output); } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java index 2f991a1..1461505 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java @@ -1,16 +1,15 @@ - package org.usfirst.frc4388.utility; import java.util.ArrayList; +//import com.ctre.CANTalon.TalonControlMode; import com.ctre.phoenix.motorcontrol.ControlMode; public class MPTalonPIDController { protected static enum MPControlMode { STRAIGHT, TURN }; public static enum MPTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY }; - - protected ArrayList motorControllers; + protected ArrayList motorControllers; protected long periodMs; protected PIDParams pidParams; protected MotionProfileBoxCar mp; @@ -23,32 +22,27 @@ public class MPTalonPIDController protected MPTalonTurnType turnType; protected int pidSlot; - public MPTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) + public MPTalonPIDController(long periodMs, ArrayList motorControllers) { this.motorControllers = motorControllers; this.periodMs = periodMs; - setPID(pidParams); } - public void setPID(PIDParams pidParams) { + public void setPID(PIDParams pidParams, int slot) { this.pidParams = pidParams; - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPID(slot, pidParams.kP, pidParams.kI, pidParams.kD); } } - public void setPIDSlot(int slot) - { + public void setPIDSlot(int slot) { this.pidSlot = slot; - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { motorController.selectProfileSlot(slot, 0); } } - + public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2) { setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, false); } @@ -64,32 +58,36 @@ public class MPTalonPIDController // Set up the motion profile mp = new MotionProfileBoxCar(startValue, targetValue, maxVelocity, periodMs, t1, t2); - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (resetEncoder) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + motorController.setPosition(0); } - //motorController.set(mp.getStartDistance()); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.set(ControlMode.Position, mp.getStartDistance()); + motorController.setWorld(ControlMode.Position, mp.getStartDistance()); } } - //public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - // controlMode = MPControlMode.STRAIGHT; - // this.startGyroAngle = desiredAngle; - // this.useGyroLock = useGyroLock; - // - // // Set up the motion profile - // mp = MotionProfileCache.getInstance().getMP(key); - // for (CANTalonEncoder motorController : motorControllers) { - // if (resetEncoder) { - // motorController.setPosition(0); - // } - // motorController.set(mp.getStartDistance()); - // motorController.changeControlMode(TalonControlMode.Position); - // } - //} + public double getStartPosition() { + return mp != null ? mp.getStartDistance() : 0; + } + + public double getTargetPosition() { + return mp != null ? mp.getTargetDistance() : 0; + } + + public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { + controlMode = MPControlMode.STRAIGHT; + this.startGyroAngle = desiredAngle; + this.useGyroLock = useGyroLock; + + // Set up the motion profile + mp = MotionProfileCache.getInstance().getMP(key); + for (TalonSRXEncoder motorController : motorControllers) { + if (resetEncoder) { + motorController.setPosition(0); + } + motorController.setWorld(ControlMode.Position, mp.getStartDistance()); + } + } public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPTalonTurnType turnType, double trackWidth) { controlMode = MPControlMode.TURN; @@ -102,11 +100,8 @@ public class MPTalonPIDController // Set up the motion profile mp = new MotionProfileBoxCar(0, trackDistance, maxTurnRateDegPerSec, periodMs, t1, t2); - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); motorController.set(ControlMode.Position, 0); } @@ -130,26 +125,21 @@ public class MPTalonPIDController } public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); motorController.set(ControlMode.Position, 0); } } public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); } } - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position + public void resetZeroPosition(double position) { + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPositionWorld(position); } } @@ -168,29 +158,24 @@ public class MPTalonPIDController // Calculate the motion profile feed forward and gyro feedback terms double KfLeft = 0.0; double KfRight = 0.0; - + // Update the set points and Kf gains if (controlMode == MPControlMode.STRAIGHT) { double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; if (Math.abs(mpPoint.position) > 0.001) { - //KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - //KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position; - - KfLeft = -(pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; + KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position; } // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); + motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS); + motorController.setWorld(ControlMode.Position, mpPoint.position); } else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); + motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS); + motorController.setWorld(ControlMode.Position, mpPoint.position); } } } @@ -203,31 +188,27 @@ public class MPTalonPIDController KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; } - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (turnType == MPTalonTurnType.TANK) { if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); + motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS); + motorController.setWorld(ControlMode.Position, -mpPoint.position); } else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); + motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS); + motorController.setWorld(ControlMode.Position, mpPoint.position); } } else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) { if (!motorController.isRight()) { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); + motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS); + motorController.setWorld(ControlMode.Position, mpPoint.position); } } else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) { if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); + motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS); + motorController.setWorld(ControlMode.Position, -mpPoint.position); } } } @@ -235,14 +216,13 @@ public class MPTalonPIDController return false; } - - public void setTarget(double position, double Kf) - { + + public void setTarget(double position, double Kf) { // Kf gets multipled by position in the Talon double KfPerPosition = Math.abs(position) > 0.001 ? Kf / position : 0; - for (CANTalonEncoder motorController : motorControllers) { - motorController.config_kF(0, KfPerPosition, CANTalonEncoder.TIMEOUT_MS); + for (TalonSRXEncoder motorController : motorControllers) { + motorController.config_kF(0, KfPerPosition, TalonSRXEncoder.TIMEOUT_MS); motorController.setWorld(ControlMode.Position, position); } } @@ -250,4 +230,4 @@ public class MPTalonPIDController public MotionProfilePoint getCurrentPoint() { return mpPoint; } -} +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java index b9e8fca..235bb91 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java @@ -2,21 +2,20 @@ package org.usfirst.frc4388.utility; import java.util.ArrayList; +//import com.ctre.CANTalon.TalonControlMode; import com.ctre.phoenix.motorcontrol.ControlMode; -import jaci.pathfinder.Trajectory.Segment; public class MPTalonPIDPathController { - protected ArrayList motorControllers; + protected ArrayList motorControllers; protected long periodMs; protected PIDParams pidParams; - protected PathGenerator path; protected double startGyroAngle; protected double targetGyroAngle; protected double trackDistance; - public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList motorControllers) + public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList motorControllers) { this.motorControllers = motorControllers; this.periodMs = periodMs; @@ -26,87 +25,49 @@ public class MPTalonPIDPathController public void setPID(PIDParams pidParams) { this.pidParams = pidParams; - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPID(0, pidParams.kP, pidParams.kI, pidParams.kD); } } - - public void setMPPathTarget(PathGenerator path) { - this.path = path; - path.resetCounter(); - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); motorController.set(ControlMode.Position, 0); } } public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); } } public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPositionWorld(angle); } } public boolean controlLoopUpdate(double currentGyroAngle) { - Segment leftPoint = path.getLeftPoint(); - Segment rightPoint = path.getRightPoint(); - - // Check if we are finished - if (leftPoint == null) { - return true; - } // Calculate the motion profile feed forward and gyro feedback terms double KfLeft = 0.0; double KfRight = 0.0; // Update the set points and Kf gains - double gyroDelta = -path.getHeading() - currentGyroAngle; - if (Math.abs(leftPoint.position) > 0.001) { - KfLeft = (pidParams.kA * leftPoint.acceleration + pidParams.kV * leftPoint.velocity + pidParams.kG * gyroDelta) / leftPoint.position; - KfRight = (pidParams.kA * rightPoint.acceleration + pidParams.kV * rightPoint.velocity - pidParams.kG * gyroDelta) / rightPoint.position; - } // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(rightPoint.position); + motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS); +// motorController.setWorld(ControlMode.Position, rightPoint.position); } else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(leftPoint.position); + motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS); } } - path.incrementCounter(); return false; } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java index 9863dc4..35b48cd 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java @@ -2,21 +2,21 @@ package org.usfirst.frc4388.utility; import java.util.ArrayList; +//import com.ctre.CANTalon.TalonControlMode; import com.ctre.phoenix.motorcontrol.ControlMode; -import jaci.pathfinder.Trajectory.Segment; public class MPTalonPIDPathVelocityController { - protected ArrayList motorControllers; + protected ArrayList motorControllers; protected long periodMs; protected PIDParams pidParams; - protected PathGenerator path; +// protected PathGenerator path; protected double startGyroAngle; protected double targetGyroAngle; protected double trackDistance; - public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList motorControllers) + public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList motorControllers) { this.motorControllers = motorControllers; this.periodMs = periodMs; @@ -26,86 +26,46 @@ public class MPTalonPIDPathVelocityController public void setPID(PIDParams pidParams) { this.pidParams = pidParams; - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - //motorController.configNominalOutputVoltage(+0.0f, -0.0f); - //motorController.configPeakOutputVoltage(+12.0f, -12.0f); - //motorController.setProfile(0); - motorController.config_kP(0, pidParams.kP, 0); - motorController.config_kI(0, pidParams.kI, 0); - motorController.config_kD(0, pidParams.kD, 0); - motorController.config_kF(0, pidParams.kF, 0); - motorController.configNominalOutputForward(+0.0f, 0); - motorController.configNominalOutputReverse(-0.0f, 0); - motorController.configPeakOutputForward(+1.0f, 0); - motorController.configPeakOutputReverse(-1.0f, 0); - motorController.selectProfileSlot(0, 0); + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF); + motorController.configVoltageCompSaturation(12.0, TalonSRXEncoder.TIMEOUT_MS); + motorController.enableVoltageCompensation(true); + motorController.configNominalOutputForward(0.0, TalonSRXEncoder.TIMEOUT_MS); + motorController.configNominalOutputReverse(0.0, TalonSRXEncoder.TIMEOUT_MS); + motorController.configPeakOutputForward(+1.0f, TalonSRXEncoder.TIMEOUT_MS); + motorController.configPeakOutputReverse(-1.0f, TalonSRXEncoder.TIMEOUT_MS); + motorController.selectProfileSlot(0, TalonSRXEncoder.PID_IDX); } } - public void setMPPathTarget(PathGenerator path) { - this.path = path; - path.resetCounter(); - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Speed); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Velocity, 0); - } - } public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); motorController.set(ControlMode.Position, 0); } } public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPosition(0); } } public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position + for (TalonSRXEncoder motorController : motorControllers) { + motorController.setPositionWorld(angle); } - } +// } - public boolean controlLoopUpdate(double currentGyroAngle) { - Segment leftPoint = path.getLeftPoint(); - Segment rightPoint = path.getRightPoint(); - - // Check if we are finished - if (leftPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double rightVelocity = rightPoint.velocity; - double leftVelocity = leftPoint.velocity; // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - motorController.setVelocityWorld(rightVelocity); } else { - motorController.setVelocityWorld(leftVelocity); } } - path.incrementCounter(); - - return false; } } \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java index eb2367c..9e08b13 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java @@ -171,7 +171,7 @@ public class MotionProfileBoxCar public static void main(String[] args) { long startTime = System.nanoTime(); - MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 96, 120, 10, 600, 300); + MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 70, 120, 10, 600, 300); System.out.println("Time, Position, Velocity, Acceleration"); MotionProfilePoint point = new MotionProfilePoint(); while(mp.getNextPoint(point) != null) { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java new file mode 100644 index 0000000..8ff5f91 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java @@ -0,0 +1,49 @@ +package org.usfirst.frc4388.utility; + +import java.text.DecimalFormat; +import java.util.Hashtable; + +public class MotionProfileCache { + + private Hashtable cache; + private DecimalFormat df = new DecimalFormat("#.000"); + private static MotionProfileCache instance; + + private MotionProfileCache() { + cache = new Hashtable(); + } + + public String addMP(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) { + String key = generateKey(startDistance, targetDistance, maxVelocity, itp, t1, t2); + + if (!cache.containsKey(key)) { + MotionProfileBoxCar mp = new MotionProfileBoxCar(startDistance, targetDistance, maxVelocity, itp, t1, t2); + this.addMP(key, mp); + } + + return key; + } + + public void addMP(String key, MotionProfileBoxCar mp) { + cache.put(key, mp); + } + + public MotionProfileBoxCar getMP(String key) { + return cache.get(key); + } + + public static MotionProfileCache getInstance() { + if(instance == null) { + instance = new MotionProfileCache(); + } + return instance; + } + + public void release() { + instance = null; + } + + private String generateKey(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) { + return df.format(startDistance) + df.format(targetDistance) + df.format(maxVelocity) + df.format(itp) + df.format(t1) + df.format(t2); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java index c989f65..9daee69 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java @@ -15,11 +15,6 @@ public class PIDParams public PIDParams() { } - public PIDParams(double kP) - { - this.kP = kP; - } - public PIDParams(double kP, double kI, double kD) { this.kP = kP; this.kI = kI; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java new file mode 100644 index 0000000..341dfd0 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java @@ -0,0 +1,74 @@ +package org.usfirst.frc4388.utility; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.lang.reflect.Field; +import java.util.concurrent.ConcurrentLinkedDeque; + +/** + * Writes data to a CSV file + */ +public class ReflectingCSVWriter { + ConcurrentLinkedDeque mLinesToWrite = new ConcurrentLinkedDeque<>(); + PrintWriter mOutput = null; + Field[] mFields; + + public ReflectingCSVWriter(String fileName, Class typeClass) { + mFields = typeClass.getFields(); + try { + mOutput = new PrintWriter(fileName); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + // Write field names. + StringBuffer line = new StringBuffer(); + for (Field field : mFields) { + if (line.length() != 0) { + line.append(", "); + } + line.append(field.getName()); + } + writeLine(line.toString()); + } + + public void add(T value) { + StringBuffer line = new StringBuffer(); + for (Field field : mFields) { + if (line.length() != 0) { + line.append(", "); + } + try { + line.append(field.get(value).toString()); + } catch (IllegalArgumentException e) { + e.printStackTrace(); + } catch (IllegalAccessException e) { + e.printStackTrace(); + } + } + mLinesToWrite.add(line.toString()); + } + + protected synchronized void writeLine(String line) { + if (mOutput != null) { + mOutput.println(line); + } + } + + // Call this periodically from any thread to write to disk. + public void write() { + while (true) { + String val = mLinesToWrite.pollFirst(); + if (val == null) { + break; + } + writeLine(val); + } + } + + public synchronized void flush() { + if (mOutput != null) { + write(); + mOutput.flush(); + } + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java index 7bd6abd..3e41b63 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java @@ -4,12 +4,12 @@ import java.util.ArrayList; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; +//import com.ctre.CANTalon.TalonControlMode; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class SoftwarePIDController { - protected ArrayList motorControllers; + protected ArrayList motorControllers; protected PIDParams pidParams; protected boolean useGyroLock; protected double targetGyroAngle; @@ -21,7 +21,7 @@ public class SoftwarePIDController protected double prevError = 0.0; // the prior error (used to compute velocity) protected double totalError = 0.0; // the sum of the errors for use in the integral calc - public SoftwarePIDController( PIDParams pidParams, ArrayList motorControllers) + public SoftwarePIDController( PIDParams pidParams, ArrayList motorControllers) { this.motorControllers = motorControllers; setPID(pidParams); @@ -31,22 +31,15 @@ public class SoftwarePIDController this.pidParams = pidParams; } - public void setPIDTurnTarget(double targetAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { this.turnType = turnType; this.targetGyroAngle = targetAngleDeg; this.useGyroLock = true; this.maxError = maxError; this.maxPrevError = maxPrevError; - - // NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - + prevError = 0.0; totalError = 0.0; - } public boolean controlLoopUpdate() { @@ -63,7 +56,6 @@ public class SoftwarePIDController return true; } - if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) { totalError += error; } @@ -79,32 +71,27 @@ public class SoftwarePIDController // Update the controllers set point. if (turnType == MPSoftwareTurnType.TANK) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.set(output); + for (TalonSRXEncoder motorController : motorControllers) { motorController.set(ControlMode.PercentOutput, output); } } else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(0); motorController.set(ControlMode.PercentOutput, 0); } else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); + motorController.set(ControlMode.PercentOutput, 2.0 * output); } } } else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { + for (TalonSRXEncoder motorController : motorControllers) { if (motorController.isRight()) { - //motorController.set(2.0 * output); motorController.set(ControlMode.PercentOutput, 2.0 * output); } else { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); + motorController.set(ControlMode.PercentOutput, 0); } } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java new file mode 100644 index 0000000..9f795ca --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java @@ -0,0 +1,208 @@ +package org.usfirst.frc4388.utility; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj.MotorSafety; + +/** + * Creates TalonSRX objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor + * parameters are not set, as these are expected to be set by the application. + */ +public class TalonSRXFactory { + + public static class Configuration { + public boolean LIMIT_SWITCH_NORMALLY_OPEN = true; + public double MAX_OUTPUT = 1; + public double NOMINAL_OUTPUT = 0; + public double PEAK_OUTPUT = 12; + public NeutralMode ENABLE_BRAKE = NeutralMode.Brake; + public boolean ENABLE_CURRENT_LIMIT = false; + public boolean ENABLE_SOFT_LIMIT = false; + public boolean ENABLE_LIMIT_SWITCH = false; + public int CURRENT_LIMIT = 0; + //public double EXPIRATION_TIMEOUT_SECONDS = MotorSafety.DEFAULT_SAFETY_EXPIRATION; + public double FORWARD_SOFT_LIMIT = 0; + public boolean INVERTED = false; + public double NOMINAL_CLOSED_LOOP_VOLTAGE = 12; + public double REVERSE_SOFT_LIMIT = 0; + public boolean SAFETY_ENABLED = false; + + public int CONTROL_FRAME_PERIOD_MS = 5; + public int MOTION_CONTROL_FRAME_PERIOD_MS = 100; + public int GENERAL_STATUS_FRAME_RATE_MS = 5; + public int FEEDBACK_STATUS_FRAME_RATE_MS = 100; + public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 100; + public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 100; + public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 100; + + public VelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = VelocityMeasPeriod.Period_100Ms; + public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64; + + public double VOLTAGE_COMPENSATION_RAMP_RATE = 0; + public double VOLTAGE_RAMP_RATE = 0; + } + + private static final Configuration kDefaultConfiguration = new Configuration(); + private static final Configuration kSlaveConfiguration = new Configuration(); + + static { + kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 1000; + kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000; + kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; + } + + // Create a TalonSRX with the default (out of the box) configuration. + public static TalonSRX createDefaultTalon(int id) { + return createTalon(id, kDefaultConfiguration); + } + + public static TalonSRXEncoder createTalonEncoder(int id, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { + TalonSRXEncoder talon = new TalonSRXEncoder(id, encoderTicksToWorld, feedbackDevice); +// initializeTalon(talon, kDefaultConfiguration); + return talon; + } + + public static TalonSRXEncoder createTalonEncoder(int id, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) { + TalonSRXEncoder talon = new TalonSRXEncoder(id, encoderTicksToWorld, isRight, feedbackDevice); +// initializeTalon(talon, kDefaultConfiguration); + return talon; + } + + public static TalonSRX createPermanentSlaveTalon(int id, int master_id) { + final TalonSRX talon = createTalon(id, kSlaveConfiguration); + talon.set(ControlMode.Follower, master_id); + return talon; + } + + public static TalonSRX createTalon(int id, Configuration config) { + TalonSRX talon = new TalonSRX(id); +// initializeTalon(talon, config); + return talon; + } + + public static void initializeTalon(TalonSRX talon, Configuration config) { +// talon.setControlFramePeriod(config.CONTROL_FRAME_PERIOD_MS, TalonSRXEncoder.TIMEOUT_MS); + talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS); + talon.setIntegralAccumulator(0, TalonSRXEncoder.PID_IDX, TalonSRXEncoder.TIMEOUT_MS); + talon.clearMotionProfileHasUnderrun(TalonSRXEncoder.TIMEOUT_MS); + talon.clearMotionProfileTrajectories(); + talon.clearStickyFaults(TalonSRXEncoder.TIMEOUT_MS); + talon.configPeakOutputForward(config.MAX_OUTPUT, TalonSRXEncoder.TIMEOUT_MS); + talon.configPeakOutputReverse(-config.MAX_OUTPUT, TalonSRXEncoder.TIMEOUT_MS); + talon.configNominalOutputForward(config.NOMINAL_OUTPUT, TalonSRXEncoder.TIMEOUT_MS); + talon.configNominalOutputReverse(-config.NOMINAL_OUTPUT, TalonSRXEncoder.TIMEOUT_MS); + talon.setNeutralMode(config.ENABLE_BRAKE); + talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT); + talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, TalonSRXEncoder.TIMEOUT_MS); + talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, TalonSRXEncoder.TIMEOUT_MS); + talon.setInverted(config.INVERTED); + talon.setSensorPhase(false); + talon.setSelectedSensorPosition(0, TalonSRXEncoder.PID_IDX, TalonSRXEncoder.TIMEOUT_MS); + talon.selectProfileSlot(0, TalonSRXEncoder.TIMEOUT_MS); + talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, TalonSRXEncoder.TIMEOUT_MS); + talon.configVelocityMeasurementWindow(config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, TalonSRXEncoder.TIMEOUT_MS); + + talon.setStatusFramePeriod(StatusFrame.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS); + talon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS); + talon.setStatusFramePeriod(StatusFrame.Status_4_AinTempVbat, config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS); + +// talon.setStatusFrameRateMs(TalonSRX.StatusFrameRate.QuadEncoder, config.QUAD_ENCODER_STATUS_FRAME_RATE_MS); +// talon.setStatusFrameRateMs(TalonSRX.StatusFrameRate.PulseWidth, config.PULSE_WIDTH_STATUS_FRAME_RATE_MS); + } + + + /** + * Run this on a fresh talon to produce good values for the defaults. + */ + public static String getFullTalonInfo(TalonSRX talon) { + StringBuilder sb = new StringBuilder().append("isRevLimitSwitchClosed = "); +// .append(talon.isRevLimitSwitchClosed()).append("\n").append("getBusVoltage = ") +// .append(talon.getBusVoltage()).append("\n").append("isForwardSoftLimitEnabled = ") +// .append(talon.isForwardSoftLimitEnabled()).append("\n").append("getFaultRevSoftLim = ") +// .append(talon.getFaultRevSoftLim()).append("\n").append("getStickyFaultOverTemp = ") +// .append(talon.getStickyFaultOverTemp()).append("\n").append("isZeroSensorPosOnFwdLimitEnabled = ") +// .append(talon.isZeroSensorPosOnFwdLimitEnabled()).append("\n") +// .append("getMotionProfileTopLevelBufferCount = ").append(talon.getMotionProfileTopLevelBufferCount()) +// .append("\n").append("getNumberOfQuadIdxRises = ").append(talon.getNumberOfQuadIdxRises()).append("\n") +// .append("getInverted = ").append(talon.getInverted()).append("\n") +// .append("getPulseWidthRiseToRiseUs = ").append(talon.getPulseWidthRiseToRiseUs()).append("\n") +// .append("getError = ").append(talon.getError()).append("\n").append("isSensorPresent = ") +// .append(talon.isSensorPresent(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)).append("\n") +// .append("isControlEnabled = ").append(talon.isControlEnabled()).append("\n").append("getTable = ") +// .append(talon.getTable()).append("\n").append("isEnabled = ").append(talon.isEnabled()).append("\n") +// .append("isZeroSensorPosOnRevLimitEnabled = ").append(talon.isZeroSensorPosOnRevLimitEnabled()) +// .append("\n").append("isSafetyEnabled = ").append(talon.isSafetyEnabled()).append("\n") +// .append("getOutputVoltage = ").append(talon.getOutputVoltage()).append("\n").append("getTemperature = ") +// .append(talon.getTemperature()).append("\n").append("getSmartDashboardType = ") +// .append(talon.getSmartDashboardType()).append("\n").append("getPulseWidthPosition = ") +// .append(talon.getPulseWidthPosition()).append("\n").append("getOutputCurrent = ") +// .append(talon.getOutputCurrent()).append("\n").append("get = ").append(talon.get()).append("\n") +// .append("isZeroSensorPosOnIndexEnabled = ").append(talon.isZeroSensorPosOnIndexEnabled()).append("\n") +// .append("getMotionMagicCruiseVelocity = ").append(talon.getMotionMagicCruiseVelocity()).append("\n") +// .append("getStickyFaultRevSoftLim = ").append(talon.getStickyFaultRevSoftLim()).append("\n") +// .append("getFaultRevLim = ").append(talon.getFaultRevLim()).append("\n").append("getEncPosition = ") +// .append(talon.getEncPosition()).append("\n").append("getIZone = ").append(talon.getIZone()).append("\n") +// .append("getAnalogInPosition = ").append(talon.getAnalogInPosition()).append("\n") +// .append("getFaultUnderVoltage = ").append(talon.getFaultUnderVoltage()).append("\n") +// .append("getCloseLoopRampRate = ").append(talon.getCloseLoopRampRate()).append("\n") +// .append("toString = ").append(talon.toString()).append("\n") +// // .append("getMotionMagicActTrajPosition = +// // ").append(talon.getMotionMagicActTrajPosition()).append("\n") +// .append("getF = ").append(talon.getF()).append("\n").append("getClass = ").append(talon.getClass()) +// .append("\n").append("getAnalogInVelocity = ").append(talon.getAnalogInVelocity()).append("\n") +// .append("getI = ").append(talon.getI()).append("\n").append("isReverseSoftLimitEnabled = ") +// .append(talon.isReverseSoftLimitEnabled()).append("\n").append("getPIDSourceType = ") +// .append(talon.getPIDSourceType()).append("\n").append("getEncVelocity = ") +// .append(talon.getEncVelocity()).append("\n").append("GetVelocityMeasurementPeriod = ") +// .append(talon.GetVelocityMeasurementPeriod()).append("\n").append("getP = ").append(talon.getP()) +// .append("\n").append("GetVelocityMeasurementWindow = ").append(talon.GetVelocityMeasurementWindow()) +// .append("\n").append("getDeviceID = ").append(talon.getDeviceID()).append("\n") +// .append("getStickyFaultRevLim = ").append(talon.getStickyFaultRevLim()).append("\n") +// // .append("getMotionMagicActTrajVelocity = +// // ").append(talon.getMotionMagicActTrajVelocity()).append("\n") +// .append("getReverseSoftLimit = ").append(talon.getReverseSoftLimit()).append("\n").append("getD = ") +// .append(talon.getD()).append("\n").append("getFaultOverTemp = ").append(talon.getFaultOverTemp()) +// .append("\n").append("getForwardSoftLimit = ").append(talon.getForwardSoftLimit()).append("\n") +// .append("GetFirmwareVersion = ").append(talon.GetFirmwareVersion()).append("\n") +// .append("getLastError = ").append(talon.getLastError()).append("\n").append("isAlive = ") +// .append(talon.isAlive()).append("\n").append("getPinStateQuadIdx = ").append(talon.getPinStateQuadIdx()) +// .append("\n").append("getAnalogInRaw = ").append(talon.getAnalogInRaw()).append("\n") +// .append("getFaultForLim = ").append(talon.getFaultForLim()).append("\n").append("getSpeed = ") +// .append(talon.getSpeed()).append("\n").append("getStickyFaultForLim = ") +// .append(talon.getStickyFaultForLim()).append("\n").append("getFaultForSoftLim = ") +// .append(talon.getFaultForSoftLim()).append("\n").append("getStickyFaultForSoftLim = ") +// .append(talon.getStickyFaultForSoftLim()).append("\n").append("getClosedLoopError = ") +// .append(talon.getClosedLoopError()).append("\n").append("getSetpoint = ").append(talon.getSetpoint()) +// .append("\n").append("isMotionProfileTopLevelBufferFull = ") +// .append(talon.isMotionProfileTopLevelBufferFull()).append("\n").append("getDescription = ") +// .append(talon.getDescription()).append("\n").append("hashCode = ").append(talon.hashCode()).append("\n") +// .append("isFwdLimitSwitchClosed = ").append(talon.isFwdLimitSwitchClosed()).append("\n") +// .append("getPinStateQuadA = ").append(talon.getPinStateQuadA()).append("\n") +// .append("getPinStateQuadB = ").append(talon.getPinStateQuadB()).append("\n").append("GetIaccum = ") +// .append(talon.GetIaccum()).append("\n").append("getFaultHardwareFailure = ") +// .append(talon.getFaultHardwareFailure()).append("\n").append("pidGet = ").append(talon.pidGet()) +// .append("\n").append("getBrakeEnableDuringNeutral = ").append(talon.getBrakeEnableDuringNeutral()) +// .append("\n").append("getStickyFaultUnderVoltage = ").append(talon.getStickyFaultUnderVoltage()) +// .append("\n").append("getPulseWidthVelocity = ").append(talon.getPulseWidthVelocity()).append("\n") +// .append("GetNominalClosedLoopVoltage = ").append(talon.GetNominalClosedLoopVoltage()).append("\n") +// .append("getPosition = ").append(talon.getPosition()).append("\n").append("getExpiration = ") +// .append(talon.getExpiration()).append("\n").append("getPulseWidthRiseToFallUs = ") +// .append(talon.getPulseWidthRiseToFallUs()).append("\n") +// // .append("createTableListener = ").append(talon.createTableListener()).append("\n") +// .append("getControlMode = ").append(talon.getControlMode()).append("\n") +// .append("getMotionMagicAcceleration = ").append(talon.getMotionMagicAcceleration()).append("\n") +// .append("getControlMode = ").append(talon.getControlMode()); + + return sb.toString(); + } + + } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java new file mode 100644 index 0000000..bcba002 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java @@ -0,0 +1,46 @@ +package org.usfirst.frc4388.utility; + +import java.util.List; + +/** + * Contains basic functions that are used often. + */ +public class Util { + /** Prevent this class from being instantiated. */ + private Util() { + } + + /** + * Limits the given input to the given magnitude. + */ + public static double limit(double v, double maxMagnitude) { + return limit(v, -maxMagnitude, maxMagnitude); + } + + public static double limit(double v, double min, double max) { + return Math.min(max, Math.max(min, v)); + } + + public static String joinStrings(String delim, List strings) { + StringBuilder sb = new StringBuilder(); + for (int i = 0; i < strings.size(); ++i) { + sb.append(strings.get(i).toString()); + if (i < strings.size() - 1) { + sb.append(delim); + } + } + return sb.toString(); + } + + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean allCloseTo(List list, double value, double epsilon) { + boolean result = true; + for (Double value_in : list) { + result &= epsilonEquals(value_in, value, epsilon); + } + return result; + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java new file mode 100644 index 0000000..519e3b0 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java @@ -0,0 +1,200 @@ +package org.usfirst.frc4388.utility.control; + +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; +import org.usfirst.frc4388.utility.math.Twist2d; + +/** + * Implements an adaptive pure pursuit controller. See: + * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 .pdf + * + * Basically, we find a spot on the path we'd like to follow and calculate the arc necessary to make us land on that + * spot. The target spot is a specified distance ahead of us, and we look further ahead the greater our tracking error. + * We also return the maximum speed we'd like to be going when we reach the target spot. + */ + +public class AdaptivePurePursuitController { + private static final double kReallyBigNumber = 1E6; + + public static class Command { + public Twist2d delta = Twist2d.identity(); + public double cross_track_error; + public double max_velocity; + public double end_velocity; + public Translation2d lookahead_point; + public double remaining_path_length; + + public Command() { + } + + public Command(Twist2d delta, double cross_track_error, double max_velocity, double end_velocity, + Translation2d lookahead_point, double remaining_path_length) { + this.delta = delta; + this.cross_track_error = cross_track_error; + this.max_velocity = max_velocity; + this.end_velocity = end_velocity; + this.lookahead_point = lookahead_point; + this.remaining_path_length = remaining_path_length; + } + } + + Path mPath; + boolean mAtEndOfPath = false; + final boolean mReversed; + final Lookahead mLookahead; + + public AdaptivePurePursuitController(Path path, boolean reversed, Lookahead lookahead) { + mPath = path; + mReversed = reversed; + mLookahead = lookahead; + } + + /** + * Gives the RigidTransform2d.Delta that the robot should take to follow the path + * + * @param pose + * robot pose + * @return movement command for the robot to follow + */ + public Command update(RigidTransform2d pose) { + if (mReversed) { + pose = new RigidTransform2d(pose.getTranslation(), + pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI))); + } + + final Path.TargetPointReport report = mPath.getTargetPoint(pose.getTranslation(), mLookahead); + + if (isFinished()) { + // Stop. + return new Command(Twist2d.identity(), report.closest_point_distance, report.max_speed, 0.0, + report.lookahead_point, report.remaining_path_distance); + } + + final Arc arc = new Arc(pose, report.lookahead_point); + double scale_factor = 1.0; + // Ensure we don't overshoot the end of the path (once the lookahead speed drops to zero). + if (report.lookahead_point_speed < 1E-6 && report.remaining_path_distance < arc.length) { + scale_factor = Math.max(0.0, report.remaining_path_distance / arc.length); + mAtEndOfPath = true; + } else { + mAtEndOfPath = false; + } + if (mReversed) { + scale_factor *= -1; + } + + return new Command( + new Twist2d(scale_factor * arc.length, 0.0, + arc.length * getDirection(pose, report.lookahead_point) * Math.abs(scale_factor) / arc.radius), + report.closest_point_distance, report.max_speed, + report.lookahead_point_speed * Math.signum(scale_factor), report.lookahead_point, + report.remaining_path_distance); + } + + public boolean hasPassedMarker(String marker) { + return mPath.hasPassedMarker(marker); + } + + public static class Arc { + public Translation2d center; + public double radius; + public double length; + + public Arc(RigidTransform2d pose, Translation2d point) { + center = getCenter(pose, point); + radius = new Translation2d(center, point).norm(); + length = getLength(pose, point, center, radius); + } + } + + /** + * Gives the center of the circle joining the lookahead point and robot pose + * + * @param pose + * robot pose + * @param point + * lookahead point + * @return center of the circle joining the lookahead point and robot pose + */ + public static Translation2d getCenter(RigidTransform2d pose, Translation2d point) { + final Translation2d poseToPointHalfway = pose.getTranslation().interpolate(point, 0.5); + final Rotation2d normal = pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal(); + final RigidTransform2d perpendicularBisector = new RigidTransform2d(poseToPointHalfway, normal); + final RigidTransform2d normalFromPose = new RigidTransform2d(pose.getTranslation(), + pose.getRotation().normal()); + if (normalFromPose.isColinear(perpendicularBisector.normal())) { + // Special case: center is poseToPointHalfway. + return poseToPointHalfway; + } + return normalFromPose.intersection(perpendicularBisector); + } + + /** + * Gives the radius of the circle joining the lookahead point and robot pose + * + * @param pose + * robot pose + * @param point + * lookahead point + * @return radius of the circle joining the lookahead point and robot pose + */ + public static double getRadius(RigidTransform2d pose, Translation2d point) { + Translation2d center = getCenter(pose, point); + return new Translation2d(center, point).norm(); + } + + /** + * Gives the length of the arc joining the lookahead point and robot pose (assuming forward motion). + * + * @param pose + * robot pose + * @param point + * lookahead point + * @return the length of the arc joining the lookahead point and robot pose + */ + public static double getLength(RigidTransform2d pose, Translation2d point) { + final double radius = getRadius(pose, point); + final Translation2d center = getCenter(pose, point); + return getLength(pose, point, center, radius); + } + + public static double getLength(RigidTransform2d pose, Translation2d point, Translation2d center, double radius) { + if (radius < kReallyBigNumber) { + final Translation2d centerToPoint = new Translation2d(center, point); + final Translation2d centerToPose = new Translation2d(center, pose.getTranslation()); + // If the point is behind pose, we want the opposite of this angle. To determine if the point is behind, + // check the sign of the cross-product between the normal vector and the vector from pose to point. + final boolean behind = Math.signum( + Translation2d.cross(pose.getRotation().normal().toTranslation(), + new Translation2d(pose.getTranslation(), point))) > 0.0; + final Rotation2d angle = Translation2d.getAngle(centerToPose, centerToPoint); + return radius * (behind ? 2.0 * Math.PI - Math.abs(angle.getRadians()) : Math.abs(angle.getRadians())); + } else { + return new Translation2d(pose.getTranslation(), point).norm(); + } + } + + /** + * Gives the direction the robot should turn to stay on the path + * + * @param pose + * robot pose + * @param point + * lookahead point + * @return the direction the robot should turn: -1 is left, +1 is right + */ + public static int getDirection(RigidTransform2d pose, Translation2d point) { + Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point); + Translation2d robot = pose.getRotation().toTranslation(); + double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x(); + return (cross < 0) ? -1 : 1; // if robot < pose turn left + } + + /** + * @return has the robot reached the end of the path + */ + public boolean isFinished() { + return mAtEndOfPath; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java new file mode 100644 index 0000000..4859868 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java @@ -0,0 +1,83 @@ +package org.usfirst.frc4388.utility.control; + +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Twist2d; + +/** + * Provides forward and inverse kinematics equations for the robot modeling the wheelbase as a differential drive (with + * a corrective factor to account for skidding). + */ + +public class Kinematics { + private static final double kEpsilon = 1E-9; + + /** + * Forward kinematics using only encoders, rotation is implicit (less accurate than below, but useful for predicting + * motion) + */ + public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta) { + double delta_v = (right_wheel_delta - left_wheel_delta) / 2 * Constants.kTrackScrubFactor; + double delta_rotation = delta_v * 2 / Constants.kTrackWidthInches; + return forwardKinematics(left_wheel_delta, right_wheel_delta, delta_rotation); + } + + /** + * Forward kinematics using encoders and explicitly measured rotation (ex. from gyro) + */ + public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta, + double delta_rotation_rads) { + final double dx = (left_wheel_delta + right_wheel_delta) / 2.0; + return new Twist2d(dx, 0, delta_rotation_rads); + } + + /** + * For convenience, forward kinematic with an absolute rotation and previous rotation. + */ + public static Twist2d forwardKinematics(Rotation2d prev_heading, double left_wheel_delta, double right_wheel_delta, + Rotation2d current_heading) { + return forwardKinematics(left_wheel_delta, right_wheel_delta, + prev_heading.inverse().rotateBy(current_heading).getRadians()); + } + + /** Append the result of forward kinematics to a previous pose. */ + public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta, + double right_wheel_delta, Rotation2d current_heading) { + Twist2d with_gyro = forwardKinematics(current_pose.getRotation(), left_wheel_delta, right_wheel_delta, + current_heading); + return integrateForwardKinematics(current_pose, with_gyro); + } + + /** + * For convenience, integrate forward kinematics with a Twist2d and previous rotation. + */ + public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, + Twist2d forward_kinematics) { + return current_pose.transformBy(RigidTransform2d.exp(forward_kinematics)); + } + + /** + * Class that contains left and right wheel velocities + */ + public static class DriveVelocity { + public final double left; + public final double right; + + public DriveVelocity(double left, double right) { + this.left = left; + this.right = right; + } + } + + /** + * Uses inverse kinematics to convert a Twist2d into left and right wheel velocities + */ + public static DriveVelocity inverseKinematics(Twist2d velocity) { + if (Math.abs(velocity.dtheta) < kEpsilon) { + return new DriveVelocity(velocity.dx, velocity.dx); + } + double delta_v = Constants.kTrackWidthInches * velocity.dtheta / (2 * Constants.kTrackScrubFactor); + return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java new file mode 100644 index 0000000..2b91876 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java @@ -0,0 +1,28 @@ +package org.usfirst.frc4388.utility.control; + +/** + * A utility class for interpolating lookahead distance based on current speed. + */ +public class Lookahead { + public final double min_distance; + public final double max_distance; + public final double min_speed; + public final double max_speed; + + protected final double delta_distance; + protected final double delta_speed; + + public Lookahead(double min_distance, double max_distance, double min_speed, double max_speed) { + this.min_distance = min_distance; + this.max_distance = max_distance; + this.min_speed = min_speed; + this.max_speed = max_speed; + delta_distance = max_distance - min_distance; + delta_speed = max_speed - min_speed; + } + + public double getLookaheadForSpeed(double speed) { + double lookahead = delta_distance * (speed - min_speed) / delta_speed + min_distance; + return Double.isNaN(lookahead) ? min_distance : Math.max(min_distance, Math.min(max_distance, lookahead)); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java new file mode 100644 index 0000000..3b9fde4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java @@ -0,0 +1,235 @@ +package org.usfirst.frc4388.utility.control; + +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; + +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.utility.math.Translation2d; +import org.usfirst.frc4388.utility.motion.MotionState; + +/** + * Class representing the robot's autonomous path. + * + * Field Coordinate System: Uses a right hand coordinate system. Positive x is right, positive y is up, and the origin + * is at the bottom left corner of the field. For angles, 0 degrees is facing right (1, 0) and angles increase as you + * turn counter clockwise. + * + * +Y + * ^ + * | + * |------------------------------ + * | | <--- FRC Field Boundary + * | | + * | | + * | ----- | + * | |robot|--> 0 deg | + * | ----- | + * ----------------------------------> +X + * (0,0) + * + * Notes: + * 1) The starting point needs to match the first point (x,y) with the proper starting rotation + * 2) When chaining together paths, keep all coordinates for both paths in the reference frame above. + * The second path should start where the first stopped. + * 3) When moving in reverse, you need to set the isReversed flag on the path. + * + */ + +public class Path { + List segments; + PathSegment prevSegment; + HashSet mMarkersCrossed = new HashSet(); + + public void extrapolateLast() { + PathSegment last = segments.get(segments.size() - 1); + last.extrapolateLookahead(true); + } + + public Translation2d getEndPosition() { + return segments.get(segments.size() - 1).getEnd(); + } + + public Path() { + segments = new ArrayList(); + } + + /** + * add a segment to the Path + * + * @param segment + * the segment to add + */ + public void addSegment(PathSegment segment) { + segments.add(segment); + } + + /** + * @return the last MotionState in the path + */ + public MotionState getLastMotionState() { + if (segments.size() > 0) { + MotionState endState = segments.get(segments.size() - 1).getEndState(); + return new MotionState(0.0, 0.0, endState.vel(), endState.acc()); + } else { + return new MotionState(0, 0, 0, 0); + } + } + + /** + * get the remaining distance left for the robot to travel on the current segment + * + * @param robotPos + * robot position + * @return remaining distance on current segment + */ + public double getSegmentRemainingDist(Translation2d robotPos) { + PathSegment currentSegment = segments.get(0); + return currentSegment.getRemainingDistance(currentSegment.getClosestPoint(robotPos)); + } + + /** + * @return the length of the current segment + */ + public double getSegmentLength() { + PathSegment currentSegment = segments.get(0); + return currentSegment.getLength(); + } + + public static class TargetPointReport { + public Translation2d closest_point; + public double closest_point_distance; + public double closest_point_speed; + public Translation2d lookahead_point; + public double max_speed; + public double lookahead_point_speed; + public double remaining_segment_distance; + public double remaining_path_distance; + + public TargetPointReport() { + } + } + + /** + * Gives the position of the lookahead point (and removes any segments prior to this point). + * + * @param robot + * Translation of the current robot pose. + * @return report containing everything we might want to know about the target point. + */ + public TargetPointReport getTargetPoint(Translation2d robot, Lookahead lookahead) { + TargetPointReport rv = new TargetPointReport(); + PathSegment currentSegment = segments.get(0); + rv.closest_point = currentSegment.getClosestPoint(robot); + rv.closest_point_distance = new Translation2d(robot, rv.closest_point).norm(); + /* + * if (segments.size() > 1) { // Check next segment to see if it is closer. final Translation2d + * next_segment_closest_point = segments.get(1).getClosestPoint(robot); final double + * next_segment_closest_point_distance = new Translation2d(robot, next_segment_closest_point) .norm(); if + * (next_segment_closest_point_distance < rv.closest_point_distance) { rv.closest_point = + * next_segment_closest_point; rv.closest_point_distance = next_segment_closest_point_distance; + * removeCurrentSegment(); currentSegment = segments.get(0); } } + */ + rv.remaining_segment_distance = currentSegment.getRemainingDistance(rv.closest_point); + rv.remaining_path_distance = rv.remaining_segment_distance; + for (int i = 1; i < segments.size(); ++i) { + rv.remaining_path_distance += segments.get(i).getLength(); + } + rv.closest_point_speed = currentSegment + .getSpeedByDistance(currentSegment.getLength() - rv.remaining_segment_distance); + double lookahead_distance = lookahead.getLookaheadForSpeed(rv.closest_point_speed) + rv.closest_point_distance; + if (rv.remaining_segment_distance < lookahead_distance && segments.size() > 1) { + lookahead_distance -= rv.remaining_segment_distance; + for (int i = 1; i < segments.size(); ++i) { + currentSegment = segments.get(i); + final double length = currentSegment.getLength(); + if (length < lookahead_distance && i < segments.size() - 1) { + lookahead_distance -= length; + } else { + break; + } + } + } else { + lookahead_distance += (currentSegment.getLength() - rv.remaining_segment_distance); + } + rv.max_speed = currentSegment.getMaxSpeed(); + rv.lookahead_point = currentSegment.getPointByDistance(lookahead_distance); + rv.lookahead_point_speed = currentSegment.getSpeedByDistance(lookahead_distance); + checkSegmentDone(rv.closest_point); + return rv; + } + + /** + * Gives the speed the robot should be traveling at the given position + * + * @param robotPos + * position of the robot + * @return speed robot should be traveling + */ + public double getSpeed(Translation2d robotPos) { + PathSegment currentSegment = segments.get(0); + return currentSegment.getSpeedByClosestPoint(robotPos); + } + + /** + * Checks if the robot has finished traveling along the current segment then removes it from the path if it has + * + * @param robotPos + * robot position + */ + public void checkSegmentDone(Translation2d robotPos) { + PathSegment currentSegment = segments.get(0); + double remainingDist = currentSegment.getRemainingDistance(currentSegment.getClosestPoint(robotPos)); + if (remainingDist < Constants.kSegmentCompletionTolerance) { +// System.out.println("Removed segment from path: " + currentSegment); + removeCurrentSegment(); + } + } + + public void removeCurrentSegment() { + prevSegment = segments.remove(0); + String marker = prevSegment.getMarker(); + if (marker != null) + mMarkersCrossed.add(marker); + } + + /** + * Ensures that all speeds in the path are attainable and robot can slow down in time + */ + public void verifySpeeds() { + double maxStartSpeed = 0.0; + double[] startSpeeds = new double[segments.size() + 1]; + startSpeeds[segments.size()] = 0.0; + for (int i = segments.size() - 1; i >= 0; i--) { + PathSegment segment = segments.get(i); + maxStartSpeed += Math + .sqrt(maxStartSpeed * maxStartSpeed + 2 * Constants.kPathFollowingMaxAccel * segment.getLength()); + startSpeeds[i] = segment.getStartState().vel(); + // System.out.println(maxStartSpeed + ", " + startSpeeds[i]); + if (startSpeeds[i] > maxStartSpeed) { + startSpeeds[i] = maxStartSpeed; + // System.out.println("Segment starting speed is too high!"); + } + maxStartSpeed = startSpeeds[i]; + } + for (int i = 0; i < segments.size(); i++) { + PathSegment segment = segments.get(i); + double endSpeed = startSpeeds[i + 1]; + MotionState startState = (i > 0) ? segments.get(i - 1).getEndState() : new MotionState(0, 0, 0, 0); + startState = new MotionState(0, 0, startState.vel(), startState.vel()); + segment.createMotionProfiler(startState, endSpeed); + } + } + + public boolean hasPassedMarker(String marker) { + return mMarkersCrossed.contains(marker); + } + + public String toString() { + String str = ""; + for (PathSegment s : segments) { + str += s.toString() + "\n"; + } + return str; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java new file mode 100644 index 0000000..272f0c5 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java @@ -0,0 +1,198 @@ +package org.usfirst.frc4388.utility.control; + +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Twist2d; +import org.usfirst.frc4388.utility.motion.MotionProfileConstraints; +import org.usfirst.frc4388.utility.motion.MotionProfileGoal; +import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior; +import org.usfirst.frc4388.utility.motion.MotionState; +import org.usfirst.frc4388.utility.motion.ProfileFollower; + +/** + * A PathFollower follows a predefined path using a combination of feedforward and feedback control. It uses an + * AdaptivePurePursuitController to choose a reference pose and generate a steering command (curvature), and then a + * ProfileFollower to generate a profile (displacement and velocity) command. + */ +public class PathFollower { + private static final double kReallyBigNumber = 1E6; + + public static class DebugOutput { + public double t; + public double pose_x; + public double pose_y; + public double pose_theta; + public double linear_displacement; + public double linear_velocity; + public double profile_displacement; + public double profile_velocity; + public double velocity_command_dx; + public double velocity_command_dy; + public double velocity_command_dtheta; + public double steering_command_dx; + public double steering_command_dy; + public double steering_command_dtheta; + public double cross_track_error; + public double along_track_error; + public double lookahead_point_x; + public double lookahead_point_y; + public double lookahead_point_velocity; + } + + public static class Parameters { + public final Lookahead lookahead; + public final double inertia_gain; + public final double profile_kp; + public final double profile_ki; + public final double profile_kv; + public final double profile_kffv; + public final double profile_kffa; + public final double profile_max_abs_vel; + public final double profile_max_abs_acc; + public final double goal_pos_tolerance; + public final double goal_vel_tolerance; + public final double stop_steering_distance; + + public Parameters(Lookahead lookahead, double inertia_gain, double profile_kp, double profile_ki, + double profile_kv, double profile_kffv, double profile_kffa, double profile_max_abs_vel, + double profile_max_abs_acc, double goal_pos_tolerance, double goal_vel_tolerance, + double stop_steering_distance) { + this.lookahead = lookahead; + this.inertia_gain = inertia_gain; + this.profile_kp = profile_kp; + this.profile_ki = profile_ki; + this.profile_kv = profile_kv; + this.profile_kffv = profile_kffv; + this.profile_kffa = profile_kffa; + this.profile_max_abs_vel = profile_max_abs_vel; + this.profile_max_abs_acc = profile_max_abs_acc; + this.goal_pos_tolerance = goal_pos_tolerance; + this.goal_vel_tolerance = goal_vel_tolerance; + this.stop_steering_distance = stop_steering_distance; + } + } + + AdaptivePurePursuitController mSteeringController; + Twist2d mLastSteeringDelta; + ProfileFollower mVelocityController; + final double mInertiaGain; + boolean overrideFinished = false; + boolean doneSteering = false; + DebugOutput mDebugOutput = new DebugOutput(); + + double mMaxProfileVel; + double mMaxProfileAcc; + final double mGoalPosTolerance; + final double mGoalVelTolerance; + final double mStopSteeringDistance; + double mCrossTrackError = 0.0; + double mAlongTrackError = 0.0; + + /** + * Create a new PathFollower for a given path. + */ + public PathFollower(Path path, boolean reversed, Parameters parameters) { + mSteeringController = new AdaptivePurePursuitController(path, reversed, parameters.lookahead); + mLastSteeringDelta = Twist2d.identity(); + mVelocityController = new ProfileFollower(parameters.profile_kp, parameters.profile_ki, parameters.profile_kv, + parameters.profile_kffv, parameters.profile_kffa); + mVelocityController.setConstraints( + new MotionProfileConstraints(parameters.profile_max_abs_vel, parameters.profile_max_abs_acc)); + mMaxProfileVel = parameters.profile_max_abs_vel; + mMaxProfileAcc = parameters.profile_max_abs_acc; + mGoalPosTolerance = parameters.goal_pos_tolerance; + mGoalVelTolerance = parameters.goal_vel_tolerance; + mInertiaGain = parameters.inertia_gain; + mStopSteeringDistance = parameters.stop_steering_distance; + } + + /** + * Get new velocity commands to follow the path. + * + * @param t + * The current timestamp + * @param pose + * The current robot pose + * @param displacement + * The current robot displacement (total distance driven). + * @param velocity + * The current robot velocity. + * @return The velocity command to apply + */ + public synchronized Twist2d update(double t, RigidTransform2d pose, double displacement, double velocity) { + if (!mSteeringController.isFinished()) { + final AdaptivePurePursuitController.Command steering_command = mSteeringController.update(pose); + mDebugOutput.lookahead_point_x = steering_command.lookahead_point.x(); + mDebugOutput.lookahead_point_y = steering_command.lookahead_point.y(); + mDebugOutput.lookahead_point_velocity = steering_command.end_velocity; + mDebugOutput.steering_command_dx = steering_command.delta.dx; + mDebugOutput.steering_command_dy = steering_command.delta.dy; + mDebugOutput.steering_command_dtheta = steering_command.delta.dtheta; + mCrossTrackError = steering_command.cross_track_error; + mLastSteeringDelta = steering_command.delta; + mVelocityController.setGoalAndConstraints( + new MotionProfileGoal(displacement + steering_command.delta.dx, + Math.abs(steering_command.end_velocity), CompletionBehavior.VIOLATE_MAX_ACCEL, + mGoalPosTolerance, mGoalVelTolerance), + new MotionProfileConstraints(Math.min(mMaxProfileVel, steering_command.max_velocity), + mMaxProfileAcc)); + + if (steering_command.remaining_path_length < mStopSteeringDistance) { + doneSteering = true; + } + } + + final double velocity_command = mVelocityController.update(new MotionState(t, displacement, velocity, 0.0), t); + mAlongTrackError = mVelocityController.getPosError(); + final double curvature = mLastSteeringDelta.dtheta / mLastSteeringDelta.dx; + double dtheta = mLastSteeringDelta.dtheta; + if (!Double.isNaN(curvature) && Math.abs(curvature) < kReallyBigNumber) { + // Regenerate angular velocity command from adjusted curvature. + final double abs_velocity_setpoint = Math.abs(mVelocityController.getSetpoint().vel()); + dtheta = mLastSteeringDelta.dx * curvature * (1.0 + mInertiaGain * abs_velocity_setpoint); + } + final double scale = velocity_command / mLastSteeringDelta.dx; + final Twist2d rv = new Twist2d(mLastSteeringDelta.dx * scale, 0.0, dtheta * scale); + + // Fill out debug. + mDebugOutput.t = t; + mDebugOutput.pose_x = pose.getTranslation().x(); + mDebugOutput.pose_y = pose.getTranslation().y(); + mDebugOutput.pose_theta = pose.getRotation().getRadians(); + mDebugOutput.linear_displacement = displacement; + mDebugOutput.linear_velocity = velocity; + mDebugOutput.profile_displacement = mVelocityController.getSetpoint().pos(); + mDebugOutput.profile_velocity = mVelocityController.getSetpoint().vel(); + mDebugOutput.velocity_command_dx = rv.dx; + mDebugOutput.velocity_command_dy = rv.dy; + mDebugOutput.velocity_command_dtheta = rv.dtheta; + mDebugOutput.cross_track_error = mCrossTrackError; + mDebugOutput.along_track_error = mAlongTrackError; + + return rv; + } + + public double getCrossTrackError() { + return mCrossTrackError; + } + + public double getAlongTrackError() { + return mAlongTrackError; + } + + public DebugOutput getDebug() { + return mDebugOutput; + } + + public boolean isFinished() { + return (mSteeringController.isFinished() && mVelocityController.isFinishedProfile() + && mVelocityController.onTarget()) || overrideFinished; + } + + public void forceFinish() { + overrideFinished = true; + } + + public boolean hasPassedMarker(String marker) { + return mSteeringController.hasPassedMarker(marker); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java new file mode 100644 index 0000000..0645f2e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java @@ -0,0 +1,287 @@ +package org.usfirst.frc4388.utility.control; + +import java.util.Optional; + +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Translation2d; +import org.usfirst.frc4388.utility.motion.MotionProfile; +import org.usfirst.frc4388.utility.motion.MotionProfileConstraints; +import org.usfirst.frc4388.utility.motion.MotionProfileGenerator; +import org.usfirst.frc4388.utility.motion.MotionProfileGoal; +import org.usfirst.frc4388.utility.motion.MotionState; + + +/** + * Class representing a segment of the robot's autonomous path. + */ + +public class PathSegment { + private Translation2d start; + private Translation2d end; + private Translation2d center; + private Translation2d deltaStart; + private Translation2d deltaEnd; + private double maxSpeed; + private boolean isLine; + private MotionProfile speedController; + private boolean extrapolateLookahead; + private String marker; + + /** + * Constructor for a linear segment + * + * @param x1 + * start x + * @param y1 + * start y + * @param x2 + * end x + * @param y2 + * end y + * @param maxSpeed + * maximum speed allowed on the segment + */ + public PathSegment(double x1, double y1, double x2, double y2, double maxSpeed, MotionState startState, + double endSpeed) { + this.start = new Translation2d(x1, y1); + this.end = new Translation2d(x2, y2); + + this.deltaStart = new Translation2d(start, end); + + this.maxSpeed = maxSpeed; + extrapolateLookahead = false; + isLine = true; + createMotionProfiler(startState, endSpeed); + } + + public PathSegment(double x1, double y1, double x2, double y2, double maxSpeed, MotionState startState, + double endSpeed, String marker) { + this.start = new Translation2d(x1, y1); + this.end = new Translation2d(x2, y2); + + this.deltaStart = new Translation2d(start, end); + + this.maxSpeed = maxSpeed; + extrapolateLookahead = false; + isLine = true; + this.marker = marker; + createMotionProfiler(startState, endSpeed); + } + + /** + * Constructor for an arc segment + * + * @param x1 + * start x + * @param y1 + * start y + * @param x2 + * end x + * @param y2 + * end y + * @param cx + * center x + * @param cy + * center y + * @param maxSpeed + * maximum speed allowed on the segment + */ + public PathSegment(double x1, double y1, double x2, double y2, double cx, double cy, double maxSpeed, + MotionState startState, double endSpeed) { + this.start = new Translation2d(x1, y1); + this.end = new Translation2d(x2, y2); + this.center = new Translation2d(cx, cy); + + this.deltaStart = new Translation2d(center, start); + this.deltaEnd = new Translation2d(center, end); + + this.maxSpeed = maxSpeed; + extrapolateLookahead = false; + isLine = false; + createMotionProfiler(startState, endSpeed); + } + + public PathSegment(double x1, double y1, double x2, double y2, double cx, double cy, double maxSpeed, + MotionState startState, double endSpeed, String marker) { + this.start = new Translation2d(x1, y1); + this.end = new Translation2d(x2, y2); + this.center = new Translation2d(cx, cy); + + this.deltaStart = new Translation2d(center, start); + this.deltaEnd = new Translation2d(center, end); + + this.maxSpeed = maxSpeed; + extrapolateLookahead = false; + isLine = false; + this.marker = marker; + createMotionProfiler(startState, endSpeed); + } + + /** + * @return max speed of the segment + */ + public double getMaxSpeed() { + return maxSpeed; + } + + public void createMotionProfiler(MotionState start_state, double end_speed) { + MotionProfileConstraints motionConstraints = new MotionProfileConstraints(maxSpeed, + Constants.kPathFollowingMaxAccel); + MotionProfileGoal goal_state = new MotionProfileGoal(getLength(), end_speed); + speedController = MotionProfileGenerator.generateProfile(motionConstraints, goal_state, start_state); + // System.out.println(speedController); + } + + /** + * @return starting point of the segment + */ + public Translation2d getStart() { + return start; + } + + /** + * @return end point of the segment + */ + public Translation2d getEnd() { + return end; + } + + /** + * @return the total length of the segment + */ + public double getLength() { + if (isLine) { + return deltaStart.norm(); + } else { + return deltaStart.norm() * Translation2d.getAngle(deltaStart, deltaEnd).getRadians(); + } + } + + /** + * Set whether or not to extrapolate the lookahead point. Should only be true for the last segment in the path + * + * @param val + */ + public void extrapolateLookahead(boolean val) { + extrapolateLookahead = val; + } + + /** + * Gets the point on the segment closest to the robot + * + * @param position + * the current position of the robot + * @return the point on the segment closest to the robot + */ + public Translation2d getClosestPoint(Translation2d position) { + if (isLine) { + Translation2d delta = new Translation2d(start, end); + double u = ((position.x() - start.x()) * delta.x() + (position.y() - start.y()) * delta.y()) + / (delta.x() * delta.x() + delta.y() * delta.y()); + if (u >= 0 && u <= 1) + return new Translation2d(start.x() + u * delta.x(), start.y() + u * delta.y()); + return (u < 0) ? start : end; + } else { + Translation2d deltaPosition = new Translation2d(center, position); + deltaPosition = deltaPosition.scale(deltaStart.norm() / deltaPosition.norm()); + if (Translation2d.cross(deltaPosition, deltaStart) * Translation2d.cross(deltaPosition, deltaEnd) < 0) { + return center.translateBy(deltaPosition); + } else { + Translation2d startDist = new Translation2d(position, start); + Translation2d endDist = new Translation2d(position, end); + return (endDist.norm() < startDist.norm()) ? end : start; + } + } + } + + /** + * Calculates the point on the segment dist distance from the starting point along the segment. + * + * @param dist + * distance from the starting point + * @return point on the segment dist distance from the starting point + */ + public Translation2d getPointByDistance(double dist) { + double length = getLength(); + if (!extrapolateLookahead && dist > length) { + dist = length; + } + if (isLine) { + return start.translateBy(deltaStart.scale(dist / length)); + } else { + double deltaAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians() + * ((Translation2d.cross(deltaStart, deltaEnd) >= 0) ? 1 : -1); + deltaAngle *= dist / length; + Translation2d t = deltaStart.rotateBy(Rotation2d.fromRadians(deltaAngle)); + return center.translateBy(t); + } + } + + /** + * Gets the remaining distance left on the segment from point point + * + * @param point + * result of getClosestPoint() + * @return distance remaining + */ + public double getRemainingDistance(Translation2d position) { + if (isLine) { + return new Translation2d(end, position).norm(); + } else { + Translation2d deltaPosition = new Translation2d(center, position); + double angle = Translation2d.getAngle(deltaEnd, deltaPosition).getRadians(); + double totalAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians(); + return angle / totalAngle * getLength(); + } + } + + private double getDistanceTravelled(Translation2d robotPosition) { + Translation2d pathPosition = getClosestPoint(robotPosition); + double remainingDist = getRemainingDistance(pathPosition); + return getLength() - remainingDist; + + } + + public double getSpeedByDistance(double dist) { + if (dist < speedController.startPos()) { + dist = speedController.startPos(); + } else if (dist > speedController.endPos()) { + dist = speedController.endPos(); + } + Optional state = speedController.firstStateByPos(dist); + if (state.isPresent()) { + return state.get().vel(); + } else { + System.out.println("Velocity does not exist at that position!"); + return 0.0; + } + } + + public double getSpeedByClosestPoint(Translation2d robotPosition) { + return getSpeedByDistance(getDistanceTravelled(robotPosition)); + } + + public MotionState getEndState() { + return speedController.endState(); + } + + public MotionState getStartState() { + return speedController.startState(); + } + + public String getMarker() { + return marker; + } + + public String toString() { + if (isLine) { + return "(" + "start: " + start + ", end: " + end + ", speed: " + maxSpeed // + ", profile: " + + // speedController + + ")"; + } else { + return "(" + "start: " + start + ", end: " + end + ", center: " + center + ", speed: " + maxSpeed + + ")"; // + ", profile: " + speedController + ")"; + } + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java new file mode 100644 index 0000000..690c1d0 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java @@ -0,0 +1,133 @@ +package org.usfirst.frc4388.utility.control; + +import java.util.Map; + +import org.usfirst.frc4388.robot.Robot.OperationMode; +import org.usfirst.frc4388.utility.math.InterpolatingDouble; +import org.usfirst.frc4388.utility.math.InterpolatingTreeMap; +import org.usfirst.frc4388.utility.math.RigidTransform2d; +import org.usfirst.frc4388.utility.math.Rotation2d; +import org.usfirst.frc4388.utility.math.Twist2d; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * RobotState keeps track of the poses of various coordinate frames throughout the match. A coordinate frame is simply a + * point and direction in space that defines an (x,y) coordinate system. Transforms (or poses) keep track of the spatial + * relationship between different frames. + * + * Robot frames of interest (from parent to child): + * + * 1. Field frame: origin is where the robot is turned on + * + * 2. Vehicle frame: origin is the center of the robot wheelbase, facing forwards + * + * 3. Camera frame: origin is the center of the camera imager relative to the robot base. + * + * 4. Goal frame: origin is the center of the boiler (note that orientation in this frame is arbitrary). Also note that + * there can be multiple goal frames. + * + * As a kinematic chain with 4 frames, there are 3 transforms of interest: + * + * 1. Field-to-vehicle: This is tracked over time by integrating encoder and gyro measurements. It will inevitably + * drift, but is usually accurate over short time periods. + * + * 2. Vehicle-to-camera: This is a constant. + * + * 3. Camera-to-goal: This is a pure translation, and is measured by the vision system. + */ + +public class RobotState { + private static RobotState instance_ = new RobotState(); + + public static RobotState getInstance() { + return instance_; + } + + private static final int kObservationBufferSize = 100; + + // FPGATimestamp -> RigidTransform2d or Rotation2d + private InterpolatingTreeMap field_to_vehicle_; + private Twist2d vehicle_velocity_predicted_; + private Twist2d vehicle_velocity_measured_; + private double distance_driven_; + + private RobotState() { + reset(0, new RigidTransform2d()); + } + + /** + * Resets the field to robot transform (robot's position on the field) + */ + public synchronized void reset(double start_time, RigidTransform2d initial_field_to_vehicle) { + field_to_vehicle_ = new InterpolatingTreeMap<>(kObservationBufferSize); + field_to_vehicle_.put(new InterpolatingDouble(start_time), initial_field_to_vehicle); + vehicle_velocity_predicted_ = Twist2d.identity(); + vehicle_velocity_measured_ = Twist2d.identity(); + distance_driven_ = 0.0; + } + + public synchronized void resetDistanceDriven() { + distance_driven_ = 0.0; + } + + /** + * Returns the robot's position on the field at a certain time. Linearly interpolates between stored robot positions + * to fill in the gaps. + */ + public synchronized RigidTransform2d getFieldToVehicle(double timestamp) { + return field_to_vehicle_.getInterpolated(new InterpolatingDouble(timestamp)); + } + + public synchronized Map.Entry getLatestFieldToVehicle() { + return field_to_vehicle_.lastEntry(); + } + + public synchronized RigidTransform2d getPredictedFieldToVehicle(double lookahead_time) { + return getLatestFieldToVehicle().getValue() + .transformBy(RigidTransform2d.exp(vehicle_velocity_predicted_.scaled(lookahead_time))); + } + + public synchronized void addFieldToVehicleObservation(double timestamp, RigidTransform2d observation) { + field_to_vehicle_.put(new InterpolatingDouble(timestamp), observation); + } + + public synchronized void addObservations(double timestamp, Twist2d measured_velocity, + Twist2d predicted_velocity) { + addFieldToVehicleObservation(timestamp, + Kinematics.integrateForwardKinematics(getLatestFieldToVehicle().getValue(), measured_velocity)); + vehicle_velocity_measured_ = measured_velocity; + vehicle_velocity_predicted_ = predicted_velocity; + } + + public synchronized Twist2d generateOdometryFromSensors(double left_encoder_delta_distance, + double right_encoder_delta_distance, Rotation2d current_gyro_angle) { + final RigidTransform2d last_measurement = getLatestFieldToVehicle().getValue(); + final Twist2d delta = Kinematics.forwardKinematics(last_measurement.getRotation(), + left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle); + distance_driven_ += delta.dx; + return delta; + } + + public synchronized double getDistanceDriven() { + return distance_driven_; + } + + public synchronized Twist2d getPredictedVelocity() { + return vehicle_velocity_predicted_; + } + + public synchronized Twist2d getMeasuredVelocity() { + return vehicle_velocity_measured_; + } + + public void updateStatus(OperationMode operationMode) { + if (operationMode == OperationMode.TEST) { + RigidTransform2d odometry = getLatestFieldToVehicle().getValue(); + SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x()); + SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y()); + SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees()); + SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx); + } + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java new file mode 100644 index 0000000..1bb35e3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java @@ -0,0 +1,333 @@ +package org.usfirst.frc4388.utility.control; + +import edu.wpi.first.hal.util.BoundaryException; + +/** + * This class implements a PID Control Loop. + * + * Does all computation synchronously (i.e. the calculate() function must be called by the user from his own thread) + */ +public class SynchronousPIDF { + private double m_P; // factor for "proportional" control + private double m_I; // factor for "integral" control + private double m_D; // factor for "derivative" control + private double m_F; // factor for feed forward gain + private double m_maximumOutput = 1.0; // |maximum output| + private double m_minimumOutput = -1.0; // |minimum output| + private double m_maximumInput = 0.0; // maximum input - limit setpoint to + // this + private double m_minimumInput = 0.0; // minimum input - limit setpoint to + // this + private boolean m_continuous = false; // do the endpoints wrap around? eg. + // Absolute encoder + private double m_prevError = 0.0; // the prior sensor input (used to compute + // velocity) + private double m_totalError = 0.0; // the sum of the errors for use in the + // integral calc + private double m_setpoint = 0.0; + private double m_error = 0.0; + private double m_result = 0.0; + private double m_last_input = Double.NaN; + private double m_deadband = 0.0; // If the absolute error is less than + // deadband + // then treat error for the proportional + // term as 0 + + public SynchronousPIDF() { + } + + /** + * Allocate a PID object with the given constants for P, I, D + * + * @param Kp + * the proportional coefficient + * @param Ki + * the integral coefficient + * @param Kd + * the derivative coefficient + */ + public SynchronousPIDF(double Kp, double Ki, double Kd) { + m_P = Kp; + m_I = Ki; + m_D = Kd; + m_F = 0; + } + + /** + * Allocate a PID object with the given constants for P, I, D + * + * @param Kp + * the proportional coefficient + * @param Ki + * the integral coefficient + * @param Kd + * the derivative coefficient + * @param Kf + * the feed forward gain coefficient + */ + public SynchronousPIDF(double Kp, double Ki, double Kd, double Kf) { + m_P = Kp; + m_I = Ki; + m_D = Kd; + m_F = Kf; + } + + /** + * Read the input, calculate the output accordingly, and write to the output. This should be called at a constant + * rate by the user (ex. in a timed thread) + * + * @param input + * the input + * @param dt + * time passed since previous call to calculate + */ + public double calculate(double input, double dt) { + if (dt < 1E-6) + dt = 1E-6; + m_last_input = input; + m_error = m_setpoint - input; + if (m_continuous) { + if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) { + if (m_error > 0) { + m_error = m_error - m_maximumInput + m_minimumInput; + } else { + m_error = m_error + m_maximumInput - m_minimumInput; + } + } + } + + if ((m_error * m_P < m_maximumOutput) && (m_error * m_P > m_minimumOutput)) { + m_totalError += m_error * dt; + } else { + m_totalError = 0; + } + + // Don't blow away m_error so as to not break derivative + double proportionalError = Math.abs(m_error) < m_deadband ? 0 : m_error; + + m_result = (m_P * proportionalError + m_I * m_totalError + m_D * (m_error - m_prevError) / dt + + m_F * m_setpoint); + m_prevError = m_error; + + if (m_result > m_maximumOutput) { + m_result = m_maximumOutput; + } else if (m_result < m_minimumOutput) { + m_result = m_minimumOutput; + } + return m_result; + } + + /** + * Set the PID controller gain parameters. Set the proportional, integral, and differential coefficients. + * + * @param p + * Proportional coefficient + * @param i + * Integral coefficient + * @param d + * Differential coefficient + */ + public void setPID(double p, double i, double d) { + m_P = p; + m_I = i; + m_D = d; + } + + /** + * Set the PID controller gain parameters. Set the proportional, integral, and differential coefficients. + * + * @param p + * Proportional coefficient + * @param i + * Integral coefficient + * @param d + * Differential coefficient + * @param f + * Feed forward coefficient + */ + public void setPID(double p, double i, double d, double f) { + m_P = p; + m_I = i; + m_D = d; + m_F = f; + } + + /** + * Get the Proportional coefficient + * + * @return proportional coefficient + */ + public double getP() { + return m_P; + } + + /** + * Get the Integral coefficient + * + * @return integral coefficient + */ + public double getI() { + return m_I; + } + + /** + * Get the Differential coefficient + * + * @return differential coefficient + */ + public double getD() { + return m_D; + } + + /** + * Get the Feed forward coefficient + * + * @return feed forward coefficient + */ + public double getF() { + return m_F; + } + + /** + * Return the current PID result This is always centered on zero and constrained the the max and min outs + * + * @return the latest calculated output + */ + public double get() { + return m_result; + } + + /** + * Set the PID controller to consider the input to be continuous, Rather then using the max and min in as + * constraints, it considers them to be the same point and automatically calculates the shortest route to the + * setpoint. + * + * @param continuous + * Set to true turns on continuous, false turns off continuous + */ + public void setContinuous(boolean continuous) { + m_continuous = continuous; + } + + public void setDeadband(double deadband) { + m_deadband = deadband; + } + + /** + * Set the PID controller to consider the input to be continuous, Rather then using the max and min in as + * constraints, it considers them to be the same point and automatically calculates the shortest route to the + * setpoint. + */ + public void setContinuous() { + this.setContinuous(true); + } + + /** + * Sets the maximum and minimum values expected from the input. + * + * @param minimumInput + * the minimum value expected from the input + * @param maximumInput + * the maximum value expected from the output + */ + public void setInputRange(double minimumInput, double maximumInput) { + if (minimumInput > maximumInput) { + throw new BoundaryException("Lower bound is greater than upper bound"); + } + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + setSetpoint(m_setpoint); + } + + /** + * Sets the minimum and maximum values to write. + * + * @param minimumOutput + * the minimum value to write to the output + * @param maximumOutput + * the maximum value to write to the output + */ + public void setOutputRange(double minimumOutput, double maximumOutput) { + if (minimumOutput > maximumOutput) { + throw new BoundaryException("Lower bound is greater than upper bound"); + } + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; + } + + /** + * Set the setpoint for the PID controller + * + * @param setpoint + * the desired setpoint + */ + public void setSetpoint(double setpoint) { + if (m_maximumInput > m_minimumInput) { + if (setpoint > m_maximumInput) { + m_setpoint = m_maximumInput; + } else if (setpoint < m_minimumInput) { + m_setpoint = m_minimumInput; + } else { + m_setpoint = setpoint; + } + } else { + m_setpoint = setpoint; + } + } + + /** + * Returns the current setpoint of the PID controller + * + * @return the current setpoint + */ + public double getSetpoint() { + return m_setpoint; + } + + /** + * Returns the current difference of the input from the setpoint + * + * @return the current error + */ + public double getError() { + return m_error; + } + + /** + * Return true if the error is within the tolerance + * + * @return true if the error is less than the tolerance + */ + public boolean onTarget(double tolerance) { + return m_last_input != Double.NaN && Math.abs(m_last_input - m_setpoint) < tolerance; + } + + /** + * Reset all internal terms. + */ + public void reset() { + m_last_input = Double.NaN; + m_prevError = 0; + m_totalError = 0; + m_result = 0; + m_setpoint = 0; + } + + public void resetIntegrator() { + m_totalError = 0; + } + + public String getState() { + String lState = ""; + + lState += "Kp: " + m_P + "\n"; + lState += "Ki: " + m_I + "\n"; + lState += "Kd: " + m_D + "\n"; + + return lState; + } + + public String getType() { + return "PIDController"; + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java new file mode 100644 index 0000000..359e181 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java @@ -0,0 +1,24 @@ +package org.usfirst.frc4388.utility.math; + +/** + * Interpolable is an interface used by an Interpolating Tree as the Value type. Given two end points and an + * interpolation parameter on [0, 1], it calculates a new Interpolable representing the interpolated value. + * + * @param + * The Type of Interpolable + * @see InterpolatingTreeMap + */ +public interface Interpolable { + /** + * Interpolates between this value and an other value according to a given parameter. If x is 0, the method should + * return this value. If x is 1, the method should return the other value. If 0 < x < 1, the return value should be + * interpolated proportionally between the two. + * + * @param other + * The value of the upper bound + * @param x + * The requested value. Should be between 0 and 1. + * @return Interpolable The estimated average between the surrounding data + */ + public T interpolate(T other, double x); +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java new file mode 100644 index 0000000..28cc4c2 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java @@ -0,0 +1,47 @@ +package org.usfirst.frc4388.utility.math; + +/** + * A Double that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingDouble implements Interpolable, InverseInterpolable, + Comparable { + public Double value = 0.0; + + public InterpolatingDouble(Double val) { + value = val; + } + + @Override + public InterpolatingDouble interpolate(InterpolatingDouble other, double x) { + Double dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingDouble(searchY); + } + + @Override + public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) { + double upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) { + return 0; + } + double query_to_lower = query.value - value; + if (query_to_lower <= 0) { + return 0; + } + return query_to_lower / upper_to_lower; + } + + @Override + public int compareTo(InterpolatingDouble other) { + if (other.value < value) { + return 1; + } else if (other.value > value) { + return -1; + } else { + return 0; + } + } + +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java new file mode 100644 index 0000000..1303252 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java @@ -0,0 +1,46 @@ +package org.usfirst.frc4388.utility.math; + +/** + * A Long that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingLong implements Interpolable, InverseInterpolable, + Comparable { + public Long value = 0L; + + public InterpolatingLong(Long val) { + value = val; + } + + @Override + public InterpolatingLong interpolate(InterpolatingLong other, double x) { + Long dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingLong(searchY.longValue()); + } + + @Override + public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) { + long upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) { + return 0; + } + long query_to_lower = query.value - value; + if (query_to_lower <= 0) { + return 0; + } + return query_to_lower / (double) upper_to_lower; + } + + @Override + public int compareTo(InterpolatingLong other) { + if (other.value < value) { + return 1; + } else if (other.value > value) { + return -1; + } else { + return 0; + } + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java new file mode 100644 index 0000000..260a445 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java @@ -0,0 +1,88 @@ +package org.usfirst.frc4388.utility.math; + +import java.util.Map; +import java.util.TreeMap; + +/** + * Interpolating Tree Maps are used to get values at points that are not defined by making a guess from points that are + * defined. This uses linear interpolation. + * + * @param + * The type of the key (must implement InverseInterpolable) + * @param + * The type of the value (must implement Interpolable) + */ +public class InterpolatingTreeMap & Comparable, V extends Interpolable> + extends TreeMap { + private static final long serialVersionUID = 8347275262778054124L; + + int max_; + + public InterpolatingTreeMap(int maximumSize) { + max_ = maximumSize; + } + + public InterpolatingTreeMap() { + this(0); + } + + /** + * Inserts a key value pair, and trims the tree if a max size is specified + * + * @param key + * Key for inserted data + * @param value + * Value for inserted data + * @return the value + */ + @Override + public V put(K key, V value) { + if (max_ > 0 && max_ <= size()) { + // "Prune" the tree if it is oversize + K first = firstKey(); + remove(first); + } + + super.put(key, value); + + return value; + } + + @Override + public void putAll(Map map) { + System.out.println("Unimplemented Method"); + } + + /** + * + * @param key + * Lookup for a value (does not have to exist) + * @return V or null; V if it is Interpolable or exists, null if it is at a bound and cannot average + */ + public V getInterpolated(K key) { + V gotval = get(key); + if (gotval == null) { + /** Get surrounding keys for interpolation */ + K topBound = ceilingKey(key); + K bottomBound = floorKey(key); + + /** + * If attempting interpolation at ends of tree, return the nearest data point + */ + if (topBound == null && bottomBound == null) { + return null; + } else if (topBound == null) { + return get(bottomBound); + } else if (bottomBound == null) { + return get(topBound); + } + + /** Get surrounding values for interpolation */ + V topElem = get(topBound); + V bottomElem = get(bottomBound); + return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); + } else { + return gotval; + } + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java new file mode 100644 index 0000000..99341ab --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java @@ -0,0 +1,23 @@ +package org.usfirst.frc4388.utility.math; + +/** + * InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two endpoint keys and a + * third query key, an InverseInterpolable object can calculate the interpolation parameter of the query key on the + * interval [0, 1]. + * + * @param + * The Type of InverseInterpolable + * @see InterpolatingTreeMap + */ +public interface InverseInterpolable { + /** + * Given this point (lower), a query point (query), and an upper point (upper), estimate how far (on [0, 1]) between + * 'lower' and 'upper' the query point lies. + * + * @param upper + * @param query + * @return The interpolation parameter on [0, 1] representing how far between this point and the upper point the + * query point lies. + */ + public double inverseInterpolate(T upper, T query); +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java similarity index 50% rename from 2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java rename to 2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java index c48b552..b8c6f51 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java @@ -1,29 +1,23 @@ -package org.usfirst.frc4388.utility; +package org.usfirst.frc4388.utility.math; + +import static org.usfirst.frc4388.utility.Util.epsilonEquals; /** - * Represents a 2d pose (rigid transform) containing translational and - * rotational elements. + * Represents a 2d pose (rigid transform) containing translational and rotational elements. * * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) */ public class RigidTransform2d implements Interpolable { - private final static double kEps = 1E-9; + protected static final double kEpsilon = 1E-9; - // Movement along an arc at constant curvature and velocity. We can use - // ideas from "differential calculus" to create new RigidTransform2d's from - // a Delta. - public static class Delta { - public final double dx; - public final double dy; - public final double dtheta; + protected static final RigidTransform2d kIdentity = new RigidTransform2d(); - public Delta(double dx, double dy, double dtheta) { - this.dx = dx; - this.dy = dy; - this.dtheta = dtheta; - } + public static final RigidTransform2d identity() { + return kIdentity; } + private final static double kEps = 1E-9; + protected Translation2d translation_; protected Rotation2d rotation_; @@ -54,7 +48,7 @@ public class RigidTransform2d implements Interpolable { * Obtain a new RigidTransform2d from a (constant curvature) velocity. See: * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp */ - public static RigidTransform2d fromVelocity(Delta delta) { + public static RigidTransform2d exp(Twist2d delta) { double sin_theta = Math.sin(delta.dtheta); double cos_theta = Math.cos(delta.dtheta); double s, c; @@ -69,6 +63,24 @@ public class RigidTransform2d implements Interpolable { new Rotation2d(cos_theta, sin_theta, false)); } + /** + * Logical inverse of the above. + */ + public static Twist2d log(RigidTransform2d transform) { + final double dtheta = transform.getRotation().getRadians(); + final double half_dtheta = 0.5 * dtheta; + final double cos_minus_one = transform.getRotation().cos() - 1.0; + double halftheta_by_tan_of_halfdtheta; + if (Math.abs(cos_minus_one) < kEps) { + halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one; + } + final Translation2d translation_part = transform.getTranslation() + .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false)); + return new Twist2d(translation_part.x(), translation_part.y(), dtheta); + } + public Translation2d getTranslation() { return translation_; } @@ -86,8 +98,8 @@ public class RigidTransform2d implements Interpolable { } /** - * Transforming this RigidTransform2d means first translating by - * other.translation and then rotating by other.rotation + * Transforming this RigidTransform2d means first translating by other.translation and then rotating by + * other.rotation * * @param other * The other transform. @@ -99,8 +111,7 @@ public class RigidTransform2d implements Interpolable { } /** - * The inverse of this transform "undoes" the effect of translating by this - * transform. + * The inverse of this transform "undoes" the effect of translating by this transform. * * @return The opposite of this transform. */ @@ -109,9 +120,49 @@ public class RigidTransform2d implements Interpolable { return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted); } + public RigidTransform2d normal() { + return new RigidTransform2d(translation_, rotation_.normal()); + } + /** - * Do linear interpolation of this transform (there are more accurate ways - * using constant curvature, but this is good enough). + * Finds the point where the heading of this transform intersects the heading of another. Returns (+INF, +INF) if + * parallel. + */ + public Translation2d intersection(RigidTransform2d other) { + final Rotation2d other_rotation = other.getRotation(); + if (rotation_.isParallel(other_rotation)) { + // Lines are parallel. + return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); + } + if (Math.abs(rotation_.cos()) < Math.abs(other_rotation.cos())) { + return intersectionInternal(this, other); + } else { + return intersectionInternal(other, this); + } + } + + /** + * Return true if the heading of this transform is colinear with the heading of another. + */ + public boolean isColinear(RigidTransform2d other) { + final Twist2d twist = log(inverse().transformBy(other)); + return (epsilonEquals(twist.dy, 0.0, kEpsilon) && epsilonEquals(twist.dtheta, 0.0, kEpsilon)); + } + + private static Translation2d intersectionInternal(RigidTransform2d a, RigidTransform2d b) { + final Rotation2d a_r = a.getRotation(); + final Rotation2d b_r = b.getRotation(); + final Translation2d a_t = a.getTranslation(); + final Translation2d b_t = b.getTranslation(); + + final double tan_b = b_r.tan(); + final double t = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) + / (a_r.sin() - a_r.cos() * tan_b); + return a_t.translateBy(a_r.toTranslation().scale(t)); + } + + /** + * Do twist interpolation of this transform assuming constant curvature. */ @Override public RigidTransform2d interpolate(RigidTransform2d other, double x) { @@ -120,8 +171,8 @@ public class RigidTransform2d implements Interpolable { } else if (x >= 1) { return new RigidTransform2d(other); } - return new RigidTransform2d(translation_.interpolate(other.translation_, x), - rotation_.interpolate(other.rotation_, x)); + final Twist2d twist = RigidTransform2d.log(inverse().transformBy(other)); + return transformBy(RigidTransform2d.exp(twist.scaled(x))); } @Override diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java new file mode 100644 index 0000000..e57f297 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java @@ -0,0 +1,144 @@ +package org.usfirst.frc4388.utility.math; + +import static org.usfirst.frc4388.utility.Util.epsilonEquals; + +import java.text.DecimalFormat; + +/** + * A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). + * + * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) + */ +public class Rotation2d implements Interpolable { + protected static final Rotation2d kIdentity = new Rotation2d(); + + public static final Rotation2d identity() { + return kIdentity; + } + + protected static final double kEpsilon = 1E-9; + + protected double cos_angle_; + protected double sin_angle_; + + public Rotation2d() { + this(1, 0, false); + } + + public Rotation2d(double x, double y, boolean normalize) { + cos_angle_ = x; + sin_angle_ = y; + if (normalize) { + normalize(); + } + } + + public Rotation2d(Rotation2d other) { + cos_angle_ = other.cos_angle_; + sin_angle_ = other.sin_angle_; + } + + public Rotation2d(Translation2d direction, boolean normalize) { + this(direction.x(), direction.y(), normalize); + } + + public static Rotation2d fromRadians(double angle_radians) { + return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false); + } + + public static Rotation2d fromDegrees(double angle_degrees) { + return fromRadians(Math.toRadians(angle_degrees)); + } + + /** + * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object we might accumulate rounding errors. + * Normalizing forces us to re-scale the sin and cos to reset rounding errors. + */ + public void normalize() { + double magnitude = Math.hypot(cos_angle_, sin_angle_); + if (magnitude > kEpsilon) { + sin_angle_ /= magnitude; + cos_angle_ /= magnitude; + } else { + sin_angle_ = 0; + cos_angle_ = 1; + } + } + + public double cos() { + return cos_angle_; + } + + public double sin() { + return sin_angle_; + } + + public double tan() { + if (Math.abs(cos_angle_) < kEpsilon) { + if (sin_angle_ >= 0.0) { + return Double.POSITIVE_INFINITY; + } else { + return Double.NEGATIVE_INFINITY; + } + } + return sin_angle_ / cos_angle_; + } + + public double getRadians() { + return Math.atan2(sin_angle_, cos_angle_); + } + + public double getDegrees() { + return Math.toDegrees(getRadians()); + } + + /** + * We can rotate this Rotation2d by adding together the effects of it and another rotation. + * + * @param other + * The other rotation. See: https://en.wikipedia.org/wiki/Rotation_matrix + * @return This rotation rotated by other. + */ + public Rotation2d rotateBy(Rotation2d other) { + return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_, + cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true); + } + + public Rotation2d normal() { + return new Rotation2d(-sin_angle_, cos_angle_, false); + } + + /** + * The inverse of a Rotation2d "undoes" the effect of this rotation. + * + * @return The opposite of this rotation. + */ + public Rotation2d inverse() { + return new Rotation2d(cos_angle_, -sin_angle_, false); + } + + public boolean isParallel(Rotation2d other) { + return epsilonEquals(Translation2d.cross(toTranslation(), other.toTranslation()), 0.0, kEpsilon); + } + + public Translation2d toTranslation() { + return new Translation2d(cos_angle_, sin_angle_); + } + + @Override + public Rotation2d interpolate(Rotation2d other, double x) { + if (x <= 0) { + return new Rotation2d(this); + } else if (x >= 1) { + return new Rotation2d(other); + } + double angle_diff = inverse().rotateBy(other).getRadians(); + return this.rotateBy(Rotation2d.fromRadians(angle_diff * x)); + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(getDegrees()) + " deg)"; + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java new file mode 100644 index 0000000..e32e62a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java @@ -0,0 +1,141 @@ +package org.usfirst.frc4388.utility.math; + +import java.text.DecimalFormat; + +/** + * A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane. + */ +public class Translation2d implements Interpolable { + protected static final Translation2d kIdentity = new Translation2d(); + + public static final Translation2d identity() { + return kIdentity; + } + + protected double x_; + protected double y_; + + public Translation2d() { + x_ = 0; + y_ = 0; + } + + public Translation2d(double x, double y) { + x_ = x; + y_ = y; + } + + public Translation2d(Translation2d other) { + x_ = other.x_; + y_ = other.y_; + } + + public Translation2d(Translation2d start, Translation2d end) { + x_ = end.x_ - start.x_; + y_ = end.y_ - start.y_; + } + + /** + * The "norm" of a transform is the Euclidean distance in x and y. + * + * @return sqrt(x^2 + y^2) + */ + public double norm() { + return Math.hypot(x_, y_); + } + + public double norm2() { + return x_ * x_ + y_ * y_; + } + + public double x() { + return x_; + } + + public double y() { + return y_; + } + + public void setX(double x) { + x_ = x; + } + + public void setY(double y) { + y_ = y; + } + + /** + * We can compose Translation2d's by adding together the x and y shifts. + * + * @param other + * The other translation to add. + * @return The combined effect of translating by this object and the other. + */ + public Translation2d translateBy(Translation2d other) { + return new Translation2d(x_ + other.x_, y_ + other.y_); + } + + /** + * We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix + * + * @param rotation + * The rotation to apply. + * @return This translation rotated by rotation. + */ + public Translation2d rotateBy(Rotation2d rotation) { + return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos()); + } + + public Rotation2d direction() { + return new Rotation2d(x_, y_, true); + } + + /** + * The inverse simply means a Translation2d that "undoes" this object. + * + * @return Translation by -x and -y. + */ + public Translation2d inverse() { + return new Translation2d(-x_, -y_); + } + + @Override + public Translation2d interpolate(Translation2d other, double x) { + if (x <= 0) { + return new Translation2d(this); + } else if (x >= 1) { + return new Translation2d(other); + } + return extrapolate(other, x); + } + + public Translation2d extrapolate(Translation2d other, double x) { + return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_); + } + + public Translation2d scale(double s) { + return new Translation2d(x_ * s, y_ * s); + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")"; + } + + public static double dot(Translation2d a, Translation2d b) { + return a.x_ * b.x_ + a.y_ * b.y_; + } + + public static Rotation2d getAngle(Translation2d a, Translation2d b) { + double cos_angle = dot(a, b) / (a.norm() * b.norm()); + if (Double.isNaN(cos_angle)) { + return new Rotation2d(); + } + return Rotation2d.fromRadians(Math.acos(Math.min(1.0, Math.max(cos_angle, -1.0)))); + } + + public static double cross(Translation2d a, Translation2d b) { + return a.x_ * b.y_ - a.y_ * b.x_; + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java new file mode 100644 index 0000000..fd4ec14 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java @@ -0,0 +1,29 @@ +package org.usfirst.frc4388.utility.math; + +/** + * A movement along an arc at constant curvature and velocity. We can use ideas from "differential calculus" to create + * new RigidTransform2d's from a Twist2d and visa versa. + * + * A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc. + */ +public class Twist2d { + protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0); + + public static final Twist2d identity() { + return kIdentity; + } + + public final double dx; + public final double dy; + public final double dtheta; // Radians! + + public Twist2d(double dx, double dy, double dtheta) { + this.dx = dx; + this.dy = dy; + this.dtheta = dtheta; + } + + public Twist2d scaled(double scale) { + return new Twist2d(dx * scale, dy * scale, dtheta * scale); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java new file mode 100644 index 0000000..176498e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4388.utility.motion; + +import org.usfirst.frc4388.utility.math.Rotation2d; + +/** + * Class to deal with angle wrapping for following a heading profile. All states are assumed to be in units of degrees, + * and wrap on the interval of [-180, 180]. + */ +public class HeadingProfileFollower extends ProfileFollower { + + public HeadingProfileFollower(double kp, double ki, double kv, double kffv, double kffa) { + super(kp, ki, kv, kffv, kffa); + } + + @Override + public double update(MotionState latest_state, double t) { + final Rotation2d goal_rotation_inverse = Rotation2d.fromDegrees(mGoal.pos()).inverse(); + // Update both the setpoint and latest state to be relative to the new goal. + if (mLatestSetpoint != null) { + mLatestSetpoint.motion_state = new MotionState(mLatestSetpoint.motion_state.t(), + mGoal.pos() + goal_rotation_inverse + .rotateBy(Rotation2d.fromDegrees(mLatestSetpoint.motion_state.pos())).getDegrees(), + mLatestSetpoint.motion_state.vel(), mLatestSetpoint.motion_state.acc()); + } + final MotionState latest_state_unwrapped = new MotionState(latest_state.t(), + mGoal.pos() + goal_rotation_inverse.rotateBy(Rotation2d.fromDegrees(latest_state.pos())).getDegrees(), + latest_state.vel(), latest_state.acc()); + double result = super.update(latest_state_unwrapped, t); + // Reset the integrator when we are close to the goal (encourage stiction!). + if (Math.abs(latest_state_unwrapped.pos() - mGoal.pos()) < mGoal.pos_tolerance()) { + result = 0.0; + super.resetIntegral(); + } + return result; + } + + /** + * Convert a motion state representing an angle to a properly wrapped angle. + */ + public static MotionState canonicalize(MotionState state) { + return new MotionState(state.t(), Rotation2d.fromDegrees(state.pos()).getDegrees(), state.vel(), state.acc()); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java new file mode 100644 index 0000000..a04f94e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java @@ -0,0 +1,326 @@ +package org.usfirst.frc4388.utility.motion; + +import static org.usfirst.frc4388.utility.Util.epsilonEquals; +import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon; + +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; +import java.util.Optional; + +/** + * A motion profile specifies a 1D time-parameterized trajectory. The trajectory is composed of successively coincident + * MotionSegments from which the desired state of motion at any given distance or time can be calculated. + */ +public class MotionProfile { + protected List mSegments; + + /** + * Create an empty MotionProfile. + */ + public MotionProfile() { + mSegments = new ArrayList<>(); + } + + /** + * Create a MotionProfile from an existing list of segments (note that validity is not checked). + * + * @param segments + * The new segments of the profile. + */ + public MotionProfile(List segments) { + mSegments = segments; + } + + /** + * Checks if the given MotionProfile is valid. This checks that: + * + * 1. All segments are valid. + * + * 2. Successive segments are C1 continuous in position and C0 continuous in velocity. + * + * @return True if the MotionProfile is valid. + */ + public boolean isValid() { + MotionSegment prev_segment = null; + for (MotionSegment s : mSegments) { + if (!s.isValid()) { + return false; + } + if (prev_segment != null && !s.start().coincident(prev_segment.end())) { + // Adjacent segments are not continuous. + System.err.println("Segments not continuous! End: " + prev_segment.end() + ", Start: " + s.start()); + return false; + } + prev_segment = s; + } + return true; + } + + /** + * Check if the profile is empty. + * + * @return True if there are no segments. + */ + public boolean isEmpty() { + return mSegments.isEmpty(); + } + + /** + * Get the interpolated MotionState at any given time. + * + * @param t + * The time to query. + * @return Empty if the time is outside the time bounds of the profile, or the resulting MotionState otherwise. + */ + public Optional stateByTime(double t) { + if (t < startTime() && t + kEpsilon >= startTime()) { + return Optional.of(startState()); + } + if (t > endTime() && t - kEpsilon <= endTime()) { + return Optional.of(endState()); + } + for (MotionSegment s : mSegments) { + if (s.containsTime(t)) { + return Optional.of(s.start().extrapolate(t)); + } + } + return Optional.empty(); + } + + /** + * Get the interpolated MotionState at any given time, clamping to the endpoints if time is out of bounds. + * + * @param t + * The time to query. + * @return The MotionState at time t, or closest to it if t is outside the profile. + */ + public MotionState stateByTimeClamped(double t) { + if (t < startTime()) { + return startState(); + } else if (t > endTime()) { + return endState(); + } + for (MotionSegment s : mSegments) { + if (s.containsTime(t)) { + return s.start().extrapolate(t); + } + } + // Should never get here. + return MotionState.kInvalidState; + } + + /** + * Get the interpolated MotionState by distance (the "pos()" field of MotionState). Note that since a profile may + * reverse, this method only returns the *first* instance of this position. + * + * @param pos + * The position to query. + * @return Empty if the profile never crosses pos or if the profile is invalid, or the resulting MotionState + * otherwise. + */ + public Optional firstStateByPos(double pos) { + for (MotionSegment s : mSegments) { + if (s.containsPos(pos)) { + if (epsilonEquals(s.end().pos(), pos, kEpsilon)) { + return Optional.of(s.end()); + } + final double t = Math.min(s.start().nextTimeAtPos(pos), s.end().t()); + if (Double.isNaN(t)) { + System.err.println("Error! We should reach 'pos' but we don't"); + return Optional.empty(); + } + return Optional.of(s.start().extrapolate(t)); + } + } + // We never reach pos. + return Optional.empty(); + } + + /** + * Remove all parts of the profile prior to the query time. This eliminates whole segments and also shortens any + * segments containing t. + * + * @param t + * The query time. + */ + public void trimBeforeTime(double t) { + for (Iterator iterator = mSegments.iterator(); iterator.hasNext();) { + MotionSegment s = iterator.next(); + if (s.end().t() <= t) { + // Segment is fully before t. + iterator.remove(); + continue; + } + if (s.start().t() <= t) { + // Segment begins before t; let's shorten the segment. + s.setStart(s.start().extrapolate(t)); + } + break; + } + } + + /** + * Remove all segments. + */ + public void clear() { + mSegments.clear(); + } + + /** + * Remove all segments and initialize to the desired state (actually a segment of length 0 that starts and ends at + * initial_state). + * + * @param initial_state + * The MotionState to initialize to. + */ + public void reset(MotionState initial_state) { + clear(); + mSegments.add(new MotionSegment(initial_state, initial_state)); + } + + /** + * Remove redundant segments (segments whose start and end states are coincident). + */ + public void consolidate() { + for (Iterator iterator = mSegments.iterator(); iterator.hasNext() && mSegments.size() > 1;) { + MotionSegment s = iterator.next(); + if (s.start().coincident(s.end())) { + iterator.remove(); + } + } + } + + /** + * Add to the profile by applying an acceleration control for a given time. This is appended to the previous last + * state. + * + * @param acc + * The acceleration to apply. + * @param dt + * The period of time to apply the given acceleration. + */ + public void appendControl(double acc, double dt) { + if (isEmpty()) { + System.err.println("Error! Trying to append to empty profile"); + return; + } + MotionState last_end_state = mSegments.get(mSegments.size() - 1).end(); + MotionState new_start_state = new MotionState(last_end_state.t(), last_end_state.pos(), last_end_state.vel(), + acc); + appendSegment(new MotionSegment(new_start_state, new_start_state.extrapolate(new_start_state.t() + dt))); + } + + /** + * Add to the profile by inserting a new segment. No validity checking is done. + * + * @param segment + * The segment to add. + */ + public void appendSegment(MotionSegment segment) { + mSegments.add(segment); + } + + /** + * Add to the profile by inserting a new profile after the final state. No validity checking is done. + * + * @param profile + * The profile to add. + */ + public void appendProfile(MotionProfile profile) { + for (MotionSegment s : profile.segments()) { + appendSegment(s); + } + } + + /** + * @return The number of segments. + */ + public int size() { + return mSegments.size(); + } + + /** + * @return The list of segments. + */ + public List segments() { + return mSegments; + } + + /** + * @return The first state in the profile (or kInvalidState if empty). + */ + public MotionState startState() { + if (isEmpty()) { + return MotionState.kInvalidState; + } + return mSegments.get(0).start(); + } + + /** + * @return The time of the first state in the profile (or NaN if empty). + */ + public double startTime() { + return startState().t(); + } + + /** + * @return The pos of the first state in the profile (or NaN if empty). + */ + public double startPos() { + return startState().pos(); + } + + /** + * @return The last state in the profile (or kInvalidState if empty). + */ + public MotionState endState() { + if (isEmpty()) { + return MotionState.kInvalidState; + } + return mSegments.get(mSegments.size() - 1).end(); + } + + /** + * @return The time of the last state in the profile (or NaN if empty). + */ + public double endTime() { + return endState().t(); + } + + /** + * @return The pos of the last state in the profile (or NaN if empty). + */ + public double endPos() { + return endState().pos(); + } + + /** + * @return The duration of the entire profile. + */ + public double duration() { + return endTime() - startTime(); + } + + /** + * @return The total distance covered by the profile. Note that distance is the sum of absolute distances of all + * segments, so a reversing profile will count the distance covered in each direction. + */ + public double length() { + double length = 0.0; + for (MotionSegment s : segments()) { + length += Math.abs(s.end().pos() - s.start().pos()); + } + return length; + } + + @Override + public String toString() { + StringBuilder builder = new StringBuilder("Profile:"); + for (MotionSegment s : segments()) { + builder.append("\n\t"); + builder.append(s); + } + return builder.toString(); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java new file mode 100644 index 0000000..4ff1e61 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4388.utility.motion; + +/** + * Constraints for constructing a MotionProfile. + */ +public class MotionProfileConstraints { + protected double max_abs_vel = Double.POSITIVE_INFINITY; + protected double max_abs_acc = Double.POSITIVE_INFINITY; + + public MotionProfileConstraints(double max_vel, double max_acc) { + this.max_abs_vel = Math.abs(max_vel); + this.max_abs_acc = Math.abs(max_acc); + } + + /** + * @return The (positive) maximum allowed velocity. + */ + public double max_abs_vel() { + return max_abs_vel; + } + + /** + * @return The (positive) maximum allowed acceleration. + */ + public double max_abs_acc() { + return max_abs_acc; + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof MotionProfileConstraints)) { + return false; + } + final MotionProfileConstraints other = (MotionProfileConstraints) obj; + return (other.max_abs_acc() == max_abs_acc()) && (other.max_abs_vel() == max_abs_vel()); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java new file mode 100644 index 0000000..ff28e22 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java @@ -0,0 +1,133 @@ +package org.usfirst.frc4388.utility.motion; + +import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior; + +/** + * A MotionProfileGenerator generates minimum-time MotionProfiles to travel from a given MotionState to a given + * MotionProfileGoal while obeying a set of MotionProfileConstraints. + */ +public class MotionProfileGenerator { + // Static class. + private MotionProfileGenerator() { + } + + protected static MotionProfile generateFlippedProfile(MotionProfileConstraints constraints, + MotionProfileGoal goal_state, MotionState prev_state) { + MotionProfile profile = generateProfile(constraints, goal_state.flipped(), prev_state.flipped()); + for (MotionSegment s : profile.segments()) { + s.setStart(s.start().flipped()); + s.setEnd(s.end().flipped()); + } + return profile; + } + + /** + * Generate a motion profile. + * + * @param constraints + * The constraints to use. + * @param goal_state + * The goal to use. + * @param prev_state + * The initial state to use. + * @return A motion profile from prev_state to goal_state that satisfies constraints. + */ + public synchronized static MotionProfile generateProfile(MotionProfileConstraints constraints, + MotionProfileGoal goal_state, + MotionState prev_state) { + double delta_pos = goal_state.pos() - prev_state.pos(); + if (delta_pos < 0.0 || (delta_pos == 0.0 && prev_state.vel() < 0.0)) { + // For simplicity, we always assume the goal requires positive movement. If negative, we flip to solve, then + // flip the solution. + return generateFlippedProfile(constraints, goal_state, prev_state); + } + // Invariant from this point on: delta_pos >= 0.0 + // Clamp the start state to be valid. + MotionState start_state = new MotionState(prev_state.t(), prev_state.pos(), + Math.signum(prev_state.vel()) * Math.min(Math.abs(prev_state.vel()), constraints.max_abs_vel()), + Math.signum(prev_state.acc()) * Math.min(Math.abs(prev_state.acc()), constraints.max_abs_acc())); + MotionProfile profile = new MotionProfile(); + profile.reset(start_state); + // If our velocity is headed away from the goal, the first thing we need to do is to stop. + if (start_state.vel() < 0.0 && delta_pos > 0.0) { + final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc()); + profile.appendControl(constraints.max_abs_acc(), stopping_time); + start_state = profile.endState(); + delta_pos = goal_state.pos() - start_state.pos(); + } + // Invariant from this point on: start_state.vel() >= 0.0 + final double min_abs_vel_at_goal_sqr = start_state.vel2() - 2.0 * constraints.max_abs_acc() * delta_pos; + final double min_abs_vel_at_goal = Math.sqrt(Math.abs(min_abs_vel_at_goal_sqr)); + final double max_abs_vel_at_goal = Math.sqrt(start_state.vel2() + 2.0 * constraints.max_abs_acc() * delta_pos); + double goal_vel = goal_state.max_abs_vel(); + double max_acc = constraints.max_abs_acc(); + if (min_abs_vel_at_goal_sqr > 0.0 + && min_abs_vel_at_goal > (goal_state.max_abs_vel() + goal_state.vel_tolerance())) { + // Overshoot is unavoidable with the current constraints. Look at completion_behavior to see what we should + // do. + if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ABS_VEL) { + // Adjust the goal velocity. + goal_vel = min_abs_vel_at_goal; + } else if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ACCEL) { + if (Math.abs(delta_pos) < goal_state.pos_tolerance()) { + // Special case: We are at the goal but moving too fast. This requires 'infinite' acceleration, + // which will result in NaNs below, so we can return the profile immediately. + profile.appendSegment(new MotionSegment( + new MotionState(profile.endTime(), profile.endPos(), profile.endState().vel(), + Double.NEGATIVE_INFINITY), + new MotionState(profile.endTime(), profile.endPos(), goal_vel, Double.NEGATIVE_INFINITY))); + profile.consolidate(); + return profile; + } + // Adjust the max acceleration. + max_acc = Math.abs(goal_vel * goal_vel - start_state.vel2()) / (2.0 * delta_pos); + } else { + // We are going to overshoot the goal, so the first thing we need to do is come to a stop. + final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc()); + profile.appendControl(-constraints.max_abs_acc(), stopping_time); + // Now we need to travel backwards, so generate a flipped profile. + profile.appendProfile(generateFlippedProfile(constraints, goal_state, profile.endState())); + profile.consolidate(); + return profile; + } + } + goal_vel = Math.min(goal_vel, max_abs_vel_at_goal); + // Invariant from this point forward: We can achieve goal_vel at goal_state.pos exactly using no more than +/- + // max_acc. + + // What is the maximum velocity we can reach (Vmax)? This is the intersection of two curves: one accelerating + // towards the goal from profile.finalState(), the other coming from the goal at max vel (in reverse). If Vmax + // is greater than constraints.max_abs_vel, we will clamp and cruise. + // Solve the following three equations to find Vmax (by substitution): + // Vmax^2 = Vstart^2 + 2*a*d_accel + // Vgoal^2 = Vmax^2 - 2*a*d_decel + // delta_pos = d_accel + d_decel + final double v_max = Math.min(constraints.max_abs_vel(), + Math.sqrt((start_state.vel2() + goal_vel * goal_vel) / 2.0 + delta_pos * max_acc)); + + // Accelerate to v_max + if (v_max > start_state.vel()) { + final double accel_time = (v_max - start_state.vel()) / max_acc; + profile.appendControl(max_acc, accel_time); + start_state = profile.endState(); + } + // Figure out how much distance will be covered during deceleration. + final double distance_decel = Math.max(0.0, + (start_state.vel2() - goal_vel * goal_vel) / (2.0 * constraints.max_abs_acc())); + final double distance_cruise = Math.max(0.0, goal_state.pos() - start_state.pos() - distance_decel); + // Cruise at constant velocity. + if (distance_cruise > 0.0) { + final double cruise_time = distance_cruise / start_state.vel(); + profile.appendControl(0.0, cruise_time); + start_state = profile.endState(); + } + // Decelerate to goal velocity. + if (distance_decel > 0.0) { + final double decel_time = (start_state.vel() - goal_vel) / max_acc; + profile.appendControl(-max_acc, decel_time); + } + + profile.consolidate(); + return profile; + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java new file mode 100644 index 0000000..1afaefe --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java @@ -0,0 +1,143 @@ +package org.usfirst.frc4388.utility.motion; + +import static org.usfirst.frc4388.utility.Util.epsilonEquals; + +/** + * A MotionProfileGoal defines a desired position and maximum velocity (at this position), along with the behavior that + * should be used to determine if we are at the goal and what to do if it is infeasible to reach the goal within the + * desired velocity bounds. + * + */ +public class MotionProfileGoal { + /** + * A goal consists of a desired position and specified maximum velocity magnitude. But what should we do if we would + * reach the goal at a velocity greater than the maximum? This enum allows a user to specify a preference on + * behavior in this case. + * + * Example use-cases of each: + * + * OVERSHOOT - Generally used with a goal max_abs_vel of 0.0 to stop at the desired pos without violating any + * constraints. + * + * VIOLATE_MAX_ACCEL - If we absolutely do not want to pass the goal and are unwilling to violate the max_abs_vel + * (for example, there is an obstacle in front of us - slam the brakes harder than we'd like in order to avoid + * hitting it). + * + * VIOLATE_MAX_ABS_VEL - If the max velocity is just a general guideline and not a hard performance limit, it's + * better to slightly exceed it to avoid skidding wheels. + */ + public static enum CompletionBehavior { + // Overshoot the goal if necessary (at a velocity greater than max_abs_vel) and come back. + // Only valid if the goal velocity is 0.0 (otherwise VIOLATE_MAX_ACCEL will be used). + OVERSHOOT, + // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the max accel + // constraint. + VIOLATE_MAX_ACCEL, + // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the goal velocity. + VIOLATE_MAX_ABS_VEL + } + + protected double pos; + protected double max_abs_vel; + protected CompletionBehavior completion_behavior = CompletionBehavior.OVERSHOOT; + protected double pos_tolerance = 1E-3; + protected double vel_tolerance = 1E-2; + + public MotionProfileGoal() { + } + + public MotionProfileGoal(double pos) { + this.pos = pos; + this.max_abs_vel = 0.0; + sanityCheck(); + } + + public MotionProfileGoal(double pos, double max_abs_vel) { + this.pos = pos; + this.max_abs_vel = max_abs_vel; + sanityCheck(); + } + + public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior) { + this.pos = pos; + this.max_abs_vel = max_abs_vel; + this.completion_behavior = completion_behavior; + sanityCheck(); + } + + public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior, + double pos_tolerance, double vel_tolerance) { + this.pos = pos; + this.max_abs_vel = max_abs_vel; + this.completion_behavior = completion_behavior; + this.pos_tolerance = pos_tolerance; + this.vel_tolerance = vel_tolerance; + sanityCheck(); + } + + public MotionProfileGoal(MotionProfileGoal other) { + this(other.pos, other.max_abs_vel, other.completion_behavior, other.pos_tolerance, other.vel_tolerance); + } + + /** + * @return A flipped MotionProfileGoal (where the position is negated, but all other attributes remain the same). + */ + public MotionProfileGoal flipped() { + return new MotionProfileGoal(-pos, max_abs_vel, completion_behavior, pos_tolerance, vel_tolerance); + } + + public double pos() { + return pos; + } + + public double max_abs_vel() { + return max_abs_vel; + } + + public double pos_tolerance() { + return pos_tolerance; + } + + public double vel_tolerance() { + return vel_tolerance; + } + + public CompletionBehavior completion_behavior() { + return completion_behavior; + } + + public boolean atGoalState(MotionState state) { + return atGoalPos(state.pos()) && (Math.abs(state.vel()) < (max_abs_vel + vel_tolerance) + || completion_behavior == CompletionBehavior.VIOLATE_MAX_ABS_VEL); + } + + public boolean atGoalPos(double pos) { + return epsilonEquals(pos, this.pos, pos_tolerance); + } + + /** + * This method makes sure that the completion behavior is compatible with the max goal velocity. + */ + protected void sanityCheck() { + if (max_abs_vel > vel_tolerance && completion_behavior == CompletionBehavior.OVERSHOOT) { + completion_behavior = CompletionBehavior.VIOLATE_MAX_ACCEL; + } + } + + @Override + public String toString() { + return "pos: " + pos + " (+/- " + pos_tolerance + "), max_abs_vel: " + max_abs_vel + " (+/- " + vel_tolerance + + "), completion behavior: " + completion_behavior.name(); + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof MotionProfileGoal)) { + return false; + } + final MotionProfileGoal other = (MotionProfileGoal) obj; + return (other.completion_behavior() == completion_behavior()) && (other.pos() == pos()) + && (other.max_abs_vel() == max_abs_vel()) && (other.pos_tolerance() == pos_tolerance()) + && (other.vel_tolerance() == vel_tolerance()); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java new file mode 100644 index 0000000..5c18542 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java @@ -0,0 +1,80 @@ +package org.usfirst.frc4388.utility.motion; + +import static org.usfirst.frc4388.utility.Util.epsilonEquals; +import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon; + +/** + * A MotionSegment is a movement from a start MotionState to an end MotionState with a constant acceleration. + */ +public class MotionSegment { + protected MotionState mStart; + protected MotionState mEnd; + + public MotionSegment(MotionState start, MotionState end) { + mStart = start; + mEnd = end; + } + + /** + * Verifies that: + * + * 1. All segments have a constant acceleration. + * + * 2. All segments have monotonic position (sign of velocity doesn't change). + * + * 3. The time, position, velocity, and acceleration of the profile are consistent. + */ + public boolean isValid() { + if (!epsilonEquals(start().acc(), end().acc(), kEpsilon)) { + // Acceleration is not constant within the segment. + System.err.println( + "Segment acceleration not constant! Start acc: " + start().acc() + ", End acc: " + end().acc()); + return false; + } + if (Math.signum(start().vel()) * Math.signum(end().vel()) < 0.0 && !epsilonEquals(start().vel(), 0.0, kEpsilon) + && !epsilonEquals(end().vel(), 0.0, kEpsilon)) { + // Velocity direction reverses within the segment. + System.err.println("Segment velocity reverses! Start vel: " + start().vel() + ", End vel: " + end().vel()); + return false; + } + if (!start().extrapolate(end().t()).equals(end())) { + // A single segment is not consistent. + if (start().t() == end().t() && Double.isInfinite(start().acc())) { + // One allowed exception: If acc is infinite and dt is zero. + return true; + } + System.err.println("Segment not consistent! Start: " + start() + ", End: " + end()); + return false; + } + return true; + } + + public boolean containsTime(double t) { + return t >= start().t() && t <= end().t(); + } + + public boolean containsPos(double pos) { + return pos >= start().pos() && pos <= end().pos() || pos <= start().pos() && pos >= end().pos(); + } + + public MotionState start() { + return mStart; + } + + public void setStart(MotionState start) { + mStart = start; + } + + public MotionState end() { + return mEnd; + } + + public void setEnd(MotionState end) { + mEnd = end; + } + + @Override + public String toString() { + return "Start: " + start() + ", End: " + end(); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java new file mode 100644 index 0000000..ec4d95f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java @@ -0,0 +1,166 @@ +package org.usfirst.frc4388.utility.motion; + +import static org.usfirst.frc4388.utility.Util.epsilonEquals; +import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon; + +/** + * A MotionState is a completely specified state of 1D motion through time. + */ +public class MotionState { + protected final double t; + protected final double pos; + protected final double vel; + protected final double acc; + + public static MotionState kInvalidState = new MotionState(Double.NaN, Double.NaN, Double.NaN, Double.NaN); + + public MotionState(double t, double pos, double vel, double acc) { + this.t = t; + this.pos = pos; + this.vel = vel; + this.acc = acc; + } + + public MotionState(MotionState state) { + this(state.t, state.pos, state.vel, state.acc); + } + + public double t() { + return t; + } + + public double pos() { + return pos; + } + + public double vel() { + return vel; + } + + public double vel2() { + return vel * vel; + } + + public double acc() { + return acc; + } + + /** + * Extrapolates this MotionState to the specified time by applying this MotionState's acceleration. + * + * @param t + * The time of the new MotionState. + * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state. + */ + public MotionState extrapolate(double t) { + return extrapolate(t, acc); + } + + /** + * + * Extrapolates this MotionState to the specified time by applying a given acceleration to the (t, pos, vel) portion + * of this MotionState. + * + * @param t + * The time of the new MotionState. + * @param acc + * The acceleration to apply. + * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state (with the + * specified accel). + */ + public MotionState extrapolate(double t, double acc) { + final double dt = t - this.t; + return new MotionState(t, pos + vel * dt + .5 * acc * dt * dt, vel + acc * dt, acc); + } + + /** + * Find the next time (first time > MotionState.t()) that this MotionState will be at pos. This is an inverse of the + * extrapolate() method. + * + * @param pos + * The position to query. + * @return The time when we are next at pos() if we are extrapolating with a positive dt. NaN if we never reach pos. + */ + public double nextTimeAtPos(double pos) { + if (epsilonEquals(pos, this.pos, kEpsilon)) { + // Already at pos. + return t; + } + if (epsilonEquals(acc, 0.0, kEpsilon)) { + // Zero acceleration case. + final double delta_pos = pos - this.pos; + if (!epsilonEquals(vel, 0.0, kEpsilon) && Math.signum(delta_pos) == Math.signum(vel)) { + // Constant velocity heading towards pos. + return delta_pos / vel + t; + } + return Double.NaN; + } + + // Solve the quadratic formula. + // ax^2 + bx + c == 0 + // x = dt + // a = .5 * acc + // b = vel + // c = this.pos - pos + final double disc = vel * vel - 2.0 * acc * (this.pos - pos); + if (disc < 0.0) { + // Extrapolating this MotionState never reaches the desired pos. + return Double.NaN; + } + final double sqrt_disc = Math.sqrt(disc); + final double max_dt = (-vel + sqrt_disc) / acc; + final double min_dt = (-vel - sqrt_disc) / acc; + if (min_dt >= 0.0 && (max_dt < 0.0 || min_dt < max_dt)) { + return t + min_dt; + } + if (max_dt >= 0.0) { + return t + max_dt; + } + // We only reach the desired pos in the past. + return Double.NaN; + } + + @Override + public String toString() { + return "(t=" + t + ", pos=" + pos + ", vel=" + vel + ", acc=" + acc + ")"; + } + + /** + * Checks if two MotionStates are epsilon-equals (all fields are equal within a nominal tolerance). + */ + @Override + public boolean equals(Object other) { + return (other instanceof MotionState) && equals((MotionState) other, kEpsilon); + } + + /** + * Checks if two MotionStates are epsilon-equals (all fields are equal within a specified tolerance). + */ + public boolean equals(MotionState other, double epsilon) { + return coincident(other, epsilon) && epsilonEquals(acc, other.acc, epsilon); + } + + /** + * Checks if two MotionStates are coincident (t, pos, and vel are equal within a nominal tolerance, but acceleration + * may be different). + */ + public boolean coincident(MotionState other) { + return coincident(other, kEpsilon); + } + + /** + * Checks if two MotionStates are coincident (t, pos, and vel are equal within a specified tolerance, but + * acceleration may be different). + */ + public boolean coincident(MotionState other, double epsilon) { + return epsilonEquals(t, other.t, epsilon) && epsilonEquals(pos, other.pos, epsilon) + && epsilonEquals(vel, other.vel, epsilon); + } + + /** + * Returns a MotionState that is the mirror image of this one. Pos, vel, and acc are all negated, but time is not. + */ + public MotionState flipped() { + return new MotionState(t, -pos, -vel, -acc); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java new file mode 100644 index 0000000..dc912c8 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java @@ -0,0 +1,8 @@ +package org.usfirst.frc4388.utility.motion; + +public class MotionUtil { + /** + * A constant for consistent floating-point equality checking within this library. + */ + public static double kEpsilon = 1e-6; +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java new file mode 100644 index 0000000..a7390b5 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java @@ -0,0 +1,203 @@ +package org.usfirst.frc4388.utility.motion; + +import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior; + +/** + * A controller for tracking a profile generated to attain a MotionProfileGoal. The controller uses feedforward on + * acceleration, velocity, and position; proportional feedback on velocity and position; and integral feedback on + * position. + */ +public class ProfileFollower { + protected double mKp; + protected double mKi; + protected double mKv; + protected double mKffv; + protected double mKffa; + + protected double mMinOutput = Double.NEGATIVE_INFINITY; + protected double mMaxOutput = Double.POSITIVE_INFINITY; + protected MotionState mLatestActualState; + protected MotionState mInitialState; + protected double mLatestPosError; + protected double mLatestVelError; + protected double mTotalError; + + protected MotionProfileGoal mGoal = null; + protected MotionProfileConstraints mConstraints = null; + protected SetpointGenerator mSetpointGenerator = new SetpointGenerator(); + protected SetpointGenerator.Setpoint mLatestSetpoint = null; + + /** + * Create a new ProfileFollower. + * + * @param kp + * The proportional gain on position error. + * @param ki + * The integral gain on position error. + * @param kv + * The proportional gain on velocity error (or derivative gain on position error). + * @param kffv + * The feedforward gain on velocity. Should be 1.0 if the units of the profile match the units of the + * output. + * @param kffa + * The feedforward gain on acceleration. + */ + public ProfileFollower(double kp, double ki, double kv, double kffv, double kffa) { + resetProfile(); + setGains(kp, ki, kv, kffv, kffa); + } + + public void setGains(double kp, double ki, double kv, double kffv, double kffa) { + mKp = kp; + mKi = ki; + mKv = kv; + mKffv = kffv; + mKffa = kffa; + } + + /** + * Completely clear all state related to the current profile (min and max outputs are maintained). + */ + public void resetProfile() { + mTotalError = 0.0; + mInitialState = MotionState.kInvalidState; + mLatestActualState = MotionState.kInvalidState; + mLatestPosError = Double.NaN; + mLatestVelError = Double.NaN; + mSetpointGenerator.reset(); + mGoal = null; + mConstraints = null; + resetSetpoint(); + } + + /** + * Specify a goal and constraints for achieving the goal. + */ + public void setGoalAndConstraints(MotionProfileGoal goal, MotionProfileConstraints constraints) { + if (mGoal != null && !mGoal.equals(goal) && mLatestSetpoint != null) { + // Clear the final state bit since the goal has changed. + mLatestSetpoint.final_setpoint = false; + } + mGoal = goal; + mConstraints = constraints; + } + + public void setGoal(MotionProfileGoal goal) { + setGoalAndConstraints(goal, mConstraints); + } + + /** + * @return The current goal (null if no goal has been set since the latest call to reset()). + */ + public MotionProfileGoal getGoal() { + return mGoal; + } + + public void setConstraints(MotionProfileConstraints constraints) { + setGoalAndConstraints(mGoal, constraints); + } + + public MotionState getSetpoint() { + return (mLatestSetpoint == null ? MotionState.kInvalidState : mLatestSetpoint.motion_state); + } + + /** + * Reset just the setpoint. This means that the latest_state provided to update() will be used rather than feeding + * forward the previous setpoint the next time update() is called. This almost always forces a MotionProfile update, + * and may be warranted if tracking error gets very large. + */ + public void resetSetpoint() { + mLatestSetpoint = null; + } + + public void resetIntegral() { + mTotalError = 0.0; + } + + /** + * Update the setpoint and apply the control gains to generate a control output. + * + * @param latest_state + * The latest *actual* state, used only for feedback purposes (unless this is the first iteration or + * reset()/resetSetpoint() was just called, in which case this is the new start state for the profile). + * @param t + * The timestamp for which the setpoint is desired. + * @return An output that reflects the control output to apply to achieve the new setpoint. + */ + public synchronized double update(MotionState latest_state, double t) { + mLatestActualState = latest_state; + MotionState prev_state = latest_state; + if (mLatestSetpoint != null) { + prev_state = mLatestSetpoint.motion_state; + } else { + mInitialState = prev_state; + } + final double dt = Math.max(0.0, t - prev_state.t()); + mLatestSetpoint = mSetpointGenerator.getSetpoint(mConstraints, mGoal, prev_state, t); + + // Update error. + mLatestPosError = mLatestSetpoint.motion_state.pos() - latest_state.pos(); + mLatestVelError = mLatestSetpoint.motion_state.vel() - latest_state.vel(); + + // Calculate the feedforward and proportional terms. + double output = mKp * mLatestPosError + mKv * mLatestVelError + mKffv * mLatestSetpoint.motion_state.vel() + + (Double.isNaN(mLatestSetpoint.motion_state.acc()) ? 0.0 : mKffa * mLatestSetpoint.motion_state.acc()); + if (output >= mMinOutput && output <= mMaxOutput) { + // Update integral. + mTotalError += mLatestPosError * dt; + output += mKi * mTotalError; + } else { + // Reset integral windup. + mTotalError = 0.0; + } + // Clamp to limits. + output = Math.max(mMinOutput, Math.min(mMaxOutput, output)); + + return output; + } + + public void setMinOutput(double min_output) { + mMinOutput = min_output; + } + + public void setMaxOutput(double max_output) { + mMaxOutput = max_output; + } + + public double getPosError() { + return mLatestPosError; + } + + public double getVelError() { + return mLatestVelError; + } + + /** + * We are finished the profile when the final setpoint has been generated. Note that this does not check whether we + * are anywhere close to the final setpoint, however. + * + * @return True if the final setpoint has been generated for the current goal. + */ + public boolean isFinishedProfile() { + return mGoal != null && mLatestSetpoint != null && mLatestSetpoint.final_setpoint; + } + + /** + * We are on target if our actual state achieves the goal (where the definition of achievement depends on the goal's + * completion behavior). + * + * @return True if we have actually achieved the current goal. + */ + public boolean onTarget() { + if (mGoal == null || mLatestSetpoint == null) { + return false; + } + // For the options that don't achieve the goal velocity exactly, also count any instance where we have passed + // the finish line. + final double goal_to_start = mGoal.pos() - mInitialState.pos(); + final double goal_to_actual = mGoal.pos() - mLatestActualState.pos(); + final boolean passed_goal_state = Math.signum(goal_to_start) * Math.signum(goal_to_actual) < 0.0; + return mGoal.atGoalState(mLatestActualState) + || (mGoal.completion_behavior() != CompletionBehavior.OVERSHOOT && passed_goal_state); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java new file mode 100644 index 0000000..f3ccb0c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java @@ -0,0 +1,114 @@ +package org.usfirst.frc4388.utility.motion; + +import java.util.Optional; + +/** + * A SetpointGenerate does just-in-time motion profile generation to supply a stream of setpoints that obey the given + * constraints to a controller. The profile is regenerated when any of the inputs change, but is cached (and trimmed as + * we go) if the only update is to the current state. + * + * Note that typically for smooth control, a user will feed the last iteration's setpoint as the argument to + * getSetpoint(), and should only use a measured state directly on the first iteration or if a large disturbance is + * detected. + */ +public class SetpointGenerator { + /** + * A Setpoint is just a MotionState and an additional flag indicating whether this is setpoint achieves the goal + * (useful for higher-level logic to know that it is now time to do something else). + */ + public static class Setpoint { + public MotionState motion_state; + public boolean final_setpoint; + + public Setpoint(MotionState motion_state, boolean final_setpoint) { + this.motion_state = motion_state; + this.final_setpoint = final_setpoint; + } + } + + protected MotionProfile mProfile = null; + protected MotionProfileGoal mGoal = null; + protected MotionProfileConstraints mConstraints = null; + + public SetpointGenerator() { + } + + /** + * Force a reset of the profile. + */ + public void reset() { + mProfile = null; + mGoal = null; + mConstraints = null; + } + + /** + * Get a new Setpoint (and generate a new MotionProfile if necessary). + * + * @param constraints + * The constraints to use. + * @param goal + * The goal to use. + * @param prev_state + * The previous setpoint (or measured state of the system to do a reset). + * @param t + * The time to generate a setpoint for. + * @return The new Setpoint at time t. + */ + public synchronized Setpoint getSetpoint(MotionProfileConstraints constraints, MotionProfileGoal goal, + MotionState prev_state, + double t) { + boolean regenerate = mConstraints == null || !mConstraints.equals(constraints) || mGoal == null + || !mGoal.equals(goal) || mProfile == null; + if (!regenerate && !mProfile.isEmpty()) { + Optional expected_state = mProfile.stateByTime(prev_state.t()); + regenerate = !expected_state.isPresent() || !expected_state.get().equals(prev_state); + } + if (regenerate) { + // Regenerate the profile, as our current profile does not satisfy the inputs. + mConstraints = constraints; + mGoal = goal; + mProfile = MotionProfileGenerator.generateProfile(constraints, goal, prev_state); + // System.out.println("Regenerating profile: " + mProfile); + } + + // Sample the profile at time t. + Setpoint rv = null; + if (!mProfile.isEmpty() && mProfile.isValid()) { + MotionState setpoint; + if (t > mProfile.endTime()) { + setpoint = mProfile.endState(); + } else if (t < mProfile.startTime()) { + setpoint = mProfile.startState(); + } else { + setpoint = mProfile.stateByTime(t).get(); + } + // Shorten the profile and return the new setpoint. + mProfile.trimBeforeTime(t); + rv = new Setpoint(setpoint, mProfile.isEmpty() || mGoal.atGoalState(setpoint)); + } + + // Invalid or empty profile - just output the same state again. + if (rv == null) { + rv = new Setpoint(prev_state, true); + } + + if (rv.final_setpoint) { + // Ensure the final setpoint matches the goal exactly. + rv.motion_state = new MotionState(rv.motion_state.t(), mGoal.pos(), + Math.signum(rv.motion_state.vel()) * Math.max(mGoal.max_abs_vel(), Math.abs(rv.motion_state.vel())), + 0.0); + } + + return rv; + } + + /** + * Get the full profile from the latest call to getSetpoint(). Useful to check estimated time or distance to goal. + * + * @return The profile from the latest call to getSetpoint(), or null if there is not yet a profile. + */ + public MotionProfile getProfile() { + return mProfile; + } +}