Geometry Classes
Throughout the code, we use various geometry classes that makes working in 2d space easier.
Take a look at Pose Key for an overview.
Each of these classes implement interface State<S>, which means they all provide a way to add(S other) two states together and you can find the distance(S other) from another state.
This makes finding the difference between two states very easy.
Consider the following example, where you're trying to control a linear axis where + is where we are ^ is where we're trying to go:
--------^----------+
-10 0 +10
Where we want to go: -1 Where we are: +10
Our "error" is therefore -11 - we need to move 11 units to the left, or more simply -11 units
More generally, we use the following formula for error
error = setpoint - process value
Displacement1d
For simplicity's sake, we start with Displacement1d.
Displacement1d simply wraps a double.
To find the error between two Displacement1ds, there may not be a .minus() function, but we can add the inverse.
Displacement1d current = new Displacement1d(10);
Displacement1d goal = new Displacement1d(-1);
System.out.println(goal.add(current.inverse()).x())
When run, this outputs:
-11Translation2d
Translation2ds are (x, y) points or 2d vectors.
We can add them just as we can with any other state.
Consider we are at (4,4) and we want to go to (3,6).
Taking the difference between two points becomes:
(3-4, 6-4) = (-1, 2)
Conceptually, this calculates "In the reference frame of where I am, what do I need to do to get where I'm trying to go?"
In code, this looks like:
Translation2d current = new Translation2d(4, 4);
Translation2d goal = new Translation2d(3, 6);
System.out.println(goal.add(current.inverse()))
When run, this outputs:
(-1.00, 2.00)
Pose2d
We use poses to represent where the robot is on the field. Consider the following code snippet and diagram below.
import React from "react";
import AcesFieldPlot, { Pose2dPlotParams } from "./AcesFieldPlot";
import { Pose2d, Translation2d, Rotation2d } from "./geometry";
export default function Pose2dExample1() {
const p1 = new Pose2d(new Translation2d(10, 0), Rotation2d.identity());
const p2 = p1.transformBy(
new Pose2d(new Translation2d(5, 20), Rotation2d.identity()),
);
const p3 = new Pose2d(
new Translation2d(-40, -20),
Rotation2d.fromDegrees(45),
);
const p4 = p3.transformBy(
new Pose2d(new Translation2d(20, 5), Rotation2d.identity()),
);
return (
<AcesFieldPlot
robotPose={null}
otherPoses={[
new Pose2dPlotParams(p1).setLabel("p1"),
new Pose2dPlotParams(p2).setStrokeStyle("lightgreen").setLabel("p2"),
new Pose2dPlotParams(p3).setLabel("p3"),
new Pose2dPlotParams(p4).setLabel("p4"),
]}
/>
);
}
The following diagram plots the following two custom poses. Try changing p1's thetaDegree. Notice how the transform is applied "in the reference frame of" p1
const p1 = new Pose2d(new Translation2d(p1data.x, p1data.y), Rotation2d.fromDegrees(p1data.thetaDegrees))
const p2 = p1.transformBy(new Pose2d(new Translation2d(p2data.x, p2data.y), Rotation2d.fromDegrees(p2data.thetaDegrees)))