3 | 4 | Autonomous motion planning and control library for wheeled mobile robots. 5 |
6 | Successor to Pathfinder. 7 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
2 | 
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
15 | * Pathfinder contains many hardware components that are not used anywhere 16 | * in Pathfinder's source code. This is done for standardization purposes - 17 | * a generic sensor, such as {@link me.wobblyyyy.pathfinder2.robot.sensors.Accelerometer}, 18 | * can now be used across many different projects without relying on 19 | * vendor-specific classes. 20 | *
21 | */ 22 | package me.wobblyyyy.pathfinder2.robot; 23 | -------------------------------------------------------------------------------- /pathfinder2-OdometryCore/src/main/java/me/wobblyyyy/pathfinder2/odometrycore/TrackableWrapper.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.odometrycore; 12 | 13 | import com.tejasmehta.OdometryCore.trackables.OdometryTrackable; 14 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 15 | import me.wobblyyyy.pathfinder2.robot.AbstractOdometry; 16 | 17 | public abstract class TrackableWrapper 18 | extends AbstractOdometry 19 | implements OdometryTrackable { 20 | 21 | @Override 22 | public PointXYZ getRawPosition() { 23 | return OdometryCoreUtils.fromOdometryPosition(getCurrentPosition()); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/pathgen/GridScaling.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.pathgen; 12 | 13 | public class GridScaling { 14 | private final double scaleX; 15 | private final double scaleY; 16 | 17 | public GridScaling(double scale) { 18 | this(scale, scale); 19 | } 20 | 21 | public GridScaling(double scaleX, double scaleY) { 22 | this.scaleX = scaleX; 23 | this.scaleY = scaleY; 24 | } 25 | 26 | public double getScaleX() { 27 | return scaleX; 28 | } 29 | 30 | public double getScaleY() { 31 | return scaleY; 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/robot/sensors/MetersDistanceSensor.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.robot.sensors; 12 | 13 | import me.wobblyyyy.pathfinder2.units.Conversions; 14 | import me.wobblyyyy.pathfinder2.units.Unit; 15 | 16 | public abstract class MetersDistanceSensor implements DistanceSensor { 17 | 18 | @Override 19 | public double getDistanceInches() { 20 | return Conversions.mToInches(getDistanceMeters()); 21 | } 22 | 23 | @Override 24 | public double getDistanceCentimeters() { 25 | return Conversions.convert(Unit.M, Unit.CM, getDistanceMeters()); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/robot/led/LedController.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.robot.led; 12 | 13 | /** 14 | * An interface for controlling an LED. 15 | * 16 | * @author Colin Robertson 17 | * @since 0.6.1 18 | */ 19 | public interface LedController { 20 | /** 21 | * Set the color of the LEDs. 22 | * 23 | * @param color the color to set to the LEDs. 24 | */ 25 | void setColor(LedColor color); 26 | 27 | /** 28 | * Set the pattern of the LEDs. 29 | * 30 | * @param pattern the pattern to set to the LEDs. 31 | */ 32 | void setPattern(LedPattern pattern); 33 | } 34 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/robot/sensors/CentimetersDistanceSensor.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.robot.sensors; 12 | 13 | import me.wobblyyyy.pathfinder2.units.Conversions; 14 | import me.wobblyyyy.pathfinder2.units.Unit; 15 | 16 | public abstract class CentimetersDistanceSensor implements DistanceSensor { 17 | 18 | @Override 19 | public double getDistanceInches() { 20 | return Conversions.cmToInches(getDistanceCentimeters()); 21 | } 22 | 23 | @Override 24 | public double getDistanceMeters() { 25 | return Conversions.convert(Unit.CM, Unit.M, getDistanceCentimeters()); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/robot/simulated/EmptyOdometry.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.robot.simulated; 12 | 13 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 14 | import me.wobblyyyy.pathfinder2.robot.AbstractOdometry; 15 | 16 | /** 17 | * An entirely empty odometry implementation. This will always return a 18 | * position of (0, 0, 0) using {@link PointXYZ#zero()}. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.1.0 22 | */ 23 | public class EmptyOdometry extends AbstractOdometry { 24 | 25 | @Override 26 | public PointXYZ getRawPosition() { 27 | return PointXYZ.zero(); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /.idea/compiler.xml: -------------------------------------------------------------------------------- 1 | 2 |30 | * Classes in this package are pretty much only useful inside the library. 31 | *
32 | */ 33 | package me.wobblyyyy.pathfinder2.execution; 34 | -------------------------------------------------------------------------------- /.github/project_structure.md: -------------------------------------------------------------------------------- 1 |pathfinder2-corepathfinder2-geometrypathfinder2-kinematicspathfinder2-OdometryCorepathfinder2-examples18 | * Internally, this simply converts the point slope into a slope intercept. 19 | * This means you can't get "the point." 20 | *
21 | * 22 | * @author Colin Robertson 23 | * @since 0.1.0 24 | */ 25 | public class PointSlope extends SlopeIntercept { 26 | 27 | /** 28 | * Create a new {@code PointSlope} instance. 29 | * 30 | * @param point the point to use. 31 | * @param slope the slope to use. 32 | */ 33 | public PointSlope(PointXY point, double slope) { 34 | super(slope, (slope * -point.x()) + point.y()); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /pathfinder2-OdometryCore/src/main/java/com/tejasmehta/OdometryCore/trackables/ThreeWheelTrackable.java: -------------------------------------------------------------------------------- 1 | /**** 2 | * Made by Tejas Mehta 3 | * Made on Monday, March 29, 2021 4 | * File Name: ThreeWheelTrackable 5 | * Package: com.tejasmehta.OdometryCore.trackables*/ 6 | package com.tejasmehta.OdometryCore.trackables; 7 | 8 | import com.tejasmehta.OdometryCore.localization.OdometryPosition; 9 | 10 | public class ThreeWheelTrackable implements OdometryTrackable { 11 | private final double wheelDiameter; 12 | private final double cpr; 13 | private final double leftOffset; 14 | private final double rightOffset; 15 | private final double frontBackOffset; 16 | 17 | ThreeWheelTrackable( 18 | double cpr, 19 | double wheelDiameter, 20 | double leftOffset, 21 | double rightOffset, 22 | double frontBackOffset 23 | ) { 24 | this.wheelDiameter = wheelDiameter; 25 | this.cpr = cpr; 26 | this.leftOffset = leftOffset; 27 | this.rightOffset = rightOffset; 28 | this.frontBackOffset = frontBackOffset; 29 | } 30 | 31 | @Override 32 | public OdometryPosition getCurrentPosition() { 33 | return null; 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/robot/sensors/ARGB.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.robot.sensors; 12 | 13 | /** 14 | * A representation of an ARGB color. 15 | * 16 | * @author Colin Robertson 17 | * @since 0.7.1 18 | */ 19 | public class ARGB { 20 | private final int alpha; 21 | private final int red; 22 | private final int green; 23 | private final int blue; 24 | 25 | public ARGB(int alpha, int red, int green, int blue) { 26 | this.alpha = alpha; 27 | this.red = red; 28 | this.green = green; 29 | this.blue = blue; 30 | } 31 | 32 | public int alpha() { 33 | return alpha; 34 | } 35 | 36 | public int red() { 37 | return red; 38 | } 39 | 40 | public int green() { 41 | return green; 42 | } 43 | 44 | public int blue() { 45 | return blue; 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/exceptions/InvalidSpeedException.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.exceptions; 12 | 13 | /** 14 | * Exception to be thrown whenever a speed value is invalid. 15 | * 16 | *17 | * In most cases, speed values should always fit within the range of 0.0 18 | * to 1.0. If you're getting this exception during program execution, actually, 19 | * it's fairly likely the cause is a speed value outside of that range. 20 | *
21 | * 22 | * @author Colin Robertson 23 | * @since 0.0.0 24 | */ 25 | public class InvalidSpeedException extends RuntimeException { 26 | 27 | public InvalidSpeedException(String s) { 28 | super(s); 29 | } 30 | 31 | public static void throwIfInvalid(String message, double speed) { 32 | if (speed < 0 || speed > 1) { 33 | throw new InvalidSpeedException(message); 34 | } 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /.idea/gradle.xml: -------------------------------------------------------------------------------- 1 | 2 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
2 |
3 |
4 | Autonomous motion planning and control library for wheeled mobile robots.
5 |
6 | Successor to Pathfinder.
7 |
24 | * Interestingly enough, this code... sucks. If you can rewrite it so it's 25 | * cleaner and/or faster, you're more than welcome to do so. 26 | *
27 | */ 28 | package me.wobblyyyy.pathfinder2.pathgen; 29 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/trajectory/EmptyTrajectory.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.trajectory; 12 | 13 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 14 | 15 | /** 16 | * An {@code EmptyTrajectory} is a trajectory that does absolutely nothing. 17 | * The next marker point is always the current point, the trajectory is never 18 | * done, and the speed is always 0. This should be used when you want 19 | * Pathfinder to do absolutely nothing. 20 | * 21 | * @author Colin Robertson 22 | * @since 0.0.0 23 | */ 24 | public class EmptyTrajectory implements Trajectory { 25 | 26 | @Override 27 | public PointXYZ nextMarker(PointXYZ current) { 28 | return current; 29 | } 30 | 31 | @Override 32 | public boolean isDone(PointXYZ current) { 33 | return false; 34 | } 35 | 36 | @Override 37 | public double speed(PointXYZ current) { 38 | return 0; 39 | } 40 | 41 | @Override 42 | public String toString() { 43 | return "EmptyTrajectory"; 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /pathfinder2-core/src/jmh/java/me/wobblyyyy/pathfinder2/trajectory/BenchmarkLinearTrajectory.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.trajectory; 12 | 13 | import me.wobblyyyy.pathfinder2.geometry.Angle; 14 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 15 | import org.openjdk.jmh.annotations.Benchmark; 16 | import org.openjdk.jmh.annotations.Scope; 17 | import org.openjdk.jmh.annotations.State; 18 | import org.openjdk.jmh.infra.Blackhole; 19 | 20 | @State(Scope.Benchmark) 21 | public class BenchmarkLinearTrajectory extends GenericTrajectoryBenchmarker { 22 | private static final double speed = 0.5; 23 | private static final double tolerance = 2; 24 | private static final Angle angleTolerance = Angle.fromDeg(5); 25 | 26 | private static final Trajectory trajectory = new LinearTrajectory( 27 | new PointXYZ(-10, -10, 45), 28 | speed, 29 | tolerance, 30 | angleTolerance 31 | ); 32 | 33 | @Benchmark 34 | public void benchmark(Blackhole bh) { 35 | followTrajectories(bh, trajectory); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/pathgen/AStarPathFinder.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.pathgen; 12 | 13 | import java.util.List; 14 | import me.wobblyyyy.pathfinder2.geometry.PointXY; 15 | import me.wobblyyyy.pathfinder2.zones.Zone; 16 | 17 | /** 18 | * I'm not entirely sure why I made this class, to be honest. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.1.0 22 | */ 23 | public class AStarPathFinder { 24 | private final LocalizedPathGen pathGen; 25 | 26 | public AStarPathFinder( 27 | GridScaling scale, 28 | double robotWidth, 29 | double robotLength, 30 | List15 | * As you can (probably) see, the Pathfinder project is split up into many 16 | * different packages for organizational purposes. The main 17 | * {@link me.wobblyyyy.pathfinder2.Pathfinder} class is the only class that's 18 | * in the package {@code me.wobblyyyy.pathfinder2} - all other classes are 19 | * in subpackages, such as {@code robot} or {@code geometry}. 20 | *
21 | * 22 | *23 | * The name of each of the subpackages should describe it's purpose fairly 24 | * well. If you're browsing Pathfinder's source code, you might want to take 25 | * note of the module structure there is - there's {@code pathfinder2-geometry}, 26 | * as well as {@code pathfinder2-core} et al. 27 | *
28 | * 29 | *30 | * There's a lot of content in this library, so I can't exactly list everything 31 | * in one convenient document, but if you'd like to get started learning the 32 | * library, you should check out the {@link me.wobblyyyy.pathfinder2.Pathfinder} 33 | * class and anything associated with it. Another solid idea is to check out 34 | * tutorials linked on the project's GitHub page - those might help. 35 | *
36 | */ 37 | package me.wobblyyyy.pathfinder2; 38 | -------------------------------------------------------------------------------- /pathfinder2-geometry/src/main/java/me/wobblyyyy/pathfinder2/math/LinearSpline.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.math; 12 | 13 | import me.wobblyyyy.pathfinder2.geometry.LinearEquation; 14 | import me.wobblyyyy.pathfinder2.geometry.PointXY; 15 | import me.wobblyyyy.pathfinder2.utils.StringUtils; 16 | 17 | /** 18 | * A spline based on a linear equation. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.6.1 22 | */ 23 | public class LinearSpline implements Spline { 24 | private final LinearEquation equation; 25 | 26 | public LinearSpline(LinearEquation equation) { 27 | this.equation = equation; 28 | } 29 | 30 | @Override 31 | public double interpolateY(double x) { 32 | return equation.getY(x); 33 | } 34 | 35 | @Override 36 | public PointXY interpolate(double x) { 37 | return new PointXY(x, this.equation.getY(x)); 38 | } 39 | 40 | @Override 41 | public PointXY getStartPoint() { 42 | return interpolate(0); 43 | } 44 | 45 | @Override 46 | public PointXY getEndPoint() { 47 | return interpolate(0); 48 | } 49 | 50 | @Override 51 | public String toString() { 52 | return StringUtils.format( 53 | "LinearSpline (slope: <%s>)", 54 | equation.getSlope() 55 | ); 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /pathfinder2-commands/src/main/java/me/wobblyyyy/pathfinder2/commands/ScriptingCommands.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.commands; 12 | 13 | import java.util.Arrays; 14 | import me.wobblyyyy.pathfinder2.logging.Logger; 15 | import me.wobblyyyy.pathfinder2.utils.StringUtils; 16 | 17 | public class ScriptingCommands { 18 | public static final String DEF_PREFIX = "var_"; 19 | 20 | private ScriptingCommands() {} 21 | 22 | public static final Command DEF_COMMAND = new Command( 23 | "def", 24 | (pathfinder, args) -> { 25 | String name = args[0]; 26 | String[] valueArray = new String[args.length - 1]; 27 | System.arraycopy(args, 1, valueArray, 0, valueArray.length); 28 | String value = StringUtils.concat(valueArray); 29 | pathfinder.putData(DEF_PREFIX + name, value); 30 | Logger.debug( 31 | ScriptingCommands.class, 32 | "Defined variable <%s> with value <%s> (args: <%s>)", 33 | DEF_PREFIX + name, 34 | value, 35 | Arrays.toString(args) 36 | ); 37 | }, 38 | 2, 39 | Integer.MAX_VALUE 40 | ); 41 | 42 | public static void addScriptingCommands(CommandRegistry registry) { 43 | registry.unsafeAdd(DEF_COMMAND); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/robot/simulated/SimulatedWrapper.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.robot.simulated; 12 | 13 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 14 | import me.wobblyyyy.pathfinder2.robot.Robot; 15 | 16 | /** 17 | * A wrapper for instances of {@link SimulatedDrive} and 18 | * {@link SimulatedOdometry}. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.15.0 22 | */ 23 | public class SimulatedWrapper { 24 | private final SimulatedDrive drive; 25 | private final SimulatedOdometry odometry; 26 | 27 | public SimulatedWrapper(SimulatedDrive drive, SimulatedOdometry odometry) { 28 | this.drive = drive; 29 | this.odometry = odometry; 30 | 31 | this.drive.setDriveModifier( 32 | translation -> { 33 | PointXYZ pos = odometry.getPosition(); 34 | 35 | odometry.setRawPosition(pos.applyTranslation(translation)); 36 | 37 | return translation; 38 | } 39 | ); 40 | } 41 | 42 | public SimulatedDrive getDrive() { 43 | return drive; 44 | } 45 | 46 | public SimulatedOdometry getOdometry() { 47 | return odometry; 48 | } 49 | 50 | public Robot getRobot() { 51 | return new Robot(drive, odometry); 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /pathfinder2-geometry/src/main/java/me/wobblyyyy/pathfinder2/geometry/BoundingBox.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.geometry; 12 | 13 | /** 14 | * Utilities for verifying numbers. I'd encourage you to not use this class, 15 | * it's only still here so nothing breaks. 16 | * 17 | * @author Colin Robertson 18 | * @since 0.1.0 19 | */ 20 | public class BoundingBox { 21 | 22 | private BoundingBox() {} 23 | 24 | public static boolean validate(double number, double min, double max) { 25 | return (min - 0.01) <= number && number <= (max + 0.01); 26 | } 27 | 28 | public static boolean isPointInBox( 29 | PointXY point, 30 | double minX, 31 | double minY, 32 | double maxX, 33 | double maxY 34 | ) { 35 | if (point == null) return false; 36 | 37 | double x = point.x(); 38 | double y = point.y(); 39 | 40 | boolean validX = validate(x, minX, maxX); 41 | boolean validY = validate(y, minY, maxY); 42 | 43 | return validX && validY; 44 | } 45 | 46 | public static boolean isPointInLine(PointXY point, Line line) { 47 | return isPointInBox( 48 | point, 49 | line.getMinX(), 50 | line.getMinY(), 51 | line.getMaxX(), 52 | line.getMaxY() 53 | ); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /pathfinder2-frc/src/main/java/me/wobblyyyy/pathfinder2/ctre/TalonFXMotor.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.ctre; 12 | 13 | import com.ctre.phoenix.motorcontrol.TalonFXControlMode; 14 | import com.ctre.phoenix.motorcontrol.can.TalonFX; 15 | import me.wobblyyyy.pathfinder2.robot.components.AbstractMotor; 16 | 17 | /** 18 | * Wrapper for {@link TalonFX}. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.10.8 22 | */ 23 | public class TalonFXMotor extends AbstractMotor { 24 | private final TalonFX talon; 25 | 26 | /** 27 | * Create a new {@code TalonFXMotor}. 28 | * 29 | * @param talon the {@link TalonFX} to use. 30 | */ 31 | public TalonFXMotor(TalonFX talon) { 32 | super( 33 | power -> talon.set(TalonFXControlMode.PercentOutput, power), 34 | talon::getMotorOutputPercent 35 | ); 36 | this.talon = talon; 37 | } 38 | 39 | /** 40 | * Create a new {@code TalonFXMotor}. 41 | * 42 | * @param deviceNumber the Talon's device number. 43 | */ 44 | public TalonFXMotor(int deviceNumber) { 45 | this(new TalonFX(deviceNumber)); 46 | } 47 | 48 | /** 49 | * Get the internal TalonFX. 50 | * 51 | * @return the internal TalonFX. 52 | */ 53 | public TalonFX getTalon() { 54 | return talon; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /pathfinder2-kinematics/src/main/java/me/wobblyyyy/pathfinder2/control/SimpleSwerveModuleController.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.control; 12 | 13 | /** 14 | * The simplest form of control for a swerve module. This class is an 15 | * extension of the {@link AngleDeltaController} class and only provides 16 | * a single constructor with a single parameter - the coefficient. Higher 17 | * coefficient values will mean the swerve module will turn faster. Lower 18 | * coefficient values will mean the swerve module will turn slower. It 19 | * typically takes some fine tuning to get this right. 20 | * 21 | * @author Colin Robertson 22 | * @since 0.0.0 23 | */ 24 | public class SimpleSwerveModuleController extends AngleDeltaController { 25 | 26 | /** 27 | * Create a new {@code SimpleSwerveModuleController}. 28 | * 29 | * @param coefficient the turn speed coefficient. Higher values will 30 | * make the swerve module turn faster, and lower values 31 | * will make the swerve module turn slower. This value 32 | * should almost always be determined experimentally, 33 | * but a decent place to start is around 0.02. 34 | */ 35 | public SimpleSwerveModuleController(double coefficient) { 36 | super(coefficient); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /pathfinder2-commands/src/main/java/me/wobblyyyy/pathfinder2/commands/TestCommands.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.commands; 12 | 13 | import me.wobblyyyy.pathfinder2.geometry.Angle; 14 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 15 | import me.wobblyyyy.pathfinder2.utils.AssertionUtils; 16 | 17 | public class TestCommands { 18 | 19 | private TestCommands() {} 20 | 21 | public static final Command ASSERT_IS_NEAR_COMMAND = new Command( 22 | "assertIsNear", 23 | (pathfinder, args) -> { 24 | PointXYZ target = PointXYZ.parse(args[0]); 25 | double tolerance = 2; 26 | Angle angleTolerance = Angle.fromDeg(5); 27 | if (args.length > 1) { 28 | tolerance = Double.parseDouble(args[1]); 29 | } 30 | if (args.length > 2) { 31 | angleTolerance = Angle.parse(args[2]); 32 | } 33 | PointXYZ current = pathfinder.getPosition(); 34 | AssertionUtils.assertIsNear( 35 | target, 36 | current, 37 | tolerance, 38 | angleTolerance 39 | ); 40 | }, 41 | 1, 42 | 3 43 | ); 44 | 45 | public static void addTestCommands(CommandRegistry registry) { 46 | registry.unsafeAdd(ASSERT_IS_NEAR_COMMAND); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /pathfinder2-frc/src/main/java/me/wobblyyyy/pathfinder2/ctre/TalonSRXMotor.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.ctre; 12 | 13 | import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; 14 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 15 | import me.wobblyyyy.pathfinder2.robot.components.AbstractMotor; 16 | 17 | /** 18 | * Wrapper for {@link TalonSRX}. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.10.8 22 | */ 23 | public class TalonSRXMotor extends AbstractMotor { 24 | private final TalonSRX talon; 25 | 26 | /** 27 | * Create a new {@code TalonSRXMotor}. 28 | * 29 | * @param talon the {@link TalonSRX} to use. 30 | */ 31 | public TalonSRXMotor(TalonSRX talon) { 32 | super( 33 | power -> talon.set(TalonSRXControlMode.PercentOutput, power), 34 | talon::getMotorOutputPercent 35 | ); 36 | this.talon = talon; 37 | } 38 | 39 | /** 40 | * Create a new {@code TalonSRXMotor}. 41 | * 42 | * @param deviceNumber the Talon's device number. 43 | */ 44 | public TalonSRXMotor(int deviceNumber) { 45 | this(new TalonSRX(deviceNumber)); 46 | } 47 | 48 | /** 49 | * Get the internal Talon SRX. 50 | * 51 | * @return the internal Talon SRX. 52 | */ 53 | public TalonSRX getTalon() { 54 | return talon; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/listening/PathfinderListener.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.listening; 12 | 13 | import java.util.function.Predicate; 14 | import java.util.function.Supplier; 15 | import me.wobblyyyy.pathfinder2.Pathfinder; 16 | 17 | /** 18 | * A listener that accepts {@code Pathfinder} as a parameter. 19 | * 20 | * @author Colin Robertson 21 | * @since 0.7.1 22 | * @deprecated this is some pretty bad code and I wouldn't recommend you make 23 | * use of it... 24 | */ 25 | @Deprecated 26 | public class PathfinderListener implements Supplier20 | * The most commonly-used parts of the geometry package are {@code PointXY}, 21 | * {@code PointXYZ}, and {@code Angle}. Rectangles, triangles, and shapes 22 | * in general are made available, but you won't need to use them to get 23 | * the library up and running. 24 | *
25 | * 26 | *27 | * This is also the only part of the Pathfinder2 project that is not meant 28 | * exclusively for use within Pathfinder2 - none of the code here is very 29 | * specific and can be used just about anywhere. 30 | *
31 | * 32 | *33 | * This isn't really an API guide or anything, but you're more than welcome 34 | * to add exhaustive tests. In order for this library to function properly, 35 | * the geometry portion has to function perfectly, and adding more tests can 36 | * never hurt, right? Right. 37 | *
38 | */ 39 | package me.wobblyyyy.pathfinder2.geometry; 40 | -------------------------------------------------------------------------------- /pathfinder2-core/src/main/java/me/wobblyyyy/pathfinder2/plugin/bundled/GyroOdometry.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022. 3 | * 4 | * This file is part of the "Pathfinder2" project, available here: 5 | * GitHub 6 | * 7 | * This project is licensed under the GNU GPL V3 license. 8 | * GNU GPL V3 9 | */ 10 | 11 | package me.wobblyyyy.pathfinder2.plugin.bundled; 12 | 13 | import java.util.function.Supplier; 14 | import me.wobblyyyy.pathfinder2.Pathfinder; 15 | import me.wobblyyyy.pathfinder2.geometry.PointXYZ; 16 | import me.wobblyyyy.pathfinder2.plugin.PathfinderPlugin; 17 | 18 | /** 19 | * @author Colin Robertson 20 | * @since 0.10.3 21 | */ 22 | public class GyroOdometry extends PathfinderPlugin { 23 | private static final String NAME = "GyroOdometry"; 24 | 25 | private final Supplier