Examples with Angle me.wobblyyyy.pathfinder2.geometry.Angle used on opensource projects

Example 11 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class TrajectoryFactory method getLinearTrajectories.

public static List<Trajectory> getLinearTrajectories(List<PointXYZ> points, double speed, double tolerance, Angle angleTolerance) {
    List<Trajectory> trajectories = new ArrayList<>(points.size() - 1);
    for (PointXYZ point : points) {
        LinearTrajectory trajectory = new LinearTrajectory(point, speed, tolerance, angleTolerance);
        trajectories.add(trajectory);
    }
    return trajectories;
}

Example 12 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class MovementProfiler method capture.

/**
 * Capture a single {@link MovementSnapshot}.
 *
 * @param position the robot's current position.
 * @param timeMs   the current time, in milliseconds.
 * @return a new {@link MovementSnapshot}.
 */
public MovementSnapshot capture(PointXYZ position, double timeMs) {
    if (position == null)
        return new MovementSnapshot();
    if (this.timeMs == 0)
        this.timeMs = timeMs;
    Logger.trace(MovementProfiler.class, "Current position: %s, previous position: %s", position, this.position);
    double deltaMs = fixValue(timeMs - this.timeMs);
    double deltaSec = fixValue(deltaMs / 1_000);
    double dx = fixValue(this.position.distanceX(position));
    double dy = fixValue(this.position.distanceY(position));
    double dxy = fixValue(this.position.distance(position));
    double dzDeg = fixValue(position.z().deg() - this.position.z().deg());
    double dxPs = fixValue(dx / deltaSec);
    double dyPs = fixValue(dy / deltaSec);
    double dzDegPs = fixValue(dzDeg / deltaSec);
    double d = fixValue(dxy / deltaSec);
    double dXp = fixValue(dxPs - xUnitsPerSec);
    double dYp = fixValue(dxPs - yUnitsPerSec);
    double dZp = fixValue(dzDegPs - zDegreesPerSec);
    double dXps = fixValue(dXp / deltaSec);
    double dYps = fixValue(dYp / deltaSec);
    double dZps = fixValue(dZp / deltaSec);
    double dps = fixValue(d / deltaSec);
    Angle angle = this.position.angleTo(position);
    this.position = position;
    this.xUnitsPerSec = dxPs;
    this.yUnitsPerSec = dyPs;
    this.zDegreesPerSec = dyPs;
    this.timeMs = timeMs;
    Logger.trace(MovementProfiler.class, "delta ms: %s, delta sec: %s, dx: %s, dy: %s, dxy: %s, dzDeg: %s" + ", dxPs: %s, dyPs: %s, dzDegPs: %s, d: %s, dXp: %s, dYp: %s, dZp" + ": %s, dXps: %s, dYps: %s, dZps: %s, dps: %s, angle: %s, " + "position: %s, xUnitsPerSec: %s, yUnitsPerSec: %s, zDegreesPer" + "Sec: %s, timeMs: %s", Rounding.fastRound(deltaMs), Rounding.fastRound(deltaSec), Rounding.fastRound(dx), Rounding.fastRound(dy), Rounding.fastRound(dxy), Rounding.fastRound(dzDeg), Rounding.fastRound(dxPs), Rounding.fastRound(dyPs), Rounding.fastRound(dzDegPs), Rounding.fastRound(d), Rounding.fastRound(dXp), Rounding.fastRound(dYp), Rounding.fastRound(dZp), Rounding.fastRound(dXps), Rounding.fastRound(dYps), Rounding.fastRound(dZps), Rounding.fastRound(dps), angle, position, Rounding.fastRound(xUnitsPerSec), Rounding.fastRound(yUnitsPerSec), Rounding.fastRound(zDegreesPerSec), Rounding.fastRound(timeMs));
    MovementSnapshot snapshot = new MovementSnapshot().setAccelerationXY(dps).setAccelerationX(dXps).setAccelerationY(dYps).setAccelerationZ(Angle.fromDeg(dZps)).setVelocity(new Velocity(d, angle)).setVelocityXY(d).setVelocityX(dxPs).setVelocityY(dyPs).setVelocityZ(Angle.fromDeg(dzDegPs));
    lastSnapshot = snapshot;
    return snapshot;
}

Example 13 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class GenericFollower method tick.

@Override
public boolean tick(PointXYZ current, Consumer<Translation> consumer) {
    // alright - i'm writing this comment maybe... say... four months?
    // after writing this initial bit of code, and i would just like to
    // say - if you're here because you're trying to figure out what makes
    // the robot move, congrats! you've found it! enjoy some stupidly
    // thoroughly commented and insanely simple code!
    // with love,
    // - colin <3
    // Tue Jan 18 22:34:58
    // p.s. i also made all of the comments in this method lowercase now
    // so it matches the vibe better. you know what i mean?
    ValidationUtils.validate(current, "current");
    ValidationUtils.validate(consumer, "consumer");
    Logger.trace(GenericFollower.class, "Ticking follower (current pos: <%s>)", current, consumer);
    // right here.
    if (trajectory.isDone(current)) {
        Logger.debug(GenericFollower.class, "Finished follower for trajectory <%s>", trajectory);
        // because the trajectory has finished, we give the robot a
        // translation of zero - there's no point in moving anymore, right?
        consumer.accept(Translation.ZERO);
        // rest of the method won't get executed.
        return true;
    }
    // assuming the follower hasn't finished executing its trajectory,
    // we continue by finding a couple values and generating a shiny
    // new translation for the robot to follow.
    // get the next marker by calling the trajectory's nextMarker
    // method. this method should always return a target point we can
    // use to generate a new translation.
    PointXYZ nextMarker = trajectory.nextMarker(current);
    // and of course, the speed, because some trajectories can control
    // speed differently.
    double speed = trajectory.speed(current);
    // determine the delta between the two angles, so we can calculate
    // turn in just a moment.
    double angleDelta = Angle.minimumDelta(current.z(), nextMarker.z());
    // and calculate turn. remember, the turn controller's only job is to
    // get the delta to zero, so by feeding it the delta it should output
    // a value that will minimize that aforementioned delta.
    double turn = turnController.calculate(angleDelta);
    // getRelativeTranslation returns a RELATIVE translation (meaning it
    // can be applied to the robot)
    Translation translation = Follower.getRelativeTranslation(current, nextMarker, speed, turn);
    Logger.trace(GenericFollower.class, "Next marker: <%s>, speed: <%s>, angle delta: <%s deg>, " + "turn value: <%s>, current translation: <%s>", nextMarker, speed, angleDelta, turn, translation);
    // use the consumer to accept a translation we create.
    // instead of creating an absolute translation, which would only help
    // if the robot is facing straight forwards, we have to use a relative
    // translation. the follower interface's static method that we
    // call here (getRelativeTranslation) first generates an absolute
    // translation and then converts it to a relative one. epic sauce.
    consumer.accept(translation);
    // we're not done yet, so return false.
    return false;
}

Example 14 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class EasyOdometry method buildOdometry.

public static Odometry buildOdometry(Supplier<Double> xPos, Supplier<Double> yPos, Supplier<Angle> zPos) {
    return new AbstractOdometry() {

        @Override
        public PointXYZ getRawPosition() {
            double x = xPos.get();
            double y = yPos.get();
            Angle z = zPos.get();
            return new PointXYZ(x, y, z);
        }
    };
}

Example 15 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class SwerveChassisOdometry method getRawPosition.

@Override
public PointXYZ getRawPosition() {
    double currentTimeMs = Time.ms();
    Angle gyroAngle = getGyroAngle.get();
    SwerveModuleState frontRightState = frontRightOdometry.getState();
    SwerveModuleState frontLeftState = frontLeftOdometry.getState();
    SwerveModuleState backRightState = backRightOdometry.getState();
    SwerveModuleState backLeftState = backLeftOdometry.getState();
    SwerveState state = new SwerveState(frontRightState, frontLeftState, backRightState, backLeftState);
    return odometry.updateWithTime(currentTimeMs, gyroAngle, state);
}