├── calibration ├── .gitignore ├── requirements.txt ├── main.py ├── Calibration_Pipeline.vpr └── calibration.py ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── src ├── main │ └── java │ │ └── com │ │ └── team254 │ │ ├── lib │ │ ├── util │ │ │ ├── CSVWritable.java │ │ │ ├── LatchedBoolean.java │ │ │ ├── CrashTrackingRunnable.java │ │ │ ├── TimeDelayedBoolean.java │ │ │ ├── InverseInterpolable.java │ │ │ ├── MinTimeBoolean.java │ │ │ ├── DelayedBoolean.java │ │ │ ├── Interpolable.java │ │ │ ├── MovingAverage.java │ │ │ ├── Units.java │ │ │ ├── MultiTrigger.java │ │ │ ├── MovingAverageTwist2d.java │ │ │ ├── CircularBuffer.java │ │ │ ├── InterpolatingLong.java │ │ │ ├── InterpolatingDouble.java │ │ │ ├── CircularBufferGeneric.java │ │ │ ├── DriveOutput.java │ │ │ ├── OpenLoopCheesyDriveHelper.java │ │ │ ├── DriveSignal.java │ │ │ ├── CrashTracker.java │ │ │ ├── ReflectingCSVWriter.java │ │ │ ├── StatFinder.java │ │ │ ├── InterpolatingTreeMap.java │ │ │ ├── Util.java │ │ │ └── ShootingParameters.java │ │ ├── geometry │ │ │ ├── IRotation2d.java │ │ │ ├── ITranslation2d.java │ │ │ ├── ICurvature.java │ │ │ ├── IPose2d.java │ │ │ ├── State.java │ │ │ ├── Displacement1d.java │ │ │ └── Twist2d.java │ │ ├── motion │ │ │ ├── MotionUtil.java │ │ │ ├── MotionProfileConstraints.java │ │ │ └── MotionSegment.java │ │ ├── trajectory │ │ │ ├── IPathFollower.java │ │ │ ├── TrajectoryView.java │ │ │ ├── TrajectoryPoint.java │ │ │ ├── timing │ │ │ │ ├── CentripetalAccelerationConstraint.java │ │ │ │ ├── TimingConstraint.java │ │ │ │ ├── VelocityLimitRegionConstraint.java │ │ │ │ ├── DifferentialDriveDynamicsConstraint.java │ │ │ │ └── TimedState.java │ │ │ ├── TrajectorySamplePoint.java │ │ │ ├── TrajectoryIterator.java │ │ │ ├── TimedView.java │ │ │ └── DistanceView.java │ │ ├── drivers │ │ │ ├── TalonUtil.java │ │ │ ├── LazyTalonSRX.java │ │ │ ├── LazyTalonFX.java │ │ │ ├── TalonChecker.java │ │ │ └── TalonFXChecker.java │ │ ├── vision │ │ │ ├── TargetInfo.java │ │ │ └── AimingParameters.java │ │ ├── spline │ │ │ ├── Spline.java │ │ │ ├── CubicHermiteSpline.java │ │ │ └── SplineGenerator.java │ │ ├── physics │ │ │ └── DCMotorTransmission.java │ │ └── wpilib │ │ │ └── TimedRobot.java │ │ └── frc2020 │ │ ├── loops │ │ ├── ILooper.java │ │ ├── Loop.java │ │ └── Looper.java │ │ ├── controlboard │ │ ├── IControlBoard.java │ │ ├── IDriveControlBoard.java │ │ ├── IButtonControlBoard.java │ │ ├── MainDriveControlBoard.java │ │ ├── GamepadDriveControlBoard.java │ │ ├── XboxController.java │ │ └── CardinalDirection.java │ │ ├── limelight │ │ ├── undistort │ │ │ ├── UndistortMap.java │ │ │ └── UndistortConstants.java │ │ ├── CameraResolution.java │ │ ├── LimelightManager.java │ │ ├── PipelineConfiguration.java │ │ └── constants │ │ │ └── LimelightConstants.java │ │ ├── auto │ │ ├── AutoModeEndedException.java │ │ ├── actions │ │ │ ├── OverrideTrajectoryAction.java │ │ │ ├── NoopAction.java │ │ │ ├── LambdaAction.java │ │ │ ├── RunOnceAction.java │ │ │ ├── RunIntakeAction.java │ │ │ ├── StopIntakingAction.java │ │ │ ├── WaitAction.java │ │ │ ├── DeployIntakeAction.java │ │ │ ├── AutoAimAction.java │ │ │ ├── DriveOpenLoopAction.java │ │ │ ├── Action.java │ │ │ ├── ParallelAction.java │ │ │ ├── SeriesAction.java │ │ │ ├── ShootAction.java │ │ │ └── DriveTrajectoryAction.java │ │ ├── modes │ │ │ ├── DoNothingAutoMode.java │ │ │ ├── TestTrajectoryFollowingMode.java │ │ │ ├── FarWOF8Ball.java │ │ │ ├── NearWOF10Ball.java │ │ │ ├── FarWOF10Ball.java │ │ │ └── AutoModeBase.java │ │ └── AutoModeExecutor.java │ │ ├── Main.java │ │ ├── states │ │ ├── LEDState.java │ │ └── TimedLEDState.java │ │ ├── subsystems │ │ ├── Subsystem.java │ │ ├── Turret.java │ │ ├── Infrastructure.java │ │ ├── Canifier.java │ │ └── RobotStateEstimator.java │ │ ├── RobotType.java │ │ ├── Kinematics.java │ │ └── SubsystemManager.java └── test │ └── java │ └── com │ └── team254 │ ├── frc2020 │ ├── TestSystemTest.java │ ├── paths │ │ └── TrajectoryGeneratorTest.java │ └── ConstantsTest.java │ └── lib │ ├── util │ └── DeadbandTest.java │ ├── motion │ ├── MotionSegmentTest.java │ ├── MotionTestUtil.java │ └── SetpointGeneratorTest.java │ ├── spline │ ├── SplineGeneratorTest.java │ └── QuinticHermiteOptimizerTest.java │ └── trajectory │ ├── DistanceViewTest.java │ ├── timing │ └── TimedStateTest.java │ ├── PurePursuitControllerTest.java │ └── TrajectoryIteratorTest.java ├── .wpilib └── wpilib_preferences.json ├── characterization-project ├── gradle │ └── wrapper │ │ ├── gradle-wrapper.jar │ │ └── gradle-wrapper.properties ├── src │ └── main │ │ └── java │ │ └── dc │ │ ├── Main.java │ │ └── SimEnabler.java ├── settings.gradle ├── vendordeps │ ├── navx_frc.json │ └── REVRobotics.json ├── build.gradle └── gradlew.bat ├── .vscode ├── settings.json └── launch.json ├── .github └── workflows │ └── gradle.yml ├── settings.gradle ├── LICENSE ├── Turret_LL_Pipeline.vpr ├── robotconfig.py └── gradlew.bat /calibration/.gitignore: -------------------------------------------------------------------------------- 1 | venv 2 | __pycache__ 3 | .idea 4 | *.jpg 5 | *.png -------------------------------------------------------------------------------- /calibration/requirements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Team254/FRC-2020-Public/HEAD/calibration/requirements.txt -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Team254/FRC-2020-Public/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/CSVWritable.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | public interface CSVWritable { 4 | String toCSV(); 5 | } 6 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/loops/ILooper.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.loops; 2 | 3 | public interface ILooper { 4 | void register(Loop loop); 5 | } 6 | -------------------------------------------------------------------------------- /.wpilib/wpilib_preferences.json: -------------------------------------------------------------------------------- 1 | { 2 | "enableCppIntellisense": false, 3 | "currentLanguage": "java", 4 | "projectYear": "2020", 5 | "teamNumber": 254 6 | } -------------------------------------------------------------------------------- /characterization-project/gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Team254/FRC-2020-Public/HEAD/characterization-project/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/IRotation2d.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | public interface IRotation2d extends State { 4 | Rotation2d getRotation(); 5 | } 6 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/IControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | public interface IControlBoard extends IDriveControlBoard, IButtonControlBoard {} -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/ITranslation2d.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | public interface ITranslation2d extends State { 4 | Translation2d getTranslation(); 5 | } 6 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/ICurvature.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | public interface ICurvature extends State { 4 | double getCurvature(); 5 | 6 | double getDCurvatureDs(); 7 | } 8 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/limelight/undistort/UndistortMap.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.limelight.undistort; 2 | 3 | public interface UndistortMap { 4 | double[] getUndistortedPoint(double x, double y); 5 | 6 | boolean getReady(); 7 | } 8 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/IPose2d.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | public interface IPose2d extends IRotation2d, ITranslation2d { 4 | Pose2d getPose(); 5 | 6 | S transformBy(Pose2d transform); 7 | 8 | S mirror(); 9 | } 10 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/motion/MotionUtil.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | public class MotionUtil { 4 | /** 5 | * A constant for consistent floating-point equality checking within this library. 6 | */ 7 | public static double kEpsilon = 1e-6; 8 | } 9 | -------------------------------------------------------------------------------- /characterization-project/gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /characterization-project/src/main/java/dc/Main.java: -------------------------------------------------------------------------------- 1 | package dc; 2 | 3 | import edu.wpi.first.wpilibj.RobotBase; 4 | 5 | public final class Main { 6 | private Main() { 7 | } 8 | 9 | public static void main(String... args) { 10 | RobotBase.startRobot(Robot::new); 11 | } 12 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/AutoModeEndedException.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto; 2 | 3 | /** 4 | * Exception thrown when an auto mode ends unexpectedly 5 | */ 6 | public class AutoModeEndedException extends Exception { 7 | private static final long serialVersionUID = 1411131586291540143L; 8 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/IPathFollower.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Twist2d; 5 | 6 | public interface IPathFollower { 7 | Twist2d steer(Pose2d current_pose); 8 | 9 | boolean isDone(); 10 | } 11 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/IDriveControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | public interface IDriveControlBoard { 4 | double getThrottle(); 5 | 6 | double getTurn(); 7 | 8 | boolean getQuickTurn(); 9 | 10 | boolean getWantsLowGear(); 11 | 12 | boolean getShoot(); 13 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/OverrideTrajectoryAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Drive; 4 | 5 | public class OverrideTrajectoryAction extends RunOnceAction { 6 | @Override 7 | public void runOnce() { 8 | Drive.getInstance().overrideTrajectory(true); 9 | } 10 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/modes/DoNothingAutoMode.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.modes; 2 | 3 | import com.team254.frc2020.auto.AutoModeEndedException; 4 | 5 | public class DoNothingAutoMode extends AutoModeBase { 6 | @Override 7 | protected void routine() throws AutoModeEndedException { 8 | System.out.println("Do nothing auto mode"); 9 | } 10 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "java.configuration.updateBuildConfiguration": "automatic", 3 | "files.exclude": { 4 | "**/.git": true, 5 | "**/.svn": true, 6 | "**/.hg": true, 7 | "**/CVS": true, 8 | "**/.DS_Store": true, 9 | "bin/": true, 10 | "**/.classpath": true, 11 | "**/.project": true, 12 | "**/.settings": true, 13 | "**/.factorypath": true 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/TrajectoryView.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.State; 4 | 5 | public interface TrajectoryView> { 6 | TrajectorySamplePoint sample(final double interpolant); 7 | 8 | double first_interpolant(); 9 | 10 | double last_interpolant(); 11 | 12 | Trajectory trajectory(); 13 | } 14 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/loops/Loop.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.loops; 2 | 3 | /** 4 | * Interface for loops, which are routine that run periodically in the robot code (such as periodic gyroscope 5 | * calibration, etc.) 6 | */ 7 | public interface Loop { 8 | void onStart(double timestamp); 9 | 10 | void onLoop(double timestamp); 11 | 12 | void onStop(double timestamp); 13 | } 14 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/State.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | import com.team254.lib.util.CSVWritable; 4 | import com.team254.lib.util.Interpolable; 5 | 6 | public interface State extends Interpolable, CSVWritable { 7 | double distance(final S other); 8 | 9 | boolean equals(final Object other); 10 | 11 | String toString(); 12 | 13 | String toCSV(); 14 | } 15 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/NoopAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | public class NoopAction implements Action { 4 | 5 | @Override 6 | public void start() {} 7 | 8 | @Override 9 | public void update() {} 10 | 11 | @Override 12 | public boolean isFinished() { 13 | return true; 14 | } 15 | 16 | @Override 17 | public void done() {} 18 | } -------------------------------------------------------------------------------- /src/test/java/com/team254/frc2020/TestSystemTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020; 2 | 3 | import org.junit.Assert; 4 | import org.junit.Test; 5 | import org.junit.runner.RunWith; 6 | import org.junit.runners.JUnit4; 7 | 8 | /** 9 | * Class that tests the system test 10 | */ 11 | @RunWith(JUnit4.class) 12 | public class TestSystemTest { 13 | @Test 14 | public void test() { 15 | // assert statements 16 | Assert.assertEquals(0, 0); 17 | } 18 | } -------------------------------------------------------------------------------- /.github/workflows/gradle.yml: -------------------------------------------------------------------------------- 1 | name: Java CI 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | 8 | runs-on: ${{ matrix.os }} 9 | strategy: 10 | matrix: 11 | os: [macos-latest, ubuntu-latest, windows-latest] 12 | 13 | steps: 14 | - uses: actions/checkout@v1 15 | - name: Set up JDK 11 16 | uses: actions/setup-java@v1 17 | with: 18 | java-version: 11.0.2 19 | - name: Build with Gradle 20 | run: ./gradlew build 21 | -------------------------------------------------------------------------------- /src/test/java/com/team254/frc2020/paths/TrajectoryGeneratorTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.paths; 2 | 3 | import org.junit.Test; 4 | import org.junit.runner.RunWith; 5 | import org.junit.runners.JUnit4; 6 | 7 | /** 8 | * Ensure all trajectories are valid 9 | */ 10 | @RunWith(JUnit4.class) 11 | public class TrajectoryGeneratorTest { 12 | @Test 13 | public void testGenerateTrajectories() { 14 | TrajectoryGenerator.getInstance().generateTrajectories(); 15 | } 16 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/limelight/CameraResolution.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.limelight; 2 | 3 | public enum CameraResolution { 4 | F_320x240(320, 240), 5 | F_960x720(960, 720); 6 | 7 | final int width; 8 | final int height; 9 | 10 | CameraResolution(int width, int height) { 11 | this.width = width; 12 | this.height = height; 13 | } 14 | 15 | public int getWidth() { 16 | return width; 17 | } 18 | 19 | public int getHeight() { 20 | return height; 21 | } 22 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/LatchedBoolean.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * An iterative boolean latch. 5 | *

6 | * Returns true once if and only if the value of newValue changes from false to true. 7 | */ 8 | public class LatchedBoolean { 9 | private boolean mLast = false; 10 | 11 | public boolean update(boolean newValue) { 12 | boolean ret = false; 13 | if (newValue && !mLast) { 14 | ret = true; 15 | } 16 | mLast = newValue; 17 | return ret; 18 | } 19 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/TrajectoryPoint.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.State; 4 | 5 | public class TrajectoryPoint> { 6 | protected final S state_; 7 | protected final int index_; 8 | 9 | public TrajectoryPoint(final S state, int index) { 10 | state_ = state; 11 | index_ = index; 12 | } 13 | 14 | public S state() { 15 | return state_; 16 | } 17 | 18 | public int index() { 19 | return index_; 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/CrashTrackingRunnable.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * Runnable class with reports all uncaught throws to CrashTracker 5 | */ 6 | public abstract class CrashTrackingRunnable implements Runnable { 7 | 8 | @Override 9 | public final void run() { 10 | try { 11 | runCrashTracked(); 12 | } catch (Throwable t) { 13 | CrashTracker.logThrowableCrash(t); 14 | throw t; 15 | } 16 | } 17 | 18 | public abstract void runCrashTracked(); 19 | } 20 | -------------------------------------------------------------------------------- /src/test/java/com/team254/frc2020/ConstantsTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020; 2 | 3 | import org.junit.Assert; 4 | import org.junit.Test; 5 | import org.junit.runner.RunWith; 6 | import org.junit.runners.JUnit4; 7 | 8 | /** 9 | * Ensure we don't push bad constants 10 | */ 11 | @RunWith(JUnit4.class) 12 | public class ConstantsTest { 13 | @Test 14 | public void test() { 15 | Assert.assertFalse("Still in Gamepad Drive Mode", Constants.kUseDriveGamepad); 16 | Assert.assertFalse("Still in Hood Tuning Mode", Constants.kIsHoodTuning); 17 | } 18 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | 8 | { 9 | "type": "wpilib", 10 | "name": "WPILib Desktop Debug", 11 | "request": "launch", 12 | "desktop": true, 13 | }, 14 | { 15 | "type": "wpilib", 16 | "name": "WPILib roboRIO Debug", 17 | "request": "launch", 18 | "desktop": false, 19 | } 20 | ] 21 | } 22 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/TalonUtil.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.ctre.phoenix.ErrorCode; 4 | import edu.wpi.first.wpilibj.DriverStation; 5 | 6 | public class TalonUtil { 7 | /** 8 | * checks the specified error code for issues 9 | * 10 | * @param errorCode error code 11 | * @param message message to print if error happens 12 | */ 13 | public static void checkError(ErrorCode errorCode, String message) { 14 | if (errorCode != ErrorCode.OK) { 15 | DriverStation.reportError(message + errorCode, false); 16 | } 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/LambdaAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | public class LambdaAction implements Action { 4 | public interface VoidInterace { 5 | void f(); 6 | } 7 | 8 | VoidInterace mF; 9 | 10 | public LambdaAction(VoidInterace f) { 11 | this.mF = f; 12 | } 13 | 14 | @Override 15 | public void start() { 16 | mF.f(); 17 | } 18 | 19 | @Override 20 | public void update() {} 21 | 22 | @Override 23 | public boolean isFinished() { 24 | return true; 25 | } 26 | 27 | @Override 28 | public void done() {} 29 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/RunOnceAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | /** 4 | * Template action for something that only needs to be done once and has no need for updates. 5 | * 6 | * @see Action 7 | */ 8 | public abstract class RunOnceAction implements Action { 9 | @Override 10 | public void start() { 11 | runOnce(); 12 | } 13 | 14 | @Override 15 | public void update() {} 16 | 17 | @Override 18 | public boolean isFinished() { 19 | return true; 20 | } 21 | 22 | @Override 23 | public void done() {} 24 | 25 | public abstract void runOnce(); 26 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/limelight/undistort/UndistortConstants.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.limelight.undistort; 2 | 3 | public class UndistortConstants { 4 | private double[][] cameraMatrix; 5 | private double[] cameraDistortion; 6 | 7 | public UndistortConstants(double[] cameraDistortion, double[][] cameraMatrix) { 8 | this.cameraDistortion = cameraDistortion; 9 | this.cameraMatrix = cameraMatrix; 10 | } 11 | 12 | public double[][] getCameraMatrix() { 13 | return cameraMatrix; 14 | } 15 | 16 | public double[] getCameraDistortion() { 17 | return cameraDistortion; 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/RunIntakeAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Intake; 4 | 5 | public class RunIntakeAction implements Action { 6 | private final Intake mIntake = Intake.getInstance(); 7 | 8 | public RunIntakeAction() {} 9 | 10 | @Override 11 | public void start() { 12 | mIntake.setWantedState(Intake.WantedState.INTAKE); 13 | } 14 | 15 | @Override 16 | public void update() {} 17 | 18 | @Override 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | 23 | @Override 24 | public void done() {} 25 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/StopIntakingAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Intake; 4 | 5 | public class StopIntakingAction implements Action { 6 | private final Intake mIntake = Intake.getInstance(); 7 | 8 | public StopIntakingAction() {} 9 | 10 | @Override 11 | public void start() { 12 | mIntake.setWantedState(Intake.WantedState.IDLE); 13 | } 14 | 15 | @Override 16 | public void update() {} 17 | 18 | @Override 19 | public boolean isFinished() { 20 | return true; 21 | } 22 | 23 | @Override 24 | public void done() {} 25 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/modes/TestTrajectoryFollowingMode.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.modes; 2 | 3 | import com.team254.frc2020.auto.AutoModeEndedException; 4 | import com.team254.frc2020.auto.actions.DriveTrajectoryAction; 5 | import com.team254.frc2020.paths.TrajectoryGenerator; 6 | 7 | public class TestTrajectoryFollowingMode extends AutoModeBase { 8 | @Override 9 | protected void routine() throws AutoModeEndedException { 10 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().testTrajectory)); 11 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().testTrajectoryBack)); 12 | } 13 | } -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/util/DeadbandTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import org.junit.Assert; 4 | import org.junit.Test; 5 | import org.junit.runner.RunWith; 6 | import org.junit.runners.JUnit4; 7 | 8 | @RunWith(JUnit4.class) 9 | public class DeadbandTest { 10 | 11 | private final double kEpsilon = 1e-5; 12 | 13 | @Test 14 | public void testInDeadband() { 15 | Assert.assertEquals(Util.handleDeadband(1, .5), 1, kEpsilon); 16 | Assert.assertEquals(Util.handleDeadband(.5, .1), 0.4444444444, kEpsilon); 17 | Assert.assertEquals(Util.handleDeadband(.1, .2), 0, kEpsilon); 18 | Assert.assertEquals(Util.handleDeadband(.8, .2), 0.75, kEpsilon); 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/TimeDelayedBoolean.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | 5 | /** 6 | * This class contains a boolean value and a timer. It can set its boolean value and return whether the timer is within 7 | * a set timeout. This returns true if the stored value is true and the timeout has expired. 8 | */ 9 | public class TimeDelayedBoolean { 10 | private Timer t = new Timer(); 11 | private boolean m_old = false; 12 | 13 | public boolean update(boolean value, double timeout) { 14 | if (!m_old && value) { 15 | t.reset(); 16 | t.start(); 17 | } 18 | m_old = value; 19 | return value && t.get() >= timeout; 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/motion/MotionSegmentTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | import org.junit.Test; 4 | 5 | import static org.junit.Assert.assertFalse; 6 | import static org.junit.Assert.assertTrue; 7 | 8 | public class MotionSegmentTest { 9 | 10 | @Test 11 | public void valid() { 12 | MotionSegment a = new MotionSegment(new MotionState(0.0, 0.0, 0.0, 0.0), new MotionState(0.0, 0.0, 0.0, 1.0)); 13 | assertFalse(a.isValid()); 14 | 15 | a = new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, 0.0, 1.0, 1.0)); 16 | assertFalse(a.isValid()); 17 | 18 | a = new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, .5, 1.0, 1.0)); 19 | assertTrue(a.isValid()); 20 | } 21 | 22 | } 23 | -------------------------------------------------------------------------------- /calibration/main.py: -------------------------------------------------------------------------------- 1 | from urllib.request import urlopen 2 | from calibration import calibrate_ll 3 | 4 | lower_bound = int(input('Lower bound of the images: ')) 5 | upper_bound = int(input('Upper bound of the images: ')) 6 | 7 | for i in range(lower_bound, upper_bound + 1): 8 | try: 9 | resource = urlopen(f'http://limelight.local:5801/snapshots/{i}.jpg') 10 | except: 11 | break 12 | 13 | output = open(f'{i}.jpg', 'wb') 14 | output.write(resource.read()) 15 | output.close() 16 | print(f'Image {i} done.') 17 | 18 | mtx, scaled_mtx, dist, fovx, fovy = calibrate_ll(9, 6, lower_bound, upper_bound) 19 | 20 | print('\nMatrix (unnormalized):') 21 | print(mtx) 22 | print('Matrix:') 23 | print(scaled_mtx) 24 | print('Distortion coefficients:') 25 | print(dist) 26 | print("Horizontal FOV:", fovx) 27 | print("Vertical FOV:", fovy) 28 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/vision/TargetInfo.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.vision; 2 | 3 | /** 4 | * A container class for Targets detected by the vision system, containing the location in three-dimensional space. 5 | */ 6 | public class TargetInfo { 7 | protected double x = 1.0; 8 | protected double y; 9 | protected double z; 10 | protected double skew; 11 | 12 | public TargetInfo(double y, double z) { 13 | this.y = y; 14 | this.z = z; 15 | } 16 | 17 | public void setSkew(double skew) { 18 | this.skew = skew; 19 | } 20 | 21 | public double getX() { 22 | return x; 23 | } 24 | 25 | public double getY() { 26 | return y; 27 | } 28 | 29 | public double getZ() { 30 | return z; 31 | } 32 | 33 | public double getSkew() { 34 | return skew; 35 | } 36 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/WaitAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | 5 | /** 6 | * Action to wait for a given amount of time To use this Action, call runAction(new WaitAction(your_time)) 7 | */ 8 | public class WaitAction implements Action { 9 | private final double mTimeToWait; 10 | private double mStartTime; 11 | 12 | public WaitAction(double timeToWait) { 13 | mTimeToWait = timeToWait; 14 | } 15 | 16 | @Override 17 | public void start() { 18 | mStartTime = Timer.getFPGATimestamp(); 19 | } 20 | 21 | @Override 22 | public void update() {} 23 | 24 | @Override 25 | public boolean isFinished() { 26 | return Timer.getFPGATimestamp() - mStartTime >= mTimeToWait; 27 | } 28 | 29 | @Override 30 | public void done() {} 31 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.timing; 2 | 3 | import com.team254.lib.geometry.Pose2dWithCurvature; 4 | 5 | public class CentripetalAccelerationConstraint implements TimingConstraint { 6 | final double mMaxCentripetalAccel; 7 | 8 | public CentripetalAccelerationConstraint(final double max_centripetal_accel) { 9 | mMaxCentripetalAccel = max_centripetal_accel; 10 | } 11 | 12 | @Override 13 | public double getMaxVelocity(final Pose2dWithCurvature state) { 14 | return Math.sqrt(Math.abs(mMaxCentripetalAccel / state.getCurvature())); 15 | } 16 | 17 | @Override 18 | public MinMaxAcceleration getMinMaxAcceleration(final Pose2dWithCurvature state, final double velocity) { 19 | return MinMaxAcceleration.kNoLimits; 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/limelight/LimelightManager.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.limelight; 2 | 3 | import com.team254.frc2020.subsystems.Limelight; 4 | 5 | public class LimelightManager { 6 | 7 | private Limelight turretLimelight; 8 | 9 | private static LimelightManager mInstance; 10 | 11 | public static LimelightManager getInstance() { 12 | if (mInstance == null) { 13 | mInstance = new LimelightManager(); 14 | } 15 | return mInstance; 16 | } 17 | 18 | 19 | private LimelightManager() {} 20 | 21 | public void setTurretLimelight(Limelight limelight) { 22 | turretLimelight = limelight; 23 | } 24 | 25 | public Limelight getTurretLimelight() { 26 | return turretLimelight; 27 | } 28 | 29 | public void writePeriodicOutputs() { 30 | turretLimelight.writePeriodicOutputs(); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/DeployIntakeAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Intake; 4 | 5 | public class DeployIntakeAction implements Action { 6 | private final Intake mIntake = Intake.getInstance(); 7 | 8 | private final boolean mShouldDeployIntake; 9 | 10 | public DeployIntakeAction(boolean should_deploy) { 11 | mShouldDeployIntake = should_deploy; 12 | } 13 | 14 | @Override 15 | public void start() { 16 | if (mShouldDeployIntake) { 17 | mIntake.deploy(); 18 | } else { 19 | mIntake.stow(); 20 | } 21 | } 22 | 23 | @Override 24 | public void update() {} 25 | 26 | @Override 27 | public boolean isFinished() { 28 | return mIntake.isStowed() == !mShouldDeployIntake; 29 | } 30 | 31 | @Override 32 | public void done() {} 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/IButtonControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | public interface IButtonControlBoard { 4 | boolean getAimCoarse(); // 2 pt 5 | 6 | boolean getAimFine(); // 3 pt 7 | 8 | boolean getMoveToZero(); 9 | 10 | CardinalDirection getTurretHint(); 11 | 12 | void reset(); 13 | 14 | boolean getIntake(); 15 | 16 | boolean getExhaust(); 17 | 18 | boolean getDeployIntake(); 19 | 20 | boolean getRetractIntake(); 21 | 22 | boolean getHumanPlayerIntake(); 23 | 24 | double getTurretJog(); 25 | 26 | double getClimbJog(); 27 | 28 | double getHoodJog(); 29 | 30 | double getStir(); 31 | 32 | boolean getCancelAutoSerialize(); 33 | 34 | boolean getFnKey(); 35 | 36 | boolean getToggleWOFMode(); 37 | 38 | boolean getToggleHangMode(); 39 | 40 | boolean getZeroGyro(); 41 | 42 | boolean getToggleInPitHangMode(); 43 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/TrajectorySamplePoint.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.State; 4 | 5 | public class TrajectorySamplePoint> { 6 | protected final S state_; 7 | protected final int index_floor_; 8 | protected final int index_ceil_; 9 | 10 | public TrajectorySamplePoint(final S state, int index_floor, int index_ceil) { 11 | state_ = state; 12 | index_floor_ = index_floor; 13 | index_ceil_ = index_ceil; 14 | } 15 | 16 | public TrajectorySamplePoint(final TrajectoryPoint point) { 17 | state_ = point.state(); 18 | index_floor_ = index_ceil_ = point.index(); 19 | } 20 | 21 | public S state() { 22 | return state_; 23 | } 24 | 25 | public int index_floor() { 26 | return index_floor_; 27 | } 28 | 29 | public int index_ceil() { 30 | return index_ceil_; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2020' 8 | File frcHome 9 | if (OperatingSystem.current().isWindows()) { 10 | String publicFolder = System.getenv('PUBLIC') 11 | if (publicFolder == null) { 12 | publicFolder = "C:\\Users\\Public" 13 | } 14 | def homeRoot = new File(publicFolder, "wpilib") 15 | frcHome = new File(homeRoot, frcYear) 16 | } else { 17 | def userFolder = System.getProperty("user.home") 18 | def homeRoot = new File(userFolder, "wpilib") 19 | frcHome = new File(homeRoot, frcYear) 20 | } 21 | def frcHomeMaven = new File(frcHome, 'maven') 22 | maven { 23 | name 'frcHome' 24 | url frcHomeMaven 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/InverseInterpolable.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two endpoint keys and a 5 | * third query key, an InverseInterpolable object can calculate the interpolation parameter of the query key on the 6 | * interval [0, 1]. 7 | * 8 | * @param The Type of InverseInterpolable 9 | * @see InterpolatingTreeMap 10 | */ 11 | public interface InverseInterpolable { 12 | /** 13 | * Given this point (lower), a query point (query), and an upper point (upper), estimate how far (on [0, 1]) between 14 | * 'lower' and 'upper' the query point lies. 15 | * 16 | * @param upper 17 | * @param query 18 | * @return The interpolation parameter on [0, 1] representing how far between this point and the upper point the 19 | * query point lies. 20 | */ 21 | double inverseInterpolate(T upper, T query); 22 | } 23 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/MinTimeBoolean.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * This boolean enforces a minimum time for the value to be true. It captures a rising edge and enforces 5 | * based on timestamp. 6 | */ 7 | public class MinTimeBoolean { 8 | private LatchedBoolean mLatchedBoolean; 9 | private final double mMinTime; 10 | private double mRisingEdgeTime; 11 | 12 | public MinTimeBoolean(double minTime) { 13 | mLatchedBoolean = new LatchedBoolean(); 14 | mMinTime = minTime; 15 | mRisingEdgeTime = Double.NaN; 16 | } 17 | 18 | public boolean update(boolean value, double timestamp) { 19 | if (mLatchedBoolean.update(value)) { 20 | mRisingEdgeTime = timestamp; 21 | } 22 | 23 | if (!value && !Double.isNaN(mRisingEdgeTime) 24 | && (timestamp - mRisingEdgeTime < mMinTime)) { 25 | return true; 26 | } 27 | return value; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /characterization-project/settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2020' 8 | File frcHome 9 | if (OperatingSystem.current().isWindows()) { 10 | String publicFolder = System.getenv('PUBLIC') 11 | if (publicFolder == null) { 12 | publicFolder = "C:\\Users\\Public" 13 | } 14 | def homeRoot = new File(publicFolder, "wpilib") 15 | frcHome = new File(homeRoot, frcYear) 16 | } else { 17 | def userFolder = System.getProperty("user.home") 18 | def homeRoot = new File(userFolder, "wpilib") 19 | frcHome = new File(homeRoot, frcYear) 20 | } 21 | def frcHomeMaven = new File(frcHome, 'maven') 22 | maven { 23 | name 'frcHome' 24 | url frcHomeMaven 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/DelayedBoolean.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * An iterative boolean latch that delays the transition from false to true. 5 | */ 6 | public class DelayedBoolean { 7 | private boolean mLastValue; 8 | private double mTransitionTimestamp; 9 | private final double mDelay; 10 | 11 | public DelayedBoolean(double timestamp, double delay) { 12 | mTransitionTimestamp = timestamp; 13 | mLastValue = false; 14 | mDelay = delay; 15 | } 16 | 17 | public boolean update(double timestamp, boolean value) { 18 | boolean result = false; 19 | 20 | if (value && !mLastValue) { 21 | mTransitionTimestamp = timestamp; 22 | } 23 | 24 | // If we are still true and we have transitioned. 25 | if (value && (timestamp - mTransitionTimestamp > mDelay)) { 26 | result = true; 27 | } 28 | 29 | mLastValue = value; 30 | return result; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/Interpolable.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * Interpolable is an interface used by an Interpolating Tree as the Value type. Given two end points and an 5 | * interpolation parameter on [0, 1], it calculates a new Interpolable representing the interpolated value. 6 | * 7 | * @param The Type of Interpolable 8 | * @see InterpolatingTreeMap 9 | */ 10 | public interface Interpolable { 11 | /** 12 | * Interpolates between this value and an other value according to a given parameter. If x is 0, the method should 13 | * return this value. If x is 1, the method should return the other value. If 0 < x < 1, the return value should be 14 | * interpolated proportionally between the two. 15 | * 16 | * @param other The value of the upper bound 17 | * @param x The requested value. Should be between 0 and 1. 18 | * @return Interpolable The estimated average between the surrounding data 19 | */ 20 | T interpolate(T other, double x); 21 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/LazyTalonSRX.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 5 | 6 | /** 7 | * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping duplicate set 8 | * commands. (By default the Talon flushes the Tx buffer on every set call). 9 | */ 10 | public class LazyTalonSRX extends TalonSRX { 11 | protected double mLastSet = Double.NaN; 12 | protected ControlMode mLastControlMode = null; 13 | 14 | public LazyTalonSRX(int deviceNumber) { 15 | super(deviceNumber); 16 | } 17 | 18 | public double getLastSet() { 19 | return mLastSet; 20 | } 21 | 22 | @Override 23 | public void set(ControlMode mode, double value) { 24 | if (value != mLastSet || mode != mLastControlMode) { 25 | mLastSet = value; 26 | mLastControlMode = mode; 27 | super.set(mode, value); 28 | } 29 | } 30 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/LazyTalonFX.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.ctre.phoenix.motorcontrol.TalonFXControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.TalonFX; 5 | 6 | /** 7 | * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping duplicate set 8 | * commands. (By default the Talon flushes the Tx buffer on every set call). 9 | */ 10 | public class LazyTalonFX extends TalonFX { 11 | protected double mLastSet = Double.NaN; 12 | protected TalonFXControlMode mLastControlMode = null; 13 | 14 | public LazyTalonFX(int deviceNumber) { 15 | super(deviceNumber); 16 | } 17 | 18 | public double getLastSet() { 19 | return mLastSet; 20 | } 21 | 22 | @Override 23 | public void set(TalonFXControlMode mode, double value) { 24 | if (value != mLastSet || mode != mLastControlMode) { 25 | mLastSet = value; 26 | mLastControlMode = mode; 27 | super.set(mode, value); 28 | } 29 | } 30 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/spline/Spline.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.spline; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Pose2dWithCurvature; 5 | import com.team254.lib.geometry.Rotation2d; 6 | import com.team254.lib.geometry.Translation2d; 7 | 8 | public abstract class Spline { 9 | public abstract Translation2d getPoint(double t); 10 | 11 | public abstract Rotation2d getHeading(double t); 12 | 13 | public abstract double getCurvature(double t); 14 | 15 | // dk/dt 16 | public abstract double getDCurvature(double t); 17 | 18 | // ds/dt 19 | public abstract double getVelocity(double t); 20 | 21 | public Pose2d getPose2d(double t) { 22 | return new Pose2d(getPoint(t), getHeading(t)); 23 | } 24 | 25 | public Pose2dWithCurvature getPose2dWithCurvature(double t) { 26 | return new Pose2dWithCurvature(getPose2d(t), getCurvature(t), getDCurvature(t) / getVelocity(t)); 27 | } 28 | 29 | // TODO add toString 30 | // public abstract String toString(); 31 | } 32 | -------------------------------------------------------------------------------- /calibration/Calibration_Pipeline.vpr: -------------------------------------------------------------------------------- 1 | area_max:3.0528476175999995 2 | area_min:0 3 | area_similarity:25 4 | aspect_max:20 5 | aspect_min:0 6 | black_level:0 7 | blue_balance:1482 8 | calibration_type:0 9 | contour_grouping:0 10 | contour_sort_final:0 11 | convexity_max:0 12 | convexity_min:0 13 | corner_approx:2 14 | cross_a_a:1 15 | cross_a_x:0 16 | cross_a_y:0 17 | cross_b_a:1.000000 18 | cross_b_x:0.000000 19 | cross_b_y:0.000000 20 | desc:Top_LL_Hatch_Pipeline 21 | desired_contour_region:0 22 | dilation_steps:1 23 | direction_filter:0 24 | dual_close_sort_origin:0 25 | erosion_steps:0 26 | exposure:20 27 | hue_max:65 28 | hue_min:55 29 | image_flip:1 30 | image_source:0 31 | img_to_show:0 32 | intersection_filter:0 33 | pipeline_led_enabled:0 34 | pipeline_res:1 35 | pipeline_type:0 36 | red_balance:1626 37 | sat_max:255 38 | sat_min:255 39 | send_corners:1 40 | send_raw_contours:0 41 | solve3d:0 42 | solve3d_algo:0 43 | solve3d_conf:0.990000 44 | solve3d_error:8 45 | solve3d_guess:0 46 | solve3d_iterations:50 47 | solve3d_precise:0 48 | val_max:255 49 | val_min:30 50 | y_max:1.000000 51 | y_min:-1.000000 52 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/MovingAverage.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.util.ArrayList; 4 | 5 | /** 6 | * Helper class for storing and calculating a moving average 7 | */ 8 | public class MovingAverage { 9 | ArrayList numbers = new ArrayList(); 10 | private int maxSize; 11 | 12 | public MovingAverage(int maxSize) { 13 | this.maxSize = maxSize; 14 | } 15 | 16 | public void add(double newNumber) { 17 | numbers.add(newNumber); 18 | if (numbers.size() > maxSize) { 19 | numbers.remove(0); 20 | } 21 | } 22 | 23 | public double getAverage() { 24 | double total = 0; 25 | 26 | for (double number : numbers) { 27 | total += number; 28 | } 29 | 30 | return total / numbers.size(); 31 | } 32 | 33 | public int getSize() { 34 | return numbers.size(); 35 | } 36 | 37 | public boolean isUnderMaxSize() { 38 | return getSize() < maxSize; 39 | } 40 | 41 | public void clear() { 42 | numbers.clear(); 43 | } 44 | 45 | } 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Team 254 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /characterization-project/vendordeps/navx_frc.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "navx_frc.json", 3 | "name": "KauaiLabs_navX_FRC", 4 | "version": "3.1.400", 5 | "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", 6 | "mavenUrls": [ 7 | "https://repo1.maven.org/maven2/" 8 | ], 9 | "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.kauailabs.navx.frc", 13 | "artifactId": "navx-java", 14 | "version": "3.1.400" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [ 19 | { 20 | "groupId": "com.kauailabs.navx.frc", 21 | "artifactId": "navx-cpp", 22 | "version": "3.1.400", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": false, 26 | "libName": "navx_frc", 27 | "skipInvalidPlatforms": true, 28 | "binaryPlatforms": [ 29 | "linuxathena", 30 | "linuxraspbian" 31 | ] 32 | } 33 | ] 34 | } 35 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/AutoAimAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Superstructure; 4 | import com.team254.lib.geometry.Rotation2d; 5 | 6 | import java.util.Optional; 7 | 8 | public class AutoAimAction implements Action { 9 | private final Superstructure mSuperstructure = Superstructure.getInstance(); 10 | 11 | private Optional mHint = Optional.empty(); 12 | 13 | public AutoAimAction() {} 14 | 15 | public AutoAimAction(Rotation2d hint) { 16 | mHint = Optional.of(hint); 17 | } 18 | 19 | @Override 20 | public void start() { 21 | if (mHint.isPresent()) { 22 | mSuperstructure.setTurretHint(mHint.get().getDegrees()); 23 | } 24 | 25 | mSuperstructure.setWantedState(Superstructure.WantedState.AIM); 26 | } 27 | 28 | @Override 29 | public void update() {} 30 | 31 | @Override 32 | public boolean isFinished() { 33 | return mSuperstructure.getSystemState() == Superstructure.SystemState.AIMING; 34 | } 35 | 36 | @Override 37 | public void done() {} 38 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/DriveOpenLoopAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Drive; 4 | import com.team254.lib.util.DriveSignal; 5 | import edu.wpi.first.wpilibj.Timer; 6 | 7 | public class DriveOpenLoopAction implements Action { 8 | private static final Drive mDrive = Drive.getInstance(); 9 | 10 | private double mStartTime; 11 | private final double mDuration, mLeft, mRight; 12 | 13 | public DriveOpenLoopAction(double left, double right, double duration) { 14 | mDuration = duration; 15 | mLeft = left; 16 | mRight = right; 17 | } 18 | 19 | @Override 20 | public void start() { 21 | mDrive.setOpenLoop(new DriveSignal(mLeft, mRight)); 22 | mStartTime = Timer.getFPGATimestamp(); 23 | } 24 | 25 | @Override 26 | public void update() {} 27 | 28 | @Override 29 | public boolean isFinished() { 30 | return Timer.getFPGATimestamp() - mStartTime > mDuration; 31 | } 32 | 33 | @Override 34 | public void done() { 35 | mDrive.setOpenLoop(new DriveSignal(0.0, 0.0)); 36 | } 37 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/Action.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | /** 4 | * Action Interface, an interface that describes an iterative action. It is run by an autonomous action, called by the 5 | * method runAction in AutoModeBase (or more commonly in autonomous modes that extend AutoModeBase) 6 | * 7 | * @see com.team254.frc2020.auto.modes.AutoModeBase#runAction 8 | */ 9 | public interface Action { 10 | /** 11 | * Run code once when the action is started, for setup 12 | */ 13 | void start(); 14 | 15 | /** 16 | * Called by runAction in AutoModeBase iteratively until isFinished returns true. Iterative logic lives in this 17 | * method 18 | */ 19 | void update(); 20 | 21 | /** 22 | * Returns whether or not the code has finished execution. When implementing this interface, this method is used by 23 | * the runAction method every cycle to know when to stop running the action 24 | * 25 | * @return boolean 26 | */ 27 | boolean isFinished(); 28 | 29 | /** 30 | * Run code once when the action finishes, usually for clean up 31 | */ 32 | void done(); 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/Units.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | public class Units { 4 | public static final double kGravityInPerSecSq = 386.09; 5 | 6 | public static double rpm_to_rads_per_sec(double rpm) { 7 | return rpm * 2.0 * Math.PI / 60.0; 8 | } 9 | 10 | public static double rads_per_sec_to_rpm(double rads_per_sec) { 11 | return rads_per_sec * 60.0 / (2.0 * Math.PI); 12 | } 13 | 14 | public static double inches_to_meters(double inches) { 15 | return inches * 0.0254; 16 | } 17 | 18 | public static double meters_to_inches(double meters) { 19 | return meters / 0.0254; 20 | } 21 | 22 | public static double feet_to_meters(double feet) { 23 | return inches_to_meters(feet * 12.0); 24 | } 25 | 26 | public static double meters_to_feet(double meters) { 27 | return meters_to_inches(meters) / 12.0; 28 | } 29 | 30 | public static double degrees_to_radians(double degrees) { 31 | return Math.toRadians(degrees); 32 | } 33 | 34 | public static double radians_to_degrees(double radians) { 35 | return Math.toDegrees(radians); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/MultiTrigger.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * Class to differentiate between tapping and holding a joystick button/trigger 5 | */ 6 | public class MultiTrigger { 7 | private final double mTimeout; 8 | private boolean lastPressed = false; 9 | private final LatchedBoolean wasTapped = new LatchedBoolean(); 10 | private final LatchedBoolean wasHeld = new LatchedBoolean(); 11 | private boolean lastTapped = false; 12 | private final TimeDelayedBoolean isHeld = new TimeDelayedBoolean(); 13 | 14 | public MultiTrigger(double timeout) { 15 | mTimeout = timeout; 16 | } 17 | 18 | public void update(boolean pressed) { 19 | lastPressed = pressed; 20 | lastTapped = wasTapped.update(pressed); 21 | isHeld.update(pressed, mTimeout); 22 | } 23 | 24 | public boolean wasTapped() { 25 | return lastTapped; 26 | } 27 | 28 | public boolean isPressed() { 29 | return lastPressed; 30 | } 31 | 32 | public boolean isHeld() { 33 | return isHeld.update(lastPressed, mTimeout); 34 | } 35 | 36 | public boolean holdStarted() { 37 | return wasHeld.update(isHeld()); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Turret_LL_Pipeline.vpr: -------------------------------------------------------------------------------- 1 | area_max:10.190460062499994 2 | area_min:0 3 | area_similarity:25 4 | aspect_max:4.940677012499998 5 | aspect_min:0.5380840124999998 6 | black_level:10 7 | blue_balance:1250 8 | calibration_type:0 9 | contour_grouping:0 10 | contour_sort_final:0 11 | convexity_max:47.7 12 | convexity_min:11.1 13 | corner_approx:2.6 14 | cross_a_a:1 15 | cross_a_x:0 16 | cross_a_y:0 17 | cross_b_a:1 18 | cross_b_x:0 19 | cross_b_y:0 20 | desc:LL_Pipeline_0 21 | desired_contour_region:0 22 | dilation_steps:1 23 | direction_filter:0 24 | dual_close_sort_origin:0 25 | erosion_steps:0 26 | exposure:2 27 | force_convex:1 28 | hue_max:83 29 | hue_min:44 30 | image_flip:0 31 | image_source:0 32 | img_to_show:1 33 | intersection_filter:0 34 | pipeline_led_enabled:1 35 | pipeline_led_power:100 36 | pipeline_res:0 37 | pipeline_type:0 38 | red_balance:2500 39 | roi_x:0 40 | roi_y:0 41 | sat_max:255 42 | sat_min:5 43 | send_corners:1 44 | send_raw_contours:0 45 | solve3d:0 46 | solve3d_algo:0 47 | solve3d_bindtarget:1 48 | solve3d_conf:0.990000 49 | solve3d_error:2 50 | solve3d_guess:0 51 | solve3d_iterations:50 52 | solve3d_precies:1 53 | solve3d_precise:0 54 | solve3d_zoffset:0 55 | val_max:255 56 | val_min:129 57 | y_max:1.000000 58 | y_min:-1.000000 59 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/Main.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) 2018 FIRST. All Rights Reserved. */ 3 | /* Open Source Software - may be modified and shared by FRC teams. The code */ 4 | /* must be accompanied by the FIRST BSD license file in the root directory of */ 5 | /* the project. */ 6 | /*----------------------------------------------------------------------------*/ 7 | 8 | package com.team254.frc2020; 9 | 10 | import edu.wpi.first.wpilibj.RobotBase; 11 | 12 | /** 13 | * Do NOT add any static variables to this class, or any initialization at all. 14 | * Unless you know what you are doing, do not modify this file except to change 15 | * the parameter class to the startRobot call. 16 | */ 17 | public final class Main { 18 | private Main() {} 19 | 20 | /** 21 | * Main initialization function. Do not perform any initialization here. 22 | * 23 | *

24 | * If you change your main robot class, change the parameter type. 25 | */ 26 | public static void main(String... args) { 27 | RobotBase.startRobot(Robot::new); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/motion/MotionProfileConstraints.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | /** 4 | * Constraints for constructing a MotionProfile. 5 | */ 6 | public class MotionProfileConstraints { 7 | protected double max_abs_vel = Double.POSITIVE_INFINITY; 8 | protected double max_abs_acc = Double.POSITIVE_INFINITY; 9 | 10 | public MotionProfileConstraints(double max_vel, double max_acc) { 11 | this.max_abs_vel = Math.abs(max_vel); 12 | this.max_abs_acc = Math.abs(max_acc); 13 | } 14 | 15 | /** 16 | * @return The (positive) maximum allowed velocity. 17 | */ 18 | public double max_abs_vel() { 19 | return max_abs_vel; 20 | } 21 | 22 | /** 23 | * @return The (positive) maximum allowed acceleration. 24 | */ 25 | public double max_abs_acc() { 26 | return max_abs_acc; 27 | } 28 | 29 | @Override 30 | public boolean equals(Object obj) { 31 | if (!(obj instanceof MotionProfileConstraints)) { 32 | return false; 33 | } 34 | final MotionProfileConstraints other = (MotionProfileConstraints) obj; 35 | return (other.max_abs_acc() == max_abs_acc()) && (other.max_abs_vel() == max_abs_vel()); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/states/LEDState.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.states; 2 | 3 | public class LEDState { 4 | public static final LEDState kOff = new LEDState(0.0, 0.0, 0.0); 5 | 6 | public static final LEDState kAiming = new LEDState(0.0, 1.0, 1.0); 7 | public static final LEDState kShooting = new LEDState(1.0, 0.0, 0.0); 8 | 9 | public static final LEDState kRobotZeroed = new LEDState(0.0, 1.0, 0.0); 10 | public static final LEDState kFault = new LEDState(0.0, 0.0, 1.0); 11 | public static final LEDState kFaultHood = new LEDState(1.0, 0.0, 1.0); 12 | 13 | public static final LEDState kClimbing = new LEDState(0.0, 0.3, 1.0); 14 | public static final LEDState kBrakeEngaged = new LEDState(1.0, 0.0, 0.0); 15 | 16 | public static final LEDState kWOF = new LEDState(1.0, 0.3, 0.0); 17 | 18 | public LEDState() {} 19 | 20 | public LEDState(double b, double g, double r) { 21 | blue = b; 22 | green = g; 23 | red = r; 24 | } 25 | 26 | public void copyFrom(LEDState other) { 27 | this.blue = other.blue; 28 | this.green = other.green; 29 | this.red = other.red; 30 | } 31 | 32 | public double blue; 33 | public double green; 34 | public double red; 35 | } 36 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/ParallelAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import java.util.ArrayList; 4 | import java.util.Arrays; 5 | import java.util.List; 6 | 7 | /** 8 | * Composite action, running all sub-actions at the same time All actions are started then updated until all actions 9 | * report being done. 10 | */ 11 | public class ParallelAction implements Action { 12 | private final ArrayList mActions; 13 | 14 | public ParallelAction(List actions) { 15 | mActions = new ArrayList<>(actions); 16 | } 17 | 18 | public ParallelAction(Action... actions) { 19 | this(Arrays.asList(actions)); 20 | } 21 | 22 | @Override 23 | public void start() { 24 | mActions.forEach(Action::start); 25 | } 26 | 27 | @Override 28 | public void update() { 29 | mActions.forEach(Action::update); 30 | } 31 | 32 | @Override 33 | public boolean isFinished() { 34 | for (Action action : mActions) { 35 | if (!action.isFinished()) { 36 | return false; 37 | } 38 | } 39 | return true; 40 | } 41 | 42 | @Override 43 | public void done() { 44 | mActions.forEach(Action::done); 45 | } 46 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/MovingAverageTwist2d.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import com.team254.lib.geometry.Twist2d; 4 | 5 | import java.util.ArrayList; 6 | 7 | /** 8 | * Helper class for storing and calculating a moving average of the Twist2d class 9 | */ 10 | public class MovingAverageTwist2d { 11 | ArrayList twists = new ArrayList(); 12 | private int maxSize; 13 | 14 | public MovingAverageTwist2d(int maxSize) { 15 | this.maxSize = maxSize; 16 | } 17 | 18 | public synchronized void add(Twist2d twist) { 19 | twists.add(twist); 20 | if (twists.size() > maxSize) { 21 | twists.remove(0); 22 | } 23 | } 24 | 25 | public synchronized Twist2d getAverage() { 26 | double x = 0.0, y = 0.0, t = 0.0; 27 | 28 | for (Twist2d twist : twists) { 29 | x += twist.dx; 30 | y += twist.dy; 31 | t += twist.dtheta; 32 | } 33 | 34 | double size = getSize(); 35 | return new Twist2d(x / size, y / size, t / size); 36 | } 37 | 38 | public int getSize() { 39 | return twists.size(); 40 | } 41 | 42 | public boolean isUnderMaxSize() { 43 | return getSize() < maxSize; 44 | } 45 | 46 | public void clear() { 47 | twists.clear(); 48 | } 49 | 50 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/timing/TimingConstraint.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.timing; 2 | 3 | import com.team254.lib.geometry.State; 4 | 5 | public interface TimingConstraint> { 6 | double getMaxVelocity(S state); 7 | 8 | MinMaxAcceleration getMinMaxAcceleration(S state, double velocity); 9 | 10 | class MinMaxAcceleration { 11 | protected final double min_acceleration_; 12 | protected final double max_acceleration_; 13 | 14 | public static MinMaxAcceleration kNoLimits = new MinMaxAcceleration(); 15 | 16 | public MinMaxAcceleration() { 17 | // No limits. 18 | min_acceleration_ = Double.NEGATIVE_INFINITY; 19 | max_acceleration_ = Double.POSITIVE_INFINITY; 20 | } 21 | 22 | public MinMaxAcceleration(double min_acceleration, double max_acceleration) { 23 | min_acceleration_ = min_acceleration; 24 | max_acceleration_ = max_acceleration; 25 | } 26 | 27 | public double min_acceleration() { 28 | return min_acceleration_; 29 | } 30 | 31 | public double max_acceleration() { 32 | return max_acceleration_; 33 | } 34 | 35 | public boolean valid() { 36 | return min_acceleration() <= max_acceleration(); 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/limelight/PipelineConfiguration.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.limelight; 2 | 3 | public class PipelineConfiguration { 4 | public final CameraResolution fullFrameCameraResolution; 5 | public final double zoomFactor; 6 | public final double crosshairX; 7 | public final double crosshairY; 8 | 9 | private final double offsetX; 10 | private final double offsetY; 11 | 12 | public PipelineConfiguration(CameraResolution fullCameraResolution, double zoomFactor) { 13 | this(fullCameraResolution, zoomFactor, 0.5, 0.5); 14 | } 15 | 16 | public PipelineConfiguration(CameraResolution fullFrameCameraResolution, double zoomFactor, double crosshairX, double crosshairY) { 17 | this.fullFrameCameraResolution = fullFrameCameraResolution; 18 | this.zoomFactor = Math.round(zoomFactor); 19 | this.crosshairX = crosshairX; 20 | this.crosshairY = crosshairY; 21 | 22 | offsetX = crosshairX - 1.0 / (2.0 * zoomFactor); 23 | offsetY = crosshairY - 1.0 / (2.0 * zoomFactor); 24 | } 25 | 26 | public double[] normalize(double[] point) { 27 | double[] transformed = new double[2]; 28 | 29 | transformed[0] = point[0] / fullFrameCameraResolution.width / zoomFactor + offsetX; 30 | transformed[1] = point[1] / fullFrameCameraResolution.height / zoomFactor + offsetY; 31 | 32 | return transformed; 33 | } 34 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.timing; 2 | 3 | import com.team254.lib.geometry.ITranslation2d; 4 | import com.team254.lib.geometry.Translation2d; 5 | 6 | public class VelocityLimitRegionConstraint> implements TimingConstraint { 7 | protected final Translation2d min_corner_; 8 | protected final Translation2d max_corner_; 9 | protected final double velocity_limit_; 10 | 11 | public VelocityLimitRegionConstraint(Translation2d min_corner, Translation2d max_corner, double velocity_limit) { 12 | min_corner_ = min_corner; 13 | max_corner_ = max_corner; 14 | velocity_limit_ = velocity_limit; 15 | } 16 | 17 | @Override 18 | public double getMaxVelocity(S state) { 19 | final Translation2d translation = state.getTranslation(); 20 | if (translation.x() <= max_corner_.x() && translation.x() >= min_corner_.x() && 21 | translation.y() <= max_corner_.y() && translation.y() >= min_corner_.y()) { 22 | return velocity_limit_; 23 | } 24 | return Double.POSITIVE_INFINITY; 25 | } 26 | 27 | @Override 28 | public TimingConstraint.MinMaxAcceleration getMinMaxAcceleration(S state, 29 | double velocity) { 30 | return MinMaxAcceleration.kNoLimits; 31 | } 32 | 33 | } 34 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/modes/FarWOF8Ball.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.modes; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.frc2020.auto.AutoModeEndedException; 5 | import com.team254.frc2020.auto.actions.*; 6 | import com.team254.frc2020.paths.TrajectoryGenerator; 7 | import com.team254.lib.geometry.Rotation2d; 8 | 9 | import java.util.List; 10 | 11 | // zero -> far wof -> shooting point 12 | public class FarWOF8Ball extends AutoModeBase { 13 | @Override 14 | protected void routine() throws AutoModeEndedException { 15 | runAction(new AutoAimAction(Rotation2d.fromDegrees(30))); 16 | runAction(new ShootAction(Constants.kCoarseShootingParams, 2)); 17 | runAction(new DeployIntakeAction(true)); 18 | runAction(new RunIntakeAction()); 19 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().startToFarWOF1)); 20 | runAction(new ParallelAction(List.of( 21 | new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().farWOF1ToShoot), 22 | new AutoAimAction(Rotation2d.fromDegrees(0)), 23 | new SeriesAction( 24 | new WaitAction(.5), 25 | new StopIntakingAction()), 26 | new DeployIntakeAction(false) 27 | ) 28 | )); 29 | runAction(new ShootAction(Constants.kFineShootingParams, 2)); 30 | } 31 | } -------------------------------------------------------------------------------- /characterization-project/src/main/java/dc/SimEnabler.java: -------------------------------------------------------------------------------- 1 | package dc; 2 | 3 | import edu.wpi.first.hal.sim.DriverStationSim; 4 | import edu.wpi.first.wpilibj.DriverStation; 5 | import edu.wpi.first.wpilibj.Sendable; 6 | import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; 7 | 8 | public class SimEnabler implements Sendable { 9 | DriverStationSim sim = new DriverStationSim(); 10 | 11 | public SimEnabler() { 12 | sim.setAutonomous(true); 13 | } 14 | 15 | public void setEnabled(boolean enabled) { 16 | sim.setEnabled(enabled); 17 | sim.notifyNewData(); 18 | DriverStation.getInstance().isNewControlData(); 19 | while (DriverStation.getInstance().isEnabled() != enabled) { 20 | try { 21 | Thread.sleep(1); 22 | } catch (InterruptedException exception) { 23 | exception.printStackTrace(); 24 | } 25 | } 26 | } 27 | 28 | @Override 29 | public String getName() { 30 | return "SimEnabler"; 31 | } 32 | 33 | @Override 34 | public void setName(String name) {} 35 | 36 | @Override 37 | public String getSubsystem() { 38 | return ""; 39 | } 40 | 41 | @Override 42 | public void setSubsystem(String subsystem) {} 43 | 44 | @Override 45 | public void initSendable(SendableBuilder builder) { 46 | builder.addBooleanProperty("Enabled", 47 | () -> DriverStation.getInstance().isEnabled(), 48 | enabled -> setEnabled(enabled)); 49 | } 50 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/MainDriveControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | import com.team254.frc2020.Constants; 4 | import edu.wpi.first.wpilibj.Joystick; 5 | 6 | public class MainDriveControlBoard implements IDriveControlBoard { 7 | private static MainDriveControlBoard mInstance = null; 8 | 9 | public static MainDriveControlBoard getInstance() { 10 | if (mInstance == null) { 11 | mInstance = new MainDriveControlBoard(); 12 | } 13 | 14 | return mInstance; 15 | } 16 | 17 | private final Joystick mThrottleStick; 18 | private final Joystick mTurnStick; 19 | 20 | private MainDriveControlBoard() { 21 | mThrottleStick = new Joystick(Constants.kMainThrottleJoystickPort); 22 | mTurnStick = new Joystick(Constants.kMainTurnJoystickPort); 23 | } 24 | 25 | @Override 26 | public double getThrottle() { 27 | return mThrottleStick.getRawAxis(1); 28 | } 29 | 30 | @Override 31 | public double getTurn() { 32 | return mTurnStick.getRawAxis(0); 33 | } 34 | 35 | @Override 36 | public boolean getQuickTurn() { 37 | return mTurnStick.getRawButton(1); 38 | } 39 | 40 | @Override 41 | public boolean getWantsLowGear() { 42 | return mThrottleStick.getRawButton(2); 43 | } 44 | 45 | @Override 46 | public boolean getShoot() { 47 | return mThrottleStick.getRawButton(1); 48 | } 49 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/GamepadDriveControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | import com.team254.frc2020.Constants; 4 | 5 | public class GamepadDriveControlBoard implements IDriveControlBoard { 6 | private static GamepadDriveControlBoard mInstance = null; 7 | 8 | public static GamepadDriveControlBoard getInstance() { 9 | if (mInstance == null) { 10 | mInstance = new GamepadDriveControlBoard(); 11 | } 12 | 13 | return mInstance; 14 | } 15 | 16 | private final XboxController mController; 17 | 18 | private GamepadDriveControlBoard() { 19 | mController = new XboxController(Constants.kDriveGamepadPort); 20 | } 21 | 22 | @Override 23 | public double getThrottle() { 24 | return mController.getJoystick(XboxController.Side.LEFT, XboxController.Axis.Y); 25 | } 26 | 27 | @Override 28 | public double getTurn() { 29 | return -mController.getJoystick(XboxController.Side.RIGHT, XboxController.Axis.X); 30 | } 31 | 32 | @Override 33 | public boolean getWantsLowGear() { 34 | return mController.getButton(XboxController.Button.LB); 35 | } 36 | 37 | @Override 38 | public boolean getQuickTurn() { 39 | return mController.getTrigger(XboxController.Side.RIGHT); 40 | } 41 | 42 | @Override 43 | public boolean getShoot() { 44 | return mController.getTrigger(XboxController.Side.LEFT); 45 | } 46 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/CircularBuffer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.util.LinkedList; 4 | 5 | /** 6 | * Implements a simple circular buffer. 7 | */ 8 | public class CircularBuffer { 9 | final int mWindowSize; 10 | final LinkedList mSamples; 11 | double mSum; 12 | 13 | public CircularBuffer(int window_size) { 14 | mWindowSize = window_size; 15 | mSamples = new LinkedList<>(); 16 | mSum = 0.0; 17 | } 18 | 19 | public void clear() { 20 | mSamples.clear(); 21 | mSum = 0.0; 22 | } 23 | 24 | public double getAverage() { 25 | if (mSamples.isEmpty()) 26 | return 0.0; 27 | return mSum / mSamples.size(); 28 | } 29 | 30 | public void recomputeAverage() { 31 | // Reset any accumulation drift. 32 | mSum = 0.0; 33 | if (mSamples.isEmpty()) 34 | return; 35 | for (Double val : mSamples) { 36 | mSum += val; 37 | } 38 | mSum /= mWindowSize; 39 | } 40 | 41 | public void addValue(double val) { 42 | mSamples.addLast(val); 43 | mSum += val; 44 | if (mSamples.size() > mWindowSize) { 45 | mSum -= mSamples.removeFirst(); 46 | } 47 | } 48 | 49 | public int getNumValues() { 50 | return mSamples.size(); 51 | } 52 | 53 | public boolean isFull() { 54 | return mWindowSize == mSamples.size(); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/SeriesAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import java.util.ArrayList; 4 | import java.util.Arrays; 5 | import java.util.List; 6 | 7 | /** 8 | * Executes one action at a time. Useful as a member of {@link ParallelAction} 9 | */ 10 | public class SeriesAction implements Action { 11 | private Action mCurrentAction; 12 | private final ArrayList mRemainingActions; 13 | 14 | public SeriesAction(List actions) { 15 | mRemainingActions = new ArrayList<>(actions); 16 | mCurrentAction = null; 17 | } 18 | 19 | public SeriesAction(Action... actions) { 20 | this(Arrays.asList(actions)); 21 | } 22 | 23 | @Override 24 | public void start() {} 25 | 26 | @Override 27 | public void update() { 28 | if (mCurrentAction == null) { 29 | if (mRemainingActions.isEmpty()) { 30 | return; 31 | } 32 | 33 | mCurrentAction = mRemainingActions.remove(0); 34 | mCurrentAction.start(); 35 | } 36 | 37 | mCurrentAction.update(); 38 | 39 | if (mCurrentAction.isFinished()) { 40 | mCurrentAction.done(); 41 | mCurrentAction = null; 42 | } 43 | } 44 | 45 | @Override 46 | public boolean isFinished() { 47 | return mRemainingActions.isEmpty() && mCurrentAction == null; 48 | } 49 | 50 | @Override 51 | public void done() {} 52 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/subsystems/Subsystem.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.subsystems; 2 | 3 | import com.team254.frc2020.loops.ILooper; 4 | 5 | /** 6 | * The Subsystem abstract class, which serves as a basic framework for all robot subsystems. Each subsystem outputs 7 | * commands to SmartDashboard, has a stop routine (for after each match), and a routine to zero all sensors, which helps 8 | * with calibration. 9 | *

10 | * All Subsystems only have one instance (after all, one robot does not have two drivetrains), and functions get the 11 | * instance of the drivetrain and act accordingly. Subsystems are also a state machine with a desired state and actual 12 | * state; the robot code will try to match the two states with actions. Each Subsystem also is responsible for 13 | * instantializing all member components at the start of the match. 14 | */ 15 | public abstract class Subsystem { 16 | public void writeToLog() {} 17 | 18 | // Optional design pattern for caching periodic reads to avoid hammering the HAL/CAN. 19 | public void readPeriodicInputs() {} 20 | 21 | // Optional design pattern for caching periodic writes to avoid hammering the HAL/CAN. 22 | public void writePeriodicOutputs() {} 23 | 24 | public void registerEnabledLoops(ILooper mEnabledLooper) {} 25 | 26 | public void zeroSensors() {} 27 | 28 | public abstract void stop(); 29 | 30 | public abstract boolean checkSystem(); 31 | 32 | public abstract void outputTelemetry(); 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/InterpolatingLong.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * A Long that can be interpolated using the InterpolatingTreeMap. 5 | * 6 | * @see InterpolatingTreeMap 7 | */ 8 | public class InterpolatingLong implements Interpolable, InverseInterpolable, 9 | Comparable { 10 | public Long value = 0L; 11 | 12 | public InterpolatingLong(Long val) { 13 | value = val; 14 | } 15 | 16 | @Override 17 | public InterpolatingLong interpolate(InterpolatingLong other, double x) { 18 | Long dydx = other.value - value; 19 | Double searchY = dydx * x + value; 20 | return new InterpolatingLong(searchY.longValue()); 21 | } 22 | 23 | @Override 24 | public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) { 25 | long upper_to_lower = upper.value - value; 26 | if (upper_to_lower <= 0) { 27 | return 0; 28 | } 29 | long query_to_lower = query.value - value; 30 | if (query_to_lower <= 0) { 31 | return 0; 32 | } 33 | return query_to_lower / (double) upper_to_lower; 34 | } 35 | 36 | @Override 37 | public int compareTo(InterpolatingLong other) { 38 | if (other.value < value) { 39 | return 1; 40 | } else if (other.value > value) { 41 | return -1; 42 | } else { 43 | return 0; 44 | } 45 | } 46 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/InterpolatingDouble.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * A Double that can be interpolated using the InterpolatingTreeMap. 5 | * 6 | * @see InterpolatingTreeMap 7 | */ 8 | public class InterpolatingDouble implements Interpolable, InverseInterpolable, 9 | Comparable { 10 | public Double value = 0.0; 11 | 12 | public InterpolatingDouble(Double val) { 13 | value = val; 14 | } 15 | 16 | @Override 17 | public InterpolatingDouble interpolate(InterpolatingDouble other, double x) { 18 | Double dydx = other.value - value; 19 | Double searchY = dydx * x + value; 20 | return new InterpolatingDouble(searchY); 21 | } 22 | 23 | @Override 24 | public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) { 25 | double upper_to_lower = upper.value - value; 26 | if (upper_to_lower <= 0) { 27 | return 0; 28 | } 29 | double query_to_lower = query.value - value; 30 | if (query_to_lower <= 0) { 31 | return 0; 32 | } 33 | return query_to_lower / upper_to_lower; 34 | } 35 | 36 | @Override 37 | public int compareTo(InterpolatingDouble other) { 38 | if (other.value < value) { 39 | return 1; 40 | } else if (other.value > value) { 41 | return -1; 42 | } else { 43 | return 0; 44 | } 45 | } 46 | 47 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/Displacement1d.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | import com.team254.lib.util.Util; 4 | 5 | import java.text.DecimalFormat; 6 | 7 | public class Displacement1d implements State { 8 | 9 | protected final double displacement_; 10 | 11 | public Displacement1d() { 12 | displacement_ = 0.0; 13 | } 14 | 15 | public Displacement1d(double displacement) { 16 | displacement_ = displacement; 17 | } 18 | 19 | public double x() { 20 | return displacement_; 21 | } 22 | 23 | @Override 24 | public Displacement1d interpolate(final Displacement1d other, double x) { 25 | return new Displacement1d(Util.interpolate(displacement_, other.displacement_, x)); 26 | } 27 | 28 | @Override 29 | public double distance(final Displacement1d other) { 30 | return Math.abs(x() - other.x()); 31 | } 32 | 33 | @Override 34 | public boolean equals(final Object other) { 35 | if (!(other instanceof Displacement1d)) { 36 | return false; 37 | } 38 | 39 | return Util.epsilonEquals(x(), ((Displacement1d) other).x()); 40 | } 41 | 42 | @Override 43 | public String toString() { 44 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 45 | return fmt.format("(" + x() + ")"); 46 | } 47 | 48 | @Override 49 | public String toCSV() { 50 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 51 | return fmt.format(x()); 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/ShootAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.subsystems.Superstructure; 4 | import com.team254.lib.util.ShootingParameters; 5 | import edu.wpi.first.wpilibj.Timer; 6 | 7 | public class ShootAction implements Action { 8 | private final Superstructure mSuperstructure = Superstructure.getInstance(); 9 | 10 | private final ShootingParameters mShootingParameters; 11 | 12 | private double mShotDuration; 13 | private double mStartTime = 0.0; 14 | private boolean mHasStarted = false; 15 | 16 | public ShootAction(ShootingParameters params, double duration) { 17 | mShootingParameters = params; 18 | mShotDuration = duration; 19 | } 20 | 21 | @Override 22 | public void start() { 23 | mSuperstructure.setShootingParams(mShootingParameters); 24 | mSuperstructure.setWantedState(Superstructure.WantedState.SHOOT); 25 | } 26 | 27 | @Override 28 | public void update() { 29 | if (mSuperstructure.getSystemState() == Superstructure.SystemState.SHOOT && !mHasStarted) { 30 | mStartTime = Timer.getFPGATimestamp(); 31 | mHasStarted = true; 32 | } 33 | } 34 | 35 | 36 | @Override 37 | public boolean isFinished() { 38 | return (Timer.getFPGATimestamp() - mStartTime) >= mShotDuration && mHasStarted; 39 | } 40 | 41 | @Override 42 | public void done() { 43 | mSuperstructure.setWantedState(Superstructure.WantedState.IDLE); 44 | } 45 | } -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/spline/SplineGeneratorTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.spline; 2 | 3 | import com.team254.lib.geometry.*; 4 | import com.team254.lib.util.Util; 5 | import org.junit.Assert; 6 | import org.junit.Test; 7 | import org.junit.runner.RunWith; 8 | import org.junit.runners.JUnit4; 9 | 10 | import java.util.List; 11 | 12 | @RunWith(JUnit4.class) 13 | public class SplineGeneratorTest { 14 | public static final double kTestEpsilon = Util.kEpsilon; 15 | 16 | @Test 17 | public void test() { 18 | // Create the test spline 19 | Pose2d p1 = new Pose2d(new Translation2d(0, 0), new Rotation2d()); 20 | Pose2d p2 = new Pose2d(new Translation2d(15, 10), new Rotation2d(1, -5, true)); 21 | Spline s = new QuinticHermiteSpline(p1, p2); 22 | 23 | List samples = SplineGenerator.parameterizeSpline(s); 24 | 25 | double arclength = 0; 26 | Pose2dWithCurvature cur_pose = samples.get(0); 27 | for (Pose2dWithCurvature sample : samples) { 28 | final Twist2d t = Pose2d.log(cur_pose.getPose().inverse().transformBy(sample.getPose())); 29 | arclength += t.dx; 30 | cur_pose = sample; 31 | } 32 | 33 | Assert.assertEquals(cur_pose.getTranslation().x(), 15.0, kTestEpsilon); 34 | Assert.assertEquals(cur_pose.getTranslation().y(), 10.0, kTestEpsilon); 35 | Assert.assertEquals(cur_pose.getRotation().getDegrees(), -78.69006752597981, kTestEpsilon); 36 | Assert.assertEquals(arclength, 23.17291953186379, kTestEpsilon); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/CircularBufferGeneric.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.util.LinkedList; 4 | 5 | /** 6 | * Implements a simple circular buffer. 7 | * Can be used for any class. 8 | */ 9 | public class CircularBufferGeneric { 10 | final int mWindowSize; 11 | final LinkedList mSamples; 12 | double mSum; 13 | 14 | public CircularBufferGeneric(int window_size) { 15 | mWindowSize = window_size; 16 | mSamples = new LinkedList<>(); 17 | mSum = 0.0; 18 | } 19 | 20 | 21 | public void clear() { 22 | mSamples.clear(); 23 | mSum = 0.0; 24 | } 25 | 26 | public void addValue(E val) { 27 | mSamples.addLast(val); 28 | if (mSamples.size() > mWindowSize) { 29 | mSamples.removeFirst(); 30 | } 31 | } 32 | 33 | public int getNumValues() { 34 | return mSamples.size(); 35 | } 36 | 37 | public boolean isFull() { 38 | return mWindowSize == mSamples.size(); 39 | } 40 | 41 | public LinkedList getLinkedList() { 42 | /* 43 | * NOTE: To get an Array of the specific class type which the instance is using, 44 | * you have to use this specific code: 45 | * specificCircularBufferGeneric.getLinkedList().toArray(new ClassThatIWant[specificCircularBufferGeneric 46 | * .getLinkedList().size()]); 47 | * The reason is that for some reason an array of a generic class(i.e. E[]) cannot be created because 48 | * of some archaic data flow ambiguities 49 | */ 50 | 51 | return mSamples; 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/DriveOutput.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * Represents a closed loop output to the drivebase 5 | */ 6 | public class DriveOutput { 7 | public double left_velocity; // rad/s 8 | public double right_velocity; // rad/s 9 | 10 | public double left_accel; // rad/s^2 11 | public double right_accel; // rad/s^2 12 | 13 | public double left_feedforward_voltage; 14 | public double right_feedforward_voltage; 15 | 16 | public DriveOutput() { 17 | this(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); 18 | } 19 | 20 | public DriveOutput(double left_velocity, double right_velocity, double left_accel, double right_accel, 21 | double left_feedforward_voltage, double 22 | right_feedforward_voltage) { 23 | this.left_velocity = left_velocity; 24 | this.right_velocity = right_velocity; 25 | this.left_accel = left_accel; 26 | this.right_accel = right_accel; 27 | this.left_feedforward_voltage = left_feedforward_voltage; 28 | this.right_feedforward_voltage = right_feedforward_voltage; 29 | } 30 | 31 | public void flip() { 32 | double tmp_left_velocity = left_velocity; 33 | left_velocity = -right_velocity; 34 | right_velocity = -tmp_left_velocity; 35 | 36 | double tmp_left_accel = left_accel; 37 | left_accel = -right_accel; 38 | right_accel = -tmp_left_accel; 39 | 40 | double tmp_left_feedforward = left_feedforward_voltage; 41 | left_feedforward_voltage = -right_feedforward_voltage; 42 | right_feedforward_voltage = -tmp_left_feedforward; 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/trajectory/DistanceViewTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.Translation2d; 4 | import com.team254.lib.util.Util; 5 | import org.junit.Assert; 6 | import org.junit.Test; 7 | import org.junit.runner.RunWith; 8 | import org.junit.runners.JUnit4; 9 | 10 | import java.util.Arrays; 11 | import java.util.List; 12 | 13 | @RunWith(JUnit4.class) 14 | public class DistanceViewTest { 15 | public static final double kTestEpsilon = Util.kEpsilon; 16 | 17 | @Test 18 | public void test() { 19 | // Specify desired waypoints. 20 | List waypoints = Arrays.asList( 21 | new Translation2d(0.0, 0.0), 22 | new Translation2d(24.0, 0.0), 23 | new Translation2d(36.0, 0.0), 24 | new Translation2d(36.0, 24.0), 25 | new Translation2d(60.0, 24.0)); 26 | 27 | // Create the reference trajectory (straight line motion between waypoints). 28 | Trajectory trajectory = new Trajectory<>(waypoints); 29 | final DistanceView distance_view = new DistanceView<>(trajectory); 30 | 31 | Assert.assertEquals(0.0, distance_view.first_interpolant(), kTestEpsilon); 32 | Assert.assertEquals(84.0, distance_view.last_interpolant(), kTestEpsilon); 33 | 34 | Assert.assertEquals(waypoints.get(0), distance_view.sample(0.0).state()); 35 | Assert.assertEquals(waypoints.get(0).interpolate(waypoints.get(1), 0.5), distance_view.sample(12.0).state()); 36 | Assert.assertEquals(waypoints.get(3).interpolate(waypoints.get(4), 0.5), distance_view.sample(72.0).state()); 37 | } 38 | 39 | } 40 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/Twist2d.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.geometry; 2 | 3 | import com.team254.lib.util.Util; 4 | 5 | import java.text.DecimalFormat; 6 | 7 | /** 8 | * A movement along an arc at constant curvature and velocity. We can use ideas from "differential calculus" to create 9 | * new RigidTransform2d's from a Twist2d and visa versa. 10 | *

11 | * A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc. 12 | */ 13 | public class Twist2d { 14 | protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0); 15 | 16 | public static Twist2d identity() { 17 | return kIdentity; 18 | } 19 | 20 | public final double dx; 21 | public final double dy; 22 | public final double dtheta; // Radians! 23 | 24 | public Twist2d(double dx, double dy, double dtheta) { 25 | this.dx = dx; 26 | this.dy = dy; 27 | this.dtheta = dtheta; 28 | } 29 | 30 | public Twist2d scaled(double scale) { 31 | return new Twist2d(dx * scale, dy * scale, dtheta * scale); 32 | } 33 | 34 | public double norm() { 35 | // Common case of dy == 0 36 | if (dy == 0.0) 37 | return Math.abs(dx); 38 | return Math.hypot(dx, dy); 39 | } 40 | 41 | public double curvature() { 42 | if (Math.abs(dtheta) < Util.kEpsilon && norm() < Util.kEpsilon) 43 | return 0.0; 44 | return dtheta / norm(); 45 | } 46 | 47 | @Override 48 | public String toString() { 49 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 50 | return "(" + fmt.format(dx) + "," + fmt.format(dy) + "," + fmt.format(Math.toDegrees(dtheta)) + " deg)"; 51 | } 52 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/modes/NearWOF10Ball.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.modes; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.frc2020.auto.AutoModeEndedException; 5 | import com.team254.frc2020.auto.actions.*; 6 | import com.team254.frc2020.paths.TrajectoryGenerator; 7 | import com.team254.lib.geometry.Rotation2d; 8 | 9 | // zero -> steel bars by rendezvous point -> back to zeroish -> far wof -> shooting point 10 | public class NearWOF10Ball extends AutoModeBase { 11 | @Override 12 | protected void routine() throws AutoModeEndedException { 13 | runAction(new DeployIntakeAction(true)); 14 | runAction(new RunIntakeAction()); 15 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().startingToNearWOF)); 16 | runAction(new StopIntakingAction()); 17 | runAction(new DeployIntakeAction(false)); 18 | 19 | runAction(new ParallelAction( 20 | new AutoAimAction(Rotation2d.fromDegrees(-30)), 21 | new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().nearWOFToShootingPoint1) 22 | )); 23 | runAction(new ShootAction(Constants.kCoarseShootingParams, 2.0)); 24 | 25 | runAction(new DeployIntakeAction(true)); 26 | runAction(new RunIntakeAction()); 27 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().shootingPoint1ToShootingPoint2)); 28 | runAction(new AutoAimAction(Rotation2d.fromDegrees(0))); 29 | runAction(new StopIntakingAction()); 30 | runAction(new DeployIntakeAction(false)); 31 | runAction(new ShootAction(Constants.kCoarseShootingParams, 3.0)); 32 | } 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/XboxController.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.lib.util.Util; 5 | import edu.wpi.first.wpilibj.GenericHID.RumbleType; 6 | import edu.wpi.first.wpilibj.Joystick; 7 | 8 | public class XboxController { 9 | private final Joystick mController; 10 | 11 | public enum Side { 12 | LEFT, RIGHT 13 | } 14 | 15 | public enum Axis { 16 | X, Y 17 | } 18 | 19 | public enum Button { 20 | A(1), B(2), X(3), Y(4), LB(5), RB(6), BACK(7), START(8), L_JOYSTICK(9), R_JOYSTICK(10); 21 | 22 | public final int id; 23 | 24 | Button(int id) { 25 | this.id = id; 26 | } 27 | } 28 | 29 | XboxController(int port) { 30 | mController = new Joystick(port); 31 | } 32 | 33 | double getJoystick(Side side, Axis axis) { 34 | double deadband = Constants.kJoystickThreshold; 35 | 36 | boolean left = side == Side.LEFT; 37 | boolean y = axis == Axis.Y; 38 | // multiplies by -1 if y-axis (inverted normally) 39 | return Util.handleDeadband((y ? -1 : 1) * mController.getRawAxis((left ? 0 : 4) + (y ? 1 : 0)), deadband); 40 | } 41 | 42 | boolean getTrigger(Side side) { 43 | return mController.getRawAxis(side == Side.LEFT ? 2 : 3) > Constants.kJoystickThreshold; 44 | } 45 | 46 | boolean getButton(Button button) { 47 | return mController.getRawButton(button.id); 48 | } 49 | 50 | int getDPad() { 51 | return mController.getPOV(); 52 | } 53 | 54 | public void setRumble(boolean on) { 55 | mController.setRumble(RumbleType.kRightRumble, on ? 1 : 0); 56 | } 57 | 58 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/OpenLoopCheesyDriveHelper.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.frc2020.Kinematics; 5 | import com.team254.lib.geometry.Twist2d; 6 | 7 | public class OpenLoopCheesyDriveHelper { 8 | private static OpenLoopCheesyDriveHelper mInstance; 9 | 10 | public static OpenLoopCheesyDriveHelper getInstance() { 11 | if (mInstance == null) { 12 | mInstance = new OpenLoopCheesyDriveHelper(); 13 | } 14 | 15 | return mInstance; 16 | } 17 | 18 | private OpenLoopCheesyDriveHelper() {} 19 | 20 | private final double kWheelGain = 0.05; 21 | private final double kWheelNonlinearity = 0.05; 22 | private final double kDenominator = Math.sin(Math.PI / 2.0 * kWheelNonlinearity); 23 | 24 | public synchronized DriveSignal cheesyDrive(double throttle, double wheel, boolean quickTurn) { 25 | 26 | throttle = Util.handleDeadband(throttle, Constants.kDriveThrottleDeadband); 27 | wheel = Util.handleDeadband(wheel, Constants.kDriveWheelDeadband); 28 | 29 | // Apply a sin function that's scaled to make it feel better. 30 | if (!quickTurn) { 31 | wheel = Math.sin(Math.PI / 2.0 * kWheelNonlinearity * wheel); 32 | wheel = Math.sin(Math.PI / 2.0 * kWheelNonlinearity * wheel); 33 | wheel = wheel / (kDenominator * kDenominator) * Math.abs(throttle); 34 | } 35 | 36 | wheel *= kWheelGain; 37 | DriveSignal signal = Kinematics.inverseKinematics(new Twist2d(throttle, 0.0, wheel)); 38 | double scaling_factor = Math.max(1.0, Math.max(Math.abs(signal.getLeft()), Math.abs(signal.getRight()))); 39 | return new DriveSignal(signal.getLeft() / scaling_factor, signal.getRight() / scaling_factor); 40 | } 41 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/TrajectoryIterator.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.State; 4 | 5 | public class TrajectoryIterator> { 6 | protected final TrajectoryView view_; 7 | protected double progress_ = 0.0; 8 | protected TrajectorySamplePoint current_sample_; 9 | 10 | public TrajectoryIterator(final TrajectoryView view) { 11 | view_ = view; 12 | 13 | // No effect if view is empty. 14 | current_sample_ = view_.sample(view_.first_interpolant()); 15 | progress_ = view_.first_interpolant(); 16 | } 17 | 18 | public boolean isDone() { 19 | return getRemainingProgress() == 0.0; 20 | } 21 | 22 | public double getProgress() { 23 | return progress_; 24 | } 25 | 26 | public double getRemainingProgress() { 27 | return Math.max(0.0, view_.last_interpolant() - progress_); 28 | } 29 | 30 | public TrajectorySamplePoint getSample() { 31 | return current_sample_; 32 | } 33 | 34 | public S getState() { 35 | return getSample().state(); 36 | } 37 | 38 | public TrajectorySamplePoint advance(double additional_progress) { 39 | progress_ = Math.max(view_.first_interpolant(), 40 | Math.min(view_.last_interpolant(), progress_ + additional_progress)); 41 | current_sample_ = view_.sample(progress_); 42 | return current_sample_; 43 | } 44 | 45 | public TrajectorySamplePoint preview(double additional_progress) { 46 | final double progress = Math.max(view_.first_interpolant(), 47 | Math.min(view_.last_interpolant(), progress_ + additional_progress)); 48 | return view_.sample(progress); 49 | } 50 | 51 | public Trajectory trajectory() { 52 | return view_.trajectory(); 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/trajectory/timing/TimedStateTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.timing; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Translation2d; 5 | import com.team254.lib.util.Util; 6 | import org.junit.Assert; 7 | import org.junit.Test; 8 | import org.junit.runner.RunWith; 9 | import org.junit.runners.JUnit4; 10 | 11 | @RunWith(JUnit4.class) 12 | public class TimedStateTest { 13 | public static final double kTestEpsilon = Util.kEpsilon; 14 | 15 | @Test 16 | public void test() { 17 | // At (0,0,0), t=0, v=0, acceleration=1 18 | final TimedState start_state = new TimedState<>(Pose2d.fromTranslation(new Translation2d(0.0, 0.0)), 19 | 0.0, 0.0, 1.0); 20 | 21 | // At (.5,0,0), t=1, v=1, acceleration=0 22 | final TimedState end_state = new TimedState<>(Pose2d.fromTranslation(new Translation2d(0.5, 0.0)), 1.0, 23 | 1.0, 0.0); 24 | 25 | Assert.assertEquals(start_state, start_state.interpolate(end_state, 0.0)); 26 | Assert.assertEquals(end_state, start_state.interpolate(end_state, 1.0)); 27 | Assert.assertEquals(end_state, end_state.interpolate(start_state, 0.0)); 28 | System.out.println(end_state.interpolate(start_state, 1.0)); 29 | Assert.assertEquals(start_state, end_state.interpolate(start_state, 1.0)); 30 | 31 | final TimedState intermediate_state = start_state.interpolate(end_state, 0.5); 32 | Assert.assertEquals(0.5, intermediate_state.t(), kTestEpsilon); 33 | Assert.assertEquals(start_state.acceleration(), intermediate_state.acceleration(), kTestEpsilon); 34 | Assert.assertEquals(0.5, intermediate_state.velocity(), kTestEpsilon); 35 | Assert.assertEquals(0.125, intermediate_state.state().getTranslation().x(), kTestEpsilon); 36 | } 37 | 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/vision/AimingParameters.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.vision; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Rotation2d; 5 | 6 | public class AimingParameters { 7 | private final double range; 8 | private final Pose2d field_to_vehicle; 9 | private final Pose2d field_to_goal; 10 | private final Rotation2d robot_to_goal_rotation; 11 | private final double last_seen_timestamp; 12 | private final double stability; 13 | private final int track_id; 14 | 15 | public AimingParameters(Pose2d field_to_vehicle, 16 | Pose2d field_to_goal, double last_seen_timestamp, 17 | double stability, int track_id) { 18 | this.field_to_vehicle = field_to_vehicle; 19 | this.field_to_goal = field_to_goal; 20 | final Pose2d vehicle_to_goal = field_to_vehicle.inverse().transformBy(field_to_goal); 21 | this.range = vehicle_to_goal.getTranslation().norm(); 22 | this.robot_to_goal_rotation = vehicle_to_goal.getTranslation().direction(); 23 | this.last_seen_timestamp = last_seen_timestamp; 24 | this.stability = stability; 25 | this.track_id = track_id; 26 | } 27 | 28 | public Pose2d getFieldToVehicle() { 29 | return field_to_vehicle; 30 | } 31 | 32 | public Pose2d getFieldToGoal() { 33 | return field_to_goal; 34 | } 35 | 36 | public double getRange() { 37 | return range; 38 | } 39 | 40 | public Rotation2d getRobotToGoalRotation() { 41 | return robot_to_goal_rotation; 42 | } 43 | 44 | public double getLastSeenTimestamp() { 45 | return last_seen_timestamp; 46 | } 47 | 48 | public double getStability() { 49 | return stability; 50 | } 51 | 52 | public int getTrackId() { 53 | return track_id; 54 | } 55 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/DriveSignal.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * A drivetrain command consisting of the left, right motor settings and whether 5 | * the brake mode is enabled. 6 | */ 7 | public class DriveSignal { 8 | private final double mLeftMotor; 9 | private final double mRightMotor; 10 | private final boolean mBrakeMode; 11 | 12 | public DriveSignal(double left, double right) { 13 | this(left, right, false); 14 | } 15 | 16 | public DriveSignal(double left, double right, boolean brakeMode) { 17 | mLeftMotor = left; 18 | mRightMotor = right; 19 | mBrakeMode = brakeMode; 20 | } 21 | 22 | public static final DriveSignal NEUTRAL = new DriveSignal(0, 0); 23 | public static final DriveSignal BRAKE = new DriveSignal(0, 0, true); 24 | 25 | public double getLeft() { 26 | return mLeftMotor; 27 | } 28 | 29 | public double getRight() { 30 | return mRightMotor; 31 | } 32 | 33 | public boolean getBrakeMode() { 34 | return mBrakeMode; 35 | } 36 | 37 | /** 38 | * @return a new DriveSignal object with the outputs normalized so the max motor 39 | * output is 1.0 40 | */ 41 | public DriveSignal normalize() { 42 | // if either of the left or right signals is greater than 1, creating a scaling 43 | // factor so that we can proportionally scale down the motor outputs so the max 44 | // output is 1.0 45 | double scaling_factor = Math.max(1.0, Math.max(Math.abs(this.getLeft()), Math.abs(this.getRight()))); 46 | 47 | // divide by scaling factor so that the max motor output is 1 48 | return new DriveSignal(this.getLeft() / scaling_factor, this.getRight() / scaling_factor); 49 | } 50 | 51 | @Override 52 | public String toString() { 53 | return "L: " + mLeftMotor + ", R: " + mRightMotor + (mBrakeMode ? ", BRAKE" : ""); 54 | } 55 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/controlboard/CardinalDirection.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.controlboard; 2 | 3 | import com.team254.lib.geometry.Rotation2d; 4 | 5 | public enum CardinalDirection { 6 | BACK(180), 7 | FRONT(0), 8 | LEFT(45, 90), 9 | RIGHT(-45, -90), 10 | NONE(0), 11 | FRONT_LEFT(45, 45), 12 | FRONT_RIGHT(-45, -45), 13 | BACK_LEFT(-45, 135), 14 | BACK_RIGHT(45, 235); 15 | 16 | public final Rotation2d rotation; 17 | private final Rotation2d inputDirection; 18 | 19 | CardinalDirection(double degrees) { 20 | this(degrees, degrees); 21 | } 22 | 23 | CardinalDirection(double degrees, double inputDirectionDegrees) { 24 | rotation = Rotation2d.fromDegrees(degrees); 25 | inputDirection = Rotation2d.fromDegrees(inputDirectionDegrees); 26 | } 27 | 28 | public static CardinalDirection findClosest(double xAxis, double yAxis) { 29 | return findClosest(new Rotation2d(yAxis, -xAxis, true)); 30 | } 31 | 32 | public static CardinalDirection findClosest(Rotation2d stickDirection) { 33 | var values = CardinalDirection.values(); 34 | 35 | CardinalDirection closest = null; 36 | double closestDistance = Double.MAX_VALUE; 37 | for (int i = 0; i < values.length; i++) { 38 | var checkDirection = values[i]; 39 | var distance = Math.abs(stickDirection.distance(checkDirection.inputDirection)); 40 | if (distance < closestDistance) { 41 | closestDistance = distance; 42 | closest = checkDirection; 43 | } 44 | } 45 | return closest; 46 | } 47 | 48 | public static boolean isDiagonal(CardinalDirection cardinal) { 49 | return cardinal == FRONT_LEFT || cardinal == FRONT_RIGHT || cardinal == BACK_LEFT || cardinal == BACK_RIGHT; 50 | } 51 | 52 | public Rotation2d getRotation() { 53 | return rotation; 54 | } 55 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/RobotType.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020; 2 | 3 | import java.net.NetworkInterface; 4 | import java.net.SocketException; 5 | import java.util.Enumeration; 6 | 7 | public class RobotType { 8 | public enum Type { 9 | COMPETITION, 10 | PRACTICE 11 | } 12 | 13 | private static String kPracticeMAC = "00-80-2F-25-B4-23"; 14 | 15 | public static Type getRobotType() { 16 | String mac = getMACAddress(); 17 | if (mac != null && !mac.equals("") && mac.equals(kPracticeMAC)) { 18 | return Type.PRACTICE; 19 | } 20 | return Type.COMPETITION; 21 | } 22 | 23 | 24 | /** 25 | * @return the MAC address of the robot 26 | */ 27 | private static String getMACAddress() { 28 | try { 29 | Enumeration nwInterface = NetworkInterface.getNetworkInterfaces(); 30 | StringBuilder ret = new StringBuilder(); 31 | while (nwInterface.hasMoreElements()) { 32 | NetworkInterface nis = nwInterface.nextElement(); 33 | if (nis != null) { 34 | byte[] mac = nis.getHardwareAddress(); 35 | if (mac != null) { 36 | for (int i = 0; i < mac.length; i++) { 37 | ret.append(String.format("%02X%s", mac[i], (i < mac.length - 1) ? "-" : "")); 38 | } 39 | return ret.toString(); 40 | } else { 41 | System.out.println("Address doesn't exist or is not accessible"); 42 | } 43 | } else { 44 | System.out.println("Network Interface for the specified address is not found."); 45 | } 46 | } 47 | } catch (SocketException | NullPointerException e) { 48 | e.printStackTrace(); 49 | } 50 | 51 | return ""; 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/actions/DriveTrajectoryAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.actions; 2 | 3 | import com.team254.frc2020.RobotState; 4 | import com.team254.frc2020.subsystems.Drive; 5 | import com.team254.lib.geometry.Pose2dWithCurvature; 6 | import com.team254.lib.trajectory.TimedView; 7 | import com.team254.lib.trajectory.Trajectory; 8 | import com.team254.lib.trajectory.TrajectoryIterator; 9 | import com.team254.lib.trajectory.timing.TimedState; 10 | import edu.wpi.first.wpilibj.Timer; 11 | 12 | public class DriveTrajectoryAction implements Action { 13 | private static final Drive mDrive = Drive.getInstance(); 14 | private static final RobotState mRobotState = RobotState.getInstance(); 15 | 16 | private final TrajectoryIterator> mTrajectory; 17 | private final boolean mResetPose; 18 | 19 | public DriveTrajectoryAction(Trajectory> trajectory) { 20 | this(trajectory, false); 21 | } 22 | 23 | 24 | public DriveTrajectoryAction(Trajectory> trajectory, boolean resetPose) { 25 | mTrajectory = new TrajectoryIterator<>(new TimedView<>(trajectory)); 26 | mResetPose = resetPose; 27 | } 28 | 29 | @Override 30 | public void start() { 31 | System.out.println("Starting trajectory! (length=" + mTrajectory.getRemainingProgress() + ")"); 32 | if (mResetPose) { 33 | mRobotState.reset(Timer.getFPGATimestamp(), mTrajectory.getState().state().getPose()); 34 | } 35 | mDrive.setTrajectory(mTrajectory); 36 | } 37 | 38 | @Override 39 | public void update() {} 40 | 41 | @Override 42 | public boolean isFinished() { 43 | if (mDrive.isDoneWithTrajectory()) { 44 | System.out.println("Trajectory finished"); 45 | return true; 46 | } 47 | return false; 48 | } 49 | 50 | @Override 51 | public void done() {} 52 | } 53 | 54 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/CrashTracker.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.io.FileWriter; 4 | import java.io.IOException; 5 | import java.io.PrintWriter; 6 | import java.util.Date; 7 | import java.util.UUID; 8 | 9 | /** 10 | * Tracks start-up and caught crash events, logging them to a file which dosn't roll over 11 | */ 12 | public class CrashTracker { 13 | 14 | private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID(); 15 | 16 | public static void logRobotConstruction() { 17 | logMarker("robot startup"); 18 | } 19 | 20 | public static void logRobotInit() { 21 | logMarker("robot init"); 22 | } 23 | 24 | public static void logTeleopInit() { 25 | logMarker("teleop init"); 26 | } 27 | 28 | public static void logAutoInit() { 29 | logMarker("auto init"); 30 | } 31 | 32 | public static void logDisabledInit() { 33 | logMarker("disabled init"); 34 | } 35 | 36 | public static void logTestInit() { 37 | logMarker("test init"); 38 | } 39 | 40 | public static void logThrowableCrash(Throwable throwable) { 41 | logMarker("Exception", throwable); 42 | } 43 | 44 | private static void logMarker(String mark) { 45 | logMarker(mark, null); 46 | } 47 | 48 | private static void logMarker(String mark, Throwable nullableException) { 49 | 50 | try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) { 51 | writer.print(RUN_INSTANCE_UUID.toString()); 52 | writer.print(", "); 53 | writer.print(mark); 54 | writer.print(", "); 55 | writer.print(new Date().toString()); 56 | 57 | if (nullableException != null) { 58 | writer.print(", "); 59 | nullableException.printStackTrace(writer); 60 | } 61 | 62 | writer.println(); 63 | } catch (IOException e) { 64 | e.printStackTrace(); 65 | } 66 | } 67 | } -------------------------------------------------------------------------------- /characterization-project/vendordeps/REVRobotics.json: -------------------------------------------------------------------------------- 1 | { 2 | "cppDependencies": [ 3 | { 4 | "artifactId": "SparkMax-cpp", 5 | "binaryPlatforms": [ 6 | "windowsx86-64", 7 | "windowsx86", 8 | "linuxaarch64bionic", 9 | "linuxx86-64", 10 | "linuxathena", 11 | "linuxraspbian" 12 | ], 13 | "groupId": "com.revrobotics.frc", 14 | "headerClassifier": "headers", 15 | "libName": "SparkMax", 16 | "sharedLibrary": false, 17 | "skipInvalidPlatforms": true, 18 | "version": "1.5.1" 19 | }, 20 | { 21 | "artifactId": "SparkMax-driver", 22 | "binaryPlatforms": [ 23 | "windowsx86-64", 24 | "windowsx86", 25 | "linuxaarch64bionic", 26 | "linuxx86-64", 27 | "linuxathena", 28 | "linuxraspbian" 29 | ], 30 | "groupId": "com.revrobotics.frc", 31 | "headerClassifier": "headers", 32 | "libName": "SparkMaxDriver", 33 | "sharedLibrary": false, 34 | "skipInvalidPlatforms": true, 35 | "version": "1.5.1" 36 | } 37 | ], 38 | "fileName": "REVRobotics.json", 39 | "javaDependencies": [ 40 | { 41 | "artifactId": "SparkMax-java", 42 | "groupId": "com.revrobotics.frc", 43 | "version": "1.5.1" 44 | } 45 | ], 46 | "jniDependencies": [ 47 | { 48 | "artifactId": "SparkMax-driver", 49 | "groupId": "com.revrobotics.frc", 50 | "isJar": false, 51 | "skipInvalidPlatforms": true, 52 | "validPlatforms": [ 53 | "windowsx86-64", 54 | "windowsx86", 55 | "linuxaarch64bionic", 56 | "linuxx86-64", 57 | "linuxathena", 58 | "linuxraspbian" 59 | ], 60 | "version": "1.5.1" 61 | } 62 | ], 63 | "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", 64 | "mavenUrls": [ 65 | "http://www.revrobotics.com/content/sw/max/sdk/maven/" 66 | ], 67 | "name": "REVRobotics", 68 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 69 | "version": "1.5.1" 70 | } -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/trajectory/PurePursuitControllerTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Rotation2d; 5 | import com.team254.lib.geometry.Translation2d; 6 | import com.team254.lib.geometry.Twist2d; 7 | import org.junit.Assert; 8 | import org.junit.Test; 9 | import org.junit.runner.RunWith; 10 | import org.junit.runners.JUnit4; 11 | 12 | import java.util.Arrays; 13 | import java.util.List; 14 | 15 | @RunWith(JUnit4.class) 16 | public class PurePursuitControllerTest { 17 | 18 | @Test 19 | public void test() { 20 | List waypoints = Arrays.asList( 21 | new Translation2d(0.0, 0.0), 22 | new Translation2d(24.0, 0.0), 23 | new Translation2d(36.0, 12.0), 24 | new Translation2d(60.0, 12.0)); 25 | 26 | // Create the reference trajectory (straight line motion between waypoints). 27 | Trajectory reference_trajectory = new Trajectory<>(waypoints); 28 | DistanceView arc_length_parameterized_trajectory = new DistanceView<>(reference_trajectory); 29 | PurePursuitController controller = new PurePursuitController<>( 30 | arc_length_parameterized_trajectory, 1.0, 6.0, 0.1); 31 | 32 | Pose2d robot_pose = new Pose2d(waypoints.get(0), Rotation2d.identity()); 33 | final int kMaxIter = 100; 34 | int i = 0; 35 | for (; i < kMaxIter; ++i) { 36 | if (controller.isDone()) 37 | break; 38 | Twist2d steering_command = controller.steer(robot_pose); 39 | steering_command = steering_command.scaled(1.0 / Math.max(1.0, steering_command.norm())); 40 | System.out.println("Iter: " + i + ", Pose: " + robot_pose + ", Steering Command: " + steering_command); 41 | robot_pose = robot_pose.transformBy(Pose2d.exp(steering_command)); 42 | } 43 | Assert.assertTrue(i < kMaxIter); 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /calibration/calibration.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | 4 | # termination criteria 5 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 6 | 7 | # image resolution 8 | w_px = 960 9 | h_px = 720 10 | 11 | 12 | def calibrate_ll(w, h, file_min, file_max): 13 | # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(h - 1,w - 1,0) 14 | pts = np.zeros((h * w, 3), np.float32) 15 | pts[:, :2] = np.mgrid[0:h, 0:w].T.reshape(-1, 2) 16 | 17 | # Arrays to store object points and image points from all the images. 18 | obj_pts = [] # 3d point in real world space 19 | img_pts = [] # 2d points in image plane. 20 | 21 | files = range(file_min, file_max + 1) 22 | 23 | for file in files: 24 | img = cv2.imread(f'{file}.jpg') 25 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 26 | 27 | # Find the chess board corners 28 | ret, corners = cv2.findChessboardCorners(gray, (h, w), None) 29 | 30 | # If found, add object points, image points (after refining them) 31 | if ret: 32 | obj_pts.append(pts) 33 | 34 | corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) 35 | img_pts.append(corners2) 36 | 37 | # Draw and display the corners 38 | img = cv2.drawChessboardCorners(img, (h, w), corners2, ret) 39 | cv2.imwrite(f'{file}-corners.png', img) 40 | 41 | cv2.destroyAllWindows() 42 | 43 | ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_pts, img_pts, gray.shape[::-1], None, None) 44 | 45 | for file in files: 46 | img = cv2.imread(f'{file}.jpg') 47 | 48 | dst = cv2.undistort(img, mtx, dist) 49 | cv2.imwrite(f'{file}-undistorted.png', dst) 50 | 51 | scaled_mtx = np.array([ 52 | list(map(lambda x: x / w_px, mtx[0])), 53 | list(map(lambda x: x / h_px, mtx[1])), 54 | mtx[2] 55 | ]) 56 | 57 | fovx, fovy, focal_length, principal_point, aspect_ratio = cv2.calibrationMatrixValues(mtx, (w_px, h_px), 10, 10) 58 | 59 | return mtx, scaled_mtx, dist, fovx, fovy 60 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/TimedView.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.State; 4 | import com.team254.lib.trajectory.timing.TimedState; 5 | import com.team254.lib.util.Util; 6 | 7 | public class TimedView> implements TrajectoryView> { 8 | protected final Trajectory> trajectory_; 9 | protected final double start_t_; 10 | protected final double end_t_; 11 | 12 | public TimedView(Trajectory> trajectory) { 13 | trajectory_ = trajectory; 14 | start_t_ = trajectory_.getState(0).t(); 15 | end_t_ = trajectory_.getState(trajectory_.length() - 1).t(); 16 | } 17 | 18 | @Override 19 | public double first_interpolant() { 20 | return start_t_; 21 | } 22 | 23 | @Override 24 | public double last_interpolant() { 25 | return end_t_; 26 | } 27 | 28 | @Override 29 | public TrajectorySamplePoint> sample(double t) { 30 | if (t >= end_t_) { 31 | return new TrajectorySamplePoint<>(trajectory_.getPoint(trajectory_.length() - 1)); 32 | } 33 | if (t <= start_t_) { 34 | return new TrajectorySamplePoint<>(trajectory_.getPoint(0)); 35 | } 36 | for (int i = 1; i < trajectory_.length(); ++i) { 37 | final TrajectoryPoint> s = trajectory_.getPoint(i); 38 | if (s.state().t() >= t) { 39 | final TrajectoryPoint> prev_s = trajectory_.getPoint(i - 1); 40 | if (Util.epsilonEquals(s.state().t(), prev_s.state().t())) { 41 | return new TrajectorySamplePoint<>(s); 42 | } 43 | return new TrajectorySamplePoint<>(prev_s.state().interpolate(s.state(), 44 | (t - prev_s.state().t()) / (s.state().t() - prev_s.state().t())), i - 1, i); 45 | } 46 | } 47 | throw new RuntimeException(); 48 | } 49 | 50 | @Override 51 | public Trajectory> trajectory() { 52 | return trajectory_; 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/timing/DifferentialDriveDynamicsConstraint.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.timing; 2 | 3 | import com.team254.lib.geometry.ICurvature; 4 | import com.team254.lib.geometry.IPose2d; 5 | import com.team254.lib.physics.DifferentialDrive; 6 | import com.team254.lib.util.Units; 7 | 8 | public class DifferentialDriveDynamicsConstraint & ICurvature> implements TimingConstraint { 9 | 10 | protected final DifferentialDrive drive_; 11 | protected final double abs_voltage_limit_; 12 | 13 | public DifferentialDriveDynamicsConstraint(final DifferentialDrive drive, double abs_voltage_limit) { 14 | drive_ = drive; 15 | abs_voltage_limit_ = abs_voltage_limit; 16 | } 17 | 18 | @Override 19 | public double getMaxVelocity(S state) { 20 | return Units.meters_to_inches(drive_.getMaxAbsVelocity( 21 | Units.meters_to_inches(state.getCurvature()), // Curvature is in inverse inches, so meters_to_inches is correct. 22 | /*Units.meters_to_inches(Units.meters_to_inches(state.getDCurvatureDs())), // DCurvature is in inverse inches^2.*/ 23 | abs_voltage_limit_)); 24 | } 25 | 26 | @Override 27 | public MinMaxAcceleration getMinMaxAcceleration(S state, 28 | double velocity) { 29 | // TODO figure out a units convention for generic states. Traditionally we use inches... 30 | // NOTE: units cancel on angular velocity. 31 | DifferentialDrive.MinMax min_max = drive_.getMinMaxAcceleration(new DifferentialDrive.ChassisState( 32 | Units.inches_to_meters(velocity), state.getCurvature() * velocity), 33 | Units.meters_to_inches(state.getCurvature()), // Curvature is in inverse inches, so meters_to_inches is correct. 34 | /*Units.meters_to_inches(Units.meters_to_inches(state.getDCurvatureDs())), // DCurvature is in inverse inches^2.*/ 35 | abs_voltage_limit_); 36 | return new MinMaxAcceleration(Units.meters_to_inches(min_max.min), Units.meters_to_inches(min_max.max)); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/subsystems/Turret.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; 4 | import com.ctre.phoenix.motorcontrol.LimitSwitchSource; 5 | import com.team254.frc2020.Constants; 6 | import com.team254.lib.drivers.TalonUtil; 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | 9 | public class Turret extends ServoMotorSubsystem { 10 | private static Turret mInstance; 11 | private Canifier mCanifier; 12 | 13 | public synchronized static Turret getInstance() { 14 | if (mInstance == null) { 15 | mInstance = new Turret(Constants.kTurretConstants); 16 | } 17 | 18 | return mInstance; 19 | } 20 | 21 | private Turret(final ServoMotorSubsystemConstants constants) { 22 | super(constants); 23 | mCanifier = Canifier.getInstance(); 24 | TalonUtil.checkError( 25 | mMaster.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen), 26 | mConstants.kName + ": Could not set forward limit switch: "); 27 | 28 | TalonUtil.checkError( 29 | mMaster.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen), 30 | mConstants.kName + ": Could not set reverse limit switch: "); 31 | 32 | mMaster.overrideLimitSwitchesEnable(true); 33 | 34 | } 35 | 36 | @Override 37 | public boolean atHomingLocation() { 38 | return mCanifier.isTurretHomed(); 39 | } 40 | 41 | @Override 42 | public void outputTelemetry() { 43 | SmartDashboard.putBoolean("Turret Reverse Limit Switch", mMaster.getSensorCollection().isRevLimitSwitchClosed() == 1); 44 | SmartDashboard.putBoolean("Turret Forward Limit Switch", mMaster.getSensorCollection().isFwdLimitSwitchClosed() == 1); 45 | super.outputTelemetry(); 46 | } 47 | 48 | // Syntactic sugar. 49 | public synchronized double getAngle() { 50 | return getPosition(); 51 | } 52 | 53 | @Override 54 | public boolean checkSystem() { 55 | return false; 56 | } 57 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/TalonChecker.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.BaseTalon; 5 | import com.team254.frc2020.subsystems.Subsystem; 6 | 7 | import java.util.ArrayList; 8 | 9 | public class TalonChecker extends MotorChecker { 10 | private static class StoredTalonSRXConfiguration { 11 | public ControlMode mMode; 12 | public double mSetValue; 13 | } 14 | 15 | protected ArrayList mStoredConfigurations = new ArrayList<>(); 16 | 17 | public static boolean checkMotors(Subsystem subsystem, 18 | ArrayList> motorsToCheck, 19 | CheckerConfig checkerConfig) { 20 | TalonChecker checker = new TalonChecker(); 21 | return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig); 22 | } 23 | 24 | @Override 25 | protected void storeConfiguration() { 26 | // record previous configuration for all talons 27 | for (MotorConfig config : mMotorsToCheck) { 28 | LazyTalonSRX talon = (LazyTalonSRX) config.mMotor; 29 | 30 | StoredTalonSRXConfiguration configuration = new StoredTalonSRXConfiguration(); 31 | configuration.mMode = talon.getControlMode(); 32 | configuration.mSetValue = talon.getLastSet(); 33 | 34 | mStoredConfigurations.add(configuration); 35 | } 36 | } 37 | 38 | @Override 39 | protected void restoreConfiguration() { 40 | for (int i = 0; i < mMotorsToCheck.size(); ++i) { 41 | mMotorsToCheck.get(i).mMotor.set(mStoredConfigurations.get(i).mMode, 42 | mStoredConfigurations.get(i).mSetValue); 43 | } 44 | } 45 | 46 | @Override 47 | protected void setMotorOutput(BaseTalon motor, double output) { 48 | motor.set(ControlMode.PercentOutput, output); 49 | } 50 | 51 | @Override 52 | protected double getMotorCurrent(BaseTalon motor) { 53 | return motor.getSupplyCurrent(); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/DistanceView.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.State; 4 | import com.team254.lib.util.Util; 5 | 6 | public class DistanceView> implements TrajectoryView { 7 | protected final Trajectory trajectory_; 8 | protected final double[] distances_; 9 | 10 | public DistanceView(final Trajectory trajectory) { 11 | trajectory_ = trajectory; 12 | distances_ = new double[trajectory_.length()]; 13 | distances_[0] = 0.0; 14 | for (int i = 1; i < trajectory_.length(); ++i) { 15 | distances_[i] = distances_[i - 1] + trajectory_.getState(i - 1).distance(trajectory_.getState(i)); 16 | } 17 | } 18 | 19 | @Override 20 | public TrajectorySamplePoint sample(double distance) { 21 | if (distance >= last_interpolant()) 22 | return new TrajectorySamplePoint(trajectory_.getPoint(trajectory_.length() - 1)); 23 | if (distance <= 0.0) 24 | return new TrajectorySamplePoint(trajectory_.getPoint(0)); 25 | for (int i = 1; i < distances_.length; ++i) { 26 | final TrajectoryPoint s = trajectory_.getPoint(i); 27 | if (distances_[i] >= distance) { 28 | final TrajectoryPoint prev_s = trajectory_.getPoint(i - 1); 29 | if (Util.epsilonEquals(distances_[i], distances_[i - 1])) { 30 | return new TrajectorySamplePoint(s); 31 | } else { 32 | return new TrajectorySamplePoint(prev_s.state().interpolate(s.state(), 33 | (distance - distances_[i - 1]) / (distances_[i] - distances_[i - 1])), i - 1, i); 34 | } 35 | } 36 | } 37 | throw new RuntimeException(); 38 | } 39 | 40 | @Override 41 | public double last_interpolant() { 42 | return distances_[distances_.length - 1]; 43 | } 44 | 45 | @Override 46 | public double first_interpolant() { 47 | return 0.0; 48 | } 49 | 50 | @Override 51 | public Trajectory trajectory() { 52 | return trajectory_; 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /robotconfig.py: -------------------------------------------------------------------------------- 1 | { 2 | # Class names of motor controllers used. 3 | # Options: 4 | # 'WPI_TalonSRX' 5 | # 'WPI_TalonFX' (for Falcon 500 motors) 6 | # 'WPI_VictorSPX' 7 | # Note: The first motor on each side should always be a Talon SRX/FX, as the 8 | # VictorSPX does not support encoder connections 9 | "rightControllerTypes": ["WPI_TalonFX", "WPI_TalonFX", "WPI_TalonFX"], 10 | "leftControllerTypes": ["WPI_TalonFX", "WPI_TalonFX", "WPI_TalonFX"], 11 | # Note: The first id in the list of ports should be the one with an encoder 12 | # Ports for the left-side motors 13 | "leftMotorPorts": [1, 2, 3], 14 | # Ports for the right-side motors 15 | "rightMotorPorts": [4, 5, 6], 16 | # Inversions for the left-side motors 17 | "leftMotorsInverted": [False, False, False], 18 | # Inversions for the right side motors 19 | "rightMotorsInverted": [True, True, True], 20 | # Wheel diameter (in units of your choice - will dictate units of analysis) 21 | "wheelDiameter": 5.9067052758, 22 | # If your robot has only one encoder, set all right encoder fields to `None` 23 | # Encoder edges-per-revolution (*NOT* cycles per revolution!) 24 | # This value should be the edges per revolution *of the wheels*, and so 25 | # should take into account gearing between the encoder and the wheels 26 | "encoderEPR": 16384, 27 | # Whether the left encoder is inverted 28 | "leftEncoderInverted": False, 29 | # Whether the right encoder is inverted: 30 | "rightEncoderInverted": False, 31 | # Your gyro type (one of "NavX", "Pigeon", "ADXRS450", "AnalogGyro", or "None") 32 | "gyroType": "Pigeon", 33 | # Whatever you put into the constructor of your gyro 34 | # Could be: 35 | # "SPI.Port.kMXP" (MXP SPI port for NavX or ADXRS450), 36 | # "SerialPort.Port.kMXP" (MXP Serial port for NavX), 37 | # "I2C.Port.kOnboard" (Onboard I2C port for NavX), 38 | # "0" (Pigeon CAN ID or AnalogGyro channel), 39 | # "new WPI_TalonSRX(3)" (Pigeon on a Talon SRX), 40 | # "leftSlave" (Pigeon on the left slave Talon SRX/FX), 41 | # "" (NavX using default SPI, ADXRS450 using onboard CS0, or no gyro) 42 | "gyroPort": "0", 43 | } 44 | 45 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/TalonFXChecker.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.BaseTalon; 5 | import com.team254.frc2020.subsystems.Subsystem; 6 | 7 | import java.util.ArrayList; 8 | 9 | public class TalonFXChecker extends MotorChecker { 10 | private static class StoredTalonFXConfiguration { 11 | public ControlMode mMode; 12 | public double mSetValue; 13 | } 14 | 15 | protected ArrayList mStoredConfigurations = new ArrayList<>(); 16 | 17 | public static boolean checkMotors(Subsystem subsystem, 18 | ArrayList> motorsToCheck, 19 | MotorChecker.CheckerConfig checkerConfig) { 20 | TalonFXChecker checker = new TalonFXChecker(); 21 | return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig); 22 | } 23 | 24 | @Override 25 | protected void storeConfiguration() { 26 | // record previous configuration for all talons 27 | for (MotorChecker.MotorConfig config : mMotorsToCheck) { 28 | LazyTalonFX talon = (LazyTalonFX) config.mMotor; 29 | 30 | TalonFXChecker.StoredTalonFXConfiguration configuration = new TalonFXChecker.StoredTalonFXConfiguration(); 31 | configuration.mMode = talon.getControlMode(); 32 | configuration.mSetValue = talon.getLastSet(); 33 | 34 | mStoredConfigurations.add(configuration); 35 | } 36 | } 37 | 38 | @Override 39 | protected void restoreConfiguration() { 40 | for (int i = 0; i < mMotorsToCheck.size(); ++i) { 41 | mMotorsToCheck.get(i).mMotor.set(mStoredConfigurations.get(i).mMode, 42 | mStoredConfigurations.get(i).mSetValue); 43 | } 44 | } 45 | 46 | @Override 47 | protected void setMotorOutput(BaseTalon motor, double output) { 48 | motor.set(ControlMode.PercentOutput, output); 49 | } 50 | 51 | @Override 52 | protected double getMotorCurrent(BaseTalon motor) { 53 | return motor.getSupplyCurrent(); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/modes/FarWOF10Ball.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.modes; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.frc2020.auto.AutoModeEndedException; 5 | import com.team254.frc2020.auto.actions.*; 6 | import com.team254.frc2020.paths.TrajectoryGenerator; 7 | import com.team254.lib.geometry.Rotation2d; 8 | 9 | import java.util.List; 10 | 11 | // zero -> steel bars by rendezvous point -> back to zeroish -> far wof -> shooting point 12 | public class FarWOF10Ball extends AutoModeBase { 13 | @Override 14 | protected void routine() throws AutoModeEndedException { 15 | runAction(new DeployIntakeAction(true)); 16 | runAction(new RunIntakeAction()); 17 | runAction(new ParallelAction( 18 | new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().startingToPickup), 19 | new AutoAimAction(Rotation2d.fromDegrees(30)))); 20 | 21 | 22 | runAction( 23 | new ParallelAction( 24 | new ShootAction(Constants.kCoarseShootingParams, 2), 25 | new SeriesAction( 26 | new WaitAction(0.5), 27 | new StopIntakingAction(), 28 | new DeployIntakeAction(false) 29 | ))); 30 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().pickupToTurningPoint)); 31 | 32 | runAction(new DeployIntakeAction(true)); 33 | runAction(new RunIntakeAction()); 34 | 35 | runAction(new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().turningPointToFarWOF2)); 36 | 37 | runAction(new ParallelAction(List.of( 38 | new DriveTrajectoryAction(TrajectoryGenerator.getInstance().getTrajectorySet().farWOF2ToShootingPoint), 39 | new AutoAimAction(Rotation2d.fromDegrees(0)), 40 | new SeriesAction( 41 | new WaitAction(1.0), 42 | new StopIntakingAction(), 43 | new DeployIntakeAction(false) 44 | ) 45 | ) 46 | )); 47 | runAction(new ShootAction(Constants.kCoarseShootingParams, 2)); 48 | } 49 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/AutoModeExecutor.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto; 2 | 3 | import com.team254.frc2020.auto.modes.AutoModeBase; 4 | import com.team254.lib.util.CrashTrackingRunnable; 5 | 6 | /** 7 | * This class selects, runs, and (if necessary) stops a specified autonomous mode. 8 | */ 9 | public class AutoModeExecutor { 10 | private static AutoModeExecutor mInstance = null; 11 | 12 | private AutoModeBase mAutoMode = null; 13 | private Thread mThread = null; 14 | 15 | public AutoModeExecutor() {} 16 | 17 | public static AutoModeExecutor getInstance() { 18 | if (mInstance == null) { 19 | mInstance = new AutoModeExecutor(); 20 | } 21 | 22 | return mInstance; 23 | } 24 | 25 | public void setAutoMode(AutoModeBase new_auto_mode) { 26 | mAutoMode = new_auto_mode; 27 | mThread = new Thread(new CrashTrackingRunnable() { 28 | @Override 29 | public void runCrashTracked() { 30 | if (mAutoMode != null) { 31 | mAutoMode.run(); 32 | } 33 | } 34 | }); 35 | } 36 | 37 | public void start() { 38 | if (mThread != null) { 39 | mThread.start(); 40 | } 41 | } 42 | 43 | public boolean isStarted() { 44 | return mAutoMode != null && mAutoMode.isActive() && mThread != null && mThread.isAlive(); 45 | } 46 | 47 | public void reset() { 48 | if (isStarted()) { 49 | stop(); 50 | } 51 | 52 | mAutoMode = null; 53 | } 54 | 55 | public void stop() { 56 | if (mAutoMode != null) { 57 | mAutoMode.stop(); 58 | } 59 | 60 | mThread = null; 61 | } 62 | 63 | public AutoModeBase getAutoMode() { 64 | return mAutoMode; 65 | } 66 | 67 | public boolean isInterrupted() { 68 | if (mAutoMode == null) { 69 | return false; 70 | } 71 | return mAutoMode.getIsInterrupted(); 72 | } 73 | 74 | public void interrupt() { 75 | if (mAutoMode == null) { 76 | return; 77 | } 78 | mAutoMode.interrupt(); 79 | } 80 | 81 | public void resume() { 82 | if (mAutoMode == null) { 83 | return; 84 | } 85 | mAutoMode.resume(); 86 | } 87 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/spline/CubicHermiteSpline.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.spline; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Rotation2d; 5 | import com.team254.lib.geometry.Translation2d; 6 | 7 | /** 8 | * Temporary spline for testing 9 | */ 10 | public class CubicHermiteSpline extends Spline { 11 | private final double ax, bx, cx, dx, ay, by, cy, dy; 12 | 13 | public CubicHermiteSpline(Pose2d p0, Pose2d p1) { 14 | double x0, x1, dx0, dx1, y0, y1, dy0, dy1; 15 | double scale = 2 * p0.getTranslation().distance(p1.getTranslation()); 16 | x0 = p0.getTranslation().x(); 17 | x1 = p1.getTranslation().x(); 18 | dx0 = p0.getRotation().cos() * scale; 19 | dx1 = p1.getRotation().cos() * scale; 20 | y0 = p0.getTranslation().y(); 21 | y1 = p1.getTranslation().y(); 22 | dy0 = p0.getRotation().sin() * scale; 23 | dy1 = p1.getRotation().sin() * scale; 24 | ax = dx0 + dx1 + 2 * x0 - 2 * x1; 25 | bx = -2 * dx0 - dx1 - 3 * x0 + 3 * x1; 26 | cx = dx0; 27 | dx = x0; 28 | ay = dy0 + dy1 + 2 * y0 - 2 * y1; 29 | by = -2 * dy0 - dy1 - 3 * y0 + 3 * y1; 30 | cy = dy0; 31 | dy = y0; 32 | } 33 | 34 | @Override 35 | public Translation2d getPoint(double t) { 36 | final double x = t * t * t * ax + t * t * bx + t * cx + dx; 37 | final double y = t * t * t * ay + t * t * by + t * cy + dy; 38 | return new Translation2d(x, y); 39 | } 40 | 41 | @Override 42 | public Rotation2d getHeading(double t) { 43 | final double dx = 3 * t * t * ax + 2 * t * bx + cx; 44 | final double dy = 3 * t * t * ay + 2 * t * by + cy; 45 | return new Rotation2d(dx, dy, true); 46 | } 47 | 48 | @Override 49 | public double getVelocity(double t) { 50 | // TODO implement this 51 | return 1.0; 52 | } 53 | 54 | @Override 55 | public double getCurvature(double t) { 56 | final double dx = 3 * t * t * ax + 2 * t * bx + cx; 57 | final double dy = 3 * t * t * ay + 2 * t * by + cy; 58 | final double ddx = 6 * t * ax + 2 * bx; 59 | final double ddy = 6 * t * ay + 2 * by; 60 | return (dx * ddy - dy * ddx) / ((dx * dx + dy * dy) * Math.sqrt(dx * dx + dy * dy)); 61 | } 62 | 63 | @Override 64 | public double getDCurvature(double t) { 65 | // TODO implement this 66 | return 0.0; 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/ReflectingCSVWriter.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.io.FileNotFoundException; 4 | import java.io.PrintWriter; 5 | import java.lang.reflect.Field; 6 | import java.util.concurrent.ConcurrentLinkedDeque; 7 | 8 | /** 9 | * Writes data to a CSV file 10 | */ 11 | public class ReflectingCSVWriter { 12 | private ConcurrentLinkedDeque mLinesToWrite = new ConcurrentLinkedDeque<>(); 13 | private PrintWriter mOutput = null; 14 | private Field[] mFields; 15 | 16 | public ReflectingCSVWriter(String fileName, Class typeClass) { 17 | mFields = typeClass.getFields(); 18 | try { 19 | mOutput = new PrintWriter(fileName); 20 | } catch (FileNotFoundException e) { 21 | e.printStackTrace(); 22 | } 23 | // Write field names. 24 | StringBuilder line = new StringBuilder(); 25 | for (Field field : mFields) { 26 | if (line.length() != 0) { 27 | line.append(", "); 28 | } 29 | line.append(field.getName()); 30 | } 31 | writeLine(line.toString()); 32 | } 33 | 34 | public void add(T value) { 35 | StringBuilder line = new StringBuilder(); 36 | for (Field field : mFields) { 37 | if (line.length() != 0) { 38 | line.append(", "); 39 | } 40 | try { 41 | if (CSVWritable.class.isAssignableFrom(field.getType())) { 42 | line.append(((CSVWritable) field.get(value)).toCSV()); 43 | } else { 44 | line.append(field.get(value).toString()); 45 | } 46 | } catch (IllegalArgumentException | IllegalAccessException e) { 47 | e.printStackTrace(); 48 | } 49 | } 50 | mLinesToWrite.add(line.toString()); 51 | } 52 | 53 | protected synchronized void writeLine(String line) { 54 | if (mOutput != null) { 55 | mOutput.println(line); 56 | } 57 | } 58 | 59 | // Call this periodically from any thread to write to disk. 60 | public void write() { 61 | while (true) { 62 | String val = mLinesToWrite.pollFirst(); 63 | if (val == null) { 64 | break; 65 | } 66 | writeLine(val); 67 | } 68 | } 69 | 70 | public synchronized void flush() { 71 | if (mOutput != null) { 72 | write(); 73 | mOutput.flush(); 74 | } 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /characterization-project/build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id "java" 3 | id "edu.wpi.first.GradleRIO" version "2020.1.2" 4 | } 5 | 6 | def ROBOT_MAIN_CLASS = "dc.Main" 7 | 8 | // Define my targets (RoboRIO) and artifacts (deployable files) 9 | // This is added by GradleRIO's backing project EmbeddedTools. 10 | deploy { 11 | targets { 12 | roboRIO("roborio") { 13 | // Team number is loaded either from the .wpilib/wpilib_preferences.json 14 | // or from command line. If not found an exception will be thrown. 15 | // You can use getTeamOrDefault(team) instead of getTeamNumber if you 16 | // want to store a team number in this file. 17 | team = frc.getTeamOrDefault(254) 18 | } 19 | } 20 | artifacts { 21 | frcJavaArtifact('frcJava') { 22 | targets << "roborio" 23 | // Debug can be overridden by command line, for use with VSCode 24 | debug = frc.getDebugOrDefault(false) 25 | } 26 | // Built in artifact to deploy arbitrary files to the roboRIO. 27 | fileTreeArtifact('frcStaticFileDeploy') { 28 | // The directory below is the local directory to deploy 29 | files = fileTree(dir: 'src/main/deploy') 30 | // Deploy to RoboRIO target, into /home/lvuser/deploy 31 | targets << "roborio" 32 | directory = '/home/lvuser/deploy' 33 | } 34 | } 35 | } 36 | 37 | // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. 38 | // Also defines JUnit 4. 39 | dependencies { 40 | implementation wpi.deps.wpilib() 41 | nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) 42 | nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) 43 | 44 | 45 | implementation wpi.deps.vendor.java() 46 | nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) 47 | nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) 48 | 49 | // In Java for now, the argument must be false 50 | simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) 51 | 52 | testImplementation 'junit:junit:4.12' 53 | } 54 | 55 | // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') 56 | // in order to make them all available at runtime. Also adding the manifest so WPILib 57 | // knows where to look for our Robot Class. 58 | jar { 59 | from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } 60 | manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) 61 | } 62 | 63 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/subsystems/Infrastructure.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.subsystems; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.frc2020.loops.ILooper; 5 | import com.team254.frc2020.loops.Loop; 6 | import edu.wpi.first.wpilibj.Compressor; 7 | 8 | /** 9 | * Subsystem to ensure the compressor never runs while the superstructure moves 10 | */ 11 | public class Infrastructure extends Subsystem { 12 | private static Infrastructure mInstance; 13 | 14 | private Superstructure mSuperstructure = Superstructure.getInstance(); 15 | private Compressor mCompressor = new Compressor(Constants.kPCMId); 16 | 17 | private boolean mIsTeleop = false; 18 | 19 | private Infrastructure() {} 20 | 21 | public static Infrastructure getInstance() { 22 | if (mInstance == null) { 23 | mInstance = new Infrastructure(); 24 | } 25 | 26 | return mInstance; 27 | } 28 | 29 | @Override 30 | public void registerEnabledLoops(ILooper mEnabledLooper) { 31 | mEnabledLooper.register(new Loop() { 32 | @Override 33 | public void onStart(double timestamp) {} 34 | 35 | @Override 36 | public void onLoop(double timestamp) { 37 | synchronized (Infrastructure.this) { 38 | boolean superstructureMoving = mSuperstructure.getSystemState() == Superstructure.SystemState.SHOOT || 39 | mSuperstructure.getSystemState() == Superstructure.SystemState.AIMING; 40 | 41 | if (superstructureMoving || !mIsTeleop) { 42 | stopCompressor(); 43 | } else { 44 | startCompressor(); 45 | } 46 | } 47 | } 48 | 49 | @Override 50 | public void onStop(double timestamp) {} 51 | }); 52 | } 53 | 54 | public synchronized void setIsTeleop(boolean isTeleop) { 55 | mIsTeleop = isTeleop; 56 | 57 | if (mIsTeleop) { 58 | startCompressor(); 59 | } 60 | } 61 | 62 | public synchronized boolean isTeleop() { 63 | return mIsTeleop; 64 | } 65 | 66 | private void startCompressor() { 67 | mCompressor.start(); 68 | } 69 | 70 | private void stopCompressor() { 71 | mCompressor.stop(); 72 | } 73 | 74 | @Override 75 | public void stop() {} 76 | 77 | @Override 78 | public boolean checkSystem() { 79 | return false; 80 | } 81 | 82 | @Override 83 | public void outputTelemetry() {} 84 | } 85 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/StatFinder.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.util.ArrayList; 4 | import java.util.DoubleSummaryStatistics; 5 | import java.util.List; 6 | 7 | /** 8 | * Finds the stats (mean, standard deviation, etc.) of a list 9 | *

10 | * Example use case: finding out how long a planner takes from the average of 100 tries 11 | */ 12 | public class StatFinder { 13 | private final List numbers = new ArrayList<>(); 14 | private final int numberToIgnore; 15 | private int numberIgnored = 0; 16 | 17 | private boolean stopped = false; 18 | 19 | /** 20 | * Creates a new StatFinder 21 | * 22 | * @param numberToIgnore the number of entries to ignore before logging (like burning the top card of a deck before dealing) 23 | */ 24 | public StatFinder(int numberToIgnore) { 25 | this.numberToIgnore = numberToIgnore; 26 | } 27 | 28 | public boolean add(double number) { 29 | if (stopped) { 30 | return false; 31 | } 32 | 33 | if (numberIgnored < numberToIgnore) { 34 | numberIgnored++; 35 | return false; 36 | } 37 | 38 | numbers.add(number); 39 | return true; 40 | } 41 | 42 | public boolean addAndPrint(double number) { 43 | boolean success = add(number); 44 | 45 | if (success) { 46 | System.out.println("added: " + number); 47 | } 48 | 49 | return success; 50 | } 51 | 52 | public DoubleSummaryStatistics getStats() { 53 | return numbers.stream().mapToDouble(x -> x).summaryStatistics(); 54 | } 55 | 56 | public double getMean() { 57 | return getStats().getAverage(); 58 | } 59 | 60 | public double getStandardDeviation() { 61 | double mean = getMean(); 62 | return Math.sqrt(numbers.stream().mapToDouble(x -> Math.pow(x - mean, 2)).sum() / (getSize() - 1)); 63 | } 64 | 65 | public double getSize() { 66 | return numbers.size(); 67 | } 68 | 69 | public void printStats() { 70 | System.out.println("mean: " + getMean()); 71 | System.out.println("standard deviation: " + getStandardDeviation()); 72 | System.out.println("min: " + getStats().getMin()); 73 | System.out.println("max: " + getStats().getMax()); 74 | System.out.println("size: " + getSize()); 75 | } 76 | 77 | public void stop() { 78 | stopped = true; 79 | } 80 | 81 | public void stopAndPrint() { 82 | if (stopped) { 83 | return; 84 | } 85 | 86 | stop(); 87 | printStats(); 88 | } 89 | 90 | } 91 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/states/TimedLEDState.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.states; 2 | 3 | public interface TimedLEDState { 4 | void getCurrentLEDState(LEDState desiredState, double timestamp); 5 | 6 | class BlinkingLEDState implements TimedLEDState { 7 | public static BlinkingLEDState kZeroingFault = new BlinkingLEDState( 8 | LEDState.kOff, LEDState.kFault, 1.0); 9 | public static BlinkingLEDState kZeroingHoodFault = new BlinkingLEDState( 10 | LEDState.kOff, LEDState.kFaultHood, 1.0); 11 | 12 | public static BlinkingLEDState kClimbing = new BlinkingLEDState( 13 | LEDState.kOff, LEDState.kClimbing, 0.5); 14 | public static BlinkingLEDState kBrakeEngaged = new BlinkingLEDState( 15 | LEDState.kOff, LEDState.kBrakeEngaged, 0.5); 16 | 17 | public static BlinkingLEDState kBlinkingAiming = new BlinkingLEDState( 18 | LEDState.kOff, LEDState.kAiming, 0.1); 19 | public static BlinkingLEDState kBlinkingShooting = new BlinkingLEDState( 20 | LEDState.kOff, LEDState.kShooting, 0.1); 21 | 22 | public static BlinkingLEDState kBlinkingWOF = new BlinkingLEDState( 23 | LEDState.kOff, LEDState.kWOF, 0.5); 24 | 25 | 26 | LEDState mStateOne = new LEDState(0.0, 0.0, 0.0); 27 | LEDState mStateTwo = new LEDState(0.0, 0.0, 0.0); 28 | double mDuration; 29 | 30 | public BlinkingLEDState(LEDState stateOne, LEDState stateTwo, double duration) { 31 | mStateOne.copyFrom(stateOne); 32 | mStateTwo.copyFrom(stateTwo); 33 | mDuration = duration; 34 | } 35 | 36 | @Override 37 | public void getCurrentLEDState(LEDState desiredState, double timestamp) { 38 | if ((int) (timestamp / mDuration) % 2 == 0) { 39 | desiredState.copyFrom(mStateOne); 40 | } else { 41 | desiredState.copyFrom(mStateTwo); 42 | } 43 | } 44 | } 45 | 46 | class StaticLEDState implements TimedLEDState { 47 | public static StaticLEDState kStaticOff = new StaticLEDState(LEDState.kOff); 48 | public static StaticLEDState kRobotZeroed = new StaticLEDState(LEDState.kRobotZeroed); 49 | 50 | 51 | LEDState mStaticState = new LEDState(0.0, 0.0, 0.0); 52 | 53 | public StaticLEDState(LEDState staticState) { 54 | mStaticState.copyFrom(staticState); 55 | } 56 | 57 | @Override 58 | public void getCurrentLEDState(LEDState desiredState, double timestamp) { 59 | desiredState.copyFrom(mStaticState); 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/Kinematics.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Rotation2d; 5 | import com.team254.lib.geometry.Twist2d; 6 | import com.team254.lib.util.DriveSignal; 7 | 8 | /** 9 | * Provides forward and inverse kinematics equations for the robot modeling the wheelbase as a differential drive (with 10 | * a corrective factor to account for skidding). 11 | */ 12 | 13 | public class Kinematics { 14 | private static final double kEpsilon = 1E-9; 15 | 16 | /** 17 | * Forward kinematics using only encoders, rotation is implicit (less accurate than below, but useful for predicting 18 | * motion) 19 | */ 20 | public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta) { 21 | double delta_rotation = (right_wheel_delta - left_wheel_delta) / (Constants.kDriveWheelTrackWidthInches * Constants.kTrackScrubFactor); 22 | return forwardKinematics(left_wheel_delta, right_wheel_delta, delta_rotation); 23 | } 24 | 25 | public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta, double delta_rotation_rads) { 26 | final double dx = (left_wheel_delta + right_wheel_delta) / 2.0; 27 | return new Twist2d(dx, 0.0, delta_rotation_rads); 28 | } 29 | 30 | public static Twist2d forwardKinematics(Rotation2d prev_heading, double left_wheel_delta, double right_wheel_delta, 31 | Rotation2d current_heading) { 32 | final double dx = (left_wheel_delta + right_wheel_delta) / 2.0; 33 | final double dy = 0.0; 34 | return new Twist2d(dx, dy, prev_heading.inverse().rotateBy(current_heading).getRadians()); 35 | } 36 | 37 | /** 38 | * For convenience, integrate forward kinematics with a Twist2d and previous rotation. 39 | */ 40 | public static Pose2d integrateForwardKinematics(Pose2d current_pose, 41 | Twist2d forward_kinematics) { 42 | return current_pose.transformBy(Pose2d.exp(forward_kinematics)); 43 | } 44 | 45 | /** 46 | * Uses inverse kinematics to convert a Twist2d into left and right wheel velocities 47 | */ 48 | public static DriveSignal inverseKinematics(Twist2d velocity) { 49 | if (Math.abs(velocity.dtheta) < kEpsilon) { 50 | return new DriveSignal(velocity.dx, velocity.dx); 51 | } 52 | double delta_v = Constants.kDriveWheelTrackWidthInches * velocity.dtheta / (2 * Constants.kTrackScrubFactor); 53 | return new DriveSignal(velocity.dx - delta_v, velocity.dx + delta_v); 54 | } 55 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/InterpolatingTreeMap.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import java.util.Map; 4 | import java.util.TreeMap; 5 | 6 | /** 7 | * Interpolating Tree Maps are used to get values at points that are not defined by making a guess from points that are 8 | * defined. This uses linear interpolation. 9 | * 10 | * @param The type of the key (must implement InverseInterpolable) 11 | * @param The type of the value (must implement Interpolable) 12 | */ 13 | public class InterpolatingTreeMap & Comparable, V extends Interpolable> 14 | extends TreeMap { 15 | private static final long serialVersionUID = 8347275262778054124L; 16 | 17 | final int max_; 18 | 19 | public InterpolatingTreeMap(int maximumSize) { 20 | max_ = maximumSize; 21 | } 22 | 23 | public InterpolatingTreeMap() { 24 | this(0); 25 | } 26 | 27 | /** 28 | * Inserts a key value pair, and trims the tree if a max size is specified 29 | * 30 | * @param key Key for inserted data 31 | * @param value Value for inserted data 32 | * @return the value 33 | */ 34 | @Override 35 | public V put(K key, V value) { 36 | if (max_ > 0 && max_ <= size()) { 37 | // "Prune" the tree if it is oversize 38 | K first = firstKey(); 39 | remove(first); 40 | } 41 | 42 | super.put(key, value); 43 | 44 | return value; 45 | } 46 | 47 | @Override 48 | public void putAll(Map map) { 49 | System.out.println("Unimplemented Method"); 50 | } 51 | 52 | /** 53 | * @param key Lookup for a value (does not have to exist) 54 | * @return V or null; V if it is Interpolable or exists, null if it is at a bound and cannot average 55 | */ 56 | public V getInterpolated(K key) { 57 | V gotval = get(key); 58 | if (gotval == null) { 59 | // get surrounding keys for interpolation 60 | K topBound = ceilingKey(key); 61 | K bottomBound = floorKey(key); 62 | 63 | // if attempting interpolation at ends of tree, return the nearest data point 64 | if (topBound == null && bottomBound == null) { 65 | return null; 66 | } else if (topBound == null) { 67 | return get(bottomBound); 68 | } else if (bottomBound == null) { 69 | return get(topBound); 70 | } 71 | 72 | // get surrounding values for interpolation 73 | V topElem = get(topBound); 74 | V bottomElem = get(bottomBound); 75 | return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); 76 | } else { 77 | return gotval; 78 | } 79 | } 80 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/limelight/constants/LimelightConstants.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.limelight.constants; 2 | 3 | import com.team254.frc2020.limelight.CameraResolution; 4 | import com.team254.frc2020.limelight.undistort.OpenCVCalculatedUndistortMap; 5 | import com.team254.frc2020.limelight.undistort.UndistortConstants; 6 | import com.team254.frc2020.limelight.undistort.UndistortMap; 7 | import com.team254.lib.geometry.Pose2d; 8 | import com.team254.lib.geometry.Rotation2d; 9 | 10 | public class LimelightConstants { 11 | private int id; 12 | private String name; 13 | private String tableName; 14 | private double height; 15 | private Pose2d turretToLens; 16 | private Rotation2d horizontalPlaneToLens; 17 | private UndistortMap undistortMap; 18 | private UndistortConstants undistortConstants; 19 | private Type type; 20 | private double horizontalFOV; 21 | private double verticalFOV; 22 | 23 | public enum Type { 24 | Shooter 25 | } 26 | 27 | public LimelightConstants(int id, Type type, String name, String tableName, double height, Pose2d turretToLens, Rotation2d horizontalPlaneToLens, UndistortMap undistortMap, UndistortConstants undistortConstants, double horizontalFOV, double verticalFOV) { 28 | this.id = id; 29 | this.type = type; 30 | this.name = name; 31 | this.tableName = tableName; 32 | this.height = height; 33 | this.turretToLens = turretToLens; 34 | this.horizontalPlaneToLens = horizontalPlaneToLens; 35 | this.undistortMap = undistortMap; 36 | this.undistortConstants = undistortConstants; 37 | if (this.undistortMap == null) { 38 | this.undistortMap = new OpenCVCalculatedUndistortMap(undistortConstants, CameraResolution.F_320x240, false); 39 | } 40 | this.horizontalFOV = horizontalFOV; 41 | this.verticalFOV = verticalFOV; 42 | } 43 | 44 | public int getId() { 45 | return id; 46 | } 47 | 48 | public String getName() { 49 | return name; 50 | } 51 | 52 | public String getTableName() { 53 | return tableName; 54 | } 55 | 56 | public double getHeight() { 57 | return height; 58 | } 59 | 60 | public Pose2d getTurretToLens() { 61 | return turretToLens; 62 | } 63 | 64 | public Rotation2d getHorizontalPlaneToLens() { 65 | return horizontalPlaneToLens; 66 | } 67 | 68 | public UndistortMap getUndistortMap() { 69 | return undistortMap; 70 | } 71 | 72 | public UndistortConstants getUndistortConstants() { 73 | return undistortConstants; 74 | } 75 | 76 | public Type getType() { 77 | return type; 78 | } 79 | 80 | public double getHorizontalFOV() { 81 | return horizontalFOV; 82 | } 83 | 84 | public double getVerticalFOV() { 85 | return verticalFOV; 86 | } 87 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/motion/MotionSegment.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | import static com.team254.lib.motion.MotionUtil.kEpsilon; 4 | import static com.team254.lib.util.Util.epsilonEquals; 5 | 6 | /** 7 | * A MotionSegment is a movement from a start MotionState to an end MotionState with a constant acceleration. 8 | */ 9 | public class MotionSegment { 10 | protected MotionState mStart; 11 | protected MotionState mEnd; 12 | 13 | public MotionSegment(MotionState start, MotionState end) { 14 | mStart = start; 15 | mEnd = end; 16 | } 17 | 18 | /** 19 | * Verifies that: 20 | *

21 | * 1. All segments have a constant acceleration. 22 | *

23 | * 2. All segments have monotonic position (sign of velocity doesn't change). 24 | *

25 | * 3. The time, position, velocity, and acceleration of the profile are consistent. 26 | */ 27 | public boolean isValid() { 28 | if (!epsilonEquals(start().acc(), end().acc(), kEpsilon)) { 29 | // Acceleration is not constant within the segment. 30 | System.err.println( 31 | "Segment acceleration not constant! Start acc: " + start().acc() + ", End acc: " + end().acc()); 32 | return false; 33 | } 34 | if (Math.signum(start().vel()) * Math.signum(end().vel()) < 0.0 && !epsilonEquals(start().vel(), 0.0, kEpsilon) 35 | && !epsilonEquals(end().vel(), 0.0, kEpsilon)) { 36 | // Velocity direction reverses within the segment. 37 | System.err.println("Segment velocity reverses! Start vel: " + start().vel() + ", End vel: " + end().vel()); 38 | return false; 39 | } 40 | if (!start().extrapolate(end().t()).equals(end())) { 41 | // A single segment is not consistent. 42 | if (start().t() == end().t() && Double.isInfinite(start().acc())) { 43 | // One allowed exception: If acc is infinite and dt is zero. 44 | return true; 45 | } 46 | System.err.println("Segment not consistent! Start: " + start() + ", End: " + end()); 47 | return false; 48 | } 49 | return true; 50 | } 51 | 52 | public boolean containsTime(double t) { 53 | return t >= start().t() && t <= end().t(); 54 | } 55 | 56 | public boolean containsPos(double pos) { 57 | return pos >= start().pos() && pos <= end().pos() || pos <= start().pos() && pos >= end().pos(); 58 | } 59 | 60 | public MotionState start() { 61 | return mStart; 62 | } 63 | 64 | public void setStart(MotionState start) { 65 | mStart = start; 66 | } 67 | 68 | public MotionState end() { 69 | return mEnd; 70 | } 71 | 72 | public void setEnd(MotionState end) { 73 | mEnd = end; 74 | } 75 | 76 | @Override 77 | public String toString() { 78 | return "Start: " + start() + ", End: " + end(); 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/loops/Looper.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.loops; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.lib.util.CrashTrackingRunnable; 5 | import edu.wpi.first.wpilibj.Notifier; 6 | import edu.wpi.first.wpilibj.Timer; 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | 9 | import java.util.ArrayList; 10 | import java.util.List; 11 | 12 | /** 13 | * This code runs all of the robot's loops. Loop objects are stored in a List object. They are started when the robot 14 | * powers up and stopped after the match. 15 | */ 16 | public class Looper implements ILooper { 17 | public final double kPeriod = Constants.kLooperDt; 18 | 19 | private boolean mRunning; 20 | 21 | private final Notifier mNotifier; 22 | private final List mLoops; 23 | private final Object mTaskRunningLock = new Object(); 24 | private double mTimestamp = 0; 25 | private double mDT = 0; 26 | 27 | private final CrashTrackingRunnable runnable_ = new CrashTrackingRunnable() { 28 | @Override 29 | public void runCrashTracked() { 30 | synchronized (mTaskRunningLock) { 31 | if (mRunning) { 32 | double now = Timer.getFPGATimestamp(); 33 | 34 | for (Loop loop : mLoops) { 35 | loop.onLoop(now); 36 | } 37 | 38 | mDT = now - mTimestamp; 39 | mTimestamp = now; 40 | } 41 | } 42 | } 43 | }; 44 | 45 | public Looper() { 46 | mNotifier = new Notifier(runnable_); 47 | mRunning = false; 48 | mLoops = new ArrayList<>(); 49 | } 50 | 51 | @Override 52 | public synchronized void register(Loop loop) { 53 | synchronized (mTaskRunningLock) { 54 | mLoops.add(loop); 55 | } 56 | } 57 | 58 | public synchronized void start() { 59 | if (!mRunning) { 60 | System.out.println("Starting loops"); 61 | 62 | synchronized (mTaskRunningLock) { 63 | mTimestamp = Timer.getFPGATimestamp(); 64 | for (Loop loop : mLoops) { 65 | loop.onStart(mTimestamp); 66 | } 67 | mRunning = true; 68 | } 69 | 70 | mNotifier.startPeriodic(kPeriod); 71 | } 72 | } 73 | 74 | public synchronized void stop() { 75 | if (mRunning) { 76 | System.out.println("Stopping loops"); 77 | mNotifier.stop(); 78 | 79 | synchronized (mTaskRunningLock) { 80 | mRunning = false; 81 | mTimestamp = Timer.getFPGATimestamp(); 82 | for (Loop loop : mLoops) { 83 | System.out.println("Stopping " + loop); 84 | loop.onStop(mTimestamp); 85 | } 86 | } 87 | } 88 | } 89 | 90 | public void outputToSmartDashboard() { 91 | SmartDashboard.putNumber("looper_dt", mDT); 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/subsystems/Canifier.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.subsystems; 2 | 3 | import com.ctre.phoenix.CANifier; 4 | import com.ctre.phoenix.CANifierStatusFrame; 5 | import com.team254.frc2020.Constants; 6 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 7 | 8 | public class Canifier extends Subsystem { 9 | 10 | private static Canifier mInstance; 11 | 12 | public synchronized static Canifier getInstance() { 13 | if (mInstance == null) { 14 | mInstance = new Canifier(); 15 | } 16 | return mInstance; 17 | } 18 | 19 | private PeriodicIO mPeriodicIO; 20 | private boolean mOutputsChanged; 21 | 22 | public static class PeriodicIO { 23 | boolean break_beam_triggered = false; 24 | boolean turret_homing_limit_switch = false; 25 | 26 | double g = 0.0; 27 | double r = 0.0; 28 | double b = 0.0; 29 | } 30 | 31 | private CANifier mCanifier; 32 | 33 | private Canifier() { 34 | mCanifier = new CANifier(Constants.kCanifierId); 35 | mCanifier.setStatusFramePeriod(CANifierStatusFrame.Status_2_General, 10, Constants.kLongCANTimeoutMs); 36 | 37 | mPeriodicIO = new PeriodicIO(); 38 | 39 | // Force a first update. 40 | mOutputsChanged = true; 41 | } 42 | 43 | @Override 44 | public synchronized void readPeriodicInputs() { 45 | mPeriodicIO.break_beam_triggered = mCanifier.getGeneralInput(CANifier.GeneralPin.LIMR); 46 | mPeriodicIO.turret_homing_limit_switch = !mCanifier.getGeneralInput(CANifier.GeneralPin.LIMF); 47 | } 48 | 49 | public synchronized void setLEDColor(double red, double green, double blue) { 50 | if (red != mPeriodicIO.r || 51 | green != mPeriodicIO.g || 52 | blue != mPeriodicIO.b) { 53 | mPeriodicIO.r = red; 54 | mPeriodicIO.g = green; 55 | mPeriodicIO.b = blue; 56 | mOutputsChanged = true; 57 | } 58 | } 59 | 60 | @Override 61 | public synchronized void writePeriodicOutputs() { 62 | // A: Green 63 | // B: Red 64 | // C: Blue 65 | if (mOutputsChanged) { 66 | mCanifier.setLEDOutput(mPeriodicIO.g, CANifier.LEDChannel.LEDChannelB); 67 | mCanifier.setLEDOutput(mPeriodicIO.r, CANifier.LEDChannel.LEDChannelA); 68 | mCanifier.setLEDOutput(mPeriodicIO.b, CANifier.LEDChannel.LEDChannelC); 69 | mOutputsChanged = false; 70 | } 71 | } 72 | 73 | @Override 74 | public void stop() { 75 | } 76 | 77 | @Override 78 | public boolean checkSystem() { 79 | return false; 80 | } 81 | 82 | @Override 83 | public void outputTelemetry() { 84 | SmartDashboard.putBoolean("Turret Homing Limit Switch", mPeriodicIO.turret_homing_limit_switch); 85 | } 86 | 87 | public synchronized boolean isBreamBeamSensorTriggered() { 88 | return mPeriodicIO.break_beam_triggered; 89 | } 90 | 91 | public synchronized boolean isTurretHomed() { 92 | return mPeriodicIO.turret_homing_limit_switch; 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/trajectory/TrajectoryIteratorTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.geometry.Translation2d; 4 | import com.team254.lib.util.Util; 5 | import org.junit.Assert; 6 | import org.junit.Test; 7 | import org.junit.runner.RunWith; 8 | import org.junit.runners.JUnit4; 9 | 10 | import java.util.Arrays; 11 | import java.util.List; 12 | 13 | @RunWith(JUnit4.class) 14 | public class TrajectoryIteratorTest { 15 | public static final double kTestEpsilon = Util.kEpsilon; 16 | 17 | public static final List kWaypoints = Arrays.asList( 18 | new Translation2d(0.0, 0.0), 19 | new Translation2d(24.0, 0.0), 20 | new Translation2d(36.0, 12.0), 21 | new Translation2d(60.0, 12.0)); 22 | 23 | @Test 24 | public void test() { 25 | Trajectory traj = new Trajectory<>(kWaypoints); 26 | TrajectoryIterator iterator = new TrajectoryIterator<>(traj.getIndexView()); 27 | 28 | // Initial conditions. 29 | Assert.assertEquals(0.0, iterator.getProgress(), kTestEpsilon); 30 | Assert.assertEquals(3.0, iterator.getRemainingProgress(), kTestEpsilon); 31 | Assert.assertEquals(kWaypoints.get(0), iterator.getState()); 32 | Assert.assertFalse(iterator.isDone()); 33 | 34 | // Advance forward. 35 | Assert.assertEquals(kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.5), iterator.preview(0.5).state()); 36 | Assert.assertEquals(kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.5), iterator.advance(0.5).state()); 37 | Assert.assertEquals(0.5, iterator.getProgress(), kTestEpsilon); 38 | Assert.assertEquals(2.5, iterator.getRemainingProgress(), kTestEpsilon); 39 | Assert.assertFalse(iterator.isDone()); 40 | 41 | // Advance backwards. 42 | Assert.assertEquals(kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.25), iterator.preview(-0.25).state()); 43 | Assert.assertEquals(kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.25), iterator.advance(-0.25).state()); 44 | Assert.assertEquals(0.25, iterator.getProgress(), kTestEpsilon); 45 | Assert.assertEquals(2.75, iterator.getRemainingProgress(), kTestEpsilon); 46 | Assert.assertFalse(iterator.isDone()); 47 | 48 | // Advance past end. 49 | Assert.assertEquals(kWaypoints.get(3), iterator.preview(5.0).state()); 50 | Assert.assertEquals(kWaypoints.get(3), iterator.advance(5.0).state()); 51 | Assert.assertEquals(3.0, iterator.getProgress(), kTestEpsilon); 52 | Assert.assertEquals(0.0, iterator.getRemainingProgress(), kTestEpsilon); 53 | Assert.assertTrue(iterator.isDone()); 54 | 55 | // Advance past beginning. 56 | Assert.assertEquals(kWaypoints.get(0), iterator.preview(-5.0).state()); 57 | Assert.assertEquals(kWaypoints.get(0), iterator.advance(-5.0).state()); 58 | Assert.assertEquals(0.0, iterator.getProgress(), kTestEpsilon); 59 | Assert.assertEquals(3.0, iterator.getRemainingProgress(), kTestEpsilon); 60 | Assert.assertFalse(iterator.isDone()); 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/auto/modes/AutoModeBase.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.auto.modes; 2 | 3 | import com.team254.frc2020.auto.AutoModeEndedException; 4 | import com.team254.frc2020.auto.actions.Action; 5 | import com.team254.frc2020.auto.actions.NoopAction; 6 | import edu.wpi.first.wpilibj.DriverStation; 7 | 8 | /** 9 | * An abstract class that is the basis of the robot's autonomous routines. This is implemented in auto modes (which are 10 | * routines that do actions). 11 | */ 12 | public abstract class AutoModeBase { 13 | protected final double mUpdateRate = 1.0 / 50.0; 14 | protected boolean mActive = false; 15 | protected boolean mIsInterrupted = false; 16 | 17 | protected abstract void routine() throws AutoModeEndedException; 18 | 19 | public void run() { 20 | mActive = true; 21 | 22 | try { 23 | routine(); 24 | } catch (AutoModeEndedException e) { 25 | DriverStation.reportError("AUTO MODE DONE!!!! ENDED EARLY!!!!", false); 26 | return; 27 | } 28 | 29 | done(); 30 | } 31 | 32 | public void done() { 33 | System.out.println("Auto mode done"); 34 | } 35 | 36 | public void stop() { 37 | mActive = false; 38 | } 39 | 40 | public boolean isActive() { 41 | return mActive; 42 | } 43 | 44 | public boolean isActiveWithThrow() throws AutoModeEndedException { 45 | if (!isActive()) { 46 | throw new AutoModeEndedException(); 47 | } 48 | 49 | return isActive(); 50 | } 51 | 52 | public void waitForDriverConfirm() throws AutoModeEndedException { 53 | if (!mIsInterrupted) { 54 | interrupt(); 55 | } 56 | runAction(new NoopAction()); 57 | } 58 | 59 | public void interrupt() { 60 | System.out.println("** Auto mode interrrupted!"); 61 | mIsInterrupted = true; 62 | } 63 | 64 | public void resume() { 65 | System.out.println("** Auto mode resumed!"); 66 | mIsInterrupted = false; 67 | } 68 | 69 | public void runAction(Action action) throws AutoModeEndedException { 70 | isActiveWithThrow(); 71 | long waitTime = (long) (mUpdateRate * 1000.0); 72 | 73 | // Wait for interrupt state to clear 74 | while (isActiveWithThrow() && mIsInterrupted) { 75 | try { 76 | Thread.sleep(waitTime); 77 | } catch (InterruptedException e) { 78 | e.printStackTrace(); 79 | } 80 | } 81 | 82 | action.start(); 83 | 84 | // Run action, stop action on interrupt, non active mode, or done 85 | while (isActiveWithThrow() && !action.isFinished() && !mIsInterrupted) { 86 | action.update(); 87 | 88 | try { 89 | Thread.sleep(waitTime); 90 | } catch (InterruptedException e) { 91 | e.printStackTrace(); 92 | } 93 | } 94 | 95 | action.done(); 96 | 97 | } 98 | 99 | public boolean getIsInterrupted() { 100 | return mIsInterrupted; 101 | } 102 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/SubsystemManager.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020; 2 | 3 | import com.team254.frc2020.loops.ILooper; 4 | import com.team254.frc2020.loops.Loop; 5 | import com.team254.frc2020.loops.Looper; 6 | import com.team254.frc2020.subsystems.Subsystem; 7 | 8 | import java.util.ArrayList; 9 | import java.util.Arrays; 10 | import java.util.List; 11 | 12 | /** 13 | * Used to reset, start, stop, and update all subsystems at once 14 | */ 15 | public class SubsystemManager implements ILooper { 16 | public static SubsystemManager mInstance = null; 17 | 18 | private List mAllSubsystems; 19 | private List mLoops = new ArrayList<>(); 20 | 21 | private SubsystemManager() {} 22 | 23 | public static SubsystemManager getInstance() { 24 | if (mInstance == null) { 25 | mInstance = new SubsystemManager(); 26 | } 27 | 28 | return mInstance; 29 | } 30 | 31 | public void outputToSmartDashboard() { 32 | mAllSubsystems.forEach(Subsystem::outputTelemetry); 33 | } 34 | 35 | public boolean checkSubsystems() { 36 | boolean ret_val = true; 37 | 38 | for (Subsystem s : mAllSubsystems) { 39 | ret_val &= s.checkSystem(); 40 | } 41 | 42 | return ret_val; 43 | } 44 | 45 | public void stop() { 46 | mAllSubsystems.forEach(Subsystem::stop); 47 | } 48 | 49 | public List getSubsystems() { 50 | return mAllSubsystems; 51 | } 52 | 53 | public void setSubsystems(Subsystem... allSubsystems) { 54 | mAllSubsystems = Arrays.asList(allSubsystems); 55 | } 56 | 57 | private class EnabledLoop implements Loop { 58 | @Override 59 | public void onStart(double timestamp) { 60 | mLoops.forEach(l -> l.onStart(timestamp)); 61 | } 62 | 63 | @Override 64 | public void onLoop(double timestamp) { 65 | mAllSubsystems.forEach(Subsystem::readPeriodicInputs); 66 | mLoops.forEach(l -> l.onLoop(timestamp)); 67 | mAllSubsystems.forEach(Subsystem::writePeriodicOutputs); 68 | } 69 | 70 | @Override 71 | public void onStop(double timestamp) { 72 | mLoops.forEach(l -> l.onStop(timestamp)); 73 | } 74 | } 75 | 76 | private class DisabledLoop implements Loop { 77 | @Override 78 | public void onStart(double timestamp) {} 79 | 80 | @Override 81 | public void onLoop(double timestamp) { 82 | mAllSubsystems.forEach(Subsystem::readPeriodicInputs); 83 | } 84 | 85 | @Override 86 | public void onStop(double timestamp) {} 87 | } 88 | 89 | public void registerEnabledLoops(Looper enabledLooper) { 90 | mAllSubsystems.forEach(s -> s.registerEnabledLoops(this)); 91 | enabledLooper.register(new EnabledLoop()); 92 | } 93 | 94 | public void registerDisabledLoops(Looper disabledLooper) { 95 | disabledLooper.register(new DisabledLoop()); 96 | } 97 | 98 | @Override 99 | public void register(Loop loop) { 100 | mLoops.add(loop); 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/motion/MotionTestUtil.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | public class MotionTestUtil { 4 | 5 | protected abstract static class Dynamics { 6 | protected MotionState mState; 7 | 8 | public Dynamics(MotionState state) { 9 | mState = state; 10 | } 11 | 12 | public MotionState getState() { 13 | return mState; 14 | } 15 | 16 | public abstract void update(double command_vel, double dt); 17 | } 18 | 19 | protected static class IdealDynamics extends Dynamics { 20 | public IdealDynamics(MotionState state) { 21 | super(state); 22 | } 23 | 24 | @Override 25 | public void update(double command_vel, double dt) { 26 | final double acc = (command_vel - mState.vel()) / dt; 27 | mState = mState.extrapolate(mState.t() + dt, acc); 28 | } 29 | } 30 | 31 | protected static class ScaledDynamics extends Dynamics { 32 | protected double mVelRatio; 33 | 34 | public ScaledDynamics(MotionState state, double vel_ratio) { 35 | super(state); 36 | mVelRatio = vel_ratio; 37 | } 38 | 39 | @Override 40 | public void update(double command_vel, double dt) { 41 | final double acc = (command_vel * mVelRatio - mState.vel()) / dt; 42 | mState = mState.extrapolate(mState.t() + dt, acc); 43 | } 44 | } 45 | 46 | protected static class DeadbandDynamics extends Dynamics { 47 | protected double mDeadband; 48 | 49 | public DeadbandDynamics(MotionState state, double deadband) { 50 | super(state); 51 | mDeadband = deadband; 52 | } 53 | 54 | @Override 55 | public void update(double command_vel, double dt) { 56 | if (command_vel > -mDeadband && command_vel < mDeadband) { 57 | command_vel = 0.0; 58 | } else { 59 | command_vel = Math.signum(command_vel) * (Math.abs(command_vel) - mDeadband); 60 | } 61 | final double acc = (command_vel - mState.vel()) / dt; 62 | mState = mState.extrapolate(mState.t() + dt, acc); 63 | } 64 | } 65 | 66 | public static MotionState followProfile(ProfileFollower follower, Dynamics dynamics, double dt, 67 | int max_iterations) { 68 | int i = 0; 69 | for (; i < max_iterations && !follower.onTarget(); ++i) { 70 | MotionState state = dynamics.getState(); 71 | final double t = state.t() + dt; 72 | final double command_vel = follower.update(state, t); 73 | dynamics.update(command_vel, dt); 74 | System.out.println("State: " + state + ", Pos error: " + follower.getPosError() + ", Vel error: " 75 | + follower.getVelError() + ", Command: " + command_vel); 76 | if (follower.isFinishedProfile()) { 77 | System.out.println("Follower has finished profile"); 78 | } 79 | } 80 | if (i == max_iterations) { 81 | System.out.println("Iteration limit reached"); 82 | } 83 | System.out.println("Final state: " + dynamics.getState()); 84 | return dynamics.getState(); 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | 17 | @if "%DEBUG%" == "" @echo off 18 | @rem ########################################################################## 19 | @rem 20 | @rem Gradle startup script for Windows 21 | @rem 22 | @rem ########################################################################## 23 | 24 | @rem Set local scope for the variables with windows NT shell 25 | if "%OS%"=="Windows_NT" setlocal 26 | 27 | set DIRNAME=%~dp0 28 | if "%DIRNAME%" == "" set DIRNAME=. 29 | set APP_BASE_NAME=%~n0 30 | set APP_HOME=%DIRNAME% 31 | 32 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 33 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 34 | 35 | @rem Find java.exe 36 | if defined JAVA_HOME goto findJavaFromJavaHome 37 | 38 | set JAVA_EXE=java.exe 39 | %JAVA_EXE% -version >NUL 2>&1 40 | if "%ERRORLEVEL%" == "0" goto init 41 | 42 | echo. 43 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 44 | echo. 45 | echo Please set the JAVA_HOME variable in your environment to match the 46 | echo location of your Java installation. 47 | 48 | goto fail 49 | 50 | :findJavaFromJavaHome 51 | set JAVA_HOME=%JAVA_HOME:"=% 52 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 53 | 54 | if exist "%JAVA_EXE%" goto init 55 | 56 | echo. 57 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 58 | echo. 59 | echo Please set the JAVA_HOME variable in your environment to match the 60 | echo location of your Java installation. 61 | 62 | goto fail 63 | 64 | :init 65 | @rem Get command-line arguments, handling Windows variants 66 | 67 | if not "%OS%" == "Windows_NT" goto win9xME_args 68 | 69 | :win9xME_args 70 | @rem Slurp the command line arguments. 71 | set CMD_LINE_ARGS= 72 | set _SKIP=2 73 | 74 | :win9xME_args_slurp 75 | if "x%~1" == "x" goto execute 76 | 77 | set CMD_LINE_ARGS=%* 78 | 79 | :execute 80 | @rem Setup the command line 81 | 82 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 83 | 84 | @rem Execute Gradle 85 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 86 | 87 | :end 88 | @rem End local scope for the variables with windows NT shell 89 | if "%ERRORLEVEL%"=="0" goto mainEnd 90 | 91 | :fail 92 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 93 | rem the _cmd.exe /c_ return code! 94 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 95 | exit /b 1 96 | 97 | :mainEnd 98 | if "%OS%"=="Windows_NT" endlocal 99 | 100 | :omega 101 | -------------------------------------------------------------------------------- /characterization-project/gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | 17 | @if "%DEBUG%" == "" @echo off 18 | @rem ########################################################################## 19 | @rem 20 | @rem Gradle startup script for Windows 21 | @rem 22 | @rem ########################################################################## 23 | 24 | @rem Set local scope for the variables with windows NT shell 25 | if "%OS%"=="Windows_NT" setlocal 26 | 27 | set DIRNAME=%~dp0 28 | if "%DIRNAME%" == "" set DIRNAME=. 29 | set APP_BASE_NAME=%~n0 30 | set APP_HOME=%DIRNAME% 31 | 32 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 33 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 34 | 35 | @rem Find java.exe 36 | if defined JAVA_HOME goto findJavaFromJavaHome 37 | 38 | set JAVA_EXE=java.exe 39 | %JAVA_EXE% -version >NUL 2>&1 40 | if "%ERRORLEVEL%" == "0" goto init 41 | 42 | echo. 43 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 44 | echo. 45 | echo Please set the JAVA_HOME variable in your environment to match the 46 | echo location of your Java installation. 47 | 48 | goto fail 49 | 50 | :findJavaFromJavaHome 51 | set JAVA_HOME=%JAVA_HOME:"=% 52 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 53 | 54 | if exist "%JAVA_EXE%" goto init 55 | 56 | echo. 57 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 58 | echo. 59 | echo Please set the JAVA_HOME variable in your environment to match the 60 | echo location of your Java installation. 61 | 62 | goto fail 63 | 64 | :init 65 | @rem Get command-line arguments, handling Windows variants 66 | 67 | if not "%OS%" == "Windows_NT" goto win9xME_args 68 | 69 | :win9xME_args 70 | @rem Slurp the command line arguments. 71 | set CMD_LINE_ARGS= 72 | set _SKIP=2 73 | 74 | :win9xME_args_slurp 75 | if "x%~1" == "x" goto execute 76 | 77 | set CMD_LINE_ARGS=%* 78 | 79 | :execute 80 | @rem Setup the command line 81 | 82 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 83 | 84 | @rem Execute Gradle 85 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 86 | 87 | :end 88 | @rem End local scope for the variables with windows NT shell 89 | if "%ERRORLEVEL%"=="0" goto mainEnd 90 | 91 | :fail 92 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 93 | rem the _cmd.exe /c_ return code! 94 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 95 | exit /b 1 96 | 97 | :mainEnd 98 | if "%OS%"=="Windows_NT" endlocal 99 | 100 | :omega 101 | -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/spline/QuinticHermiteOptimizerTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.spline; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | import com.team254.lib.geometry.Rotation2d; 5 | import com.team254.lib.geometry.Translation2d; 6 | import com.team254.lib.util.Util; 7 | import org.junit.Assert; 8 | import org.junit.Test; 9 | import org.junit.runner.RunWith; 10 | import org.junit.runners.JUnit4; 11 | 12 | import java.util.ArrayList; 13 | import java.util.List; 14 | 15 | @RunWith(JUnit4.class) 16 | public class QuinticHermiteOptimizerTest { 17 | private static double kEpsilon = Util.kEpsilon; 18 | 19 | @Test 20 | public void test() { 21 | Pose2d a = new Pose2d(new Translation2d(0, 100), Rotation2d.fromDegrees(270)); 22 | Pose2d b = new Pose2d(new Translation2d(50, 0), Rotation2d.fromDegrees(0)); 23 | Pose2d c = new Pose2d(new Translation2d(100, 100), Rotation2d.fromDegrees(90)); 24 | 25 | List splines = new ArrayList<>(); 26 | splines.add(new QuinticHermiteSpline(a, b)); 27 | splines.add(new QuinticHermiteSpline(b, c)); 28 | 29 | long startTime = System.currentTimeMillis(); 30 | Assert.assertTrue(QuinticHermiteSpline.optimizeSpline(splines) < 0.014); 31 | System.out.println("Optimization time (ms): " + (System.currentTimeMillis() - startTime)); 32 | 33 | Pose2d d = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(90)); 34 | Pose2d e = new Pose2d(new Translation2d(0, 50), Rotation2d.fromDegrees(0)); 35 | Pose2d f = new Pose2d(new Translation2d(100, 0), Rotation2d.fromDegrees(90)); 36 | Pose2d g = new Pose2d(new Translation2d(100, 100), Rotation2d.fromDegrees(0)); 37 | 38 | List splines1 = new ArrayList<>(); 39 | splines1.add(new QuinticHermiteSpline(d, e)); 40 | splines1.add(new QuinticHermiteSpline(e, f)); 41 | splines1.add(new QuinticHermiteSpline(f, g)); 42 | 43 | startTime = System.currentTimeMillis(); 44 | Assert.assertTrue(QuinticHermiteSpline.optimizeSpline(splines1) < 0.16); 45 | System.out.println("Optimization time (ms): " + (System.currentTimeMillis() - startTime)); 46 | 47 | 48 | Pose2d h = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)); 49 | Pose2d i = new Pose2d(new Translation2d(50, 0), Rotation2d.fromDegrees(0)); 50 | Pose2d j = new Pose2d(new Translation2d(100, 50), Rotation2d.fromDegrees(45)); 51 | Pose2d k = new Pose2d(new Translation2d(150, 0), Rotation2d.fromDegrees(270)); 52 | Pose2d l = new Pose2d(new Translation2d(150, -50), Rotation2d.fromDegrees(270)); 53 | 54 | List splines2 = new ArrayList<>(); 55 | splines2.add(new QuinticHermiteSpline(h, i)); 56 | splines2.add(new QuinticHermiteSpline(i, j)); 57 | splines2.add(new QuinticHermiteSpline(j, k)); 58 | splines2.add(new QuinticHermiteSpline(k, l)); 59 | 60 | startTime = System.currentTimeMillis(); 61 | Assert.assertTrue(QuinticHermiteSpline.optimizeSpline(splines2) < 0.05); 62 | Assert.assertEquals(splines2.get(0).getCurvature(1.0), 0.0, kEpsilon); 63 | Assert.assertEquals(splines2.get(2).getCurvature(1.0), 0.0, kEpsilon); 64 | System.out.println("Optimization time (ms): " + (System.currentTimeMillis() - startTime)); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/Util.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import com.team254.frc2020.Constants; 4 | 5 | import java.util.List; 6 | 7 | /** 8 | * Contains basic functions that are used often. 9 | */ 10 | public class Util { 11 | public static final double kEpsilon = 1e-12; 12 | 13 | /** 14 | * Prevent this class from being instantiated. 15 | */ 16 | private Util() {} 17 | 18 | /** 19 | * Limits the given input to the given magnitude. 20 | */ 21 | public static double limit(double v, double maxMagnitude) { 22 | return limit(v, -maxMagnitude, maxMagnitude); 23 | } 24 | 25 | public static double limit(double v, double min, double max) { 26 | return Math.min(max, Math.max(min, v)); 27 | } 28 | 29 | public static boolean inRange(double v, double maxMagnitude) { 30 | return inRange(v, -maxMagnitude, maxMagnitude); 31 | } 32 | 33 | /** 34 | * Checks if the given input is within the range (min, max), both exclusive. 35 | */ 36 | public static boolean inRange(double v, double min, double max) { 37 | return v > min && v < max; 38 | } 39 | 40 | public static double interpolate(double a, double b, double x) { 41 | x = limit(x, 0.0, 1.0); 42 | return a + (b - a) * x; 43 | } 44 | 45 | public static String joinStrings(final String delim, final List strings) { 46 | StringBuilder sb = new StringBuilder(); 47 | for (int i = 0; i < strings.size(); ++i) { 48 | sb.append(strings.get(i).toString()); 49 | if (i < strings.size() - 1) { 50 | sb.append(delim); 51 | } 52 | } 53 | return sb.toString(); 54 | } 55 | 56 | public static boolean epsilonEquals(double a, double b, double epsilon) { 57 | return (a - epsilon <= b) && (a + epsilon >= b); 58 | } 59 | 60 | public static boolean epsilonEquals(double a, double b) { 61 | return epsilonEquals(a, b, kEpsilon); 62 | } 63 | 64 | public static boolean epsilonEquals(int a, int b, int epsilon) { 65 | return (a - epsilon <= b) && (a + epsilon >= b); 66 | } 67 | 68 | public static boolean allCloseTo(final List list, double value, double epsilon) { 69 | boolean result = true; 70 | for (Double value_in : list) { 71 | result &= epsilonEquals(value_in, value, epsilon); 72 | } 73 | return result; 74 | } 75 | 76 | /** 77 | * @param angle of turret in degrees 78 | */ 79 | public static double limitTurret(double turret_degrees) { 80 | if (turret_degrees < Constants.kTurretConstants.kMinUnitsLimit) { 81 | turret_degrees += 360.0; 82 | } 83 | if (turret_degrees > Constants.kTurretConstants.kMaxUnitsLimit) { 84 | turret_degrees -= 360.0; 85 | } 86 | 87 | return Util.limit(turret_degrees, Constants.kTurretConstants.kMinUnitsLimit, Constants.kTurretConstants.kMaxUnitsLimit); 88 | } 89 | 90 | public static double handleDeadband(double value, double deadband) { 91 | deadband = Math.abs(deadband); 92 | if (deadband == 1) { 93 | return 0; 94 | } 95 | double scaledValue = (value + (value < 0 ? deadband : -deadband)) / (1 - deadband); 96 | return (Math.abs(value) > Math.abs(deadband)) ? scaledValue : 0; 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/trajectory/timing/TimedState.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.timing; 2 | 3 | import com.team254.lib.geometry.State; 4 | import com.team254.lib.util.Util; 5 | 6 | import java.text.DecimalFormat; 7 | 8 | public class TimedState> implements State> { 9 | protected final S state_; 10 | protected double t_; // Time we achieve this state. 11 | protected double velocity_; // ds/dt 12 | protected double acceleration_; // d^2s/dt^2 13 | 14 | public TimedState(final S state) { 15 | state_ = state; 16 | } 17 | 18 | public TimedState(final S state, double t, double velocity, double acceleration) { 19 | state_ = state; 20 | t_ = t; 21 | velocity_ = velocity; 22 | acceleration_ = acceleration; 23 | } 24 | 25 | public S state() { 26 | return state_; 27 | } 28 | 29 | public void set_t(double t) { 30 | t_ = t; 31 | } 32 | 33 | public double t() { 34 | return t_; 35 | } 36 | 37 | public void set_velocity(double velocity) { 38 | velocity_ = velocity; 39 | } 40 | 41 | public double velocity() { 42 | return velocity_; 43 | } 44 | 45 | public void set_acceleration(double acceleration) { 46 | acceleration_ = acceleration; 47 | } 48 | 49 | public double acceleration() { 50 | return acceleration_; 51 | } 52 | 53 | @Override 54 | public String toString() { 55 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 56 | return state().toString() + ", t: " + fmt.format(t()) + ", v: " + fmt.format(velocity()) + ", a: " 57 | + fmt.format(acceleration()); 58 | } 59 | 60 | @Override 61 | public String toCSV() { 62 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 63 | return state().toCSV() + "," + fmt.format(t()) + "," + fmt.format(velocity()) + "," 64 | + fmt.format(acceleration()); 65 | } 66 | 67 | @Override 68 | public TimedState interpolate(TimedState other, double x) { 69 | final double new_t = Util.interpolate(t(), other.t(), x); 70 | final double delta_t = new_t - t(); 71 | if (delta_t < 0.0) { 72 | return other.interpolate(this, 1.0 - x); 73 | } 74 | boolean reversing = velocity() < 0.0 || (Util.epsilonEquals(velocity(), 0.0) && acceleration() < 0.0); 75 | final double new_v = velocity() + acceleration() * delta_t; 76 | final double new_s = (reversing ? -1.0 : 1.0) * (velocity() * delta_t + .5 * acceleration() * delta_t * delta_t); 77 | // System.out.println("x: " + x + " , new_t: " + new_t + ", new_s: " + new_s + " , distance: " + state() 78 | // .distance(other.state())); 79 | return new TimedState(state().interpolate(other.state(), new_s / state().distance(other.state())), 80 | new_t, 81 | new_v, 82 | acceleration()); 83 | } 84 | 85 | @Override 86 | public double distance(TimedState other) { 87 | return state().distance(other.state()); 88 | } 89 | 90 | @Override 91 | public boolean equals(final Object other) { 92 | if (other == null || !(other instanceof TimedState)) return false; 93 | TimedState ts = (TimedState) other; 94 | return state().equals(ts.state()) && Util.epsilonEquals(t(), ts.t()); 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/spline/SplineGenerator.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.spline; 2 | 3 | import com.team254.lib.geometry.*; 4 | 5 | import java.util.ArrayList; 6 | import java.util.List; 7 | 8 | public class SplineGenerator { 9 | private static final double kMaxDX = 2.0; //inches 10 | private static final double kMaxDY = 0.05; //inches 11 | private static final double kMaxDTheta = 0.1; //radians! 12 | private static final int kMinSampleSize = 1; 13 | 14 | /** 15 | * Converts a spline into a list of Twist2d's. 16 | * 17 | * @param s the spline to parametrize 18 | * @param t0 starting percentage of spline to parametrize 19 | * @param t1 ending percentage of spline to parametrize 20 | * @return list of Pose2dWithCurvature that approximates the original spline 21 | */ 22 | public static List parameterizeSpline(Spline s, double maxDx, double maxDy, double 23 | maxDTheta, double t0, double t1) { 24 | List rv = new ArrayList<>(); 25 | rv.add(s.getPose2dWithCurvature(0.0)); 26 | double dt = (t1 - t0); 27 | for (double t = 0; t < t1; t += dt / kMinSampleSize) { 28 | getSegmentArc(s, rv, t, t + dt / kMinSampleSize, maxDx, maxDy, maxDTheta); 29 | } 30 | return rv; 31 | } 32 | 33 | /** 34 | * Convenience function to parametrize a spline from t 0 to 1 35 | */ 36 | public static List parameterizeSpline(Spline s) { 37 | return parameterizeSpline(s, kMaxDX, kMaxDY, kMaxDTheta, 0.0, 1.0); 38 | } 39 | 40 | public static List parameterizeSpline(Spline s, double maxDx, double maxDy, double maxDTheta) { 41 | return parameterizeSpline(s, maxDx, maxDy, maxDTheta, 0.0, 1.0); 42 | } 43 | 44 | public static List parameterizeSplines(List splines) { 45 | return parameterizeSplines(splines, kMaxDX, kMaxDY, kMaxDTheta); 46 | } 47 | 48 | public static List parameterizeSplines(List splines, double maxDx, double maxDy, 49 | double maxDTheta) { 50 | List rv = new ArrayList<>(); 51 | if (splines.isEmpty()) return rv; 52 | rv.add(splines.get(0).getPose2dWithCurvature(0.0)); 53 | for (final Spline s : splines) { 54 | List samples = parameterizeSpline(s, maxDx, maxDy, maxDTheta); 55 | samples.remove(0); 56 | rv.addAll(samples); 57 | } 58 | return rv; 59 | } 60 | 61 | private static void getSegmentArc(Spline s, List rv, double t0, double t1, double maxDx, 62 | double maxDy, 63 | double maxDTheta) { 64 | Translation2d p0 = s.getPoint(t0); 65 | Translation2d p1 = s.getPoint(t1); 66 | Rotation2d r0 = s.getHeading(t0); 67 | Rotation2d r1 = s.getHeading(t1); 68 | Pose2d transformation = new Pose2d(new Translation2d(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse())); 69 | Twist2d twist = Pose2d.log(transformation); 70 | if (twist.dy > maxDy || twist.dx > maxDx || twist.dtheta > maxDTheta) { 71 | getSegmentArc(s, rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta); 72 | getSegmentArc(s, rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta); 73 | } else { 74 | rv.add(s.getPose2dWithCurvature(t1)); 75 | } 76 | } 77 | 78 | } 79 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/physics/DCMotorTransmission.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.physics; 2 | 3 | import com.team254.lib.util.Util; 4 | 5 | /** 6 | * Model of a DC motor rotating a shaft. All parameters refer to the output (e.g. should already consider gearing 7 | * and efficiency losses). The motor is assumed to be symmetric forward/reverse. 8 | */ 9 | public class DCMotorTransmission { 10 | // TODO add electrical constants? (e.g. current) 11 | 12 | // All units must be SI! 13 | protected final double speed_per_volt_; // rad/s per V (no load) 14 | protected final double torque_per_volt_; // N m per V (stall) 15 | protected final double friction_voltage_; // V 16 | 17 | public DCMotorTransmission(final double speed_per_volt, 18 | final double torque_per_volt, 19 | final double friction_voltage) { 20 | speed_per_volt_ = speed_per_volt; 21 | torque_per_volt_ = torque_per_volt; 22 | friction_voltage_ = friction_voltage; 23 | } 24 | 25 | public double speed_per_volt() { 26 | return speed_per_volt_; 27 | } 28 | 29 | public double torque_per_volt() { 30 | return torque_per_volt_; 31 | } 32 | 33 | public double friction_voltage() { 34 | return friction_voltage_; 35 | } 36 | 37 | public double free_speed_at_voltage(final double voltage) { 38 | if (voltage > Util.kEpsilon) { 39 | return Math.max(0.0, voltage - friction_voltage()) * speed_per_volt(); 40 | } else if (voltage < Util.kEpsilon) { 41 | return Math.min(0.0, voltage + friction_voltage()) * speed_per_volt(); 42 | } else { 43 | return 0.0; 44 | } 45 | } 46 | 47 | public double getTorqueForVoltage(final double output_speed, final double voltage) { 48 | double effective_voltage = voltage; 49 | if (output_speed > Util.kEpsilon) { 50 | // Forward motion, rolling friction. 51 | effective_voltage -= friction_voltage(); 52 | } else if (output_speed < -Util.kEpsilon) { 53 | // Reverse motion, rolling friction. 54 | effective_voltage += friction_voltage(); 55 | } else if (voltage > Util.kEpsilon) { 56 | // System is static, forward torque. 57 | effective_voltage = Math.max(0.0, voltage - friction_voltage()); 58 | } else if (voltage < -Util.kEpsilon) { 59 | // System is static, reverse torque. 60 | effective_voltage = Math.min(0.0, voltage + friction_voltage()); 61 | } else { 62 | // System is idle. 63 | return 0.0; 64 | } 65 | return torque_per_volt() * (-output_speed / speed_per_volt() + effective_voltage); 66 | } 67 | 68 | public double getVoltageForTorque(final double output_speed, final double torque) { 69 | double friction_voltage; 70 | if (output_speed > Util.kEpsilon) { 71 | // Forward motion, rolling friction. 72 | friction_voltage = friction_voltage(); 73 | } else if (output_speed < -Util.kEpsilon) { 74 | // Reverse motion, rolling friction. 75 | friction_voltage = -friction_voltage(); 76 | } else if (torque > Util.kEpsilon) { 77 | // System is static, forward torque. 78 | friction_voltage = friction_voltage(); 79 | } else if (torque < -Util.kEpsilon) { 80 | // System is static, reverse torque. 81 | friction_voltage = -friction_voltage(); 82 | } else { 83 | // System is idle. 84 | return 0.0; 85 | } 86 | return torque / torque_per_volt() + output_speed / speed_per_volt() + friction_voltage; 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/ShootingParameters.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import com.team254.lib.geometry.Pose2d; 4 | 5 | public class ShootingParameters { 6 | private final InterpolatingTreeMap mOldBallHoodMap; 7 | private final InterpolatingTreeMap mMediumBallHoodMap; 8 | private final InterpolatingTreeMap mNewBallHoodMap; 9 | private final InterpolatingTreeMap mShooterRPMMap; 10 | private final Pose2d mVisionTargetToGoalOffset; 11 | private final double mSpinCycleSetpoint; // percent output 12 | private final double mShooterAllowableErrorRPM; // rpm 13 | private final double mTurretAllowableErrorDegrees; // ° 14 | private final double mHoodAllowableErrorDegrees; // ° 15 | 16 | public enum BallQuality { 17 | NEW_BALL, 18 | MEDIUM_BALL, 19 | OLD_BALL 20 | } 21 | 22 | public ShootingParameters(InterpolatingTreeMap oldBallHoodMap, 23 | InterpolatingTreeMap mediumBallHoodMap, 24 | InterpolatingTreeMap newBallHoodMap, 25 | InterpolatingTreeMap shooterRPMMap, 26 | Pose2d visionTargetToGoalOffset, 27 | double spinCycleSetpoint, 28 | double shooterAllowableErrorRPM, 29 | double turretAllowableErrorDegrees, 30 | double hoodAllowableErrorDegrees) { 31 | this.mOldBallHoodMap = oldBallHoodMap; 32 | this.mMediumBallHoodMap = mediumBallHoodMap; 33 | this.mNewBallHoodMap = newBallHoodMap; 34 | this.mShooterRPMMap = shooterRPMMap; 35 | this.mVisionTargetToGoalOffset = visionTargetToGoalOffset; 36 | this.mSpinCycleSetpoint = spinCycleSetpoint; 37 | this.mShooterAllowableErrorRPM = shooterAllowableErrorRPM; 38 | this.mTurretAllowableErrorDegrees = turretAllowableErrorDegrees; 39 | this.mHoodAllowableErrorDegrees = hoodAllowableErrorDegrees; 40 | } 41 | 42 | public InterpolatingTreeMap getHoodMap(BallQuality ballQuality) { 43 | switch (ballQuality) { 44 | case NEW_BALL: 45 | return mNewBallHoodMap; 46 | case OLD_BALL: 47 | return mOldBallHoodMap; 48 | case MEDIUM_BALL: 49 | default: 50 | return mMediumBallHoodMap; 51 | } 52 | } 53 | 54 | public InterpolatingTreeMap getShooterRPMMap() { 55 | return mShooterRPMMap; 56 | } 57 | 58 | public Pose2d getVisionTargetToGoalOffset() { 59 | return mVisionTargetToGoalOffset; 60 | } 61 | 62 | public double getSpinCycleSetpoint() { 63 | return mSpinCycleSetpoint; 64 | } 65 | 66 | public synchronized boolean isShooterAtSetpoint(double current_shooter_rpm, double shooter_setpoint) { 67 | return Util.epsilonEquals(current_shooter_rpm, shooter_setpoint, mShooterAllowableErrorRPM); 68 | } 69 | 70 | public synchronized boolean isTurretAtSetpoint(double current_turret_angle, double turret_setpoint) { 71 | return Util.epsilonEquals(current_turret_angle, turret_setpoint, mTurretAllowableErrorDegrees); 72 | } 73 | 74 | public synchronized boolean isHoodAtSetpoint(double current_hood_angle, double hood_setpoint) { 75 | return Util.epsilonEquals(current_hood_angle, hood_setpoint, mHoodAllowableErrorDegrees); 76 | } 77 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/wpilib/TimedRobot.java: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------------*/ 2 | /* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ 3 | /* Open Source Software - may be modified and shared by FRC teams. The code */ 4 | /* must be accompanied by the FIRST BSD license file in the root directory of */ 5 | /* the project. */ 6 | /*----------------------------------------------------------------------------*/ 7 | 8 | package com.team254.lib.wpilib; 9 | 10 | import edu.wpi.first.hal.FRCNetComm.tInstances; 11 | import edu.wpi.first.hal.FRCNetComm.tResourceType; 12 | import edu.wpi.first.hal.HAL; 13 | import edu.wpi.first.hal.NotifierJNI; 14 | import edu.wpi.first.wpilibj.RobotController; 15 | 16 | /** 17 | * TimedRobot implements the IterativeRobotBase robot program framework. 18 | * 19 | *

20 | * The TimedRobot class is intended to be subclassed by a user creating a robot 21 | * program. 22 | * 23 | *

24 | * periodic() functions from the base class are called on an interval by a 25 | * Notifier instance. 26 | */ 27 | public class TimedRobot extends IterativeRobotBase { 28 | public static final double kDefaultPeriod = 0.02; 29 | 30 | // The C pointer to the notifier object. We don't use it directly, it is 31 | // just passed to the JNI bindings. 32 | private final int m_notifier = NotifierJNI.initializeNotifier(); 33 | 34 | // The absolute expiration time 35 | private double m_expirationTime; 36 | 37 | /** 38 | * Constructor for TimedRobot. 39 | */ 40 | protected TimedRobot() { 41 | this(kDefaultPeriod); 42 | } 43 | 44 | /** 45 | * Constructor for TimedRobot. 46 | * 47 | * @param period Period in seconds. 48 | */ 49 | protected TimedRobot(double period) { 50 | super(period); 51 | NotifierJNI.setNotifierName(m_notifier, "TimedRobot"); 52 | 53 | HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed); 54 | } 55 | 56 | @Override 57 | @SuppressWarnings("NoFinalizer") 58 | protected void finalize() { 59 | NotifierJNI.stopNotifier(m_notifier); 60 | NotifierJNI.cleanNotifier(m_notifier); 61 | } 62 | 63 | /** 64 | * Provide an alternate "main loop" via startCompetition(). 65 | */ 66 | @Override 67 | @SuppressWarnings("UnsafeFinalization") 68 | public void startCompetition() { 69 | robotInit(); 70 | 71 | if (isSimulation()) { 72 | simulationInit(); 73 | } 74 | 75 | // Tell the DS that the robot is ready to be enabled 76 | HAL.observeUserProgramStarting(); 77 | 78 | m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period; 79 | updateAlarm(); 80 | 81 | // Loop forever, calling the appropriate mode-dependent function 82 | while (true) { 83 | long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); 84 | if (curTime == 0) { 85 | break; 86 | } 87 | 88 | m_expirationTime += m_period; 89 | updateAlarm(); 90 | 91 | loopFunc(); 92 | } 93 | } 94 | 95 | /** 96 | * Ends the main loop in startCompetition(). 97 | */ 98 | @Override 99 | public void endCompetition() { 100 | NotifierJNI.stopNotifier(m_notifier); 101 | } 102 | 103 | /** 104 | * Get time period between calls to Periodic() functions. 105 | */ 106 | public double getPeriod() { 107 | return m_period; 108 | } 109 | 110 | /** 111 | * Update the alarm hardware to reflect the next alarm. 112 | */ 113 | @SuppressWarnings("UnsafeFinalization") 114 | private void updateAlarm() { 115 | NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6)); 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2020/subsystems/RobotStateEstimator.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2020.subsystems; 2 | 3 | import com.team254.frc2020.Constants; 4 | import com.team254.frc2020.Kinematics; 5 | import com.team254.frc2020.RobotState; 6 | import com.team254.frc2020.loops.ILooper; 7 | import com.team254.frc2020.loops.Loop; 8 | import com.team254.lib.geometry.Pose2d; 9 | import com.team254.lib.geometry.Rotation2d; 10 | import com.team254.lib.geometry.Twist2d; 11 | 12 | public class RobotStateEstimator extends Subsystem { 13 | static RobotStateEstimator mInstance = new RobotStateEstimator(); 14 | private RobotState mRobotState = RobotState.getInstance(); 15 | private Drive mDrive = Drive.getInstance(); 16 | private double left_encoder_prev_distance_ = 0.0; 17 | private double right_encoder_prev_distance_ = 0.0; 18 | private double prev_timestamp_ = -1.0; 19 | private Rotation2d prev_heading_ = null; 20 | 21 | public static RobotStateEstimator getInstance() { 22 | if (mInstance == null) { 23 | mInstance = new RobotStateEstimator(); 24 | } 25 | 26 | return mInstance; 27 | } 28 | 29 | private RobotStateEstimator() {} 30 | 31 | @Override 32 | public void registerEnabledLoops(ILooper looper) { 33 | looper.register(new EnabledLoop()); 34 | } 35 | 36 | private class EnabledLoop implements Loop { 37 | @Override 38 | public synchronized void onStart(double timestamp) { 39 | left_encoder_prev_distance_ = mDrive.getLeftEncoderDistance(); 40 | right_encoder_prev_distance_ = mDrive.getRightEncoderDistance(); 41 | prev_timestamp_ = timestamp; 42 | } 43 | 44 | @Override 45 | public synchronized void onLoop(double timestamp) { 46 | if (prev_heading_ == null) { 47 | prev_heading_ = mRobotState.getLatestFieldToVehicle().getValue().getRotation(); 48 | } 49 | final double dt = timestamp - prev_timestamp_; 50 | final double left_distance = mDrive.getLeftEncoderDistance(); 51 | final double right_distance = mDrive.getRightEncoderDistance(); 52 | final double delta_left = left_distance - left_encoder_prev_distance_; 53 | final double delta_right = right_distance - right_encoder_prev_distance_; 54 | final Rotation2d gyro_angle = mDrive.getHeading(); 55 | Twist2d odometry_twist; 56 | synchronized (mRobotState) { 57 | final Pose2d last_measurement = mRobotState.getLatestFieldToVehicle().getValue(); 58 | odometry_twist = Kinematics.forwardKinematics(last_measurement.getRotation(), delta_left, 59 | delta_right, gyro_angle); 60 | } 61 | final Twist2d measured_velocity = Kinematics.forwardKinematics( 62 | delta_left, delta_right, prev_heading_.inverse().rotateBy(gyro_angle).getRadians()).scaled(1.0 / dt); 63 | final Twist2d predicted_velocity = Kinematics.forwardKinematics(mDrive.getLeftLinearVelocity(), 64 | mDrive.getRightLinearVelocity()).scaled(dt); 65 | 66 | mRobotState.addVehicleToTurretObservation(timestamp, 67 | new Pose2d(Constants.kVehicleToTurretTranslation, Rotation2d.fromDegrees(Turret.getInstance().getAngle()))); 68 | 69 | mRobotState.addObservations(timestamp, odometry_twist, measured_velocity, 70 | predicted_velocity); 71 | left_encoder_prev_distance_ = left_distance; 72 | right_encoder_prev_distance_ = right_distance; 73 | prev_heading_ = gyro_angle; 74 | prev_timestamp_ = timestamp; 75 | } 76 | 77 | @Override 78 | public void onStop(double timestamp) {} 79 | } 80 | 81 | @Override 82 | public void stop() {} 83 | 84 | @Override 85 | public boolean checkSystem() { 86 | return true; 87 | } 88 | 89 | @Override 90 | public void outputTelemetry() { 91 | mRobotState.outputToSmartDashboard(); 92 | } 93 | } -------------------------------------------------------------------------------- /src/test/java/com/team254/lib/motion/SetpointGeneratorTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | import org.junit.Test; 4 | 5 | import static org.junit.Assert.assertTrue; 6 | 7 | public class SetpointGeneratorTest { 8 | 9 | public MotionState followProfile(SetpointGenerator spg, MotionProfileConstraints constraints, 10 | MotionProfileGoal goal, MotionState start_state, double dt, int max_iterations) { 11 | MotionState prev_state = start_state; 12 | 13 | System.out.println("Goal: " + goal); 14 | System.out.println("Start state: " + prev_state); 15 | int i = 0; 16 | for (; i < max_iterations; ++i) { 17 | SetpointGenerator.Setpoint setpoint = spg.getSetpoint(constraints, goal, prev_state, prev_state.t() + dt); 18 | prev_state = setpoint.motion_state; 19 | System.out.println(prev_state); 20 | if (setpoint.final_setpoint) { 21 | System.out.println("Goal reached"); 22 | break; 23 | } 24 | } 25 | if (i == max_iterations) { 26 | System.out.println("Iteration limit reached"); 27 | } 28 | return prev_state; 29 | } 30 | 31 | @Test 32 | public void testStationaryToStationary() { 33 | MotionProfileConstraints constraints = new MotionProfileConstraints(10.0, 10.0); 34 | MotionProfileGoal goal = new MotionProfileGoal(100.0); 35 | MotionState start_state = new MotionState(0.0, 0.0, 0.0, 0.0); 36 | final double dt = 0.01; 37 | 38 | SetpointGenerator spg = new SetpointGenerator(); 39 | MotionState final_setpoint = followProfile(spg, constraints, goal, start_state, dt, 1500); 40 | assertTrue(goal.atGoalState(final_setpoint)); 41 | } 42 | 43 | @Test 44 | public void testUpdateGoal() { 45 | MotionProfileConstraints constraints = new MotionProfileConstraints(10.0, 10.0); 46 | MotionProfileGoal goal = new MotionProfileGoal(100.0); 47 | MotionState start_state = new MotionState(0.0, 0.0, 0.0, 0.0); 48 | final double dt = 0.01; 49 | 50 | SetpointGenerator spg = new SetpointGenerator(); 51 | MotionState final_setpoint = followProfile(spg, constraints, goal, start_state, dt, 500); 52 | assertTrue(!goal.atGoalState(final_setpoint)); 53 | 54 | goal = new MotionProfileGoal(0.0); 55 | final_setpoint = followProfile(spg, constraints, goal, final_setpoint, dt, 1000); 56 | assertTrue(goal.atGoalState(final_setpoint)); 57 | } 58 | 59 | @Test 60 | public void testUpdateState() { 61 | MotionProfileConstraints constraints = new MotionProfileConstraints(10.0, 10.0); 62 | MotionProfileGoal goal = new MotionProfileGoal(100.0); 63 | MotionState start_state = new MotionState(0.0, 0.0, 0.0, 0.0); 64 | final double dt = 0.01; 65 | 66 | SetpointGenerator spg = new SetpointGenerator(); 67 | MotionState final_setpoint = followProfile(spg, constraints, goal, start_state, dt, 500); 68 | assertTrue(!goal.atGoalState(final_setpoint)); 69 | 70 | start_state = new MotionState(5.0, 50.0, 0.0, 0.0); 71 | final_setpoint = followProfile(spg, constraints, goal, start_state, dt, 1500); 72 | assertTrue(goal.atGoalState(final_setpoint)); 73 | } 74 | 75 | @Test 76 | public void testUpdateConstraints() { 77 | MotionProfileConstraints constraints = new MotionProfileConstraints(10.0, 10.0); 78 | MotionProfileGoal goal = new MotionProfileGoal(100.0); 79 | MotionState start_state = new MotionState(0.0, 0.0, 0.0, 0.0); 80 | final double dt = 0.01; 81 | 82 | SetpointGenerator spg = new SetpointGenerator(); 83 | MotionState final_setpoint = followProfile(spg, constraints, goal, start_state, dt, 500); 84 | assertTrue(!goal.atGoalState(final_setpoint)); 85 | 86 | constraints = new MotionProfileConstraints(20.0, 20.0); 87 | final_setpoint = followProfile(spg, constraints, goal, final_setpoint, dt, 1500); 88 | assertTrue(goal.atGoalState(final_setpoint)); 89 | } 90 | } 91 | --------------------------------------------------------------------------------