diff --git a/trajectory/res/field.png b/trajectory/res/field.png new file mode 100644 index 0000000..bf20a96 Binary files /dev/null and b/trajectory/res/field.png differ diff --git a/trajectory/com/team254/lib/control/AdaptivePurePursuitController.java b/trajectory/src/com/team254/lib/control/AdaptivePurePursuitController.java similarity index 100% rename from trajectory/com/team254/lib/control/AdaptivePurePursuitController.java rename to trajectory/src/com/team254/lib/control/AdaptivePurePursuitController.java diff --git a/trajectory/com/team254/lib/control/Lookahead.java b/trajectory/src/com/team254/lib/control/Lookahead.java similarity index 100% rename from trajectory/com/team254/lib/control/Lookahead.java rename to trajectory/src/com/team254/lib/control/Lookahead.java diff --git a/trajectory/com/team254/lib/control/Path.java b/trajectory/src/com/team254/lib/control/Path.java similarity index 100% rename from trajectory/com/team254/lib/control/Path.java rename to trajectory/src/com/team254/lib/control/Path.java diff --git a/trajectory/com/team254/lib/control/PathFollower.java b/trajectory/src/com/team254/lib/control/PathFollower.java similarity index 100% rename from trajectory/com/team254/lib/control/PathFollower.java rename to trajectory/src/com/team254/lib/control/PathFollower.java diff --git a/trajectory/com/team254/lib/control/PathSegment.java b/trajectory/src/com/team254/lib/control/PathSegment.java similarity index 100% rename from trajectory/com/team254/lib/control/PathSegment.java rename to trajectory/src/com/team254/lib/control/PathSegment.java diff --git a/trajectory/com/team254/lib/geometry/Displacement1d.java b/trajectory/src/com/team254/lib/geometry/Displacement1d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/Displacement1d.java rename to trajectory/src/com/team254/lib/geometry/Displacement1d.java diff --git a/trajectory/com/team254/lib/geometry/ICurvature.java b/trajectory/src/com/team254/lib/geometry/ICurvature.java similarity index 100% rename from trajectory/com/team254/lib/geometry/ICurvature.java rename to trajectory/src/com/team254/lib/geometry/ICurvature.java diff --git a/trajectory/com/team254/lib/geometry/IPose2d.java b/trajectory/src/com/team254/lib/geometry/IPose2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/IPose2d.java rename to trajectory/src/com/team254/lib/geometry/IPose2d.java diff --git a/trajectory/com/team254/lib/geometry/IRotation2d.java b/trajectory/src/com/team254/lib/geometry/IRotation2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/IRotation2d.java rename to trajectory/src/com/team254/lib/geometry/IRotation2d.java diff --git a/trajectory/com/team254/lib/geometry/ITranslation2d.java b/trajectory/src/com/team254/lib/geometry/ITranslation2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/ITranslation2d.java rename to trajectory/src/com/team254/lib/geometry/ITranslation2d.java diff --git a/trajectory/com/team254/lib/geometry/Pose2d.java b/trajectory/src/com/team254/lib/geometry/Pose2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/Pose2d.java rename to trajectory/src/com/team254/lib/geometry/Pose2d.java diff --git a/trajectory/com/team254/lib/geometry/Pose2dWithCurvature.java b/trajectory/src/com/team254/lib/geometry/Pose2dWithCurvature.java similarity index 100% rename from trajectory/com/team254/lib/geometry/Pose2dWithCurvature.java rename to trajectory/src/com/team254/lib/geometry/Pose2dWithCurvature.java diff --git a/trajectory/com/team254/lib/geometry/Rotation2d.java b/trajectory/src/com/team254/lib/geometry/Rotation2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/Rotation2d.java rename to trajectory/src/com/team254/lib/geometry/Rotation2d.java diff --git a/trajectory/com/team254/lib/geometry/State.java b/trajectory/src/com/team254/lib/geometry/State.java similarity index 100% rename from trajectory/com/team254/lib/geometry/State.java rename to trajectory/src/com/team254/lib/geometry/State.java diff --git a/trajectory/com/team254/lib/geometry/Translation2d.java b/trajectory/src/com/team254/lib/geometry/Translation2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/Translation2d.java rename to trajectory/src/com/team254/lib/geometry/Translation2d.java diff --git a/trajectory/com/team254/lib/geometry/Twist2d.java b/trajectory/src/com/team254/lib/geometry/Twist2d.java similarity index 100% rename from trajectory/com/team254/lib/geometry/Twist2d.java rename to trajectory/src/com/team254/lib/geometry/Twist2d.java diff --git a/trajectory/com/team254/lib/motion/MotionProfile.java b/trajectory/src/com/team254/lib/motion/MotionProfile.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionProfile.java rename to trajectory/src/com/team254/lib/motion/MotionProfile.java diff --git a/trajectory/com/team254/lib/motion/MotionProfileConstraints.java b/trajectory/src/com/team254/lib/motion/MotionProfileConstraints.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionProfileConstraints.java rename to trajectory/src/com/team254/lib/motion/MotionProfileConstraints.java diff --git a/trajectory/com/team254/lib/motion/MotionProfileGenerator.java b/trajectory/src/com/team254/lib/motion/MotionProfileGenerator.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionProfileGenerator.java rename to trajectory/src/com/team254/lib/motion/MotionProfileGenerator.java diff --git a/trajectory/com/team254/lib/motion/MotionProfileGoal.java b/trajectory/src/com/team254/lib/motion/MotionProfileGoal.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionProfileGoal.java rename to trajectory/src/com/team254/lib/motion/MotionProfileGoal.java diff --git a/trajectory/com/team254/lib/motion/MotionSegment.java b/trajectory/src/com/team254/lib/motion/MotionSegment.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionSegment.java rename to trajectory/src/com/team254/lib/motion/MotionSegment.java diff --git a/trajectory/com/team254/lib/motion/MotionState.java b/trajectory/src/com/team254/lib/motion/MotionState.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionState.java rename to trajectory/src/com/team254/lib/motion/MotionState.java diff --git a/trajectory/com/team254/lib/motion/MotionUtil.java b/trajectory/src/com/team254/lib/motion/MotionUtil.java similarity index 100% rename from trajectory/com/team254/lib/motion/MotionUtil.java rename to trajectory/src/com/team254/lib/motion/MotionUtil.java diff --git a/trajectory/com/team254/lib/motion/ProfileFollower.java b/trajectory/src/com/team254/lib/motion/ProfileFollower.java similarity index 100% rename from trajectory/com/team254/lib/motion/ProfileFollower.java rename to trajectory/src/com/team254/lib/motion/ProfileFollower.java diff --git a/trajectory/com/team254/lib/motion/SetpointGenerator.java b/trajectory/src/com/team254/lib/motion/SetpointGenerator.java similarity index 100% rename from trajectory/com/team254/lib/motion/SetpointGenerator.java rename to trajectory/src/com/team254/lib/motion/SetpointGenerator.java diff --git a/trajectory/com/team254/lib/physics/DCMotorTransmission.java b/trajectory/src/com/team254/lib/physics/DCMotorTransmission.java similarity index 100% rename from trajectory/com/team254/lib/physics/DCMotorTransmission.java rename to trajectory/src/com/team254/lib/physics/DCMotorTransmission.java diff --git a/trajectory/com/team254/lib/physics/DifferentialDrive.java b/trajectory/src/com/team254/lib/physics/DifferentialDrive.java similarity index 100% rename from trajectory/com/team254/lib/physics/DifferentialDrive.java rename to trajectory/src/com/team254/lib/physics/DifferentialDrive.java diff --git a/trajectory/com/team254/lib/physics/DriveCharacterization.java b/trajectory/src/com/team254/lib/physics/DriveCharacterization.java similarity index 100% rename from trajectory/com/team254/lib/physics/DriveCharacterization.java rename to trajectory/src/com/team254/lib/physics/DriveCharacterization.java diff --git a/trajectory/com/team254/lib/spline/CubicHermiteSpline.java b/trajectory/src/com/team254/lib/spline/CubicHermiteSpline.java similarity index 100% rename from trajectory/com/team254/lib/spline/CubicHermiteSpline.java rename to trajectory/src/com/team254/lib/spline/CubicHermiteSpline.java diff --git a/trajectory/com/team254/lib/spline/QuinticHermiteSpline.java b/trajectory/src/com/team254/lib/spline/QuinticHermiteSpline.java similarity index 100% rename from trajectory/com/team254/lib/spline/QuinticHermiteSpline.java rename to trajectory/src/com/team254/lib/spline/QuinticHermiteSpline.java diff --git a/trajectory/com/team254/lib/spline/Spline.java b/trajectory/src/com/team254/lib/spline/Spline.java similarity index 100% rename from trajectory/com/team254/lib/spline/Spline.java rename to trajectory/src/com/team254/lib/spline/Spline.java diff --git a/trajectory/com/team254/lib/spline/SplineGenerator.java b/trajectory/src/com/team254/lib/spline/SplineGenerator.java similarity index 100% rename from trajectory/com/team254/lib/spline/SplineGenerator.java rename to trajectory/src/com/team254/lib/spline/SplineGenerator.java diff --git a/trajectory/com/team254/lib/trajectory/DistanceView.java b/trajectory/src/com/team254/lib/trajectory/DistanceView.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/DistanceView.java rename to trajectory/src/com/team254/lib/trajectory/DistanceView.java diff --git a/trajectory/com/team254/lib/trajectory/IPathFollower.java b/trajectory/src/com/team254/lib/trajectory/IPathFollower.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/IPathFollower.java rename to trajectory/src/com/team254/lib/trajectory/IPathFollower.java diff --git a/trajectory/com/team254/lib/trajectory/PurePursuitController.java b/trajectory/src/com/team254/lib/trajectory/PurePursuitController.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/PurePursuitController.java rename to trajectory/src/com/team254/lib/trajectory/PurePursuitController.java diff --git a/trajectory/com/team254/lib/trajectory/TimedView.java b/trajectory/src/com/team254/lib/trajectory/TimedView.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/TimedView.java rename to trajectory/src/com/team254/lib/trajectory/TimedView.java diff --git a/trajectory/com/team254/lib/trajectory/Trajectory.java b/trajectory/src/com/team254/lib/trajectory/Trajectory.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/Trajectory.java rename to trajectory/src/com/team254/lib/trajectory/Trajectory.java diff --git a/trajectory/com/team254/lib/trajectory/TrajectoryIterator.java b/trajectory/src/com/team254/lib/trajectory/TrajectoryIterator.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/TrajectoryIterator.java rename to trajectory/src/com/team254/lib/trajectory/TrajectoryIterator.java diff --git a/trajectory/com/team254/lib/trajectory/TrajectoryPoint.java b/trajectory/src/com/team254/lib/trajectory/TrajectoryPoint.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/TrajectoryPoint.java rename to trajectory/src/com/team254/lib/trajectory/TrajectoryPoint.java diff --git a/trajectory/com/team254/lib/trajectory/TrajectorySamplePoint.java b/trajectory/src/com/team254/lib/trajectory/TrajectorySamplePoint.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/TrajectorySamplePoint.java rename to trajectory/src/com/team254/lib/trajectory/TrajectorySamplePoint.java diff --git a/trajectory/com/team254/lib/trajectory/TrajectoryUtil.java b/trajectory/src/com/team254/lib/trajectory/TrajectoryUtil.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/TrajectoryUtil.java rename to trajectory/src/com/team254/lib/trajectory/TrajectoryUtil.java diff --git a/trajectory/com/team254/lib/trajectory/TrajectoryView.java b/trajectory/src/com/team254/lib/trajectory/TrajectoryView.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/TrajectoryView.java rename to trajectory/src/com/team254/lib/trajectory/TrajectoryView.java diff --git a/trajectory/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java b/trajectory/src/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java rename to trajectory/src/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java diff --git a/trajectory/com/team254/lib/trajectory/timing/DifferentialDriveDynamicsConstraint.java b/trajectory/src/com/team254/lib/trajectory/timing/DifferentialDriveDynamicsConstraint.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/timing/DifferentialDriveDynamicsConstraint.java rename to trajectory/src/com/team254/lib/trajectory/timing/DifferentialDriveDynamicsConstraint.java diff --git a/trajectory/com/team254/lib/trajectory/timing/TimedState.java b/trajectory/src/com/team254/lib/trajectory/timing/TimedState.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/timing/TimedState.java rename to trajectory/src/com/team254/lib/trajectory/timing/TimedState.java diff --git a/trajectory/com/team254/lib/trajectory/timing/TimingConstraint.java b/trajectory/src/com/team254/lib/trajectory/timing/TimingConstraint.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/timing/TimingConstraint.java rename to trajectory/src/com/team254/lib/trajectory/timing/TimingConstraint.java diff --git a/trajectory/com/team254/lib/trajectory/timing/TimingUtil.java b/trajectory/src/com/team254/lib/trajectory/timing/TimingUtil.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/timing/TimingUtil.java rename to trajectory/src/com/team254/lib/trajectory/timing/TimingUtil.java diff --git a/trajectory/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java b/trajectory/src/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java similarity index 100% rename from trajectory/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java rename to trajectory/src/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java diff --git a/trajectory/com/team254/lib/util/CSVWritable.java b/trajectory/src/com/team254/lib/util/CSVWritable.java similarity index 100% rename from trajectory/com/team254/lib/util/CSVWritable.java rename to trajectory/src/com/team254/lib/util/CSVWritable.java diff --git a/trajectory/com/team254/lib/util/CircularBuffer.java b/trajectory/src/com/team254/lib/util/CircularBuffer.java similarity index 100% rename from trajectory/com/team254/lib/util/CircularBuffer.java rename to trajectory/src/com/team254/lib/util/CircularBuffer.java diff --git a/trajectory/com/team254/lib/util/CircularBufferGeneric.java b/trajectory/src/com/team254/lib/util/CircularBufferGeneric.java similarity index 96% rename from trajectory/com/team254/lib/util/CircularBufferGeneric.java rename to trajectory/src/com/team254/lib/util/CircularBufferGeneric.java index 6ad82ca..fa9d5d4 100644 --- a/trajectory/com/team254/lib/util/CircularBufferGeneric.java +++ b/trajectory/src/com/team254/lib/util/CircularBufferGeneric.java @@ -1,53 +1,53 @@ -package com.team254.lib.util; - -import java.util.LinkedList; - -/** - * Implements a simple circular buffer. - * Can be used for any class. - */ -public class CircularBufferGeneric { - final int mWindowSize; - final LinkedList mSamples; - double mSum; - - public CircularBufferGeneric(int window_size) { - mWindowSize = window_size; - mSamples = new LinkedList<>(); - mSum = 0.0; - } - - - public void clear() { - mSamples.clear(); - mSum = 0.0; - } - - public void addValue(E val) { - mSamples.addLast(val); - if (mSamples.size() > mWindowSize) { - mSamples.removeFirst(); - } - } - - public int getNumValues() { - return mSamples.size(); - } - - public boolean isFull() { - return mWindowSize == mSamples.size(); - } - - public LinkedList getLinkedList() { - /* - * NOTE: To get an Array of the specific class type which the instance is using, - * you have to use this specific code: - * specificCircularBufferGeneric.getLinkedList().toArray(new ClassThatIWant[specificCircularBufferGeneric - * .getLinkedList().size()]); - * The reason is that for some reason an array of a generic class(i.e. E[]) cannot be created because - * of some archaic data flow ambiguities - */ - - return mSamples; - } -} +package com.team254.lib.util; + +import java.util.LinkedList; + +/** + * Implements a simple circular buffer. + * Can be used for any class. + */ +public class CircularBufferGeneric { + final int mWindowSize; + final LinkedList mSamples; + double mSum; + + public CircularBufferGeneric(int window_size) { + mWindowSize = window_size; + mSamples = new LinkedList<>(); + mSum = 0.0; + } + + + public void clear() { + mSamples.clear(); + mSum = 0.0; + } + + public void addValue(E val) { + mSamples.addLast(val); + if (mSamples.size() > mWindowSize) { + mSamples.removeFirst(); + } + } + + public int getNumValues() { + return mSamples.size(); + } + + public boolean isFull() { + return mWindowSize == mSamples.size(); + } + + public LinkedList getLinkedList() { + /* + * NOTE: To get an Array of the specific class type which the instance is using, + * you have to use this specific code: + * specificCircularBufferGeneric.getLinkedList().toArray(new ClassThatIWant[specificCircularBufferGeneric + * .getLinkedList().size()]); + * The reason is that for some reason an array of a generic class(i.e. E[]) cannot be created because + * of some archaic data flow ambiguities + */ + + return mSamples; + } +} diff --git a/trajectory/com/team254/lib/util/CrashTracker.java b/trajectory/src/com/team254/lib/util/CrashTracker.java similarity index 100% rename from trajectory/com/team254/lib/util/CrashTracker.java rename to trajectory/src/com/team254/lib/util/CrashTracker.java diff --git a/trajectory/com/team254/lib/util/CrashTrackingRunnable.java b/trajectory/src/com/team254/lib/util/CrashTrackingRunnable.java similarity index 100% rename from trajectory/com/team254/lib/util/CrashTrackingRunnable.java rename to trajectory/src/com/team254/lib/util/CrashTrackingRunnable.java diff --git a/trajectory/com/team254/lib/util/Deadband.java b/trajectory/src/com/team254/lib/util/Deadband.java similarity index 100% rename from trajectory/com/team254/lib/util/Deadband.java rename to trajectory/src/com/team254/lib/util/Deadband.java diff --git a/trajectory/com/team254/lib/util/DelayedBoolean.java b/trajectory/src/com/team254/lib/util/DelayedBoolean.java similarity index 100% rename from trajectory/com/team254/lib/util/DelayedBoolean.java rename to trajectory/src/com/team254/lib/util/DelayedBoolean.java diff --git a/trajectory/com/team254/lib/util/DriveSignal.java b/trajectory/src/com/team254/lib/util/DriveSignal.java similarity index 100% rename from trajectory/com/team254/lib/util/DriveSignal.java rename to trajectory/src/com/team254/lib/util/DriveSignal.java diff --git a/trajectory/com/team254/lib/util/Interpolable.java b/trajectory/src/com/team254/lib/util/Interpolable.java similarity index 100% rename from trajectory/com/team254/lib/util/Interpolable.java rename to trajectory/src/com/team254/lib/util/Interpolable.java diff --git a/trajectory/com/team254/lib/util/InterpolatingDouble.java b/trajectory/src/com/team254/lib/util/InterpolatingDouble.java similarity index 100% rename from trajectory/com/team254/lib/util/InterpolatingDouble.java rename to trajectory/src/com/team254/lib/util/InterpolatingDouble.java diff --git a/trajectory/com/team254/lib/util/InterpolatingLong.java b/trajectory/src/com/team254/lib/util/InterpolatingLong.java similarity index 100% rename from trajectory/com/team254/lib/util/InterpolatingLong.java rename to trajectory/src/com/team254/lib/util/InterpolatingLong.java diff --git a/trajectory/com/team254/lib/util/InterpolatingTreeMap.java b/trajectory/src/com/team254/lib/util/InterpolatingTreeMap.java similarity index 100% rename from trajectory/com/team254/lib/util/InterpolatingTreeMap.java rename to trajectory/src/com/team254/lib/util/InterpolatingTreeMap.java diff --git a/trajectory/com/team254/lib/util/InverseInterpolable.java b/trajectory/src/com/team254/lib/util/InverseInterpolable.java similarity index 97% rename from trajectory/com/team254/lib/util/InverseInterpolable.java rename to trajectory/src/com/team254/lib/util/InverseInterpolable.java index 6d06ad1..a18bf49 100644 --- a/trajectory/com/team254/lib/util/InverseInterpolable.java +++ b/trajectory/src/com/team254/lib/util/InverseInterpolable.java @@ -1,22 +1,22 @@ -package com.team254.lib.util; - -/** - * InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two endpoint keys and a - * third query key, an InverseInterpolable object can calculate the interpolation parameter of the query key on the - * interval [0, 1]. - * - * @param The Type of InverseInterpolable - * @see InterpolatingTreeMap - */ -public interface InverseInterpolable { - /** - * Given this point (lower), a query point (query), and an upper point (upper), estimate how far (on [0, 1]) between - * 'lower' and 'upper' the query point lies. - * - * @param upper - * @param query - * @return The interpolation parameter on [0, 1] representing how far between this point and the upper point the - * query point lies. - */ - double inverseInterpolate(T upper, T query); -} +package com.team254.lib.util; + +/** + * InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two endpoint keys and a + * third query key, an InverseInterpolable object can calculate the interpolation parameter of the query key on the + * interval [0, 1]. + * + * @param The Type of InverseInterpolable + * @see InterpolatingTreeMap + */ +public interface InverseInterpolable { + /** + * Given this point (lower), a query point (query), and an upper point (upper), estimate how far (on [0, 1]) between + * 'lower' and 'upper' the query point lies. + * + * @param upper + * @param query + * @return The interpolation parameter on [0, 1] representing how far between this point and the upper point the + * query point lies. + */ + double inverseInterpolate(T upper, T query); +} diff --git a/trajectory/com/team254/lib/util/LatchedBoolean.java b/trajectory/src/com/team254/lib/util/LatchedBoolean.java similarity index 100% rename from trajectory/com/team254/lib/util/LatchedBoolean.java rename to trajectory/src/com/team254/lib/util/LatchedBoolean.java diff --git a/trajectory/com/team254/lib/util/MinTimeBoolean.java b/trajectory/src/com/team254/lib/util/MinTimeBoolean.java similarity index 100% rename from trajectory/com/team254/lib/util/MinTimeBoolean.java rename to trajectory/src/com/team254/lib/util/MinTimeBoolean.java diff --git a/trajectory/com/team254/lib/util/MovingAverage.java b/trajectory/src/com/team254/lib/util/MovingAverage.java similarity index 100% rename from trajectory/com/team254/lib/util/MovingAverage.java rename to trajectory/src/com/team254/lib/util/MovingAverage.java diff --git a/trajectory/com/team254/lib/util/MovingAverageTwist2d.java b/trajectory/src/com/team254/lib/util/MovingAverageTwist2d.java similarity index 100% rename from trajectory/com/team254/lib/util/MovingAverageTwist2d.java rename to trajectory/src/com/team254/lib/util/MovingAverageTwist2d.java diff --git a/trajectory/com/team254/lib/util/PolynomialRegression.java b/trajectory/src/com/team254/lib/util/PolynomialRegression.java similarity index 100% rename from trajectory/com/team254/lib/util/PolynomialRegression.java rename to trajectory/src/com/team254/lib/util/PolynomialRegression.java diff --git a/trajectory/com/team254/lib/util/ReflectingCSVWriter.java b/trajectory/src/com/team254/lib/util/ReflectingCSVWriter.java similarity index 100% rename from trajectory/com/team254/lib/util/ReflectingCSVWriter.java rename to trajectory/src/com/team254/lib/util/ReflectingCSVWriter.java diff --git a/trajectory/com/team254/lib/util/StatFinder.java b/trajectory/src/com/team254/lib/util/StatFinder.java similarity index 100% rename from trajectory/com/team254/lib/util/StatFinder.java rename to trajectory/src/com/team254/lib/util/StatFinder.java diff --git a/trajectory/com/team254/lib/util/Units.java b/trajectory/src/com/team254/lib/util/Units.java similarity index 100% rename from trajectory/com/team254/lib/util/Units.java rename to trajectory/src/com/team254/lib/util/Units.java diff --git a/trajectory/com/team254/lib/util/Util.java b/trajectory/src/com/team254/lib/util/Util.java similarity index 100% rename from trajectory/com/team254/lib/util/Util.java rename to trajectory/src/com/team254/lib/util/Util.java diff --git a/trajectory/com/team254/lib/vision/AimingParameters.java b/trajectory/src/com/team254/lib/vision/AimingParameters.java similarity index 100% rename from trajectory/com/team254/lib/vision/AimingParameters.java rename to trajectory/src/com/team254/lib/vision/AimingParameters.java diff --git a/trajectory/com/team254/lib/vision/TargetInfo.java b/trajectory/src/com/team254/lib/vision/TargetInfo.java similarity index 100% rename from trajectory/com/team254/lib/vision/TargetInfo.java rename to trajectory/src/com/team254/lib/vision/TargetInfo.java diff --git a/trajectory/com/team319/follower/SrxMotionProfile.java b/trajectory/src/com/team319/follower/SrxMotionProfile.java similarity index 97% rename from trajectory/com/team319/follower/SrxMotionProfile.java rename to trajectory/src/com/team319/follower/SrxMotionProfile.java index 75486ed..3e61173 100644 --- a/trajectory/com/team319/follower/SrxMotionProfile.java +++ b/trajectory/src/com/team319/follower/SrxMotionProfile.java @@ -1,40 +1,40 @@ -/** - * Copyright (c) 2018 Team319 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package com.team319.follower; - -//Generic Motion Profile Class -public class SrxMotionProfile { - - public int numPoints; - // Position (rotations) Velocity (RPM) Duration (ms) - public double[][] points; - - public SrxMotionProfile() { - - } - - public SrxMotionProfile(int numPoints, double[][] points) { - this.numPoints = numPoints; - this.points = points; - } -} +/** + * Copyright (c) 2018 Team319 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package com.team319.follower; + +//Generic Motion Profile Class +public class SrxMotionProfile { + + public int numPoints; + // Position (rotations) Velocity (RPM) Duration (ms) + public double[][] points; + + public SrxMotionProfile() { + + } + + public SrxMotionProfile(int numPoints, double[][] points) { + this.numPoints = numPoints; + this.points = points; + } +} diff --git a/trajectory/com/team319/follower/SrxTrajectory.java b/trajectory/src/com/team319/follower/SrxTrajectory.java similarity index 97% rename from trajectory/com/team319/follower/SrxTrajectory.java rename to trajectory/src/com/team319/follower/SrxTrajectory.java index 80c0442..ceddf3c 100644 --- a/trajectory/com/team319/follower/SrxTrajectory.java +++ b/trajectory/src/com/team319/follower/SrxTrajectory.java @@ -1,42 +1,42 @@ -/** - * Copyright (c) 2018 Team319 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package com.team319.follower; - -//Combines left and right motion profiles in one object -public class SrxTrajectory { - public boolean flipped; - public boolean highGear; - public SrxMotionProfile leftProfile; - public SrxMotionProfile centerProfile; - public SrxMotionProfile rightProfile; - - public SrxTrajectory() { - - } - - public SrxTrajectory(SrxMotionProfile left, SrxMotionProfile center, SrxMotionProfile right) { - this.leftProfile = left; - this.centerProfile = center; - this.rightProfile = right; - } -} +/** + * Copyright (c) 2018 Team319 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package com.team319.follower; + +//Combines left and right motion profiles in one object +public class SrxTrajectory { + public boolean flipped; + public boolean highGear; + public SrxMotionProfile leftProfile; + public SrxMotionProfile centerProfile; + public SrxMotionProfile rightProfile; + + public SrxTrajectory() { + + } + + public SrxTrajectory(SrxMotionProfile left, SrxMotionProfile center, SrxMotionProfile right) { + this.leftProfile = left; + this.centerProfile = center; + this.rightProfile = right; + } +} diff --git a/trajectory/src/jama/.DS_Store b/trajectory/src/jama/.DS_Store new file mode 100644 index 0000000..d001d09 Binary files /dev/null and b/trajectory/src/jama/.DS_Store differ diff --git a/trajectory/jama/CholeskyDecomposition.java b/trajectory/src/jama/CholeskyDecomposition.java similarity index 100% rename from trajectory/jama/CholeskyDecomposition.java rename to trajectory/src/jama/CholeskyDecomposition.java diff --git a/trajectory/jama/EigenvalueDecomposition.java b/trajectory/src/jama/EigenvalueDecomposition.java similarity index 100% rename from trajectory/jama/EigenvalueDecomposition.java rename to trajectory/src/jama/EigenvalueDecomposition.java diff --git a/trajectory/jama/LUDecomposition.java b/trajectory/src/jama/LUDecomposition.java similarity index 100% rename from trajectory/jama/LUDecomposition.java rename to trajectory/src/jama/LUDecomposition.java diff --git a/trajectory/jama/Matrix.java b/trajectory/src/jama/Matrix.java similarity index 100% rename from trajectory/jama/Matrix.java rename to trajectory/src/jama/Matrix.java diff --git a/trajectory/jama/QRDecomposition.java b/trajectory/src/jama/QRDecomposition.java similarity index 100% rename from trajectory/jama/QRDecomposition.java rename to trajectory/src/jama/QRDecomposition.java diff --git a/trajectory/jama/SingularValueDecomposition.java b/trajectory/src/jama/SingularValueDecomposition.java similarity index 100% rename from trajectory/jama/SingularValueDecomposition.java rename to trajectory/src/jama/SingularValueDecomposition.java diff --git a/trajectory/jama/util/Maths.java b/trajectory/src/jama/util/Maths.java similarity index 94% rename from trajectory/jama/util/Maths.java rename to trajectory/src/jama/util/Maths.java index e841ddc..e96fb92 100644 --- a/trajectory/jama/util/Maths.java +++ b/trajectory/src/jama/util/Maths.java @@ -1,20 +1,20 @@ -package jama.util; - -public class Maths { - - /** sqrt(a^2 + b^2) without under/overflow. **/ - - public static double hypot(double a, double b) { - double r; - if (Math.abs(a) > Math.abs(b)) { - r = b / a; - r = Math.abs(a) * Math.sqrt(1 + r * r); - } else if (b != 0) { - r = a / b; - r = Math.abs(b) * Math.sqrt(1 + r * r); - } else { - r = 0.0; - } - return r; - } -} +package jama.util; + +public class Maths { + + /** sqrt(a^2 + b^2) without under/overflow. **/ + + public static double hypot(double a, double b) { + double r; + if (Math.abs(a) > Math.abs(b)) { + r = b / a; + r = Math.abs(a) * Math.sqrt(1 + r * r); + } else if (b != 0) { + r = a / b; + r = Math.abs(b) * Math.sqrt(1 + r * r); + } else { + r = 0.0; + } + return r; + } +} diff --git a/trajectory/module-info.java b/trajectory/src/module-info.java similarity index 95% rename from trajectory/module-info.java rename to trajectory/src/module-info.java index c751956..ddfac1d 100644 --- a/trajectory/module-info.java +++ b/trajectory/src/module-info.java @@ -1,7 +1,7 @@ -module aluminatipath2 { - opens org.aluminati3555.aluminatipath2; - - requires javafx.base; - requires javafx.controls; - requires java.desktop; +module aluminatipath2 { + opens org.aluminati3555.aluminatipath2; + + requires javafx.base; + requires javafx.controls; + requires java.desktop; } \ No newline at end of file diff --git a/trajectory/org/aluminati3555/aluminatipath2/AluminatiPath2.java b/trajectory/src/org/aluminati3555/aluminatipath2/AluminatiPath2.java similarity index 97% rename from trajectory/org/aluminati3555/aluminatipath2/AluminatiPath2.java rename to trajectory/src/org/aluminati3555/aluminatipath2/AluminatiPath2.java index be3cad9..fd196e6 100644 --- a/trajectory/org/aluminati3555/aluminatipath2/AluminatiPath2.java +++ b/trajectory/src/org/aluminati3555/aluminatipath2/AluminatiPath2.java @@ -1,70 +1,70 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.aluminatipath2; - -import javafx.application.Application; -import javafx.application.Platform; -import javafx.scene.Scene; -import javafx.stage.Stage; - -/** - * This is the main class of the AluminatiPath2 path generator - * - * @author Caleb Heydon - */ -public class AluminatiPath2 extends Application { - /** - * This main method starts JavaFx - * @param args - */ - public static void main(String[] args) { - // Launch JavaFx application - launch(args); - } - - /** - * This is the real main method of the application - */ - public void start(Stage window) { - // Initialize basic settings - - // Shutdown nicely when the window is closed - window.setOnCloseRequest(e -> { - - // Use Platform.runLater() to prevent the window from not closing smoothly (freezes and then closes) - Platform.runLater(() -> { - System.exit(0); - }); - }); - - // Set scene to field scene - FieldPane fieldPane = new FieldPane(); - Scene scene = new Scene(fieldPane, fieldPane.getField().getWidth() + 200, fieldPane.getField().getHeight()); - window.setScene(scene); - - // Initialize window - window.setTitle("AluminatiPath2"); - window.setResizable(false); - window.show(); - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.aluminatipath2; + +import javafx.application.Application; +import javafx.application.Platform; +import javafx.scene.Scene; +import javafx.stage.Stage; + +/** + * This is the main class of the AluminatiPath2 path generator + * + * @author Caleb Heydon + */ +public class AluminatiPath2 extends Application { + /** + * This main method starts JavaFx + * @param args + */ + public static void main(String[] args) { + // Launch JavaFx application + launch(args); + } + + /** + * This is the real main method of the application + */ + public void start(Stage window) { + // Initialize basic settings + + // Shutdown nicely when the window is closed + window.setOnCloseRequest(e -> { + + // Use Platform.runLater() to prevent the window from not closing smoothly (freezes and then closes) + Platform.runLater(() -> { + System.exit(0); + }); + }); + + // Set scene to field scene + FieldPane fieldPane = new FieldPane(); + Scene scene = new Scene(fieldPane, fieldPane.getField().getWidth() + 200, fieldPane.getField().getHeight()); + window.setScene(scene); + + // Initialize window + window.setTitle("AluminatiPath2"); + window.setResizable(false); + window.show(); + } +} diff --git a/trajectory/org/aluminati3555/aluminatipath2/Field.java b/trajectory/src/org/aluminati3555/aluminatipath2/Field.java similarity index 97% rename from trajectory/org/aluminati3555/aluminatipath2/Field.java rename to trajectory/src/org/aluminati3555/aluminatipath2/Field.java index f8c0aeb..c939de4 100644 --- a/trajectory/org/aluminati3555/aluminatipath2/Field.java +++ b/trajectory/src/org/aluminati3555/aluminatipath2/Field.java @@ -1,49 +1,49 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.aluminatipath2; - -import java.io.File; -import java.io.IOException; - -import javax.imageio.ImageIO; - -/** - * This class holds dimensions and ratios of the field - * - * @author Caleb Heydon - */ -public class Field { - public static final double FIELD_WIDTH_INCHES = 54 * 12; - - // The ratio between inches and pixels - public static double ratio; // Should be 1.75 - - static { - try { - int pixels = ImageIO.read(new File(FieldPane.FIELD_IMAGE)).getWidth(); - ratio = FIELD_WIDTH_INCHES / pixels; - } catch (IOException e) { - ratio = 1.75; - } - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.aluminatipath2; + +import java.io.File; +import java.io.IOException; + +import javax.imageio.ImageIO; + +/** + * This class holds dimensions and ratios of the field + * + * @author Caleb Heydon + */ +public class Field { + public static final double FIELD_WIDTH_INCHES = 54 * 12; + + // The ratio between inches and pixels + public static double ratio; // Should be 1.75 + + static { + try { + int pixels = ImageIO.read(new File(FieldPane.FIELD_IMAGE)).getWidth(); + ratio = FIELD_WIDTH_INCHES / pixels; + } catch (IOException e) { + ratio = 1.75; + } + } +} diff --git a/trajectory/org/aluminati3555/aluminatipath2/FieldPane.java b/trajectory/src/org/aluminati3555/aluminatipath2/FieldPane.java similarity index 96% rename from trajectory/org/aluminati3555/aluminatipath2/FieldPane.java rename to trajectory/src/org/aluminati3555/aluminatipath2/FieldPane.java index 122eda4..5cd9bdc 100644 --- a/trajectory/org/aluminati3555/aluminatipath2/FieldPane.java +++ b/trajectory/src/org/aluminati3555/aluminatipath2/FieldPane.java @@ -1,502 +1,502 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.aluminatipath2; - -import java.text.DecimalFormat; -import java.util.ArrayList; - -import org.aluminati3555.lib.data.AluminatiData; -import org.aluminati3555.lib.trajectory.AluminatiTrajectory; - -import com.team254.lib.geometry.Pose2d; -import com.team254.lib.geometry.Rotation2d; - -import javafx.beans.property.SimpleDoubleProperty; -import javafx.beans.value.ChangeListener; -import javafx.geometry.Insets; -import javafx.scene.control.Button; -import javafx.scene.control.CheckBox; -import javafx.scene.control.Label; -import javafx.scene.control.TextField; -import javafx.scene.image.Image; -import javafx.scene.image.ImageView; -import javafx.scene.input.MouseButton; -import javafx.scene.input.MouseEvent; -import javafx.scene.layout.BorderPane; -import javafx.scene.layout.VBox; -import javafx.scene.paint.Color; -import javafx.scene.shape.Ellipse; - -/** - * This class provides the basis for the field component of the gui - * - * @author Caleb Heydon - */ -public class FieldPane extends BorderPane { - // The constant name of the field image file - public static final String FIELD_IMAGE = "field.png"; - - // The image object of the field - private Image field; - - private ImageView fieldView; - private PropertiesPane propertiesPane; - - // List of robots - private ArrayList robots; - - // Properties - private SimpleDoubleProperty lengthValue = new SimpleDoubleProperty(39); - private SimpleDoubleProperty widthValue = new SimpleDoubleProperty(33); - - private double startVelocityValue = 0; - private double endVelocityValue = 0; - private double maxVelocityValue = 100; - private double maxAccelerationValue = 100; - private double dtValue = 0.01; - - // Current trajectory - private AluminatiTrajectory currentTrajectory; - - /** - * This method returns the image of the field - * - * @return - */ - public Image getField() { - return field; - } - - /** - * Unselects all robots - */ - public void unselectAll() { - for (int i = 0; i < robots.size(); i++) { - robots.get(i).unselect(); - } - } - - /** - * Generates an info string for the current trajectory - * - * @return - */ - public String getTrajectoryInfoString() { - if (currentTrajectory == null || robots.size() < 2) { - return null; - } - - String info = ""; - - info += "AluminatiData.wheelDiameter = " + AluminatiData.wheelDiamater + ";\n"; - info += "AluminatiData.encoderUnitsPerRotation = " + AluminatiData.encoderUnitsPerRotation + ";\n\n"; - - info += "boolean reversed = " + propertiesPane.isReversed() + ";\n"; - info += "double startAngle = " + -robots.get(0).getRobot().getRotate() + ";\n"; - info += "double startVelocity = " + startVelocityValue + ";\n"; - info += "double endVelocity = " + endVelocityValue + ";\n"; - info += "double maxVelocity = " + maxVelocityValue + ";\n"; - info += "double maxAcceleration = " + maxAccelerationValue + ";\n\n"; - - for (int i = 0; i < currentTrajectory.getWaypoints().size(); i++) { - DecimalFormat decimalFormat = new DecimalFormat("#####.##"); - - String x = decimalFormat.format(currentTrajectory.getWaypoints().get(i).getTranslation().x()); - String y = decimalFormat.format(currentTrajectory.getWaypoints().get(i).getTranslation().y() - - (robots.get(i).getRobot().getHeight() / 2.0) / Field.ratio); - String angle = decimalFormat.format(currentTrajectory.getWaypoints().get(i).getRotation().getDegrees()); - - info += "new Pose2d(" + x + ", " + y + ", Rotation2d.fromDegrees(" + angle + "));\n"; - } - - return info; - } - - /** - * Adds a robot - * - * @param e - */ - public void addRobot(MouseEvent e) { - Robot lastRobot; - if (robots.size() == 0) { - lastRobot = null; - } else { - lastRobot = robots.get(robots.size() - 1); - } - - Robot robot = new Robot(lengthValue.doubleValue(), widthValue.doubleValue()); - robot.setTranslateX(e.getSceneX() - robot.getRobot().getWidth() / 2); - robot.getRobot().widthProperty().addListener((a, b, c) -> { - robot.setTranslateX(e.getSceneX() + b.doubleValue() / 2); - robot.setTranslateX(e.getSceneX() - c.doubleValue() / 2); - }); - robot.setTranslateY(e.getSceneY()); - if (lastRobot != null) { - double angle = 0; - if (!propertiesPane.isReversed()) { - angle = (lastRobot.getTranslateX() < robot.getTranslateX()) ? 0 : 180; - } else { - angle = (lastRobot.getTranslateX() < robot.getTranslateX()) ? 180 : 0; - } - - robot.getRobot().setRotate(-angle); - } - robot.setOnMouseClicked(e1 -> { - if (e1.getButton() == MouseButton.PRIMARY) { - unselectAll(); - propertiesPane.select(robot); - robot.select(); - } else if (e1.getButton() == MouseButton.SECONDARY) { - if (robot.isSelected()) { - propertiesPane.reset(); - } - robots.remove(robot); - robot.unselect(); - render(); - } - }); - - unselectAll(); - propertiesPane.select(robot); - robot.select(); - robots.add(robot); - render(); - } - - /** - * This method updates the paths on the screen - */ - public void updatePaths() { - if (robots.size() > 1) { - ArrayList waypoints = new ArrayList(); - - for (int i = 0; i < robots.size(); i++) { - double xValue = (robots.get(i).getTranslateX() + robots.get(i).getRobot().getWidth() / 2.0) - / Field.ratio; - double yValue = (this.getHeight() - robots.get(i).getTranslateY() - + robots.get(i).getRobot().getHeight() / 2.0) / Field.ratio; - double angleValue = -robots.get(i).getRobot().getRotate(); - - waypoints.add(new Pose2d(xValue, yValue, Rotation2d.fromDegrees(angleValue))); - } - - currentTrajectory = new AluminatiTrajectory(waypoints, null, false, propertiesPane.isReversed(), - startVelocityValue, endVelocityValue, maxVelocityValue, maxAccelerationValue, dtValue); - - for (int i = 0; i < currentTrajectory.getXPoints().size(); i++) { - Ellipse ellipse = new Ellipse(5, 5); - ellipse.setFill(Color.GREENYELLOW); - - ellipse.setTranslateX(currentTrajectory.getXPoints().get(i) * Field.ratio); - ellipse.setTranslateY(this.getHeight() - currentTrajectory.getYPoints().get(i) * Field.ratio - + robots.get(0).getRobot().getHeight() / 2); - - ellipse.setOnMouseClicked(e -> { - if (e.getSceneX() <= field.getWidth()) { - addRobot(e); - } - }); - - if (ellipse.getTranslateX() <= field.getWidth()) { - this.getChildren().add(ellipse); - } - } - - int ms = 0; - for (int i = 0; i < currentTrajectory.centerProfile.numPoints; i++) { - ms += currentTrajectory.centerProfile.points[i][2]; - } - - double time = ms / 1000.0; - propertiesPane.getTimeLabel().setText("Time: " + time + " (seconds)"); - } else { - propertiesPane.getTimeLabel().setText("Time: -"); - } - } - - /** - * Updates the dimensions of the robots - */ - public void updateRobots() { - for (int i = 0; i < robots.size(); i++) { - robots.get(i).setDimensions(lengthValue.doubleValue(), widthValue.doubleValue()); - this.getChildren().add(robots.get(i)); - } - } - - /** - * This method renders the screen - */ - public void render() { - this.getChildren().clear(); - this.setLeft(fieldView); - this.setRight(propertiesPane); - - updatePaths(); - updateRobots(); - } - - public FieldPane() { - field = new Image(FIELD_IMAGE); - robots = new ArrayList(); - - fieldView = new ImageView(field); - fieldView.setOnMouseClicked(e -> { - if (e.getButton() == MouseButton.PRIMARY) { - addRobot(e); - } - }); - - propertiesPane = new PropertiesPane(); - propertiesPane.setPadding(new Insets(10, 10, 10, 10)); - - render(); - } - - /** - * This class provides the properties view - * - * @author Caleb Heydon - */ - private class PropertiesPane extends VBox { - // GUI elements - private TextField length; - private TextField width; - private TextField wheelDiameter; - private TextField encoderTicksPerRevolution; - - private TextField startVelocity; - private TextField endVelocity; - private TextField maxVelocity; - private TextField maxAcceleration; - - private TextField x; - private TextField y; - private TextField angle; - private Label time; - - private CheckBox reversed; - - // Listeners - private ChangeListener xListener; - private ChangeListener yListener; - private ChangeListener angleListener; - - /** - * Returns the time label - * - * @return - */ - public Label getTimeLabel() { - return time; - } - - /** - * Returns true if the path is reversed - * - * @return - */ - public boolean isReversed() { - return reversed.isSelected(); - } - - /** - * Resets the properties window when a robot is unselected - */ - public void reset() { - x.setDisable(true); - y.setDisable(true); - angle.setDisable(true); - - x.setText(""); - y.setText(""); - angle.setText(""); - } - - /** - * Parses the properties from the inputs - */ - private void parseProperties() { - try { - lengthValue.set(Double.parseDouble(length.getText())); - widthValue.set(Double.parseDouble(width.getText())); - AluminatiData.wheelDiamater = Double.parseDouble(wheelDiameter.getText()); - AluminatiData.encoderUnitsPerRotation = Integer.parseInt(encoderTicksPerRevolution.getText()); - - startVelocityValue = Double.parseDouble(startVelocity.getText()); - endVelocityValue = Double.parseDouble(endVelocity.getText()); - - if (maxVelocityValue > 0 && maxAccelerationValue > 0) { - maxVelocityValue = Double.parseDouble(maxVelocity.getText()); - maxAccelerationValue = Double.parseDouble(maxAcceleration.getText()); - - render(); - } - } catch (NumberFormatException e) { - return; - } - } - - /** - * Selects a robot - * - * @param robot - */ - public void select(Robot robot) { - if (xListener != null && yListener != null && angleListener != null) { - x.textProperty().removeListener(xListener); - y.textProperty().removeListener(yListener); - angle.textProperty().removeListener(angleListener); - } - - xListener = (a, b, c) -> { - try { - double value = Double.parseDouble(c) * Field.ratio - robot.getRobot().getWidth() / 2.0; - robot.setTranslateX(value); - } catch (NumberFormatException e) { - return; - } - - render(); - }; - - yListener = (a, b, c) -> { - try { - double value = this.getHeight() - (Double.parseDouble(c) * Field.ratio); - robot.setTranslateY(value); - } catch (NumberFormatException e) { - return; - } - - render(); - }; - - angleListener = (a, b, c) -> { - try { - double value = Double.parseDouble(c); - robot.getRobot().setRotate(-value); - } catch (NumberFormatException e) { - return; - } - - render(); - }; - - DecimalFormat decimalFormat = new DecimalFormat("#####.##"); - double xValue = (robot.getTranslateX() + robot.getRobot().getWidth() / 2.0) / Field.ratio; - double yValue = (this.getHeight() - robot.getTranslateY()) / Field.ratio; - double angleValue = -robot.getRobot().getRotate(); - - x.setText(decimalFormat.format(xValue)); - y.setText(decimalFormat.format(yValue)); - if (robot.getRobot().getRotate() == 0) { - angle.setText("0"); - } else { - angle.setText("" + decimalFormat.format(angleValue)); - } - - x.textProperty().addListener(xListener); - y.textProperty().addListener(yListener); - angle.textProperty().addListener(angleListener); - - x.setDisable(false); - y.setDisable(false); - angle.setDisable(false); - } - - public PropertiesPane() { - super(); - - length = new TextField("" + lengthValue.doubleValue()); - width = new TextField("" + widthValue.doubleValue()); - wheelDiameter = new TextField("" + AluminatiData.wheelDiamater); - encoderTicksPerRevolution = new TextField("" + AluminatiData.encoderUnitsPerRotation); - - startVelocity = new TextField("" + startVelocityValue); - endVelocity = new TextField("" + endVelocityValue); - maxVelocity = new TextField("" + maxVelocityValue); - maxAcceleration = new TextField("" + maxAccelerationValue); - - ChangeListener listener = (a, b, c) -> { - parseProperties(); - }; - - length.textProperty().addListener(listener); - width.textProperty().addListener(listener); - wheelDiameter.textProperty().addListener(listener); - encoderTicksPerRevolution.textProperty().addListener(listener); - startVelocity.textProperty().addListener(listener); - endVelocity.textProperty().addListener(listener); - maxVelocity.textProperty().addListener(listener); - maxAcceleration.textProperty().addListener(listener); - - this.getChildren().add(new Label("Robot length (in)")); - this.getChildren().add(length); - this.getChildren().add(new Label("Robot width (in)")); - this.getChildren().add(width); - this.getChildren().add(new Label("Wheel diameter (in)")); - this.getChildren().add(wheelDiameter); - this.getChildren().add(new Label("Encoder ticks/rev")); - this.getChildren().add(encoderTicksPerRevolution); - this.getChildren().add(new Label("Start velocity (in/s)")); - this.getChildren().add(startVelocity); - this.getChildren().add(new Label("End velocity (in/s)")); - this.getChildren().add(endVelocity); - this.getChildren().add(new Label("Max velocity (in/s)")); - this.getChildren().add(maxVelocity); - this.getChildren().add(new Label("Max acceleration (in/s^2)")); - this.getChildren().add(maxAcceleration); - - x = new TextField(); - y = new TextField(); - angle = new TextField(); - - x.setDisable(true); - y.setDisable(true); - angle.setDisable(true); - - this.getChildren().add(new Label("X (inches)")); - this.getChildren().add(x); - this.getChildren().add(new Label("Y (inches)")); - this.getChildren().add(y); - this.getChildren().add(new Label("Angle (degrees)")); - this.getChildren().add(angle); - - time = new Label("Time: -"); - this.getChildren().add(time); - - reversed = new CheckBox("Reversed"); - reversed.selectedProperty().addListener((a, b, c) -> { - parseProperties(); - }); - this.getChildren().add(reversed); - - Button displayButton = new Button("Display Output"); - displayButton.setOnMouseClicked(e -> { - new OutputWindow(getTrajectoryInfoString()).show(); - }); - this.getChildren().add(displayButton); - } - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.aluminatipath2; + +import java.text.DecimalFormat; +import java.util.ArrayList; + +import org.aluminati3555.lib.data.AluminatiData; +import org.aluminati3555.lib.trajectory.AluminatiTrajectory; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d; + +import javafx.beans.property.SimpleDoubleProperty; +import javafx.beans.value.ChangeListener; +import javafx.geometry.Insets; +import javafx.scene.control.Button; +import javafx.scene.control.CheckBox; +import javafx.scene.control.Label; +import javafx.scene.control.TextField; +import javafx.scene.image.Image; +import javafx.scene.image.ImageView; +import javafx.scene.input.MouseButton; +import javafx.scene.input.MouseEvent; +import javafx.scene.layout.BorderPane; +import javafx.scene.layout.VBox; +import javafx.scene.paint.Color; +import javafx.scene.shape.Ellipse; + +/** + * This class provides the basis for the field component of the gui + * + * @author Caleb Heydon + */ +public class FieldPane extends BorderPane { + // The constant name of the field image file + public static final String FIELD_IMAGE = "field.png"; + + // The image object of the field + private Image field; + + private ImageView fieldView; + private PropertiesPane propertiesPane; + + // List of robots + private ArrayList robots; + + // Properties + private SimpleDoubleProperty lengthValue = new SimpleDoubleProperty(39); + private SimpleDoubleProperty widthValue = new SimpleDoubleProperty(33); + + private double startVelocityValue = 0; + private double endVelocityValue = 0; + private double maxVelocityValue = 100; + private double maxAccelerationValue = 100; + private double dtValue = 0.01; + + // Current trajectory + private AluminatiTrajectory currentTrajectory; + + /** + * This method returns the image of the field + * + * @return + */ + public Image getField() { + return field; + } + + /** + * Unselects all robots + */ + public void unselectAll() { + for (int i = 0; i < robots.size(); i++) { + robots.get(i).unselect(); + } + } + + /** + * Generates an info string for the current trajectory + * + * @return + */ + public String getTrajectoryInfoString() { + if (currentTrajectory == null || robots.size() < 2) { + return null; + } + + String info = ""; + + info += "AluminatiData.wheelDiameter = " + AluminatiData.wheelDiamater + ";\n"; + info += "AluminatiData.encoderUnitsPerRotation = " + AluminatiData.encoderUnitsPerRotation + ";\n\n"; + + info += "boolean reversed = " + propertiesPane.isReversed() + ";\n"; + info += "double startAngle = " + -robots.get(0).getRobot().getRotate() + ";\n"; + info += "double startVelocity = " + startVelocityValue + ";\n"; + info += "double endVelocity = " + endVelocityValue + ";\n"; + info += "double maxVelocity = " + maxVelocityValue + ";\n"; + info += "double maxAcceleration = " + maxAccelerationValue + ";\n\n"; + + for (int i = 0; i < currentTrajectory.getWaypoints().size(); i++) { + DecimalFormat decimalFormat = new DecimalFormat("#####.##"); + + String x = decimalFormat.format(currentTrajectory.getWaypoints().get(i).getTranslation().x()); + String y = decimalFormat.format(currentTrajectory.getWaypoints().get(i).getTranslation().y() + - (robots.get(i).getRobot().getHeight() / 2.0) / Field.ratio); + String angle = decimalFormat.format(currentTrajectory.getWaypoints().get(i).getRotation().getDegrees()); + + info += "new Pose2d(" + x + ", " + y + ", Rotation2d.fromDegrees(" + angle + "));\n"; + } + + return info; + } + + /** + * Adds a robot + * + * @param e + */ + public void addRobot(MouseEvent e) { + Robot lastRobot; + if (robots.size() == 0) { + lastRobot = null; + } else { + lastRobot = robots.get(robots.size() - 1); + } + + Robot robot = new Robot(lengthValue.doubleValue(), widthValue.doubleValue()); + robot.setTranslateX(e.getSceneX() - robot.getRobot().getWidth() / 2); + robot.getRobot().widthProperty().addListener((a, b, c) -> { + robot.setTranslateX(e.getSceneX() + b.doubleValue() / 2); + robot.setTranslateX(e.getSceneX() - c.doubleValue() / 2); + }); + robot.setTranslateY(e.getSceneY()); + if (lastRobot != null) { + double angle = 0; + if (!propertiesPane.isReversed()) { + angle = (lastRobot.getTranslateX() < robot.getTranslateX()) ? 0 : 180; + } else { + angle = (lastRobot.getTranslateX() < robot.getTranslateX()) ? 180 : 0; + } + + robot.getRobot().setRotate(-angle); + } + robot.setOnMouseClicked(e1 -> { + if (e1.getButton() == MouseButton.PRIMARY) { + unselectAll(); + propertiesPane.select(robot); + robot.select(); + } else if (e1.getButton() == MouseButton.SECONDARY) { + if (robot.isSelected()) { + propertiesPane.reset(); + } + robots.remove(robot); + robot.unselect(); + render(); + } + }); + + unselectAll(); + propertiesPane.select(robot); + robot.select(); + robots.add(robot); + render(); + } + + /** + * This method updates the paths on the screen + */ + public void updatePaths() { + if (robots.size() > 1) { + ArrayList waypoints = new ArrayList(); + + for (int i = 0; i < robots.size(); i++) { + double xValue = (robots.get(i).getTranslateX() + robots.get(i).getRobot().getWidth() / 2.0) + / Field.ratio; + double yValue = (this.getHeight() - robots.get(i).getTranslateY() + + robots.get(i).getRobot().getHeight() / 2.0) / Field.ratio; + double angleValue = -robots.get(i).getRobot().getRotate(); + + waypoints.add(new Pose2d(xValue, yValue, Rotation2d.fromDegrees(angleValue))); + } + + currentTrajectory = new AluminatiTrajectory(waypoints, null, false, propertiesPane.isReversed(), + startVelocityValue, endVelocityValue, maxVelocityValue, maxAccelerationValue, dtValue); + + for (int i = 0; i < currentTrajectory.getXPoints().size(); i++) { + Ellipse ellipse = new Ellipse(5, 5); + ellipse.setFill(Color.GREENYELLOW); + + ellipse.setTranslateX(currentTrajectory.getXPoints().get(i) * Field.ratio); + ellipse.setTranslateY(this.getHeight() - currentTrajectory.getYPoints().get(i) * Field.ratio + + robots.get(0).getRobot().getHeight() / 2); + + ellipse.setOnMouseClicked(e -> { + if (e.getSceneX() <= field.getWidth()) { + addRobot(e); + } + }); + + if (ellipse.getTranslateX() <= field.getWidth()) { + this.getChildren().add(ellipse); + } + } + + int ms = 0; + for (int i = 0; i < currentTrajectory.centerProfile.numPoints; i++) { + ms += currentTrajectory.centerProfile.points[i][2]; + } + + double time = ms / 1000.0; + propertiesPane.getTimeLabel().setText("Time: " + time + " (seconds)"); + } else { + propertiesPane.getTimeLabel().setText("Time: -"); + } + } + + /** + * Updates the dimensions of the robots + */ + public void updateRobots() { + for (int i = 0; i < robots.size(); i++) { + robots.get(i).setDimensions(lengthValue.doubleValue(), widthValue.doubleValue()); + this.getChildren().add(robots.get(i)); + } + } + + /** + * This method renders the screen + */ + public void render() { + this.getChildren().clear(); + this.setLeft(fieldView); + this.setRight(propertiesPane); + + updatePaths(); + updateRobots(); + } + + public FieldPane() { + field = new Image(FIELD_IMAGE); + robots = new ArrayList(); + + fieldView = new ImageView(field); + fieldView.setOnMouseClicked(e -> { + if (e.getButton() == MouseButton.PRIMARY) { + addRobot(e); + } + }); + + propertiesPane = new PropertiesPane(); + propertiesPane.setPadding(new Insets(10, 10, 10, 10)); + + render(); + } + + /** + * This class provides the properties view + * + * @author Caleb Heydon + */ + private class PropertiesPane extends VBox { + // GUI elements + private TextField length; + private TextField width; + private TextField wheelDiameter; + private TextField encoderTicksPerRevolution; + + private TextField startVelocity; + private TextField endVelocity; + private TextField maxVelocity; + private TextField maxAcceleration; + + private TextField x; + private TextField y; + private TextField angle; + private Label time; + + private CheckBox reversed; + + // Listeners + private ChangeListener xListener; + private ChangeListener yListener; + private ChangeListener angleListener; + + /** + * Returns the time label + * + * @return + */ + public Label getTimeLabel() { + return time; + } + + /** + * Returns true if the path is reversed + * + * @return + */ + public boolean isReversed() { + return reversed.isSelected(); + } + + /** + * Resets the properties window when a robot is unselected + */ + public void reset() { + x.setDisable(true); + y.setDisable(true); + angle.setDisable(true); + + x.setText(""); + y.setText(""); + angle.setText(""); + } + + /** + * Parses the properties from the inputs + */ + private void parseProperties() { + try { + lengthValue.set(Double.parseDouble(length.getText())); + widthValue.set(Double.parseDouble(width.getText())); + AluminatiData.wheelDiamater = Double.parseDouble(wheelDiameter.getText()); + AluminatiData.encoderUnitsPerRotation = Integer.parseInt(encoderTicksPerRevolution.getText()); + + startVelocityValue = Double.parseDouble(startVelocity.getText()); + endVelocityValue = Double.parseDouble(endVelocity.getText()); + + if (maxVelocityValue > 0 && maxAccelerationValue > 0) { + maxVelocityValue = Double.parseDouble(maxVelocity.getText()); + maxAccelerationValue = Double.parseDouble(maxAcceleration.getText()); + + render(); + } + } catch (NumberFormatException e) { + return; + } + } + + /** + * Selects a robot + * + * @param robot + */ + public void select(Robot robot) { + if (xListener != null && yListener != null && angleListener != null) { + x.textProperty().removeListener(xListener); + y.textProperty().removeListener(yListener); + angle.textProperty().removeListener(angleListener); + } + + xListener = (a, b, c) -> { + try { + double value = Double.parseDouble(c) * Field.ratio - robot.getRobot().getWidth() / 2.0; + robot.setTranslateX(value); + } catch (NumberFormatException e) { + return; + } + + render(); + }; + + yListener = (a, b, c) -> { + try { + double value = this.getHeight() - (Double.parseDouble(c) * Field.ratio); + robot.setTranslateY(value); + } catch (NumberFormatException e) { + return; + } + + render(); + }; + + angleListener = (a, b, c) -> { + try { + double value = Double.parseDouble(c); + robot.getRobot().setRotate(-value); + } catch (NumberFormatException e) { + return; + } + + render(); + }; + + DecimalFormat decimalFormat = new DecimalFormat("#####.##"); + double xValue = (robot.getTranslateX() + robot.getRobot().getWidth() / 2.0) / Field.ratio; + double yValue = (this.getHeight() - robot.getTranslateY()) / Field.ratio; + double angleValue = -robot.getRobot().getRotate(); + + x.setText(decimalFormat.format(xValue)); + y.setText(decimalFormat.format(yValue)); + if (robot.getRobot().getRotate() == 0) { + angle.setText("0"); + } else { + angle.setText("" + decimalFormat.format(angleValue)); + } + + x.textProperty().addListener(xListener); + y.textProperty().addListener(yListener); + angle.textProperty().addListener(angleListener); + + x.setDisable(false); + y.setDisable(false); + angle.setDisable(false); + } + + public PropertiesPane() { + super(); + + length = new TextField("" + lengthValue.doubleValue()); + width = new TextField("" + widthValue.doubleValue()); + wheelDiameter = new TextField("" + AluminatiData.wheelDiamater); + encoderTicksPerRevolution = new TextField("" + AluminatiData.encoderUnitsPerRotation); + + startVelocity = new TextField("" + startVelocityValue); + endVelocity = new TextField("" + endVelocityValue); + maxVelocity = new TextField("" + maxVelocityValue); + maxAcceleration = new TextField("" + maxAccelerationValue); + + ChangeListener listener = (a, b, c) -> { + parseProperties(); + }; + + length.textProperty().addListener(listener); + width.textProperty().addListener(listener); + wheelDiameter.textProperty().addListener(listener); + encoderTicksPerRevolution.textProperty().addListener(listener); + startVelocity.textProperty().addListener(listener); + endVelocity.textProperty().addListener(listener); + maxVelocity.textProperty().addListener(listener); + maxAcceleration.textProperty().addListener(listener); + + this.getChildren().add(new Label("Robot length (in)")); + this.getChildren().add(length); + this.getChildren().add(new Label("Robot width (in)")); + this.getChildren().add(width); + this.getChildren().add(new Label("Wheel diameter (in)")); + this.getChildren().add(wheelDiameter); + this.getChildren().add(new Label("Encoder ticks/rev")); + this.getChildren().add(encoderTicksPerRevolution); + this.getChildren().add(new Label("Start velocity (in/s)")); + this.getChildren().add(startVelocity); + this.getChildren().add(new Label("End velocity (in/s)")); + this.getChildren().add(endVelocity); + this.getChildren().add(new Label("Max velocity (in/s)")); + this.getChildren().add(maxVelocity); + this.getChildren().add(new Label("Max acceleration (in/s^2)")); + this.getChildren().add(maxAcceleration); + + x = new TextField(); + y = new TextField(); + angle = new TextField(); + + x.setDisable(true); + y.setDisable(true); + angle.setDisable(true); + + this.getChildren().add(new Label("X (inches)")); + this.getChildren().add(x); + this.getChildren().add(new Label("Y (inches)")); + this.getChildren().add(y); + this.getChildren().add(new Label("Angle (degrees)")); + this.getChildren().add(angle); + + time = new Label("Time: -"); + this.getChildren().add(time); + + reversed = new CheckBox("Reversed"); + reversed.selectedProperty().addListener((a, b, c) -> { + parseProperties(); + }); + this.getChildren().add(reversed); + + Button displayButton = new Button("Display Output"); + displayButton.setOnMouseClicked(e -> { + new OutputWindow(getTrajectoryInfoString()).show(); + }); + this.getChildren().add(displayButton); + } + } +} diff --git a/trajectory/org/aluminati3555/aluminatipath2/OutputWindow.java b/trajectory/src/org/aluminati3555/aluminatipath2/OutputWindow.java similarity index 97% rename from trajectory/org/aluminati3555/aluminatipath2/OutputWindow.java rename to trajectory/src/org/aluminati3555/aluminatipath2/OutputWindow.java index d749c33..10071af 100644 --- a/trajectory/org/aluminati3555/aluminatipath2/OutputWindow.java +++ b/trajectory/src/org/aluminati3555/aluminatipath2/OutputWindow.java @@ -1,51 +1,51 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.aluminatipath2; - -import javafx.scene.Scene; -import javafx.scene.control.TextArea; -import javafx.stage.Stage; - -/** - * This class is a window to view the output - * - * @author Caleb Heydon - */ -public class OutputWindow { - private Stage window; - - /** - * Displays the window - */ - public void show() { - window.show(); - } - - public OutputWindow(String text) { - window = new Stage(); - window.setTitle("AluminatiPath2 - Output"); - - Scene scene = new Scene(new TextArea(text), 800, 600); - window.setScene(scene); - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.aluminatipath2; + +import javafx.scene.Scene; +import javafx.scene.control.TextArea; +import javafx.stage.Stage; + +/** + * This class is a window to view the output + * + * @author Caleb Heydon + */ +public class OutputWindow { + private Stage window; + + /** + * Displays the window + */ + public void show() { + window.show(); + } + + public OutputWindow(String text) { + window = new Stage(); + window.setTitle("AluminatiPath2 - Output"); + + Scene scene = new Scene(new TextArea(text), 800, 600); + window.setScene(scene); + } +} diff --git a/trajectory/org/aluminati3555/aluminatipath2/Robot.java b/trajectory/src/org/aluminati3555/aluminatipath2/Robot.java similarity index 96% rename from trajectory/org/aluminati3555/aluminatipath2/Robot.java rename to trajectory/src/org/aluminati3555/aluminatipath2/Robot.java index 6b4ece1..6867f76 100644 --- a/trajectory/org/aluminati3555/aluminatipath2/Robot.java +++ b/trajectory/src/org/aluminati3555/aluminatipath2/Robot.java @@ -1,126 +1,126 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.aluminatipath2; - -import javafx.scene.Group; -import javafx.scene.paint.Color; -import javafx.scene.shape.Rectangle; -import javafx.scene.transform.Translate; - -/** - * This class is the robot object shown on the screen - * - * @author Caleb Heydon - */ -public class Robot extends Group { - private Rectangle robot; - private Rectangle cover; - - // Dimensions - private double length; - private double width; - - /** - * Returns the length of the robot in inches - * @return - */ - public double getRobotLength() { - return length; - } - - /** - * Returns the width of the robot in inches - * @return - */ - public double getRobotWidth() { - return width; - } - - /** - * Returns the robot rectangle - * @return - */ - public Rectangle getRobot() { - return robot; - } - - /** - * Highlights this robot - */ - public void select() { - robot.setFill(Color.GREENYELLOW); - } - - /** - * Unselects the robot - */ - public void unselect() { - robot.setFill(Color.RED); - } - - /** - * Returns true if this robot is selected - * @return - */ - public boolean isSelected() { - return (robot.getFill() == Color.GREENYELLOW); - } - - /** - * Sets the dimensions of the robot - * @param length - * @param width - */ - public void setDimensions(double length, double width) { - robot.setWidth(length * Field.ratio); - robot.setHeight(width * Field.ratio); - - cover.setWidth(length * Field.ratio - 10); - cover.setHeight(width * Field.ratio - 10); - } - - public Robot(double length, double width) { - this.length = length; - this.width = width; - - robot = new Rectangle(length * Field.ratio, width * Field.ratio); - robot.setArcWidth(10); - robot.setArcHeight(10); - robot.setFill(Color.RED); - robot.setOpacity(0.65); - this.getChildren().add(robot); - - cover = new Rectangle(length * Field.ratio - 10, width * Field.ratio - 10); - cover.setTranslateX(5); - cover.setTranslateY(5); - cover.rotateProperty().bind(robot.rotateProperty()); - cover.setFill(Color.DARKBLUE); - cover.setOpacity(0.65); - this.getChildren().add(cover); - - Translate translate = new Translate(); - translate.setX(0); - translate.yProperty().bind(robot.heightProperty().divide(-2)); - this.getTransforms().add(translate); - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.aluminatipath2; + +import javafx.scene.Group; +import javafx.scene.paint.Color; +import javafx.scene.shape.Rectangle; +import javafx.scene.transform.Translate; + +/** + * This class is the robot object shown on the screen + * + * @author Caleb Heydon + */ +public class Robot extends Group { + private Rectangle robot; + private Rectangle cover; + + // Dimensions + private double length; + private double width; + + /** + * Returns the length of the robot in inches + * @return + */ + public double getRobotLength() { + return length; + } + + /** + * Returns the width of the robot in inches + * @return + */ + public double getRobotWidth() { + return width; + } + + /** + * Returns the robot rectangle + * @return + */ + public Rectangle getRobot() { + return robot; + } + + /** + * Highlights this robot + */ + public void select() { + robot.setFill(Color.GREENYELLOW); + } + + /** + * Unselects the robot + */ + public void unselect() { + robot.setFill(Color.RED); + } + + /** + * Returns true if this robot is selected + * @return + */ + public boolean isSelected() { + return (robot.getFill() == Color.GREENYELLOW); + } + + /** + * Sets the dimensions of the robot + * @param length + * @param width + */ + public void setDimensions(double length, double width) { + robot.setWidth(length * Field.ratio); + robot.setHeight(width * Field.ratio); + + cover.setWidth(length * Field.ratio - 10); + cover.setHeight(width * Field.ratio - 10); + } + + public Robot(double length, double width) { + this.length = length; + this.width = width; + + robot = new Rectangle(length * Field.ratio, width * Field.ratio); + robot.setArcWidth(10); + robot.setArcHeight(10); + robot.setFill(Color.RED); + robot.setOpacity(0.65); + this.getChildren().add(robot); + + cover = new Rectangle(length * Field.ratio - 10, width * Field.ratio - 10); + cover.setTranslateX(5); + cover.setTranslateY(5); + cover.rotateProperty().bind(robot.rotateProperty()); + cover.setFill(Color.DARKBLUE); + cover.setOpacity(0.65); + this.getChildren().add(cover); + + Translate translate = new Translate(); + translate.setX(0); + translate.yProperty().bind(robot.heightProperty().divide(-2)); + this.getTransforms().add(translate); + } +} diff --git a/trajectory/org/aluminati3555/lib/auto/AluminatiAutoTask.java b/trajectory/src/org/aluminati3555/lib/auto/AluminatiAutoTask.java similarity index 96% rename from trajectory/org/aluminati3555/lib/auto/AluminatiAutoTask.java rename to trajectory/src/org/aluminati3555/lib/auto/AluminatiAutoTask.java index 1f9d1f5..ac1ad33 100644 --- a/trajectory/org/aluminati3555/lib/auto/AluminatiAutoTask.java +++ b/trajectory/src/org/aluminati3555/lib/auto/AluminatiAutoTask.java @@ -1,55 +1,55 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.lib.auto; - -/** - * This interface allows for autonomous tasks to be standardized - * - * @author Caleb Heydon - */ -public interface AluminatiAutoTask { - /** - * Initializes the task - */ - public void start(double timestamp); - - /** - * Call this every loop - */ - public void update(double timestamp); - - /** - * Returns true when the task is complete - */ - public boolean isComplete(); - - /** - * Stops the auto task - */ - public void stop(); - - /** - * Advances the state of the autonomous action - */ - public void advanceState(); -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.lib.auto; + +/** + * This interface allows for autonomous tasks to be standardized + * + * @author Caleb Heydon + */ +public interface AluminatiAutoTask { + /** + * Initializes the task + */ + public void start(double timestamp); + + /** + * Call this every loop + */ + public void update(double timestamp); + + /** + * Returns true when the task is complete + */ + public boolean isComplete(); + + /** + * Stops the auto task + */ + public void stop(); + + /** + * Advances the state of the autonomous action + */ + public void advanceState(); +} diff --git a/trajectory/org/aluminati3555/lib/auto/AluminatiAutoTaskList.java b/trajectory/src/org/aluminati3555/lib/auto/AluminatiAutoTaskList.java similarity index 96% rename from trajectory/org/aluminati3555/lib/auto/AluminatiAutoTaskList.java rename to trajectory/src/org/aluminati3555/lib/auto/AluminatiAutoTaskList.java index 394580e..b229512 100644 --- a/trajectory/org/aluminati3555/lib/auto/AluminatiAutoTaskList.java +++ b/trajectory/src/org/aluminati3555/lib/auto/AluminatiAutoTaskList.java @@ -1,127 +1,127 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.lib.auto; - -import java.util.ArrayList; - -/** - * This class allows for auto modes to be easily created. Do not add tasks once - * started - * - * @author Caleb Heydon - */ -public class AluminatiAutoTaskList implements AluminatiAutoTask { - private ArrayList taskList; - private int taskIndex; - - public void start(double timestamp) { - taskList.get(taskIndex).start(timestamp); - } - - public void update(double timestamp) { - // Completely done? - if (isComplete()) { - return; - } - - // Current task done? - if (taskList.get(taskIndex).isComplete()) { - taskIndex++; - - // Start new task if there is one - if (!isComplete()) { - taskList.get(taskIndex).start(timestamp); - } - - // Wait till next loop to update new task - return; - } - - taskList.get(taskIndex).update(timestamp); - } - - public boolean isComplete() { - return (taskIndex >= taskList.size()); - } - - public void stop() { - if (!isComplete()) { - taskList.get(taskIndex).stop(); - taskIndex = taskList.size(); - } - } - - /** - * Does nothing - */ - public void advanceState() { - - } - - /** - * Clears the task list - */ - public void clear() { - taskList.clear(); - } - - /** - * Adds a task to the list - */ - public void add(AluminatiAutoTask task) { - taskList.add(task); - } - - /** - * Removes a task - */ - public void remove(AluminatiAutoTask task) { - taskList.remove(task); - } - - /** - * Removes a task by index - */ - public void remove(int index) { - taskList.remove(index); - } - - /** - * Returns the index of the current task - */ - public int getTaskIndex() { - return taskIndex; - } - - /** - * Returns the current task - */ - public AluminatiAutoTask getCurrentTask() { - return taskList.get(taskIndex); - } - - public AluminatiAutoTaskList() { - taskList = new ArrayList(); - taskIndex = 0; - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.lib.auto; + +import java.util.ArrayList; + +/** + * This class allows for auto modes to be easily created. Do not add tasks once + * started + * + * @author Caleb Heydon + */ +public class AluminatiAutoTaskList implements AluminatiAutoTask { + private ArrayList taskList; + private int taskIndex; + + public void start(double timestamp) { + taskList.get(taskIndex).start(timestamp); + } + + public void update(double timestamp) { + // Completely done? + if (isComplete()) { + return; + } + + // Current task done? + if (taskList.get(taskIndex).isComplete()) { + taskIndex++; + + // Start new task if there is one + if (!isComplete()) { + taskList.get(taskIndex).start(timestamp); + } + + // Wait till next loop to update new task + return; + } + + taskList.get(taskIndex).update(timestamp); + } + + public boolean isComplete() { + return (taskIndex >= taskList.size()); + } + + public void stop() { + if (!isComplete()) { + taskList.get(taskIndex).stop(); + taskIndex = taskList.size(); + } + } + + /** + * Does nothing + */ + public void advanceState() { + + } + + /** + * Clears the task list + */ + public void clear() { + taskList.clear(); + } + + /** + * Adds a task to the list + */ + public void add(AluminatiAutoTask task) { + taskList.add(task); + } + + /** + * Removes a task + */ + public void remove(AluminatiAutoTask task) { + taskList.remove(task); + } + + /** + * Removes a task by index + */ + public void remove(int index) { + taskList.remove(index); + } + + /** + * Returns the index of the current task + */ + public int getTaskIndex() { + return taskIndex; + } + + /** + * Returns the current task + */ + public AluminatiAutoTask getCurrentTask() { + return taskList.get(taskIndex); + } + + public AluminatiAutoTaskList() { + taskList = new ArrayList(); + taskIndex = 0; + } +} diff --git a/trajectory/org/aluminati3555/lib/auto/AluminatiParallelAutoTask.java b/trajectory/src/org/aluminati3555/lib/auto/AluminatiParallelAutoTask.java similarity index 96% rename from trajectory/org/aluminati3555/lib/auto/AluminatiParallelAutoTask.java rename to trajectory/src/org/aluminati3555/lib/auto/AluminatiParallelAutoTask.java index 6e660b1..2dae7ee 100644 --- a/trajectory/org/aluminati3555/lib/auto/AluminatiParallelAutoTask.java +++ b/trajectory/src/org/aluminati3555/lib/auto/AluminatiParallelAutoTask.java @@ -1,77 +1,77 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.lib.auto; - -/** - * This allows for multiple auto tasks to run at the same time - * - * @author Caleb Heydon - */ -public class AluminatiParallelAutoTask implements AluminatiAutoTask { - // The tasks - private AluminatiAutoTask[] tasks; - - /** - * Returns the array of tasks - * @return - */ - public AluminatiAutoTask[] getTasks() { - return tasks; - } - - public void start(double timestamp) { - for (int i = 0; i < tasks.length; i++) { - tasks[i].start(timestamp); - } - } - - public void update(double timestamp) { - for (int i = 0; i < tasks.length; i++) { - tasks[i].update(timestamp); - } - } - - public boolean isComplete() { - for (int i = 0; i < tasks.length; i++) { - if (!tasks[i].isComplete()) { - return false; - } - } - - return true; - } - - public void stop() { - for (int i = 0; i < tasks.length; i++) { - tasks[i].stop(); - } - } - - public void advanceState() { - - } - - public AluminatiParallelAutoTask(AluminatiAutoTask ... tasks) { - this.tasks = tasks; - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.lib.auto; + +/** + * This allows for multiple auto tasks to run at the same time + * + * @author Caleb Heydon + */ +public class AluminatiParallelAutoTask implements AluminatiAutoTask { + // The tasks + private AluminatiAutoTask[] tasks; + + /** + * Returns the array of tasks + * @return + */ + public AluminatiAutoTask[] getTasks() { + return tasks; + } + + public void start(double timestamp) { + for (int i = 0; i < tasks.length; i++) { + tasks[i].start(timestamp); + } + } + + public void update(double timestamp) { + for (int i = 0; i < tasks.length; i++) { + tasks[i].update(timestamp); + } + } + + public boolean isComplete() { + for (int i = 0; i < tasks.length; i++) { + if (!tasks[i].isComplete()) { + return false; + } + } + + return true; + } + + public void stop() { + for (int i = 0; i < tasks.length; i++) { + tasks[i].stop(); + } + } + + public void advanceState() { + + } + + public AluminatiParallelAutoTask(AluminatiAutoTask ... tasks) { + this.tasks = tasks; + } +} diff --git a/trajectory/org/aluminati3555/lib/data/AluminatiData.java b/trajectory/src/org/aluminati3555/lib/data/AluminatiData.java similarity index 97% rename from trajectory/org/aluminati3555/lib/data/AluminatiData.java rename to trajectory/src/org/aluminati3555/lib/data/AluminatiData.java index b9d5858..377989d 100644 --- a/trajectory/org/aluminati3555/lib/data/AluminatiData.java +++ b/trajectory/src/org/aluminati3555/lib/data/AluminatiData.java @@ -1,119 +1,119 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.lib.data; - -/** - * This class holds basic information about the robot. The data starts out - * zeroed and it initialized by the program - * - * WARNING: These default values are for the pigeon gyro and a drivetrain with - * ctre mag encoders - * - * @author Caleb Heydon - */ -public final class AluminatiData { - // Library - public static final int LIBRARY_VERSION = 3; - - // Robot delay - public static double robotDelay = 0.02; // Seconds - - // PID - public static double encoderF = 1; - public static double encoderP = 1; - public static double encoderI = 0; - public static double encoderD = 0; - - public static double gyroF = 1; - public static double gyroP = 1; - public static double gyroI = 0; - public static double gyroD = 0; - - public static int primaryPIDSlot = 0; - public static int auxPIDSlot = 1; - - public static int iZone = 400; - public static int peakOutput = 1; - - // Deadband - public static double deadband = 0.00001; - - // Encoders - public static int encoderUnitsPerRotation = 4096; - - // Pigeon - public static double pigeonTurnUnitsPerDegree = 8192 / 360.0; - - // TalonSRX minimum firmware version - public static int minTalonSRXFirmareVersion = 0; - - // VictorSPX minimum firmware version - public static int minVictorSPXFirmwareVersion = 0; - - // Tipping detection - public static double minTippingAngle = 10; // Degrees - - // Motion profile generation - public static double maxDx = 2; - public static double maxDy = 0.25; - public static double maxDTheta = Math.toRadians(5); - public static double wheelDiamater = 4; // Inches - - // Drive characterization - public static double kV = 0.18; // V/(rad/s^2) - public static double kA = 0.008; // V/(rad/s) - public static double vIntercept = 0.3; // V - public static double linearInertia = 60; // Kg - public static double angularInertia = 120; - public static double angularDrag = 0.1; - public static double scrubFactor = 1; - public static double driveWidth = 20; // Inches - - // Pure pursuit - public static double maxTrackerDistance = 9; - public static double maxGoalTrackAge = 2.5; - public static double maxGoalTrackSmoothingTime = 0.5; - public static double cameraFrameRate = 90; - - public static double pathFollowingMaxVel = 100; - public static double pathFollowingMaxAccel = 100; - - public static double minLookAhead = 12; - public static double maxLookAhead = 48; - public static double minLookAheadSpeed = 12; - public static double maxLookAheadSpeed = 120; - - public static double inertiaSteeringGain = 1; - public static double pathFollowingProfileKP = 1; - public static double pathFollowingProfileKI = 0; - public static double pathFollowingProfileKV = 0; - public static double pathFollowingProfileKFFV = 1 / pathFollowingMaxVel; - public static double pathFollowingProfileKFFA = 1 / pathFollowingMaxAccel; - public static double pathFollowingProfileKS = 0; - public static double pathFollowingGoalPosTolerance = 1; - public static double pathFollowingGoalVelTolerance = 12; - public static double pathStopSteeringDistance = 12; - - // Loops - public static double looperDT = 0.01; -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.lib.data; + +/** + * This class holds basic information about the robot. The data starts out + * zeroed and it initialized by the program + * + * WARNING: These default values are for the pigeon gyro and a drivetrain with + * ctre mag encoders + * + * @author Caleb Heydon + */ +public final class AluminatiData { + // Library + public static final int LIBRARY_VERSION = 3; + + // Robot delay + public static double robotDelay = 0.02; // Seconds + + // PID + public static double encoderF = 1; + public static double encoderP = 1; + public static double encoderI = 0; + public static double encoderD = 0; + + public static double gyroF = 1; + public static double gyroP = 1; + public static double gyroI = 0; + public static double gyroD = 0; + + public static int primaryPIDSlot = 0; + public static int auxPIDSlot = 1; + + public static int iZone = 400; + public static int peakOutput = 1; + + // Deadband + public static double deadband = 0.00001; + + // Encoders + public static int encoderUnitsPerRotation = 4096; + + // Pigeon + public static double pigeonTurnUnitsPerDegree = 8192 / 360.0; + + // TalonSRX minimum firmware version + public static int minTalonSRXFirmareVersion = 0; + + // VictorSPX minimum firmware version + public static int minVictorSPXFirmwareVersion = 0; + + // Tipping detection + public static double minTippingAngle = 10; // Degrees + + // Motion profile generation + public static double maxDx = 2; + public static double maxDy = 0.25; + public static double maxDTheta = Math.toRadians(5); + public static double wheelDiamater = 4; // Inches + + // Drive characterization + public static double kV = 0.18; // V/(rad/s^2) + public static double kA = 0.008; // V/(rad/s) + public static double vIntercept = 0.3; // V + public static double linearInertia = 60; // Kg + public static double angularInertia = 120; + public static double angularDrag = 0.1; + public static double scrubFactor = 1; + public static double driveWidth = 20; // Inches + + // Pure pursuit + public static double maxTrackerDistance = 9; + public static double maxGoalTrackAge = 2.5; + public static double maxGoalTrackSmoothingTime = 0.5; + public static double cameraFrameRate = 90; + + public static double pathFollowingMaxVel = 100; + public static double pathFollowingMaxAccel = 100; + + public static double minLookAhead = 12; + public static double maxLookAhead = 48; + public static double minLookAheadSpeed = 12; + public static double maxLookAheadSpeed = 120; + + public static double inertiaSteeringGain = 1; + public static double pathFollowingProfileKP = 1; + public static double pathFollowingProfileKI = 0; + public static double pathFollowingProfileKV = 0; + public static double pathFollowingProfileKFFV = 1 / pathFollowingMaxVel; + public static double pathFollowingProfileKFFA = 1 / pathFollowingMaxAccel; + public static double pathFollowingProfileKS = 0; + public static double pathFollowingGoalPosTolerance = 1; + public static double pathFollowingGoalVelTolerance = 12; + public static double pathStopSteeringDistance = 12; + + // Loops + public static double looperDT = 0.01; +} diff --git a/trajectory/org/aluminati3555/lib/system/AluminatiSystem.java b/trajectory/src/org/aluminati3555/lib/system/AluminatiSystem.java similarity index 97% rename from trajectory/org/aluminati3555/lib/system/AluminatiSystem.java rename to trajectory/src/org/aluminati3555/lib/system/AluminatiSystem.java index 94827dc..d9e1714 100644 --- a/trajectory/org/aluminati3555/lib/system/AluminatiSystem.java +++ b/trajectory/src/org/aluminati3555/lib/system/AluminatiSystem.java @@ -1,37 +1,37 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.lib.system; - -/** - * This interface is for systems on the robot - * - * @author Caleb Heydon - */ -public interface AluminatiSystem { - /** - * Updates the system - * @param timestamp The timestamp in milliseconds - * @param enabled True if the robot is enabled - */ - public void update(double timestamp, boolean enabled); +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.lib.system; + +/** + * This interface is for systems on the robot + * + * @author Caleb Heydon + */ +public interface AluminatiSystem { + /** + * Updates the system + * @param timestamp The timestamp in milliseconds + * @param enabled True if the robot is enabled + */ + public void update(double timestamp, boolean enabled); } \ No newline at end of file diff --git a/trajectory/org/aluminati3555/lib/trajectory/AluminatiTrajectory.java b/trajectory/src/org/aluminati3555/lib/trajectory/AluminatiTrajectory.java similarity index 97% rename from trajectory/org/aluminati3555/lib/trajectory/AluminatiTrajectory.java rename to trajectory/src/org/aluminati3555/lib/trajectory/AluminatiTrajectory.java index dac9c6e..98e456b 100644 --- a/trajectory/org/aluminati3555/lib/trajectory/AluminatiTrajectory.java +++ b/trajectory/src/org/aluminati3555/lib/trajectory/AluminatiTrajectory.java @@ -1,299 +1,299 @@ -/** - * Copyright (c) 2019 Team 3555 - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.aluminati3555.lib.trajectory; - -import java.util.ArrayList; -import java.util.List; - -import org.aluminati3555.lib.data.AluminatiData; -import org.aluminati3555.lib.util.AluminatiUtil; - -import com.team254.lib.geometry.Pose2d; -import com.team254.lib.geometry.Pose2dWithCurvature; -import com.team254.lib.geometry.Rotation2d; -import com.team254.lib.trajectory.DistanceView; -import com.team254.lib.trajectory.TimedView; -import com.team254.lib.trajectory.Trajectory; -import com.team254.lib.trajectory.TrajectoryIterator; -import com.team254.lib.trajectory.TrajectorySamplePoint; -import com.team254.lib.trajectory.TrajectoryUtil; -import com.team254.lib.trajectory.timing.TimedState; -import com.team254.lib.trajectory.timing.TimingConstraint; -import com.team254.lib.trajectory.timing.TimingUtil; -import com.team319.follower.SrxMotionProfile; -import com.team319.follower.SrxTrajectory; - -/** - * This class generates motion profiles on the robot - * - * @author Caleb Heydon - */ -public class AluminatiTrajectory extends SrxTrajectory { - // Waypoints - ArrayList waypoints; - ArrayList x; - ArrayList y; - - // Motion profile - private double[][] centerPoints; - - // Path variables - private boolean reversed; - private double startVelocity; - private double endVelocity; - private double maxVelocity; - private double maxAcceleration; - private double dt; - - @Override - public String toString() { - return "[AluminatiTrajectory]"; - } - - /** - * Returns the ArrayList of waypoints - * - * @return - */ - public ArrayList getWaypoints() { - return waypoints; - } - - /** - * Returns true if the path is flipped - */ - public boolean isFlipped() { - return flipped; - } - - /** - * Returns true if the path is reversed - * - * @return - */ - public boolean isReversed() { - return reversed; - } - - /** - * Returns the start velocity - * - * @return - */ - public double getStartVelocity() { - return startVelocity; - } - - /** - * Returns the end velocity - */ - public double getEndVelocity() { - return endVelocity; - } - - /** - * Returns the max velocity - * - * @return - */ - public double getMaxVelocity() { - return maxVelocity; - } - - /** - * Returns the max acceleration - * - * @return - */ - public double getMaxAcceleration() { - return maxAcceleration; - } - - /** - * Returns the dt - */ - public double getDT() { - return dt; - } - - /** - * Returns the x coordinates - * @return - */ - public ArrayList getXPoints() { - return x; - } - - /** - * Returns the y coordinates - * @return - */ - public ArrayList getYPoints() { - return y; - } - - /** - * Generate trajectory method from team 254 with modifications. See - * https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/frc2018/planners/DriveMotionPlanner.java - * - * @param reversed - * @param waypoints - * @param constraints - * @param startVelocity - * @param endVelocity - * @param maxVelocity - * @param maxAcceleration - * @param maxDx - * @param maxDy - * @param maxDTheta - * @return - */ - private Trajectory> generateTrajectory(boolean reversed, List waypoints, - List> constraints, double startVelocity, double endVelocity, - double maxVelocity, double maxAcceleration, double maxDx, double maxDy, double maxDTheta) { - List waypointsMaybeFlipped = waypoints; - Pose2d flip = Pose2d.fromRotation(new Rotation2d(-1, 0, false)); - - if (reversed) { - waypointsMaybeFlipped = new ArrayList<>(waypoints.size()); - for (int i = 0; i < waypoints.size(); ++i) { - waypointsMaybeFlipped.add(waypoints.get(i).transformBy(flip)); - } - } - - // Create a trajectory from splines. - Trajectory trajectory = TrajectoryUtil.trajectoryFromSplineWaypoints(waypointsMaybeFlipped, - maxDx, maxDy, maxDTheta); - - if (reversed) { - List flippedPath = new ArrayList<>(trajectory.length()); - for (int i = 0; i < trajectory.length(); ++i) { - flippedPath.add(new Pose2dWithCurvature(trajectory.getState(i).getPose().transformBy(flip), - -trajectory.getState(i).getCurvature(), trajectory.getState(i).getDCurvatureDs())); - } - trajectory = new Trajectory<>(flippedPath); - } - - List> allConstraints = new ArrayList<>(); - if (constraints != null) { - allConstraints.addAll(constraints); - } - - // Generate the timed trajectory. - Trajectory> timedTrajectory = TimingUtil.timeParameterizeTrajectory(reversed, - new DistanceView<>(trajectory), maxDx, allConstraints, startVelocity, endVelocity, maxVelocity, - maxAcceleration); - - return timedTrajectory; - } - - /** - * Generates the entire path - */ - private void generatePath() { - Trajectory> path = generateTrajectory(reversed, waypoints, null, startVelocity, - endVelocity, maxVelocity, maxAcceleration, AluminatiData.maxDx, AluminatiData.maxDy, - AluminatiData.maxDTheta); - - TrajectoryIterator> pathIterator = new TrajectoryIterator>( - new TimedView(path)); - - ArrayList position = new ArrayList(); - ArrayList velocity = new ArrayList(); - ArrayList heading = new ArrayList(); - - boolean first = true; - while (!pathIterator.isDone()) { - TrajectorySamplePoint> sample; - - if (first) { - sample = pathIterator.getSample(); - first = false; - } else { - sample = pathIterator.advance(dt); - } - - x.add(sample.state().state().getTranslation().x()); - y.add(sample.state().state().getTranslation().y()); - - position.add(AluminatiUtil.inchesToEncoderTicks(sample.state().distance(path.getFirstState()), - AluminatiData.wheelDiamater, AluminatiData.encoderUnitsPerRotation)); - - velocity.add(AluminatiUtil.inchesToEncoderTicks(sample.state().velocity(), AluminatiData.wheelDiamater, - AluminatiData.encoderUnitsPerRotation) / 10); - - heading.add(sample.state().state().getRotation().getDegrees()); - } - - centerPoints = new double[position.size()][4]; - double pointDT = dt * 1000; - double lastAngle = heading.get(0).doubleValue(); - double accumulator = 0; - - for (int i = 0; i < centerPoints.length; i++) { - centerPoints[i][0] = isReversed() ? -position.get(i).doubleValue() : position.get(i).doubleValue(); - centerPoints[i][1] = velocity.get(i).doubleValue(); - centerPoints[i][2] = pointDT; - - double angle = heading.get(i).doubleValue(); - if (Math.abs(angle - lastAngle) > 300) { - if (angle >= 0) { - accumulator -= 360; - } else { - accumulator += 360; - } - } - centerPoints[i][3] = angle + accumulator; - - lastAngle = angle; - } - - centerProfile = new SrxMotionProfile(centerPoints.length, centerPoints); - } - - public AluminatiTrajectory(ArrayList waypoints, - ArrayList> constraints, boolean flipped, boolean reversed, - double startVelocity, double endVelocity, double maxVelocity, double maxAcceleration, - double dt) { - x = new ArrayList(); - y = new ArrayList(); - - this.waypoints = waypoints; - - this.highGear = true; - this.flipped = flipped; - - this.reversed = reversed; - this.startVelocity = startVelocity; - this.endVelocity = endVelocity; - this.maxVelocity = maxVelocity; - this.maxAcceleration = maxAcceleration; - this.dt = dt; - - generatePath(); - } - - public AluminatiTrajectory(ArrayList waypoints, double maxVelocity, double maxAcceleration) { - this(waypoints, null, false, false, 0, 0, maxVelocity, maxAcceleration, 0.01); - } -} +/** + * Copyright (c) 2019 Team 3555 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.aluminati3555.lib.trajectory; + +import java.util.ArrayList; +import java.util.List; + +import org.aluminati3555.lib.data.AluminatiData; +import org.aluminati3555.lib.util.AluminatiUtil; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Pose2dWithCurvature; +import com.team254.lib.geometry.Rotation2d; +import com.team254.lib.trajectory.DistanceView; +import com.team254.lib.trajectory.TimedView; +import com.team254.lib.trajectory.Trajectory; +import com.team254.lib.trajectory.TrajectoryIterator; +import com.team254.lib.trajectory.TrajectorySamplePoint; +import com.team254.lib.trajectory.TrajectoryUtil; +import com.team254.lib.trajectory.timing.TimedState; +import com.team254.lib.trajectory.timing.TimingConstraint; +import com.team254.lib.trajectory.timing.TimingUtil; +import com.team319.follower.SrxMotionProfile; +import com.team319.follower.SrxTrajectory; + +/** + * This class generates motion profiles on the robot + * + * @author Caleb Heydon + */ +public class AluminatiTrajectory extends SrxTrajectory { + // Waypoints + ArrayList waypoints; + ArrayList x; + ArrayList y; + + // Motion profile + private double[][] centerPoints; + + // Path variables + private boolean reversed; + private double startVelocity; + private double endVelocity; + private double maxVelocity; + private double maxAcceleration; + private double dt; + + @Override + public String toString() { + return "[AluminatiTrajectory]"; + } + + /** + * Returns the ArrayList of waypoints + * + * @return + */ + public ArrayList getWaypoints() { + return waypoints; + } + + /** + * Returns true if the path is flipped + */ + public boolean isFlipped() { + return flipped; + } + + /** + * Returns true if the path is reversed + * + * @return + */ + public boolean isReversed() { + return reversed; + } + + /** + * Returns the start velocity + * + * @return + */ + public double getStartVelocity() { + return startVelocity; + } + + /** + * Returns the end velocity + */ + public double getEndVelocity() { + return endVelocity; + } + + /** + * Returns the max velocity + * + * @return + */ + public double getMaxVelocity() { + return maxVelocity; + } + + /** + * Returns the max acceleration + * + * @return + */ + public double getMaxAcceleration() { + return maxAcceleration; + } + + /** + * Returns the dt + */ + public double getDT() { + return dt; + } + + /** + * Returns the x coordinates + * @return + */ + public ArrayList getXPoints() { + return x; + } + + /** + * Returns the y coordinates + * @return + */ + public ArrayList getYPoints() { + return y; + } + + /** + * Generate trajectory method from team 254 with modifications. See + * https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/frc2018/planners/DriveMotionPlanner.java + * + * @param reversed + * @param waypoints + * @param constraints + * @param startVelocity + * @param endVelocity + * @param maxVelocity + * @param maxAcceleration + * @param maxDx + * @param maxDy + * @param maxDTheta + * @return + */ + private Trajectory> generateTrajectory(boolean reversed, List waypoints, + List> constraints, double startVelocity, double endVelocity, + double maxVelocity, double maxAcceleration, double maxDx, double maxDy, double maxDTheta) { + List waypointsMaybeFlipped = waypoints; + Pose2d flip = Pose2d.fromRotation(new Rotation2d(-1, 0, false)); + + if (reversed) { + waypointsMaybeFlipped = new ArrayList<>(waypoints.size()); + for (int i = 0; i < waypoints.size(); ++i) { + waypointsMaybeFlipped.add(waypoints.get(i).transformBy(flip)); + } + } + + // Create a trajectory from splines. + Trajectory trajectory = TrajectoryUtil.trajectoryFromSplineWaypoints(waypointsMaybeFlipped, + maxDx, maxDy, maxDTheta); + + if (reversed) { + List flippedPath = new ArrayList<>(trajectory.length()); + for (int i = 0; i < trajectory.length(); ++i) { + flippedPath.add(new Pose2dWithCurvature(trajectory.getState(i).getPose().transformBy(flip), + -trajectory.getState(i).getCurvature(), trajectory.getState(i).getDCurvatureDs())); + } + trajectory = new Trajectory<>(flippedPath); + } + + List> allConstraints = new ArrayList<>(); + if (constraints != null) { + allConstraints.addAll(constraints); + } + + // Generate the timed trajectory. + Trajectory> timedTrajectory = TimingUtil.timeParameterizeTrajectory(reversed, + new DistanceView<>(trajectory), maxDx, allConstraints, startVelocity, endVelocity, maxVelocity, + maxAcceleration); + + return timedTrajectory; + } + + /** + * Generates the entire path + */ + private void generatePath() { + Trajectory> path = generateTrajectory(reversed, waypoints, null, startVelocity, + endVelocity, maxVelocity, maxAcceleration, AluminatiData.maxDx, AluminatiData.maxDy, + AluminatiData.maxDTheta); + + TrajectoryIterator> pathIterator = new TrajectoryIterator>( + new TimedView(path)); + + ArrayList position = new ArrayList(); + ArrayList velocity = new ArrayList(); + ArrayList heading = new ArrayList(); + + boolean first = true; + while (!pathIterator.isDone()) { + TrajectorySamplePoint> sample; + + if (first) { + sample = pathIterator.getSample(); + first = false; + } else { + sample = pathIterator.advance(dt); + } + + x.add(sample.state().state().getTranslation().x()); + y.add(sample.state().state().getTranslation().y()); + + position.add(AluminatiUtil.inchesToEncoderTicks(sample.state().distance(path.getFirstState()), + AluminatiData.wheelDiamater, AluminatiData.encoderUnitsPerRotation)); + + velocity.add(AluminatiUtil.inchesToEncoderTicks(sample.state().velocity(), AluminatiData.wheelDiamater, + AluminatiData.encoderUnitsPerRotation) / 10); + + heading.add(sample.state().state().getRotation().getDegrees()); + } + + centerPoints = new double[position.size()][4]; + double pointDT = dt * 1000; + double lastAngle = heading.get(0).doubleValue(); + double accumulator = 0; + + for (int i = 0; i < centerPoints.length; i++) { + centerPoints[i][0] = isReversed() ? -position.get(i).doubleValue() : position.get(i).doubleValue(); + centerPoints[i][1] = velocity.get(i).doubleValue(); + centerPoints[i][2] = pointDT; + + double angle = heading.get(i).doubleValue(); + if (Math.abs(angle - lastAngle) > 300) { + if (angle >= 0) { + accumulator -= 360; + } else { + accumulator += 360; + } + } + centerPoints[i][3] = angle + accumulator; + + lastAngle = angle; + } + + centerProfile = new SrxMotionProfile(centerPoints.length, centerPoints); + } + + public AluminatiTrajectory(ArrayList waypoints, + ArrayList> constraints, boolean flipped, boolean reversed, + double startVelocity, double endVelocity, double maxVelocity, double maxAcceleration, + double dt) { + x = new ArrayList(); + y = new ArrayList(); + + this.waypoints = waypoints; + + this.highGear = true; + this.flipped = flipped; + + this.reversed = reversed; + this.startVelocity = startVelocity; + this.endVelocity = endVelocity; + this.maxVelocity = maxVelocity; + this.maxAcceleration = maxAcceleration; + this.dt = dt; + + generatePath(); + } + + public AluminatiTrajectory(ArrayList waypoints, double maxVelocity, double maxAcceleration) { + this(waypoints, null, false, false, 0, 0, maxVelocity, maxAcceleration, 0.01); + } +} diff --git a/trajectory/org/aluminati3555/lib/trajectoryfollowingmotion/Kinematics.java b/trajectory/src/org/aluminati3555/lib/trajectoryfollowingmotion/Kinematics.java similarity index 100% rename from trajectory/org/aluminati3555/lib/trajectoryfollowingmotion/Kinematics.java rename to trajectory/src/org/aluminati3555/lib/trajectoryfollowingmotion/Kinematics.java diff --git a/trajectory/org/aluminati3555/lib/trajectoryfollowingmotion/PathBuilder.java b/trajectory/src/org/aluminati3555/lib/trajectoryfollowingmotion/PathBuilder.java similarity index 100% rename from trajectory/org/aluminati3555/lib/trajectoryfollowingmotion/PathBuilder.java rename to trajectory/src/org/aluminati3555/lib/trajectoryfollowingmotion/PathBuilder.java diff --git a/trajectory/org/aluminati3555/lib/trajectoryfollowingmotion/PathContainer.java b/trajectory/src/org/aluminati3555/lib/trajectoryfollowingmotion/PathContainer.java similarity index 100% rename from trajectory/org/aluminati3555/lib/trajectoryfollowingmotion/PathContainer.java rename to trajectory/src/org/aluminati3555/lib/trajectoryfollowingmotion/PathContainer.java diff --git a/trajectory/org/aluminati3555/lib/util/AluminatiUtil.java b/trajectory/src/org/aluminati3555/lib/util/AluminatiUtil.java similarity index 96% rename from trajectory/org/aluminati3555/lib/util/AluminatiUtil.java rename to trajectory/src/org/aluminati3555/lib/util/AluminatiUtil.java index b466c8b..8c908f0 100644 --- a/trajectory/org/aluminati3555/lib/util/AluminatiUtil.java +++ b/trajectory/src/org/aluminati3555/lib/util/AluminatiUtil.java @@ -1,99 +1,99 @@ -package org.aluminati3555.lib.util; - -import com.team254.lib.physics.DCMotorTransmission; -import com.team254.lib.physics.DifferentialDrive; - -import org.aluminati3555.lib.data.AluminatiData; - -/** - * This class provides utilities - * - * @author Caleb Heydon - */ -public class AluminatiUtil { - /** - * This function converts encoder ticks to inches travelled - * - * @param x - * @param wheelDiameter - * @param encoderTicks - * @return - */ - public static double inchesToEncoderTicks(double x, double wheelDiameter, int encoderTicks) { - double circumference = Math.PI * wheelDiameter; - return (x / circumference) * encoderTicks; - } - - /** - * This function converts inches to meters - */ - public static double inchesToMeters(double inches) { - return inches * 0.0254; - } - - /** - * This function returns the drivetrain model - */ - public static DifferentialDrive getDriveModel() { - DCMotorTransmission transmission = new DCMotorTransmission( - 1 / AluminatiData.kV, inchesToMeters(AluminatiData.wheelDiamater / 2) - * inchesToMeters(AluminatiData.wheelDiamater / 2) / (2 * AluminatiData.kA), - AluminatiData.vIntercept); - - DifferentialDrive model = new DifferentialDrive(AluminatiData.linearInertia, AluminatiData.angularInertia, - AluminatiData.angularDrag, inchesToMeters(AluminatiData.wheelDiamater / 2), - inchesToMeters(AluminatiData.driveWidth / 2 * AluminatiData.scrubFactor), transmission, transmission); - - return model; - } - - /** - * Converts rotations to inches - */ - public static double rotationsToInches(double rotations) { - return rotations* (AluminatiData.wheelDiamater * Math.PI); - } - - /** - * Converts rpm to inches per second - */ - public static double rpmToInchesPerSecond(double rpm) { - return rotationsToInches(rpm) / 60; - } - - /** - * Converts rpm to native units - */ - public static int convertRPMToNativeUnits(double rpm) { - return (int) (rpm * AluminatiData.encoderUnitsPerRotation / 600.0); - } - - /** - * Converts native units to rpm - */ - public static int convertNativeUnitsToRPM(double nativeUnits) { - return (int) (nativeUnits / AluminatiData.encoderUnitsPerRotation * 600.0); - } - - /** - * Converts inches to rotations - */ - public static double inchesToRotations(double inches) { - return inches / (AluminatiData.wheelDiamater * Math.PI); - } - - /** - * Converts inches per second to rpm - */ - public static double inchesPerSecondToRPM(double inchesPerSecond) { - return inchesToRotations(inchesPerSecond) * 60; - } - - /** - * Updates the path following feedforward values - */ - public static void generatePathFollowingFeedforwardValues() { - AluminatiData.pathFollowingProfileKFFV = 1 / AluminatiData.pathFollowingMaxVel; - AluminatiData.pathFollowingProfileKFFA = 1 / AluminatiData.pathFollowingMaxAccel; - } -} +package org.aluminati3555.lib.util; + +import com.team254.lib.physics.DCMotorTransmission; +import com.team254.lib.physics.DifferentialDrive; + +import org.aluminati3555.lib.data.AluminatiData; + +/** + * This class provides utilities + * + * @author Caleb Heydon + */ +public class AluminatiUtil { + /** + * This function converts encoder ticks to inches travelled + * + * @param x + * @param wheelDiameter + * @param encoderTicks + * @return + */ + public static double inchesToEncoderTicks(double x, double wheelDiameter, int encoderTicks) { + double circumference = Math.PI * wheelDiameter; + return (x / circumference) * encoderTicks; + } + + /** + * This function converts inches to meters + */ + public static double inchesToMeters(double inches) { + return inches * 0.0254; + } + + /** + * This function returns the drivetrain model + */ + public static DifferentialDrive getDriveModel() { + DCMotorTransmission transmission = new DCMotorTransmission( + 1 / AluminatiData.kV, inchesToMeters(AluminatiData.wheelDiamater / 2) + * inchesToMeters(AluminatiData.wheelDiamater / 2) / (2 * AluminatiData.kA), + AluminatiData.vIntercept); + + DifferentialDrive model = new DifferentialDrive(AluminatiData.linearInertia, AluminatiData.angularInertia, + AluminatiData.angularDrag, inchesToMeters(AluminatiData.wheelDiamater / 2), + inchesToMeters(AluminatiData.driveWidth / 2 * AluminatiData.scrubFactor), transmission, transmission); + + return model; + } + + /** + * Converts rotations to inches + */ + public static double rotationsToInches(double rotations) { + return rotations* (AluminatiData.wheelDiamater * Math.PI); + } + + /** + * Converts rpm to inches per second + */ + public static double rpmToInchesPerSecond(double rpm) { + return rotationsToInches(rpm) / 60; + } + + /** + * Converts rpm to native units + */ + public static int convertRPMToNativeUnits(double rpm) { + return (int) (rpm * AluminatiData.encoderUnitsPerRotation / 600.0); + } + + /** + * Converts native units to rpm + */ + public static int convertNativeUnitsToRPM(double nativeUnits) { + return (int) (nativeUnits / AluminatiData.encoderUnitsPerRotation * 600.0); + } + + /** + * Converts inches to rotations + */ + public static double inchesToRotations(double inches) { + return inches / (AluminatiData.wheelDiamater * Math.PI); + } + + /** + * Converts inches per second to rpm + */ + public static double inchesPerSecondToRPM(double inchesPerSecond) { + return inchesToRotations(inchesPerSecond) * 60; + } + + /** + * Updates the path following feedforward values + */ + public static void generatePathFollowingFeedforwardValues() { + AluminatiData.pathFollowingProfileKFFV = 1 / AluminatiData.pathFollowingMaxVel; + AluminatiData.pathFollowingProfileKFFA = 1 / AluminatiData.pathFollowingMaxAccel; + } +}