diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index 00adcbe73..22d576478 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java @@ -264,7 +264,11 @@ public static double distanceM(Pose2d a, Pose2d b) { } public static double distanceM(Translation2d a, Translation2d b) { - return inverse(a).plus(b).getNorm(); + return a.getDistance(b); + } + + public static double distanceM(Translation3d a, Translation3d b) { + return a.getDistance(b); } public static Translation2d inverse(Translation2d a) { @@ -368,4 +372,9 @@ public static Vector toVec(Twist3d twist) { public static Vector toVec(Translation2d t) { return VecBuilder.fill(t.getX(), t.getY()); } + + /** 3-vector for translation */ + public static Vector toVec3(Translation2d t) { + return VecBuilder.fill(t.getX(), t.getY(), 0); + } } diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java index 30d6853dc..4e5bd6eee 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java +++ b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java @@ -59,6 +59,14 @@ public GlobalVelocityR3 minus(GlobalVelocityR3 other) { return new GlobalVelocityR3(x - other.x, y - other.y, theta - other.theta); } + /** Integrate the velocity from the initial pose for time dt */ + public Pose2d integrate(Pose2d initial, double dt) { + return new Pose2d( + initial.getX() + x * dt, + initial.getY() + y * dt, + initial.getRotation().plus(new Rotation2d(theta * dt))); + } + public GlobalAccelerationR3 accel(GlobalVelocityR3 previous, double dt) { GlobalVelocityR3 v = minus(previous).div(dt); return new GlobalAccelerationR3(v.x(), v.y(), v.theta()); @@ -115,4 +123,14 @@ public static GlobalVelocityR3 fromVector(Matrix v) { public Vector toVector() { return VecBuilder.fill(x, y, theta); } + + /** Angular velocity vector, i.e. just the theta component. */ + public Vector omegaVector() { + return VecBuilder.fill(0, 0, theta); + } + + /** Cartesian velocity vector, i.e. just the x and y components. */ + public Vector vVector() { + return VecBuilder.fill(x, y, 0); + } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java new file mode 100644 index 000000000..35a0eb532 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/HolonomicPose3d.java @@ -0,0 +1,18 @@ +package org.team100.lib.geometry; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +/** + * For constructing 3d splines, paths, and trajectories. + * + * @param translation + * @param heading where we're facing, can include roll + * @param course where we're going, ignores roll. + */ +public record HolonomicPose3d( + Translation3d translation, + Rotation3d heading, + Rotation3d course) { + +} diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java new file mode 100644 index 000000000..8b17c48a6 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/geometry/Pose3dWithMotion.java @@ -0,0 +1,31 @@ +package org.team100.lib.geometry; + +/** + * HolonomicPose3d with: + * + * * the spatial rate of change in heading + * * the spatial rate of change in course + * * the spatial rate of change in course curvature + */ +public class Pose3dWithMotion { + /** Position, heading and course. */ + private final HolonomicPose3d m_pose; + /** Change in yaw per meter of motion, rad/m. */ + private final double m_headingYawRate; + + public Pose3dWithMotion( + HolonomicPose3d pose, + double headingYawRate) { + m_pose = pose; + m_headingYawRate = headingYawRate; + } + + public HolonomicPose3d getPose() { + return m_pose; + } + + public double getHeadingYawRateRad_M() { + return m_headingYawRate; + } + +} diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/README.md b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/README.md new file mode 100644 index 000000000..ca96bc19e --- /dev/null +++ b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/README.md @@ -0,0 +1,214 @@ +# 2P Parallel + +This is a study of control of two parallel prismatic joints, the simplest "redundant" mechanism, +to prepare for a more thorough analysis of the complete control problem -- drivetrain and +mechanism together. In the Calgames mech, this would be six DOF: PPRPRR. + +## Use Cases + +Why would we want redundant joints? One reason is to allow "coarse" and "fine" joints: +the drivetrain could execute "coarse" motion (large, slow, approximate) and the +mechanism could execute "fine" motion (small, quick, accurate). +([spreadsheet](https://docs.google.com/spreadsheets/d/1Z7twUzL8mwdp5JcAuysRHhibZrmlWo9Cy3-zGcI3sXY/edit?gid=1682330556#gid=1682330556)): + +### Tolerance + +A simple use case involves tolerances: within the loose tolerance of the coarse joint, +the fine joint does all the work: + + + +This might be applicable for the case where the drivetrain gets "close enough" to a target, +and then stops, to avoid upsetting a mechanism that's moving the rest of the way. + +### Cascaded PID + +A simple use case is cascaded PID control: the coarse joint responds relatively +slowly, and the fine joint responds more quickly, within its limits: + + + +The control scheme here is very simple, just "P" and "D," but it compares favorably to the same +coarse control, with the same "P" value, (but more "D" to avoid oscillation) by itself: + + + +The main benefit of this approach is simplicity. + +### Linger + +This scenario uses a fixed trajectory for the coarse axis, with a PID controller +on the fine axis, to produce a long linger time at the target (e.g. to do some task). +Here $q_1$ represents the drive base, executing a constant-acceleration U-turn, and $q_2$ +represents a mechanism driven to linger at the target for as long as possible. +Here the target is $0.75$, $q_1$ turns around with acceleration $2 m/s^2$, just +momentarily touching the target, and $q_2$ is uses a PD controller to maintain +$x$ within 0.01 m of the target for about 1 second. + + + +In this chart, you can see the $q_2$ axis reaching forward at its +limit until the target is reached, and then $q_2$ mirrors $q_1$. + +Since $q_1$ is moving fast when $q_2$ begins moving, the $q_2$ +acceleration determines the size of the little overshoot: in this +example, $q_2$ responds very fast. + +This scheme also works if $q_2$ responds more slowly, though +the arrival on target is not quite as crisp. Notice +how $q_2$ anticipates the movement required (just by using +a softer P). The transition moving the other way is sudden, +as $q_2$ hits some sort of physical hard stop. + + + +## What to do + +The use cases above are all hierarchical: the two joints have +different purposes, and are controlled separately. As such, +the conventional redundant control principles aren't really +required. + +Instead, just control them as a cascade? + +Details of conventional redundant control follow. + +
+
+
+
+ +## Kinematics + +Redundant mechanisms have nonunique inverse kinematics, so the problem needs to be solved +as some sort of constrained optimization. + +The forward kinematics are trivial, for joint configurations $q_1$ and $q_2$, and end-effector position $x$: + +```math +x = q_1 + q_2 +``` + +To solve the underdetermined inverse kinematics, there are some trivial options, for example the +joints could split the distance: + +```math +q_1 = \tfrac{x}{2} +\\ +q_2 = \tfrac{x}{2} +``` +Another trivial option is for the first joint to do all the work: + +```math +q_1 = x +\\ +q_2 = 0 +``` + +In general, the problem is of the form + +```math +A q = x +``` + +where + +```math +A = +\begin{bmatrix} +1 && 1 +\end{bmatrix} +``` + +and the solution is of the form: + +```math +q = q_p + q_n +``` + +where $q_p$ is a any particular solution, for example, +the first one above: + +```math +q = +\begin{bmatrix} +\frac{x}{2} +\\ +\space +\\ +\frac{x}{2} +\end{bmatrix} +``` + +and $q_n$ is any vector from the null space. The null space +$\bold{N}(A)$ of $A$ has basis + +```math +q = +\begin{bmatrix} +-1 +\\ +\space +\\ +1 +\end{bmatrix} +``` + +So the complete solution involves a parameter, $s$: + +```math +q = +\begin{bmatrix} +\frac{x}{2} +\\ +\space +\\ +\frac{x}{2} +\end{bmatrix} ++ +s +\begin{bmatrix} +-1 +\\ +\space +\\ +1 +\end{bmatrix} + +``` + +or, equivalently: + +```math +q_1 = \frac{x}{2} - s +\\ +\space +\\ +q_2 = \frac{x}{2} + s + +``` + +To express the limits on $q_2$ in the use cases above, +we could use a cost function with a soft minimum at the center of motion, and steep penalty near the +edges ([desmos](https://www.desmos.com/calculator/2l6ta0osg7)): + + + +Using this cost function alone would yield the same answer as the trivial case above: +$q_1$ would do all the work. + +## Control + +For a trajectory in $x$, we have two control strategies. + +* For the drivetrain, $q_1$, we take the sum of velocity feedforward and proportional +position feedback, supplied to a low-level velocity controller. +* For the mechanism, $q_2$, we supply the positional setpoint directly to a low-level +position controller, with the velocity feedforward as an extra voltage. + +TODO: finish this section + +## Reference + +* [paper about redundant manipulator IK](https://www.mdpi.com/2227-7390/13/4/624) +* [FABRIK paper](http://www.andreasaristidou.com/publications/papers/FABRIK.pdf) \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart.png new file mode 100644 index 000000000..cb266e449 Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart2.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart2.png new file mode 100644 index 000000000..444f5970f Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart2.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart3.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart3.png new file mode 100644 index 000000000..d316a85a4 Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart3.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart4.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart4.png new file mode 100644 index 000000000..d18a4dcb7 Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart4.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart5.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart5.png new file mode 100644 index 000000000..1183f249d Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart5.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart6.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart6.png new file mode 100644 index 000000000..7ce253372 Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/chart6.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/cost_fn.png b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/cost_fn.png new file mode 100644 index 000000000..29b53cb1d Binary files /dev/null and b/lib/src/main/java/org/team100/lib/subsystems/2p_parallel/cost_fn.png differ diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java index 34c4b8c64..03f73e069 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java @@ -15,8 +15,7 @@ * * There's no profile, no trajectory, just give it a point * you want to go to, and it will go there. It makes no attempt to - * impose feasibility constraints or coordinate the axes, so it tries - * to use the setpoint generator to moderate the output. + * impose feasibility constraints or coordinate the axes. * * This is really only intended for testing. */ diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java index 70f004391..5c939d842 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java @@ -47,6 +47,7 @@ public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsy private final ModelR3Logger m_log_state; private final DoubleLogger m_log_turning; private final DoubleArrayLogger m_log_pose_array; + // TODO: pull the field logger out into a separate observer. private final DoubleArrayLogger m_log_field_robot; private final EnumLogger m_log_skill; private final GlobalVelocityR3Logger m_log_input; diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java new file mode 100644 index 000000000..26863a9ad --- /dev/null +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java @@ -0,0 +1,153 @@ +package org.team100.lib.subsystems.test; + +import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.state.ModelR3; +import org.team100.lib.subsystems.r3.VelocitySubsystemR3; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N3; + +/** + * Demo of offset control, without actually changing any control + * classes. + * + * The controlled state is the "toolpoint" of the robot. + * + * The drivetrain is the delegate, and its velocity commands are + * derived from the toolpoint velocities using a fixed offset. + * + * Some of the toolpoint desired velocity is perpendicular to the offset, and so + * by adding a rotation to the delegate, we can move the toolpoint in its + * desired direction a bit faster, in exchange for some theta error. This + * essentially edits the output of the controller, so we can leave the + * controller alone. + */ +public class OffsetDrivetrain implements VelocitySubsystemR3 { + /** + * How much of the perpendicular speed to mix in. This interacts with the + * controller "P" values, so should be tuned together with them. + */ + private static final double OMEGA_MIXER = 2.0; + private final VelocitySubsystemR3 m_delegate; + private final Translation2d m_offset; + + /** + * @param delgate the real drivetrain + * @param offset from delegate to toolpoint + */ + public OffsetDrivetrain( + VelocitySubsystemR3 delegate, Translation2d offset) { + m_delegate = delegate; + m_offset = offset; + } + + @Override + public ModelR3 getState() { + return new ModelR3(toolpointPose(), toolpointVelocity()); + } + + /** + * Set delegate velocity from toolpoint velocity and offset. + * r is from toolpoint to delegate, so invert offset. + * + * @param setpoint toolpoint velocity + */ + @Override + public void setVelocity(GlobalVelocityR3 setpoint) { + // the component of the cartesian part that tries to spin + // the delegate + // adding some of this will make the toolpoint move more rapidly + // towards the cartesian goal, while injecting theta error. + GlobalVelocityR3 perpendicularOmega = omega(r(m_offset), velocity(setpoint)); + + // the component of the rotation part that tries to move the + // delegate in x and y + // respecting 100% of this velocity will keep the toolpoint + // where it wants to go (if the delegate responds perfectly) + GlobalVelocityR3 tangentialVelocity = tangentialVelocity(omega(setpoint), r(m_offset.unaryMinus())); + + m_delegate.setVelocity(setpoint + .plus(tangentialVelocity) + .plus(perpendicularOmega.times(OMEGA_MIXER))); + } + + @Override + public void stop() { + m_delegate.stop(); + } + + /** + * The perpendicular component of v across r, as an angular velocity. + * + * omega = (r \cross v) / r^2 + * + * Cartesian components are always zero. + */ + private GlobalVelocityR3 omega(Vector r, Vector v) { + return GlobalVelocityR3.fromVector( + Vector.cross(r, v).div(r.norm() * r.norm())); + } + + /** + * Computes toolpoint pose from delegate pose and offset. + */ + private Pose2d toolpointPose() { + return m_delegate.getState().pose().transformBy( + new Transform2d(m_offset, Rotation2d.kZero)); + } + + /** + * Computes toolpoint velocity from delegate velocity, pose, and offset. + */ + private GlobalVelocityR3 toolpointVelocity() { + GlobalVelocityR3 delegateVelocity = m_delegate.getState().velocity(); + return delegateVelocity.plus( + tangentialVelocity(omega(delegateVelocity), r(m_offset))); + } + + private Rotation2d delegateRotation() { + return m_delegate.getState().rotation(); + } + + /** + * Vector form of the offset, rotated by the delegate pose rotation. + */ + private Vector r(Translation2d offset) { + return GeometryUtil.toVec3( + offset.rotateBy(delegateRotation())); + } + + /** + * Cartesian component of velocity. + */ + private Vector velocity(GlobalVelocityR3 v) { + return v.vVector(); + } + + /** + * Omega component of the velocity + */ + private Vector omega(GlobalVelocityR3 v) { + return v.omegaVector(); + } + + /** + * Computes the cartesian velocity created by the rotational velocity omega, + * through the radius r. + * + * v = omega \cross r + * + * Omega component is always zero. + */ + private GlobalVelocityR3 tangentialVelocity( + Vector omega, Vector r) { + return GlobalVelocityR3.fromVector( + Vector.cross(omega, r)); + } + +} diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/README.md b/lib/src/main/java/org/team100/lib/subsystems/test/README.md new file mode 100644 index 000000000..e638bb8e3 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/subsystems/test/README.md @@ -0,0 +1,3 @@ +# lib.subsystems.test + +Subsystems only useful for testing. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java new file mode 100644 index 000000000..5fae28367 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java @@ -0,0 +1,49 @@ +package org.team100.lib.subsystems.test; + +import org.team100.lib.coherence.Cache; +import org.team100.lib.coherence.ObjectCache; +import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.geometry.GlobalVelocityR3; +import org.team100.lib.state.ModelR3; +import org.team100.lib.subsystems.r3.VelocitySubsystemR3; + +import edu.wpi.first.math.geometry.Pose2d; + +/** + * Executes desired velocity exactly. + */ +public class TrivialDrivetrain implements VelocitySubsystemR3 { + private static final double DT = TimedRobot100.LOOP_PERIOD_S; + private final ObjectCache m_stateCache; + private GlobalVelocityR3 m_setpoint; + private ModelR3 m_state; + + public TrivialDrivetrain(Pose2d initial) { + m_setpoint = new GlobalVelocityR3(0, 0, 0); + m_state = new ModelR3(initial); + m_stateCache = Cache.of(this::update); + } + + @Override + public ModelR3 getState() { + return m_stateCache.get(); + } + + @Override + public void stop() { + m_setpoint = new GlobalVelocityR3(0, 0, 0); + } + + @Override + public void setVelocity(GlobalVelocityR3 setpoint) { + m_setpoint = setpoint; + } + + private ModelR3 update() { + m_state = new ModelR3( + m_setpoint.integrate(m_state.pose(), DT), + m_setpoint); + return m_state; + } + +} diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java index 01bb02e08..5cab58027 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java @@ -78,11 +78,14 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl if (DEBUG) { System.out.printf("scale %f %f\n", scale0, scale1); } + Translation2d course0 = new Translation2d(1,0).rotateBy(p0.course()); + Translation2d course1 = new Translation2d(1,0).rotateBy(p1.course()); + double x0 = p0.translation().getX(); double x1 = p1.translation().getX(); // first derivatives are just the course - double dx0 = p0.course().getCos() * scale0; - double dx1 = p1.course().getCos() * scale1; + double dx0 = course0.getX() * scale0; + double dx1 = course1.getX() * scale1; // second derivatives are zero at the ends double ddx0 = 0; double ddx1 = 0; @@ -90,8 +93,8 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl double y0 = p0.translation().getY(); double y1 = p1.translation().getY(); // first derivatives are just the course - double dy0 = p0.course().getSin() * scale0; - double dy1 = p1.course().getSin() * scale1; + double dy0 = course0.getY() * scale0; + double dy1 = course1.getY() * scale1; // second derivatives are zero at the ends double ddy0 = 0; double ddy1 = 0; @@ -100,7 +103,7 @@ public HolonomicSpline(HolonomicPose2d p0, HolonomicPose2d p1, double mN0, doubl m_y = SplineR1.get(y0, y1, dy0, dy1, ddy0, ddy1); m_r0 = p0.heading(); - double delta = p0.heading().unaryMinus().rotateBy(p1.heading()).getRadians(); + double delta = p1.heading().minus(p0.heading()).getRadians(); // previously dtheta at the endpoints was zero, which is bad: it meant the omega // value varied all over the place, even for theta paths that should be diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java new file mode 100644 index 000000000..678b75959 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3d.java @@ -0,0 +1,141 @@ +package org.team100.lib.trajectory.path.spline; + +import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.HolonomicPose2d; +import org.team100.lib.geometry.HolonomicPose3d; +import org.team100.lib.geometry.Pose2dWithMotion; +import org.team100.lib.geometry.Pose3dWithMotion; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +/** + * Holonomic spline in three dimensions, similar to the 2d version. + * + * Internally this is five one-dimensional splines (x, y, z, phi, theta), with + * respect to a parameter [0,1]. + * + * Elsewhere, we use Rotation3d to describe spherical angles, by applying the + * rotation to the translation (1,0,0), as Pose3d does. The coordinate system + * there is "roll pitch yaw" from the reference, not "spherical coordinates" + * (with a polar angle etc). + */ +public class HolonomicSpline3d { + private static final boolean DEBUG = false; + // curvature measurement performance scales with sample count so make it kinda + // low. most splines go between 0.5 and 5 meters so this is steps of 2 to 20 cm. + private static final int SAMPLES = 25; + + private final SplineR1 m_x; + private final SplineR1 m_y; + private final SplineR1 m_z; + private final SplineR1 m_pitch; + private final SplineR1 m_yaw; + private final Rotation2d m_pitch0; + private final Rotation2d m_yaw0; + + public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1) { + this(p0, p1, 1.2, 1.2); + } + + public HolonomicSpline3d(HolonomicPose3d p0, HolonomicPose3d p1, double mN0, double mN1) { + double scale0 = mN0 * GeometryUtil.distanceM(p0.translation(), p1.translation()); + double scale1 = mN1 * GeometryUtil.distanceM(p0.translation(), p1.translation()); + + Translation3d course0 = new Translation3d(1, 0, 0).rotateBy(p0.course()); + Translation3d course1 = new Translation3d(1, 0, 0).rotateBy(p1.course()); + + double x0 = p0.translation().getX(); + double x1 = p1.translation().getX(); + // first derivatives are just the course + double dx0 = course0.getX() * scale0; + double dx1 = course1.getX() * scale1; + // second derivatives are zero at the ends + double ddx0 = 0; + double ddx1 = 0; + + double y0 = p0.translation().getY(); + double y1 = p1.translation().getY(); + // first derivatives are just the course + double dy0 = course0.getY() * scale0; + double dy1 = course1.getY() * scale1; + // second derivatives are zero at the ends + double ddy0 = 0; + double ddy1 = 0; + + double z0 = p0.translation().getZ(); + double z1 = p1.translation().getZ(); + // first derivatives are just the course + double dz0 = course0.getZ() * scale0; + double dz1 = course1.getZ() * scale1; + // second derivatives are zero at the ends + double ddz0 = 0; + double ddz1 = 0; + + m_x = SplineR1.get(x0, x1, dx0, dx1, ddx0, ddx1); + m_y = SplineR1.get(y0, y1, dy0, dy1, ddy0, ddy1); + m_z = SplineR1.get(z0, z1, dz0, dz1, ddz0, ddz1); + + m_pitch0 = new Rotation2d(p0.heading().getY()); + m_yaw0 = new Rotation2d(p0.heading().getZ()); + + Rotation3d headingDelta = p1.heading().minus(p0.heading()); + double pitchDelta = headingDelta.getY(); + double yawDelta = headingDelta.getZ(); + + double dpitch0 = pitchDelta * mN0; + double dpitch1 = pitchDelta * mN1; + // second derivatives are zero at the ends + double ddpitch0 = 0; + double ddpitch1 = 0; + m_pitch = SplineR1.get(0.0, pitchDelta, dpitch0, dpitch1, ddpitch0, ddpitch1); + + double dyaw0 = yawDelta * mN0; + double dyaw1 = yawDelta * mN1; + // second derivatives are zero at the ends + double ddyaw0 = 0; + double ddyaw1 = 0; + m_yaw = SplineR1.get(0.0, yawDelta, dyaw0, dyaw1, ddyaw0, ddyaw1); + } + + /** + * Cartesian coordinate in meters. + * + * @param t ranges from 0 to 1 + * @return the point on the spline for that t value + */ + protected Translation3d getPoint(double t) { + return new Translation3d(x(t), y(t), z(t)); + } + + double x(double t) { + return m_x.getPosition(t); + } + + double y(double t) { + return m_y.getPosition(t); + } + + double z(double t) { + return m_z.getPosition(t); + } + + public Pose3dWithMotion getPose3dWithMotion(double p) { + return null; + // return new Pose3dWithMotion( + // new HolonomicPose3d( + // getPoint(p), + // getHeading(p), + // getCourse(p).orElseThrow()), + // getDHeadingDs(p), + // getCurvature(p), + // getDCurvatureDs(p)); + } + + protected Rotation3d getHeading(double t) { + return null; + // return m_r0.rotateBy(Rotation2d.fromRadians(m_theta.getPosition(t))); + } + +} diff --git a/lib/src/main/java/org/team100/lib/visualization/RobotPoseVisualization.java b/lib/src/main/java/org/team100/lib/visualization/RobotPoseVisualization.java new file mode 100644 index 000000000..30c6bd2f6 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/visualization/RobotPoseVisualization.java @@ -0,0 +1,38 @@ +package org.team100.lib.visualization; + +import java.util.function.Supplier; + +import org.team100.lib.logging.Level; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; + +import edu.wpi.first.math.geometry.Pose2d; + +/** + * Observes a pose supplier, publishes to the glass Field2d widget. + */ +public class RobotPoseVisualization implements Runnable { + private final DoubleArrayLogger m_log_field_robot; + private final Supplier m_pose; + + public RobotPoseVisualization( + LoggerFactory fieldLogger, + Supplier pose, + String label) { + m_log_field_robot = fieldLogger.doubleArrayLogger(Level.COMP, label); + m_pose = pose; + } + + @Override + public void run() { + m_log_field_robot.log(this::poseArray); + } + + private double[] poseArray() { + Pose2d pose = m_pose.get(); + return new double[] { + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees() }; + } +} diff --git a/lib/src/test/java/org/team100/lib/testing/Timeless.java b/lib/src/test/java/org/team100/lib/testing/Timeless.java index f9ef94ab1..79b020d76 100644 --- a/lib/src/test/java/org/team100/lib/testing/Timeless.java +++ b/lib/src/test/java/org/team100/lib/testing/Timeless.java @@ -1,7 +1,6 @@ package org.team100.lib.testing; import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.BeforeEach; import org.team100.lib.coherence.Cache; import org.team100.lib.coherence.Takt; @@ -23,31 +22,29 @@ */ public interface Timeless { - /** Make sure the cache doesn't try to update stale things. */ + /** One big method because Junit */ @BeforeEach - default void clearCache() { + default void setupSim() { + // Do any time-related setup *in your test method* ! + HAL.initialize(500, 0); + SimHooks.pauseTiming(); + Takt.update(); + + // Make sure the cache doesn't try to update stale things. Cache.clear(); - } - /* Simulated motors don't move unless enabled, so enable them. */ - @BeforeEach - default void enableMotors() { + // Simulated motors don't move unless enabled, so enable them. DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); - } - /** Avoid mixing mutable values between tests. */ - @BeforeEach - default void unpublish() { + // Avoid mixing mutable values between tests. Mutable.unpublishAll(); - } - - /** Do any time-related setup *in your test method* ! */ - @BeforeEach - default void pauseTiming() { - HAL.initialize(500, 0); - SimHooks.pauseTiming(); - Takt.update(); + try { + // wait for CTRE threads + Thread.sleep(10); + } catch (InterruptedException e) { + e.printStackTrace(); + } } @AfterEach diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java new file mode 100644 index 000000000..5eeaba87c --- /dev/null +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSpline3dTest.java @@ -0,0 +1,35 @@ +package org.team100.lib.trajectory.path.spline; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.HolonomicPose3d; +import org.team100.lib.geometry.Pose3dWithMotion; +import org.team100.lib.testing.Timeless; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class HolonomicSpline3dTest implements Timeless { + private static final double DELTA = 0.001; + + @Test + void testLinear() { + HolonomicSpline3d s = new HolonomicSpline3d( + new HolonomicPose3d(new Translation3d(), new Rotation3d(), new Rotation3d()), + new HolonomicPose3d(new Translation3d(1, 0, 0), new Rotation3d(), new Rotation3d())); + Translation3d t = s.getPoint(0); + assertEquals(0, t.getX(), DELTA); + t = s.getPoint(1); + assertEquals(1, t.getX(), DELTA); + // Pose3dWithMotion p = s.getPose3dWithMotion(0); + // assertEquals(0, p.getPose().translation().getX(), DELTA); + // assertEquals(0, p.getPose().heading().getZ(), DELTA); + // assertEquals(0, p.getHeadingYawRateRad_M(), DELTA); + // p = s.getPose3dWithMotion(1); + // assertEquals(1, p.getPose().translation().getX(), DELTA); + // assertEquals(0, p.getPose().heading().getZ(), DELTA); + // assertEquals(0, p.getHeadingYawRateRad_M(), DELTA); + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 9a73f4ca7..feb15392d 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -27,7 +27,7 @@ import edu.wpi.first.math.geometry.Translation2d; class HolonomicSplineTest implements Timeless { - private static final boolean DEBUG = false; + private static final boolean DEBUG = true; private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @@ -71,7 +71,6 @@ void testLinear2() { @Test void testRotation() { - HolonomicSpline s = new HolonomicSpline( new HolonomicPose2d(new Translation2d(), new Rotation2d(), new Rotation2d()), new HolonomicPose2d(new Translation2d(1, 0), new Rotation2d(1), new Rotation2d())); @@ -107,6 +106,23 @@ void testRotation() { } + @Test + void testRotation2() { + // Make sure the rotation goes over +/- pi + HolonomicSpline s = new HolonomicSpline( + new HolonomicPose2d(new Translation2d(), new Rotation2d(2.5), new Rotation2d()), + new HolonomicPose2d(new Translation2d(1,0), new Rotation2d(-2.5), new Rotation2d())); + if (DEBUG) { + System.out.println("d, x, y, heading, course"); + for (double tt = 0; tt <= 1.0; tt += 0.01) { + var ss = s.getPose2dWithMotion(tt); + Pose2d pose = ss.getPose().pose(); + System.out.printf("%6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", + tt, pose.getX(), pose.getY(), pose.getRotation().getRadians(), ss.getCourse().getRadians()); + } + } + } + @Test void testCircle() { // four splines that make a circle should have nice even curvature and velocity diff --git a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java index 2108560aa..939bb8527 100644 --- a/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/GeometryUtilTest.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -85,6 +86,17 @@ void testSlog() { assertEquals(1.571, twist.dtheta, DELTA); } + @Test + void testDistance2() { + assertEquals(1, GeometryUtil.distanceM(new Translation2d(1, 0), new Translation2d(0, 0)), DELTA); + } + + @Test + void testDistance3() { + assertEquals(1, GeometryUtil.distanceM( + new Translation3d(1, 0, 0), new Translation3d(0, 0, 0)), DELTA); + } + @Test void testDistance() { // same pose => 0 diff --git a/studies/ballerina/src/main/java/frc/robot/Main.java b/studies/ballerina/src/main/java/frc/robot/Main.java index 071fe3888..afde3cd68 100644 --- a/studies/ballerina/src/main/java/frc/robot/Main.java +++ b/studies/ballerina/src/main/java/frc/robot/Main.java @@ -5,9 +5,10 @@ import edu.wpi.first.wpilibj.RobotBase; public final class Main { - private Main() {} + private Main() { + } - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/studies/offset_control/.gitignore b/studies/offset_control/.gitignore new file mode 100644 index 000000000..9a9ca7be7 --- /dev/null +++ b/studies/offset_control/.gitignore @@ -0,0 +1,187 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/studies/offset_control/.vscode/launch.json b/studies/offset_control/.vscode/launch.json new file mode 100644 index 000000000..5b804e844 --- /dev/null +++ b/studies/offset_control/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/studies/offset_control/.vscode/settings.json b/studies/offset_control/.vscode/settings.json new file mode 100644 index 000000000..dccbc7c39 --- /dev/null +++ b/studies/offset_control/.vscode/settings.json @@ -0,0 +1,60 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ] +} diff --git a/studies/offset_control/.wpilib/wpilib_preferences.json b/studies/offset_control/.wpilib/wpilib_preferences.json new file mode 100644 index 000000000..e90d9cfd0 --- /dev/null +++ b/studies/offset_control/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 100 +} \ No newline at end of file diff --git a/studies/offset_control/README.md b/studies/offset_control/README.md new file mode 100644 index 000000000..006cb7695 --- /dev/null +++ b/studies/offset_control/README.md @@ -0,0 +1,9 @@ +# Offset Control + +In 2025 we learned that 254 drivetrain control operated on the error +in the end-effector pose, rather than the robot pose, in order to avoid +controlling the drivetrain unnecessarily. In particular, the +rotation error of the end effector was relatively unimportant. + +How could we do the same thing? + diff --git a/studies/offset_control/WPILib-License.md b/studies/offset_control/WPILib-License.md new file mode 100644 index 000000000..e7cd597b3 --- /dev/null +++ b/studies/offset_control/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2024 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/studies/offset_control/build.gradle b/studies/offset_control/build.gradle new file mode 100644 index 000000000..37612152d --- /dev/null +++ b/studies/offset_control/build.gradle @@ -0,0 +1,112 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.3.2" +} + +sourceSets { + main { + java { + srcDir "../../lib/src/main/java" + } + } +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/studies/offset_control/gradle/wrapper/gradle-wrapper.jar b/studies/offset_control/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 000000000..a4b76b953 Binary files /dev/null and b/studies/offset_control/gradle/wrapper/gradle-wrapper.jar differ diff --git a/studies/offset_control/gradle/wrapper/gradle-wrapper.properties b/studies/offset_control/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 000000000..8e975a5f2 --- /dev/null +++ b/studies/offset_control/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/studies/offset_control/gradlew b/studies/offset_control/gradlew new file mode 100755 index 000000000..f5feea6d6 --- /dev/null +++ b/studies/offset_control/gradlew @@ -0,0 +1,252 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/studies/offset_control/gradlew.bat b/studies/offset_control/gradlew.bat new file mode 100644 index 000000000..9b42019c7 --- /dev/null +++ b/studies/offset_control/gradlew.bat @@ -0,0 +1,94 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/studies/offset_control/offset_control.code-workspace b/studies/offset_control/offset_control.code-workspace new file mode 100644 index 000000000..d3d4fbf1a --- /dev/null +++ b/studies/offset_control/offset_control.code-workspace @@ -0,0 +1,73 @@ +{ + "folders": [ + { + "path": "." + }, + { + "path": "../../lib" + } + ], + "settings": { + "chat.disableAIFeatures": true, + "debug.console.collapseIdenticalLines": false, + "editor.acceptSuggestionOnEnter": "off", + "editor.autoClosingBrackets": "never", + "editor.autoClosingComments": "never", + "editor.autoClosingDelete": "never", + "editor.autoClosingOvertype": "never", + "editor.autoClosingQuotes": "never", + "editor.codeLens": false, + "editor.detectIndentation": false, + "editor.hover.above": true, + "editor.inlayHints.enabled": "off", + "editor.inlineSuggest.enabled": false, + "editor.minimap.enabled": false, + "editor.parameterHints.cycle": false, + "editor.snippetSuggestions": "none", + "editor.stickyScroll.enabled": false, + "editor.stickyScroll.scrollWithEditor": false, + "editor.suggest.showSnippets": false, + "files.autoSave": "afterDelay", + "java.completion.favoriteStaticMembers": [ + "edu.wpi.first.wpilibj2.command.Commands.*", + "org.junit.jupiter.api.Assertions.*", + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*" + ], + "java.completion.guessMethodArguments": "insertBestGuessedArguments", + "java.completion.postfix.enabled": false, + "java.configuration.updateBuildConfiguration": "automatic", + "java.import.gradle.annotationProcessing.enabled": false, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx64G -Xms100m -Xlog:disable", + "java.saveActions.organizeImports": false, + "java.server.launchMode": "Standard", + "java.signatureHelp.description.enabled": true, + "java.signatureHelp.enabled": true, + "markdown.preview.scrollEditorWithPreview": false, + "markdown.preview.scrollPreviewWithEditor": false, + "terminal.integrated.scrollback": 100000, + "vsintellicode.java.completionsEnabled": true, + "workbench.colorCustomizations": { + "debugConsole.infoForeground": "#000000" + }, + "workbench.colorTheme": "Default Light Modern", + "workbench.editor.revealIfOpen": true, + "workbench.tree.enableStickyScroll": false + } +} diff --git a/studies/offset_control/settings.gradle b/studies/offset_control/settings.gradle new file mode 100644 index 000000000..3bc070a1d --- /dev/null +++ b/studies/offset_control/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2025' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name = 'frcHome' + url = frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/studies/offset_control/simgui-ds.json b/studies/offset_control/simgui-ds.json new file mode 100644 index 000000000..387c8b1e6 --- /dev/null +++ b/studies/offset_control/simgui-ds.json @@ -0,0 +1,112 @@ +{ + "FMS": { + "window": { + "visible": false + } + }, + "Joysticks": { + "window": { + "visible": false + } + }, + "System Joysticks": { + "window": { + "visible": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/studies/offset_control/src/main/deploy/example.txt b/studies/offset_control/src/main/deploy/example.txt new file mode 100644 index 000000000..bb82515da --- /dev/null +++ b/studies/offset_control/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/studies/offset_control/src/main/java/frc/robot/Main.java b/studies/offset_control/src/main/java/frc/robot/Main.java new file mode 100644 index 000000000..cd91dc0b2 --- /dev/null +++ b/studies/offset_control/src/main/java/frc/robot/Main.java @@ -0,0 +1,14 @@ +package frc.robot; + +import org.team100.offset_control.Robot; + +import edu.wpi.first.wpilibj.RobotBase; + +public final class Main { + private Main() { + } + + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/studies/offset_control/src/main/java/org/team100/offset_control/Robot.java b/studies/offset_control/src/main/java/org/team100/offset_control/Robot.java new file mode 100644 index 000000000..00e31c4a9 --- /dev/null +++ b/studies/offset_control/src/main/java/org/team100/offset_control/Robot.java @@ -0,0 +1,142 @@ + +package org.team100.offset_control; + +import org.team100.lib.coherence.Cache; +import org.team100.lib.coherence.Takt; +import org.team100.lib.commands.MoveAndHold; +import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.experiments.Experiments; +import org.team100.lib.hid.DriverXboxControl; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.Logging; +import org.team100.lib.profile.HolonomicProfile; +import org.team100.lib.state.ModelR3; +import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.subsystems.r3.commands.test.DriveToStateSimple; +import org.team100.lib.subsystems.test.OffsetDrivetrain; +import org.team100.lib.subsystems.test.TrivialDrivetrain; +import org.team100.lib.util.Banner; +import org.team100.lib.visualization.RobotPoseVisualization; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class Robot extends TimedRobot { + private static final double REEF_X = 3.089; + private static final double REEF_Y = 4.026; + private final Runnable m_robotViz; + private final Runnable m_toolpointViz; + private final DriverXboxControl m_controller; + + public Robot() { + Banner.printBanner(); + Experiments.instance.show(); + LoggerFactory field = Logging.instance().fieldLogger; + LoggerFactory log = Logging.instance().rootLogger; + + Translation2d offset = new Translation2d(1, 0); + TrivialDrivetrain robot = new TrivialDrivetrain( + new Pose2d(offset.unaryMinus(), Rotation2d.kZero)); + VelocitySubsystemR3 toolpoint = new OffsetDrivetrain( + robot, offset); + + m_robotViz = new RobotPoseVisualization( + field, () -> robot.getState().pose(), "robot"); + m_toolpointViz = new RobotPoseVisualization( + field, () -> toolpoint.getState().pose(), "ctrl"); + m_controller = new DriverXboxControl(0); + + setup1(log, toolpoint); + + setup2(log, toolpoint); + } + + /** Setup profiles to A and to B */ + private void setup1(LoggerFactory log, VelocitySubsystemR3 toolpoint) { + // this is intended to be *fast* + HolonomicProfile profile = HolonomicProfile.currentLimitedExponential( + 5, // v cartesian + 10, // a cartesian + 20, // stall a cartesian + 10, // omega + 10, // alpha + 20); // stall alpha + // this should try to control cartesian much harder than rotation + ControllerR3 controller = new FullStateControllerR3( + log, + 10, // p cartesian + 4, // p theta + 0.055, // p cartesian v + 0.005, // p theta v + 0.03, // x tol + 0.03, // theta tol + 0.1, // xdot tol + 0.1); + + // these aren't actually A and B, they're further apart to make the demo look + // better. + Pose2d A = new Pose2d(REEF_X, REEF_Y + 0.5, Rotation2d.kZero); + Pose2d B = new Pose2d(REEF_X, REEF_Y - 0.5, Rotation2d.kZero); + + DriveToPoseWithProfile toA = new DriveToPoseWithProfile( + log, toolpoint, controller, profile, () -> A); + new Trigger(m_controller::a).whileTrue(toA.until(toA::isDone)); + + DriveToPoseWithProfile toB = new DriveToPoseWithProfile( + log, toolpoint, controller, profile, () -> B); + new Trigger(m_controller::b).whileTrue(toB.until(toB::isDone)); + } + + /** Setup pure PID examples */ + private void setup2(LoggerFactory log, VelocitySubsystemR3 toolpoint) { + ControllerR3 gentleController = new FullStateControllerR3( + log, + 3, // p cartesian + 1, // p theta + 0.04, // p cartesian v + 0.01, // p theta v + 0.05, // x tol + 0.05, // theta tol + 1, // xdot tol + 1); // omega tol + MoveAndHold driveSimple = new DriveToStateSimple( + log, gentleController, toolpoint, new ModelR3()); + new Trigger(m_controller::x).whileTrue( + driveSimple.until(driveSimple::isDone)); + + MoveAndHold driveSimple2 = new DriveToStateSimple( + log, gentleController, toolpoint, + new ModelR3(new Pose2d(4, 4, Rotation2d.kCW_90deg))); + new Trigger(m_controller::y).whileTrue( + driveSimple2.until(driveSimple2::isDone)); + } + + @Override + public void robotPeriodic() { + Takt.update(); + Cache.refresh(); + CommandScheduler.getInstance().run(); + m_robotViz.run(); + m_toolpointViz.run(); + } + + @Override + public void teleopInit() { + + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void teleopExit() { + } + +} diff --git a/studies/offset_control/vendordeps/Phoenix5-frc2025-latest.json b/studies/offset_control/vendordeps/Phoenix5-frc2025-latest.json new file mode 100644 index 000000000..c1098dcce --- /dev/null +++ b/studies/offset_control/vendordeps/Phoenix5-frc2025-latest.json @@ -0,0 +1,171 @@ +{ + "fileName": "Phoenix5-frc2025-latest.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.35.1", + "frcYear": "2025", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + }, + { + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.35.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.35.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/studies/offset_control/vendordeps/Phoenix6-frc2025-latest.json b/studies/offset_control/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 000000000..cf873578b --- /dev/null +++ b/studies/offset_control/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,419 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.2.1", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/studies/offset_control/vendordeps/REVLib.json b/studies/offset_control/vendordeps/REVLib.json new file mode 100644 index 000000000..86ad287c8 --- /dev/null +++ b/studies/offset_control/vendordeps/REVLib.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/studies/offset_control/vendordeps/ReduxLib-2025.0.0.json b/studies/offset_control/vendordeps/ReduxLib-2025.0.0.json new file mode 100644 index 000000000..f15ff901b --- /dev/null +++ b/studies/offset_control/vendordeps/ReduxLib-2025.0.0.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib-2025.0.0.json", + "name": "ReduxLib", + "version": "2025.0.0", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.0", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.0", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/studies/offset_control/vendordeps/WPILibNewCommands.json b/studies/offset_control/vendordeps/WPILibNewCommands.json new file mode 100644 index 000000000..3718e0acd --- /dev/null +++ b/studies/offset_control/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/studies/offset_control/vendordeps/libgrapplefrc2025.json b/studies/offset_control/vendordeps/libgrapplefrc2025.json new file mode 100644 index 000000000..a49af315c --- /dev/null +++ b/studies/offset_control/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.1.3", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.1.3", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file