├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── src ├── main │ └── java │ │ └── com │ │ └── team254 │ │ ├── lib │ │ ├── util │ │ │ ├── CSVWritable.java │ │ │ ├── LatchedBoolean.java │ │ │ ├── CrashTrackingRunnable.java │ │ │ ├── TimeDelayedBoolean.java │ │ │ ├── Deadband.java │ │ │ ├── MinTimeBoolean.java │ │ │ ├── InverseInterpolable.java │ │ │ ├── DelayedBoolean.java │ │ │ ├── Interpolable.java │ │ │ ├── MovingAverage.java │ │ │ ├── Units.java │ │ │ ├── MultiTrigger.java │ │ │ ├── MovingAverageTwist2d.java │ │ │ ├── DriveSignal.java │ │ │ ├── CircularBuffer.java │ │ │ ├── InterpolatingLong.java │ │ │ ├── InterpolatingDouble.java │ │ │ ├── CircularBufferGeneric.java │ │ │ ├── CrashTracker.java │ │ │ ├── ReflectingCSVWriter.java │ │ │ ├── StatFinder.java │ │ │ ├── Util.java │ │ │ └── InterpolatingTreeMap.java │ │ ├── geometry │ │ │ ├── IRotation2d.java │ │ │ ├── ITranslation2d.java │ │ │ ├── ICurvature.java │ │ │ ├── IPose2d.java │ │ │ ├── State.java │ │ │ ├── Displacement1d.java │ │ │ ├── Twist2d.java │ │ │ └── Pose2dWithCurvature.java │ │ ├── motion │ │ │ ├── MotionUtil.java │ │ │ ├── MotionProfileConstraints.java │ │ │ └── MotionSegment.java │ │ ├── drivers │ │ │ ├── SparkMaxUtil.java │ │ │ ├── TalonSRXUtil.java │ │ │ ├── LazyTalonSRX.java │ │ │ ├── LazySparkMax.java │ │ │ ├── SparkMaxChecker.java │ │ │ ├── TalonSRXChecker.java │ │ │ └── SparkMaxFactory.java │ │ ├── vision │ │ │ ├── TargetInfo.java │ │ │ └── AimingParameters.java │ │ ├── control │ │ │ └── Lookahead.java │ │ └── wpilib │ │ │ └── TimedRobot.java │ │ └── frc2019 │ │ ├── GamePiece.java │ │ ├── loops │ │ ├── ILooper.java │ │ ├── Loop.java │ │ └── Looper.java │ │ ├── controlboard │ │ ├── IControlBoard.java │ │ ├── IDriveControlBoard.java │ │ ├── GamepadDriveControlBoard.java │ │ ├── MainDriveControlBoard.java │ │ ├── XboxController.java │ │ ├── IButtonControlBoard.java │ │ └── ControlBoard.java │ │ ├── auto │ │ ├── AutoModeEndedException.java │ │ ├── actions │ │ │ ├── ForceEndPathAction.java │ │ │ ├── NoopAction.java │ │ │ ├── GoToWantDiskAction.java │ │ │ ├── RunOnceAction.java │ │ │ ├── SetSuperstructureAction.java │ │ │ ├── LambdaAction.java │ │ │ ├── WaitUntilSeesTargetAction.java │ │ │ ├── WaitForPathMarkerAction.java │ │ │ ├── WaitAction.java │ │ │ ├── SuperstructureActionBase.java │ │ │ ├── WaitUntilInsideRegion.java │ │ │ ├── DriveOpenLoopAction.java │ │ │ ├── Action.java │ │ │ ├── ParallelAction.java │ │ │ ├── SeriesAction.java │ │ │ ├── DrivePathAction.java │ │ │ ├── AutoSteerAndIntakeAction.java │ │ │ ├── AutoAimAndScoreAction.java │ │ │ ├── CollectVelocityDataAction.java │ │ │ └── CollectAccelerationDataAction.java │ │ ├── modes │ │ │ ├── DoNothingMode.java │ │ │ ├── DriveByCameraMode.java │ │ │ ├── TestControlFlowMode.java │ │ │ ├── CharacterizeDrivebaseMode.java │ │ │ └── AutoModeBase.java │ │ └── AutoModeExecutor.java │ │ ├── paths │ │ ├── PathContainer.java │ │ ├── Hab1ToCargoShip3Path.java │ │ ├── RocketFarToCargo1Path.java │ │ ├── CargoShip1ToCargoShip2Path.java │ │ ├── Hab1ToCargoShipFrontPath.java │ │ ├── RocketNearToCargo1Path.java │ │ ├── CargoShip2ToBallPitPath.java │ │ ├── Hab1ToRocketFarPath.java │ │ ├── GetOffHab2Path.java │ │ ├── FeederToCargoShip2Path.java │ │ ├── Hab1ToCargoShip1Path.java │ │ ├── RocketFarToFeederPath.java │ │ ├── RocketNearToFeederPath.java │ │ ├── FeederToCargoShip1Path.java │ │ ├── FeederToCargoShip3Path.java │ │ ├── CargoShip1ToFeederPath.java │ │ ├── CargoShip2ToFeederPath.java │ │ ├── CargoShip3ToFeederPath.java │ │ ├── CargoShipFrontToFeederPath.java │ │ ├── Hab1ToRocketNearPath.java │ │ ├── FeederToRocketFarPath.java │ │ ├── FeederToRocketNearPath.java │ │ └── FeederToRocketFarScoringPositionPath.java │ │ ├── planners │ │ ├── ISuperstructureMotionPlanner.java │ │ └── AvoidDriveBasePlanner.java │ │ ├── states │ │ ├── SuperstructureConstants.java │ │ ├── LEDState.java │ │ ├── SuperstructureGoal.java │ │ ├── TimedLEDState.java │ │ └── SuperstructureState.java │ │ ├── Main.java │ │ ├── subsystems │ │ ├── StingerRoller.java │ │ ├── Subsystem.java │ │ ├── Wrist.java │ │ ├── Infrastructure.java │ │ ├── Kickstand.java │ │ ├── Arm.java │ │ └── RobotStateEstimator.java │ │ ├── Kinematics.java │ │ └── SubsystemManager.java └── test │ └── java │ └── com │ └── team254 │ ├── frc2019 │ ├── TestSystemTest.java │ ├── statemachines │ │ └── SuperstructureStateMachineTest.java │ ├── planners │ │ └── PlannerTestUtil.java │ └── subsystems │ │ └── LimelightTest.java │ └── lib │ ├── util │ ├── DeadbandTest.java │ └── MultiTriggerTest.java │ ├── motion │ ├── MotionSegmentTest.java │ ├── MotionTestUtil.java │ └── SetpointGeneratorTest.java │ └── physics │ └── DriveCharacterizationTest.java ├── .wpilib └── wpilib_preferences.json ├── dash ├── second fpv.html └── firstperson.html ├── .vscode ├── settings.json └── launch.json ├── settings.gradle ├── LICENSE ├── Top_LL_Hatch_Pipeline.vpr ├── Bottom_LL_Hatch_Pipeline.vpr ├── Bottom_LL_Hatch_Pipeline_Highest.vpr ├── Top_LL_Hatch_Pipeline_Highest.vpr ├── vendordeps └── REVRobotics.json └── gradlew.bat /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Team254/FRC-2019-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/frc2019/GamePiece.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019; 2 | 3 | public enum GamePiece { 4 | BALL, 5 | DISK, 6 | CLIMB, 7 | } 8 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/loops/ILooper.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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": "2019", 5 | "teamNumber": 254 6 | } -------------------------------------------------------------------------------- /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/frc2019/controlboard/IControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-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 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/AutoModeEndedException.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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/frc2019/auto/actions/ForceEndPathAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.subsystems.Drive; 4 | 5 | public class ForceEndPathAction extends RunOnceAction { 6 | 7 | @Override 8 | public synchronized void runOnce() { 9 | Drive.getInstance().forceDoneWithPath(); 10 | } 11 | } -------------------------------------------------------------------------------- /dash/second fpv.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/controlboard/IDriveControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.controlboard; 2 | 3 | public interface IDriveControlBoard { 4 | double getThrottle(); 5 | 6 | double getTurn(); 7 | 8 | boolean getQuickTurn(); 9 | 10 | boolean getShoot(); 11 | 12 | boolean getWantsLowGear(); 13 | 14 | boolean getThrust(); 15 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/modes/DoNothingMode.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.modes; 2 | 3 | import com.team254.frc2019.auto.AutoModeEndedException; 4 | 5 | public class DoNothingMode extends AutoModeBase { 6 | @Override 7 | protected void routine() throws AutoModeEndedException { 8 | System.out.println("doing nothing"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/modes/DriveByCameraMode.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.modes; 2 | 3 | import com.team254.frc2019.auto.AutoModeEndedException; 4 | 5 | public class DriveByCameraMode extends AutoModeBase { 6 | @Override 7 | protected void routine() throws AutoModeEndedException { 8 | System.out.println("Drive By Camera"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/loops/Loop.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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 | -------------------------------------------------------------------------------- /dash/firstperson.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/NoopAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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/main/java/com/team254/frc2019/paths/PathContainer.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | /** 6 | * Interface containing all information necessary for a path including the Path itself, the Path's starting pose, and 7 | * whether or not the robot should drive in reverse along the path. 8 | */ 9 | public interface PathContainer { 10 | Path buildPath(); 11 | 12 | boolean isReversed(); 13 | } 14 | -------------------------------------------------------------------------------- /src/test/java/com/team254/frc2019/TestSystemTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019; 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 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/planners/ISuperstructureMotionPlanner.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.planners; 2 | 3 | import com.team254.frc2019.states.SuperstructureGoal; 4 | 5 | public interface ISuperstructureMotionPlanner { 6 | boolean isValidGoal(SuperstructureGoal goal); 7 | 8 | /** 9 | * @return the new setpoint produced by the planner. 10 | */ 11 | SuperstructureGoal plan(SuperstructureGoal prevSetpoint, SuperstructureGoal desiredState); 12 | } 13 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/SparkMaxUtil.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.revrobotics.CANError; 4 | 5 | import edu.wpi.first.wpilibj.DriverStation; 6 | 7 | public class SparkMaxUtil { 8 | // Checks the specified error code for issues. 9 | public static void checkError(CANError errorCode, String message) { 10 | if (errorCode != CANError.kOK) { 11 | DriverStation.reportError(message + errorCode, false); 12 | } 13 | } 14 | } -------------------------------------------------------------------------------- /.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 | "**/.classpath": true, 13 | "**/.project": true, 14 | "**/.settings": true, 15 | "**/.factorypath": true 16 | }, 17 | "wpilib.online": true 18 | } 19 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/GoToWantDiskAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.statemachines.SuperstructureCommands; 4 | 5 | public class GoToWantDiskAction extends SuperstructureActionBase { 6 | 7 | public GoToWantDiskAction(boolean waitForDesired) { 8 | super(waitForDesired); 9 | } 10 | 11 | @Override 12 | public void start() { 13 | SuperstructureCommands.goToPickupDiskFromWallFront(); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /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/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/lib/util/DeadbandTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import org.junit.Assert; 4 | import org.junit.Test; 5 | 6 | public class DeadbandTest { 7 | @Test 8 | public void testInDeadband() { 9 | Assert.assertTrue(Deadband.inDeadband(.05, .1)); 10 | Assert.assertTrue(Deadband.inDeadband(-0.05, .1)); 11 | Assert.assertTrue(Deadband.inDeadband(0, .1)); 12 | Assert.assertFalse(Deadband.inDeadband(.15, .1)); 13 | Assert.assertFalse(Deadband.inDeadband(-.15, .1)); 14 | Assert.assertFalse(Deadband.inDeadband(-.6, .5)); 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /.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/TalonSRXUtil.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 TalonSRXUtil { 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/frc2019/auto/actions/RunOnceAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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/frc2019/auto/actions/SetSuperstructureAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.states.SuperstructureGoal; 4 | import com.team254.frc2019.states.SuperstructureState; 5 | 6 | public class SetSuperstructureAction extends SuperstructureActionBase { 7 | private SuperstructureState mDesiredState; 8 | 9 | public SetSuperstructureAction(SuperstructureState desiredState) { 10 | mDesiredState = desiredState; 11 | } 12 | 13 | @Override 14 | public void start() { 15 | mSuperstructure.setGoal(new SuperstructureGoal(mDesiredState)); 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/LambdaAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | public class LambdaAction implements Action { 4 | 5 | public interface VoidInterace { 6 | void f(); 7 | } 8 | 9 | VoidInterace mF; 10 | 11 | public LambdaAction(VoidInterace f) { 12 | this.mF = f; 13 | } 14 | 15 | @Override 16 | public void start() { 17 | mF.f(); 18 | } 19 | 20 | @Override 21 | public void update() {} 22 | 23 | @Override 24 | public boolean isFinished() { 25 | return true; 26 | } 27 | 28 | @Override 29 | public void done() {} 30 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/WaitUntilSeesTargetAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.Constants; 4 | import com.team254.frc2019.RobotState; 5 | 6 | public class WaitUntilSeesTargetAction implements Action { 7 | @Override 8 | public void start() {} 9 | 10 | @Override 11 | public void update() {} 12 | 13 | @Override 14 | public boolean isFinished() { 15 | return RobotState.getInstance().getAimingParameters(RobotState.getInstance().useHighTarget(), -1, Constants.kMaxGoalTrackAge).isPresent(); 16 | } 17 | 18 | @Override 19 | public void done() {} 20 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/states/SuperstructureConstants.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.states; 2 | 3 | public class SuperstructureConstants { 4 | public static final double kTurretPaddingDegrees = 5; 5 | public static final double kElevatorPaddingInches = 1; 6 | public static final double kShoulderPaddingDegrees = 5; 7 | public static final double kWristPaddingDegrees = 5; 8 | 9 | public static final double[] kPadding = { 10 | kTurretPaddingDegrees, kElevatorPaddingInches, kShoulderPaddingDegrees, kWristPaddingDegrees}; 11 | public static final double[] kPlannerPadding = {kElevatorPaddingInches, kShoulderPaddingDegrees, kWristPaddingDegrees}; 12 | } 13 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/WaitForPathMarkerAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.subsystems.Drive; 4 | 5 | public class WaitForPathMarkerAction implements Action { 6 | 7 | private Drive mDrive = Drive.getInstance(); 8 | private String mMarker; 9 | 10 | public WaitForPathMarkerAction(String marker) { 11 | mMarker = marker; 12 | } 13 | 14 | @Override 15 | public boolean isFinished() { 16 | return mDrive.hasPassedMarker(mMarker); 17 | } 18 | 19 | @Override 20 | public void update() {} 21 | 22 | @Override 23 | public void done() {} 24 | 25 | @Override 26 | public void start() {} 27 | } -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/frc2019/auto/actions/WaitAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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 | } 32 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/Deadband.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | public class Deadband { 4 | public static double applyLowHigh(double input, double lowPass, double highPass, double defaultValue) { 5 | if (inDeadbandLowHigh(input, lowPass, highPass)) { 6 | return defaultValue; 7 | } 8 | return input; 9 | } 10 | 11 | public static boolean inDeadband(double input, double window) { 12 | return inDeadbandLowHigh(input, -window, window); 13 | } 14 | 15 | public static boolean inDeadbandLowHigh(double input, double lowPass, double highPass) { 16 | return input < highPass && input > lowPass; 17 | } 18 | 19 | public static double apply(double input, double window) { 20 | if (inDeadband(input, window)) { 21 | return 0; 22 | } 23 | return input; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2019' 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 | frcHome = new File(publicFolder, "frc${frcYear}") 15 | } else { 16 | def userFolder = System.getProperty("user.home") 17 | frcHome = new File(userFolder, "frc${frcYear}") 18 | } 19 | def frcHomeMaven = new File(frcHome, 'maven') 20 | maven { 21 | name 'frcHome' 22 | url frcHomeMaven 23 | } 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/SuperstructureActionBase.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.subsystems.Superstructure; 4 | 5 | public abstract class SuperstructureActionBase implements Action { 6 | protected final Superstructure mSuperstructure = Superstructure.getInstance(); 7 | protected boolean mWaitForDesired; 8 | 9 | public SuperstructureActionBase(boolean waitForDesired) { 10 | mWaitForDesired = waitForDesired; 11 | } 12 | 13 | public SuperstructureActionBase() { 14 | mWaitForDesired = true; 15 | } 16 | 17 | @Override 18 | public abstract void start(); 19 | 20 | @Override 21 | public void update() {} 22 | 23 | @Override 24 | public boolean isFinished() { 25 | return !mWaitForDesired || mSuperstructure.isAtDesiredState(); 26 | } 27 | 28 | @Override 29 | public void done() {} 30 | } 31 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/Hab1ToCargoShip3Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class Hab1ToCargoShip3Path implements PathContainer { 9 | 10 | boolean mLeft; 11 | 12 | public Hab1ToCargoShip3Path(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new Waypoint(0, 0.0, 0, 0)); 20 | sWaypoints.add(new Waypoint(20, 0.0, 0, 40.0)); 21 | sWaypoints.add(new Waypoint(245, (mLeft ? 1.0 : -1.0) * 5, 0, 120)); 22 | 23 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 24 | } 25 | 26 | @Override 27 | public boolean isReversed() { 28 | return false; 29 | } 30 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/RocketFarToCargo1Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class RocketFarToCargo1Path implements PathContainer { 8 | 9 | boolean mLeft; 10 | 11 | public RocketFarToCargo1Path(boolean left) { 12 | mLeft = left; 13 | } 14 | 15 | @Override 16 | public Path buildPath() { 17 | ArrayList sWaypoints = new ArrayList(); 18 | 19 | sWaypoints.add(new PathBuilder.Waypoint(205, (mLeft ? 1.0 : -1.0) * 95, 0, 100)); 20 | //sWaypoints.add(new PathBuilder.Waypoint(197.5, (mLeft ? 1.0 : -1.0) * 40, 1, 100)); 21 | sWaypoints.add(new PathBuilder.Waypoint(195, 0, 0, 120)); 22 | 23 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 24 | } 25 | 26 | @Override 27 | public boolean isReversed() { 28 | return true; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/CargoShip1ToCargoShip2Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class CargoShip1ToCargoShip2Path implements PathContainer { 8 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 9 | 10 | boolean mLeft; 11 | 12 | public CargoShip1ToCargoShip2Path(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(195, (mLeft ? 1.0 : -1.0) * 0, 0, 100)); 20 | sWaypoints.add(new PathBuilder.Waypoint(220, (mLeft ? 1.0 : -1.0) * 0, 0, 100)); 21 | 22 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 23 | } 24 | 25 | @Override 26 | public boolean isReversed() { 27 | return false; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /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/frc2019/paths/Hab1ToCargoShipFrontPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class Hab1ToCargoShipFrontPath implements PathContainer { 8 | boolean mLeft; 9 | 10 | public Hab1ToCargoShipFrontPath(boolean left) { 11 | mLeft = left; 12 | } 13 | 14 | @Override 15 | public Path buildPath() { 16 | ArrayList sWaypoints = new ArrayList(); 17 | sWaypoints.add(new PathBuilder.Waypoint(0, 0, 0, 0)); 18 | sWaypoints.add(new PathBuilder.Waypoint(65, (mLeft ? 1.0 : -1.0) * -20, 19 | 15.0, 100.0)); 20 | sWaypoints.add(new PathBuilder.Waypoint(142.5, (mLeft ? 1.0 : -1.0) * -45, 0, 100.0)); 21 | 22 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 23 | } 24 | 25 | @Override 26 | public boolean isReversed() { 27 | return false; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/RocketNearToCargo1Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class RocketNearToCargo1Path implements PathContainer { 8 | 9 | boolean mLeft; 10 | 11 | public RocketNearToCargo1Path(boolean left) { 12 | mLeft = left; 13 | } 14 | 15 | @Override 16 | public Path buildPath() { 17 | ArrayList sWaypoints = new ArrayList(); 18 | sWaypoints.add(new PathBuilder.Waypoint(125, (mLeft ? 1.0 : -1.0) * 75, 0, 120)); 19 | sWaypoints.add(new PathBuilder.Waypoint(145, (mLeft ? 1.0 : -1.0) * 40, 30, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(200, (mLeft ? 1.0 : -1.0) * -5, 0, 120)); 21 | 22 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 23 | } 24 | 25 | @Override 26 | public boolean isReversed() { 27 | return false; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/CargoShip2ToBallPitPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class CargoShip2ToBallPitPath implements PathContainer { 8 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 9 | 10 | boolean mLeft; 11 | 12 | public CargoShip2ToBallPitPath(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList<>(); 19 | sWaypoints.add(new PathBuilder.Waypoint(220, 0, 0, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(160, 0, 0, 120, kTurnTurretMarker)); 21 | sWaypoints.add(new PathBuilder.Waypoint(100, 0, 0, 120)); 22 | 23 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 24 | } 25 | 26 | @Override 27 | public boolean isReversed() { 28 | return true; 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /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/test/java/com/team254/lib/util/MultiTriggerTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import org.junit.Assert; 5 | import org.junit.Test; 6 | 7 | public class MultiTriggerTest { 8 | @Test 9 | public void test() { 10 | MultiTrigger trigger = new MultiTrigger(0.1); 11 | trigger.update(true); 12 | Assert.assertTrue(trigger.wasTapped()); 13 | Assert.assertTrue(trigger.isPressed()); 14 | 15 | trigger.update(true); 16 | Assert.assertFalse(trigger.wasTapped()); 17 | Assert.assertTrue(trigger.isPressed()); 18 | 19 | Timer.delay(0.05); 20 | Assert.assertFalse(trigger.isHeld()); 21 | Assert.assertTrue(trigger.isPressed()); 22 | 23 | Timer.delay(0.1); 24 | Assert.assertTrue(trigger.isHeld()); 25 | Assert.assertTrue(trigger.isPressed()); 26 | 27 | trigger.update(false); 28 | Assert.assertFalse(trigger.wasTapped()); 29 | Assert.assertFalse(trigger.isPressed()); 30 | Assert.assertFalse(trigger.isHeld()); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/Hab1ToRocketFarPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class Hab1ToRocketFarPath implements PathContainer { 9 | public static final String kStartAutoAimingMarker = "START_AUTO_AIMING"; 10 | 11 | boolean mLeft; 12 | 13 | public Hab1ToRocketFarPath(boolean left) { 14 | mLeft = left; 15 | } 16 | 17 | @Override 18 | public Path buildPath() { 19 | ArrayList sWaypoints = new ArrayList(); 20 | sWaypoints.add(new Waypoint(0, 0, 0, 0)); 21 | sWaypoints.add(new Waypoint(20, 0, 0, 40.0)); 22 | sWaypoints.add(new Waypoint(170, 0, 83.0, 100.0, kStartAutoAimingMarker)); 23 | sWaypoints.add(new Waypoint(205, (mLeft ? 1.0 : -1.0) * 83, 0, 100.0)); 24 | 25 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 26 | } 27 | 28 | @Override 29 | public boolean isReversed() { 30 | return false; 31 | } 32 | } -------------------------------------------------------------------------------- /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) 2019 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. 22 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/WaitUntilInsideRegion.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.RobotState; 4 | import com.team254.lib.geometry.Translation2d; 5 | 6 | public class WaitUntilInsideRegion implements Action { 7 | private final static RobotState mRobotState = RobotState.getInstance(); 8 | 9 | private final Translation2d mBottomLeft; 10 | private final Translation2d mTopRight; 11 | 12 | public WaitUntilInsideRegion(Translation2d bottomLeft, Translation2d topRight) { 13 | mBottomLeft = bottomLeft; 14 | mTopRight = topRight; 15 | } 16 | 17 | @Override 18 | public boolean isFinished() { 19 | Translation2d position = mRobotState.getLatestFieldToVehicle().getValue().getTranslation(); 20 | return position.x() > mBottomLeft.x() && position.x() < mTopRight.x() 21 | && position.y() > mBottomLeft.y() && position.y() < mTopRight.y(); 22 | } 23 | 24 | @Override 25 | public void update() {} 26 | 27 | @Override 28 | public void done() {} 29 | 30 | @Override 31 | public void start() {} 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/GetOffHab2Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | import com.team254.lib.geometry.Pose2d; 6 | import com.team254.lib.geometry.Rotation2d; 7 | import com.team254.lib.geometry.Translation2d; 8 | 9 | import java.util.ArrayList; 10 | 11 | public class GetOffHab2Path implements PathContainer { 12 | 13 | @Override 14 | public Path buildPath() { 15 | ArrayList sWaypoints = new ArrayList(); 16 | sWaypoints.add(new Waypoint(0, 0, 0, 0)); 17 | sWaypoints.add(new Waypoint(25, 0, 0, 20)); 18 | //sWaypoints.add(new Waypoint(100, 0, 0.0, 100.0)); 19 | //sWaypoints.add(new Waypoint(100, 100, 0, 100)); 20 | 21 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 22 | } 23 | 24 | //@Override 25 | public Pose2d getStartPose() { 26 | return new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(180.0)); 27 | } 28 | 29 | @Override 30 | public boolean isReversed() { 31 | return false; 32 | } 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/FeederToCargoShip2Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class FeederToCargoShip2Path implements PathContainer { 9 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 10 | 11 | boolean mLeft; 12 | 13 | public FeederToCargoShip2Path(boolean left) { 14 | mLeft = left; 15 | } 16 | 17 | @Override 18 | public Path buildPath() { 19 | ArrayList sWaypoints = new ArrayList<>(); 20 | sWaypoints.add(new Waypoint(-73, (mLeft ? 1.0 : -1.0) * 80, 0, 120)); 21 | sWaypoints.add(new Waypoint(-25, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kTurnTurretMarker)); 22 | sWaypoints.add(new Waypoint(100, (mLeft ? 1.0 : -1.0) * 40, 40, 100)); 23 | sWaypoints.add(new Waypoint(219, 0, 0, 120)); 24 | 25 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 26 | } 27 | 28 | @Override 29 | public boolean isReversed() { 30 | return false; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/DriveOpenLoopAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.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/frc2019/paths/Hab1ToCargoShip1Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class Hab1ToCargoShip1Path implements PathContainer { 8 | public static final String kStartAutoAimingMarker = "START_AUTO_AIMING"; 9 | 10 | boolean mLeft; 11 | 12 | public Hab1ToCargoShip1Path(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(0, 0.0, 0, 0)); 20 | sWaypoints.add(new PathBuilder.Waypoint(30, 0.0, 0, 40.0, kStartAutoAimingMarker)); 21 | sWaypoints.add(new PathBuilder.Waypoint(110, (mLeft ? 1.0 : -1.0) * 1.5, 1, 100.0, kStartAutoAimingMarker)); 22 | sWaypoints.add(new PathBuilder.Waypoint(205, (mLeft ? 1.0 : -1.0) * 3, 0, 120)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return false; 30 | } 31 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/modes/TestControlFlowMode.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.modes; 2 | 3 | import com.team254.frc2019.auto.AutoModeEndedException; 4 | import com.team254.frc2019.auto.actions.WaitAction; 5 | 6 | public class TestControlFlowMode extends AutoModeBase { 7 | 8 | @Override 9 | protected void routine() throws AutoModeEndedException { 10 | System.out.println("***** Starting test control flow mode"); 11 | 12 | System.out.println("***** starting - first wait action"); 13 | runAction(new WaitAction(10)); 14 | System.out.println("***** done - first wait action "); 15 | 16 | waitForDriverConfirm(); // drivers do some manual stuff 17 | 18 | System.out.println("***** starting - second wait action"); 19 | runAction(new WaitAction(10)); 20 | System.out.println("***** done - second wait action "); 21 | 22 | waitForDriverConfirm(); // drivers do some manual stuff 23 | 24 | System.out.println("***** starting - third wait action"); 25 | runAction(new WaitAction(10)); 26 | System.out.println("***** done - third wait action "); 27 | } 28 | 29 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/RocketFarToFeederPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class RocketFarToFeederPath implements PathContainer { 9 | public static final String kLookForTargetMarker = "LOOK_FOR_TARGET"; 10 | 11 | boolean mLeft; 12 | 13 | public RocketFarToFeederPath(boolean left) { 14 | mLeft = left; 15 | } 16 | 17 | @Override 18 | public Path buildPath() { 19 | ArrayList sWaypoints = new ArrayList(); 20 | sWaypoints.add(new Waypoint(205, (mLeft ? 1.0 : -1.0) * 83, 0, 0)); 21 | sWaypoints.add(new Waypoint(170, (mLeft ? 1.0 : -1.0) * 40, 40, 120)); 22 | sWaypoints.add(new Waypoint(55, (mLeft ? 1.0 : -1.0) * 80, 60, 120, kLookForTargetMarker)); 23 | sWaypoints.add(new Waypoint(-20, (mLeft ? 1.0 : -1.0) * 80, 0, 120)); 24 | 25 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 26 | } 27 | 28 | @Override 29 | public boolean isReversed() { 30 | return true; 31 | } 32 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/control/Lookahead.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.control; 2 | 3 | /** 4 | * A utility class for interpolating lookahead distance based on current speed. 5 | */ 6 | public class Lookahead { 7 | public final double min_distance; 8 | public final double max_distance; 9 | public final double min_speed; 10 | public final double max_speed; 11 | 12 | protected final double delta_distance; 13 | protected final double delta_speed; 14 | 15 | public Lookahead(double min_distance, double max_distance, double min_speed, double max_speed) { 16 | this.min_distance = min_distance; 17 | this.max_distance = max_distance; 18 | this.min_speed = min_speed; 19 | this.max_speed = max_speed; 20 | delta_distance = max_distance - min_distance; 21 | delta_speed = max_speed - min_speed; 22 | } 23 | 24 | public double getLookaheadForSpeed(double speed) { 25 | double lookahead = delta_distance * (speed - min_speed) / delta_speed + min_distance; 26 | return Double.isNaN(lookahead) ? min_distance : Math.max(min_distance, Math.min(max_distance, lookahead)); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/RocketNearToFeederPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class RocketNearToFeederPath implements PathContainer { 9 | public static final String kLookForTargetMarker = "LOOK_FOR_TARGET"; 10 | 11 | boolean mLeft; 12 | 13 | public RocketNearToFeederPath(boolean left) { 14 | mLeft = left; 15 | } 16 | 17 | @Override 18 | public Path buildPath() { 19 | ArrayList sWaypoints = new ArrayList(); 20 | sWaypoints.add(new Waypoint(135, (mLeft ? 1.0 : -1.0) * 90, 0, 100)); 21 | sWaypoints.add(new Waypoint(105, (mLeft ? 1.0 : -1.0) * 70, 20, 100)); 22 | sWaypoints.add(new Waypoint(55, (mLeft ? 1.0 : -1.0) * 80, 0, 100, kLookForTargetMarker)); 23 | sWaypoints.add(new Waypoint(-20, (mLeft ? 1.0 : -1.0) * 80, 0, 100)); 24 | 25 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 26 | } 27 | 28 | @Override 29 | public boolean isReversed() { 30 | return true; 31 | } 32 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/Action.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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.frc2019.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/frc2019/paths/FeederToCargoShip1Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class FeederToCargoShip1Path implements PathContainer { 8 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 9 | 10 | boolean mLeft; 11 | 12 | public FeederToCargoShip1Path(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(-73, (mLeft ? 1.0 : -1.0) * 80, 0, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(-25, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kTurnTurretMarker)); 21 | sWaypoints.add(new PathBuilder.Waypoint(100, (mLeft ? 1.0 : -1.0) * 40, 40, 100)); 22 | sWaypoints.add(new PathBuilder.Waypoint(195, (mLeft ? 1.0 : -1.0) * 0, 0, 120)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return false; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/FeederToCargoShip3Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class FeederToCargoShip3Path implements PathContainer { 8 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 9 | 10 | boolean mLeft; 11 | 12 | public FeederToCargoShip3Path(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(-73, (mLeft ? 1.0 : -1.0) * 80, 0, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(-25, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kTurnTurretMarker)); 21 | sWaypoints.add(new PathBuilder.Waypoint(100, (mLeft ? 1.0 : -1.0) * 40, 40, 100)); 22 | sWaypoints.add(new PathBuilder.Waypoint(247.5, (mLeft ? 1.0 : -1.0) * 0, 0, 120)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return false; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/CargoShip1ToFeederPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class CargoShip1ToFeederPath implements PathContainer { 8 | public static final String kLookForTargetMarker = "LOOK_FOR_TARGET"; 9 | 10 | boolean mLeft; 11 | 12 | public CargoShip1ToFeederPath(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(205, (mLeft ? 1.0 : -1.0) * 5, 0, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(105, (mLeft ? 1.0 : -1.0) * 40, 35, 100)); 21 | sWaypoints.add(new PathBuilder.Waypoint(55, (mLeft ? 1.0 : -1.0) * 75, 0, 120, kLookForTargetMarker)); 22 | sWaypoints.add(new PathBuilder.Waypoint(-20, (mLeft ? 1.0 : -1.0) * 75, 0, 120)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return true; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/CargoShip2ToFeederPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class CargoShip2ToFeederPath implements PathContainer { 8 | public static final String kLookForTargetMarker = "LOOK_FOR_TARGET"; 9 | 10 | boolean mLeft; 11 | 12 | public CargoShip2ToFeederPath(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(219, (mLeft ? 1.0 : -1.0) * 5, 0, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(105, (mLeft ? 1.0 : -1.0) * 40, 35, 100)); 21 | sWaypoints.add(new PathBuilder.Waypoint(55, (mLeft ? 1.0 : -1.0) * 75, 0, 120, kLookForTargetMarker)); 22 | sWaypoints.add(new PathBuilder.Waypoint(-20, (mLeft ? 1.0 : -1.0) * 75, 0, 120)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return true; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/CargoShip3ToFeederPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class CargoShip3ToFeederPath implements PathContainer { 8 | public static final String kLookForTargetMarker = "LOOK_FOR_TARGET"; 9 | 10 | boolean mLeft; 11 | 12 | public CargoShip3ToFeederPath(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(250, (mLeft ? 1.0 : -1.0) * 5, 0, 120)); 20 | sWaypoints.add(new PathBuilder.Waypoint(105, (mLeft ? 1.0 : -1.0) * 40, 35, 100)); 21 | sWaypoints.add(new PathBuilder.Waypoint(55, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kLookForTargetMarker)); 22 | sWaypoints.add(new PathBuilder.Waypoint(-20, (mLeft ? 1.0 : -1.0) * 80, 0, 120)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return true; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /Top_LL_Hatch_Pipeline.vpr: -------------------------------------------------------------------------------- 1 | area_max:9.487940062500003 2 | area_min:0.03733010410000002 3 | area_similarity:10 4 | aspect_max:0.7496192000000004 5 | aspect_min:0.23718419999999987 6 | black_level:5 7 | blue_balance:1250 8 | calibration_type:0 9 | contour_grouping:1 10 | contour_sort_final:6 11 | convexity_max:100 12 | convexity_min:49.8 13 | corner_approx:6 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:1 26 | exposure:2 27 | hue_max:65 28 | hue_min:55 29 | image_flip:1 30 | image_source:0 31 | img_to_show:0 32 | intersection_filter:1 33 | pipeline_led_enabled:3 34 | pipeline_res:0 35 | pipeline_type:0 36 | red_balance:1250 37 | sat_max:255 38 | sat_min:75 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:75 50 | y_max:1.000000 51 | y_min:-1.000000 52 | -------------------------------------------------------------------------------- /Bottom_LL_Hatch_Pipeline.vpr: -------------------------------------------------------------------------------- 1 | area_max:17.6319369216 2 | area_min:0.10971993760000001 3 | area_similarity:0.000000 4 | aspect_max:3.570125000000001 5 | aspect_min:0.2518890125000001 6 | black_level:10 7 | blue_balance:1250 8 | calibration_type:0 9 | contour_grouping:1 10 | contour_sort_final:6 11 | convexity_max:100 12 | convexity_min:49.8 13 | corner_approx:4 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:Bottom_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:1 26 | exposure:2 27 | hue_max:65 28 | hue_min:55 29 | image_flip:1 30 | image_source:0 31 | img_to_show:0 32 | intersection_filter:1 33 | pipeline_led_enabled:3 34 | pipeline_res:0 35 | pipeline_type:0 36 | red_balance:1250 37 | sat_max:255 38 | sat_min:76 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:58 50 | y_max:1.000000 51 | y_min:-1.000000 52 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/CargoShipFrontToFeederPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class CargoShipFrontToFeederPath implements PathContainer { 8 | public static final String kLookForTargetMarker = "LOOK_FOR_TARGET"; 9 | 10 | boolean mLeft; 11 | 12 | public CargoShipFrontToFeederPath(boolean left) { 13 | mLeft = left; 14 | } 15 | 16 | @Override 17 | public Path buildPath() { 18 | ArrayList sWaypoints = new ArrayList(); 19 | sWaypoints.add(new PathBuilder.Waypoint(142.5, (mLeft ? 1.0 : -1.0) * -45, 0, 0)); 20 | sWaypoints.add(new PathBuilder.Waypoint(100, (mLeft ? 1.0 : -1.0) * 20, 40, 100)); 21 | sWaypoints.add(new PathBuilder.Waypoint(55, (mLeft ? 1.0 : -1.0) * 80, 0, 100, kLookForTargetMarker)); 22 | sWaypoints.add(new PathBuilder.Waypoint(-5, (mLeft ? 1.0 : -1.0) * 80, 0, 100)); 23 | 24 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 25 | } 26 | 27 | @Override 28 | public boolean isReversed() { 29 | return true; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /Bottom_LL_Hatch_Pipeline_Highest.vpr: -------------------------------------------------------------------------------- 1 | area_max:17.6319369216 2 | area_min:0.10971993760000001 3 | area_similarity:0.000000 4 | aspect_max:3.570125000000001 5 | aspect_min:0.2518890125000001 6 | black_level:5 7 | blue_balance:1250 8 | calibration_type:0 9 | contour_grouping:1 10 | contour_sort_final:2 11 | convexity_max:100 12 | convexity_min:49.8 13 | corner_approx:4 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:Bottom_LL_Hatch_Pipeline_Highest 21 | desired_contour_region:0 22 | dilation_steps:1 23 | direction_filter:0 24 | dual_close_sort_origin:0 25 | erosion_steps:1 26 | exposure:2 27 | hue_max:65 28 | hue_min:55 29 | image_flip:1 30 | image_source:0 31 | img_to_show:0 32 | intersection_filter:1 33 | pipeline_led_enabled:1 34 | pipeline_res:0 35 | pipeline_type:0 36 | red_balance:1250 37 | sat_max:255 38 | sat_min:76 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:58 50 | y_max:1.000000 51 | y_min:-1.000000 52 | -------------------------------------------------------------------------------- /Top_LL_Hatch_Pipeline_Highest.vpr: -------------------------------------------------------------------------------- 1 | area_max:9.487940062500003 2 | area_min:0.04543718559999999 3 | area_similarity:0.000000 4 | aspect_max:0.7496192000000004 5 | aspect_min:0.23718419999999987 6 | black_level:5 7 | blue_balance:1250 8 | calibration_type:0 9 | contour_grouping:1 10 | contour_sort_final:2 11 | convexity_max:100 12 | convexity_min:49.8 13 | corner_approx:6 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_Highest 21 | desired_contour_region:0 22 | dilation_steps:1 23 | direction_filter:0 24 | dual_close_sort_origin:0 25 | erosion_steps:1 26 | exposure:2 27 | hue_max:65 28 | hue_min:55 29 | image_flip:1 30 | image_source:0 31 | img_to_show:0 32 | intersection_filter:1 33 | pipeline_led_enabled:3 34 | pipeline_res:0 35 | pipeline_type:0 36 | red_balance:1250 37 | sat_max:255 38 | sat_min:75 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:75 50 | y_max:1.000000 51 | y_min:-1.000000 52 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/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.frc2019; 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 15 | * change 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 | *

If you change your main robot class, change the parameter type. 24 | */ 25 | public static void main(String... args) { 26 | RobotBase.startRobot(Robot::new); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/Hab1ToRocketNearPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class Hab1ToRocketNearPath implements PathContainer { 9 | public static final String kStartAutoAimingMarker = "START_AUTO_AIMING"; 10 | public static final String kStartRaisingElevatorMarker = "START_RAISING_ELEVATOR"; 11 | 12 | boolean mLeft; 13 | 14 | public Hab1ToRocketNearPath(boolean left) { 15 | mLeft = left; 16 | } 17 | 18 | @Override 19 | public Path buildPath() { 20 | ArrayList sWaypoints = new ArrayList(); 21 | sWaypoints.add(new Waypoint(0, 0, 0, 0)); 22 | sWaypoints.add(new Waypoint(40, 0, 0, 40.0, kStartRaisingElevatorMarker)); 23 | sWaypoints.add(new Waypoint(80, (mLeft ? 1.0 : -1.0) * 40, 40, 80, kStartAutoAimingMarker)); 24 | sWaypoints.add(new Waypoint(135, (mLeft ? 1.0 : -1.0) * 90, 0, 80)); 25 | 26 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 27 | } 28 | 29 | @Override 30 | public boolean isReversed() { 31 | return false; 32 | } 33 | } -------------------------------------------------------------------------------- /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/frc2019/paths/FeederToRocketFarPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class FeederToRocketFarPath implements PathContainer { 9 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 10 | public static final String kStartAutoAimingMarker = "START_AUTO_AIMING"; 11 | 12 | boolean mLeft; 13 | 14 | public FeederToRocketFarPath(boolean left) { 15 | mLeft = left; 16 | } 17 | 18 | @Override 19 | public Path buildPath() { 20 | ArrayList sWaypoints = new ArrayList(); 21 | sWaypoints.add(new Waypoint(-73, (mLeft ? 1.0 : -1.0) * 80, 0, 0)); 22 | sWaypoints.add(new Waypoint(-25, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kTurnTurretMarker)); 23 | sWaypoints.add(new Waypoint(165, (mLeft ? 1.0 : -1.0) * 40, 40, 100, kStartAutoAimingMarker)); 24 | sWaypoints.add(new Waypoint(205, (mLeft ? 1.0 : -1.0) * 95, 0, 100)); 25 | 26 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 27 | } 28 | 29 | @Override 30 | public boolean isReversed() { 31 | return false; 32 | } 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/FeederToRocketNearPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.frc2019.paths.PathBuilder.Waypoint; 4 | import com.team254.lib.control.Path; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class FeederToRocketNearPath implements PathContainer { 9 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 10 | public static final String kStartAutoAimingMarker = "START_AUTO_AIMING"; 11 | 12 | boolean mLeft; 13 | 14 | public FeederToRocketNearPath(boolean left) { 15 | mLeft = left; 16 | } 17 | 18 | @Override 19 | public Path buildPath() { 20 | ArrayList sWaypoints = new ArrayList(); 21 | sWaypoints.add(new Waypoint(-73, (mLeft ? 1.0 : -1.0) * 80, 0, 0)); 22 | sWaypoints.add(new Waypoint(-25, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kTurnTurretMarker)); 23 | sWaypoints.add(new Waypoint(100, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kStartAutoAimingMarker)); 24 | sWaypoints.add(new Waypoint(120, (mLeft ? 1.0 : -1.0) * 80, 0, 120)); 25 | 26 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 27 | } 28 | 29 | @Override 30 | public boolean isReversed() { 31 | return false; 32 | } 33 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/ParallelAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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/test/java/com/team254/frc2019/statemachines/SuperstructureStateMachineTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.statemachines; 2 | 3 | import com.team254.frc2019.Constants; 4 | import com.team254.frc2019.states.SuperstructureState; 5 | 6 | import org.junit.Assert; 7 | import org.junit.Test; 8 | 9 | public class SuperstructureStateMachineTest { 10 | /** 11 | * Tests specific SuperstructurePosition setpoints to make sure isOverBumper 12 | * works 13 | * 14 | * @see SuperstructureState 15 | */ 16 | @Test 17 | public void testIsOverBumper() { 18 | SuperstructureState zeroState = new SuperstructureState(Constants.kTurretConstants.kHomePosition, 19 | Constants.kElevatorConstants.kHomePosition, Constants.kArmConstants.kHomePosition, 20 | Constants.kWristConstants.kHomePosition); 21 | 22 | Assert.assertEquals(true, zeroState.isOverBumper()); 23 | Assert.assertEquals(true, SuperstructureCommands.ScoreBallLow.isOverBumper()); 24 | Assert.assertEquals(false, SuperstructureCommands.IntakeBallGroundFront.isOverBumper()); 25 | Assert.assertEquals(true, SuperstructureCommands.StowedDisk.isOverBumper()); 26 | Assert.assertEquals(true, SuperstructureCommands.StowedBall.isOverBumper()); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/paths/FeederToRocketFarScoringPositionPath.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.paths; 2 | 3 | import com.team254.lib.control.Path; 4 | 5 | import java.util.ArrayList; 6 | 7 | public class FeederToRocketFarScoringPositionPath implements PathContainer { 8 | public static final String kTurnTurretMarker = "READY_TO_TURN"; 9 | public static final String kStartAutoAimingMarker = "START_AUTO_AIMING"; 10 | 11 | boolean mLeft; 12 | 13 | public FeederToRocketFarScoringPositionPath(boolean left) { 14 | mLeft = left; 15 | } 16 | 17 | @Override 18 | public Path buildPath() { 19 | ArrayList sWaypoints = new ArrayList(); 20 | sWaypoints.add(new PathBuilder.Waypoint(-73, (mLeft ? 1.0 : -1.0) * 80, 0, 0)); 21 | sWaypoints.add(new PathBuilder.Waypoint(-25, (mLeft ? 1.0 : -1.0) * 80, 0, 120, kTurnTurretMarker)); 22 | sWaypoints.add(new PathBuilder.Waypoint(165, (mLeft ? 1.0 : -1.0) * 40, 40, 100, kStartAutoAimingMarker)); 23 | sWaypoints.add(new PathBuilder.Waypoint(213.5, (mLeft ? 1.0 : -1.0) * 80, 0, 100)); 24 | 25 | return PathBuilder.buildPathFromWaypoints(sWaypoints); 26 | } 27 | 28 | @Override 29 | public boolean isReversed() { 30 | return false; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /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/drivers/LazySparkMax.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.revrobotics.CANError; 4 | import com.revrobotics.CANSparkMax; 5 | import com.revrobotics.ControlType; 6 | 7 | /** 8 | * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU 9 | * overhead by skipping duplicate set commands. 10 | */ 11 | public class LazySparkMax extends CANSparkMax { 12 | protected double mLastSet = Double.NaN; 13 | protected ControlType mLastControlType = null; 14 | 15 | // Set if is a follower 16 | protected CANSparkMax mLeader = null; 17 | 18 | public LazySparkMax(int deviceNumber) { 19 | super(deviceNumber, MotorType.kBrushless); 20 | } 21 | 22 | public CANSparkMax getLeader() { 23 | return mLeader; 24 | } 25 | 26 | @Override 27 | public CANError follow(final CANSparkMax leader) { 28 | mLeader = leader; 29 | return super.follow(leader); 30 | } 31 | 32 | /** 33 | * wrapper method to mimic TalonSRX set method 34 | */ 35 | public void set(ControlType type, double setpoint) { 36 | if (setpoint != mLastSet || type != mLastControlType) { 37 | mLastSet = setpoint; 38 | mLastControlType = type; 39 | super.getPIDController().setReference(setpoint, type); 40 | } 41 | } 42 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/subsystems/StingerRoller.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 5 | import com.team254.frc2019.Constants; 6 | import com.team254.lib.drivers.TalonSRXFactory; 7 | 8 | /** 9 | * Controls the stinger roller for the robot's climbing mechanism at SFR and SVR 10 | */ 11 | public class StingerRoller extends Subsystem { 12 | private static StingerRoller mInstance; 13 | 14 | public synchronized static StingerRoller getInstance() { 15 | if (mInstance == null) { 16 | mInstance = new StingerRoller(); 17 | } 18 | 19 | return mInstance; 20 | } 21 | 22 | TalonSRX mMaster; 23 | 24 | private StingerRoller() { 25 | mMaster = TalonSRXFactory.createDefaultTalon(Constants.kStingerMasterId); 26 | mMaster.overrideSoftLimitsEnable(false); 27 | } 28 | 29 | public synchronized void setOpenLoop(double output) { 30 | mMaster.set(ControlMode.PercentOutput, output); 31 | } 32 | 33 | @Override 34 | public synchronized void stop() { 35 | setOpenLoop(0.0); 36 | } 37 | 38 | @Override 39 | public boolean checkSystem() { 40 | return true; 41 | } 42 | 43 | @Override 44 | public void outputTelemetry() {} 45 | } 46 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/states/LEDState.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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 kIntakeIntakingDisk = new LEDState(0.0, 1.0, 0.0); 7 | public static final LEDState kIntakeIntakingCargo = new LEDState(1.0, 1.0, 1.0); 8 | 9 | public static final LEDState kIntakeExhuasting = new LEDState(1.0, 0.0, 0.0); 10 | 11 | public static final LEDState kRobotZeroed = new LEDState(0.0, 1.0, 0.0); 12 | public static final LEDState kFault = new LEDState(0.0, 0.0, 1.0); 13 | public static final LEDState kFaultElevator = new LEDState(1.0, 0.0, 1.0); 14 | 15 | public static final LEDState kHanging = new LEDState(0.0, 0.3, 1.0); 16 | public static final LEDState kMinimalPressure = new LEDState(0.0, 1.0, 1.0); 17 | public static final LEDState kOptimalPressure = new LEDState(1.0, 0.0, 0.0); 18 | 19 | public LEDState() {} 20 | 21 | public LEDState(double b, double g, double r) { 22 | blue = b; 23 | green = g; 24 | red = r; 25 | } 26 | 27 | public void copyFrom(LEDState other) { 28 | this.blue = other.blue; 29 | this.green = other.green; 30 | this.red = other.red; 31 | } 32 | 33 | public double blue; 34 | public double green; 35 | public double red; 36 | } 37 | -------------------------------------------------------------------------------- /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 the brake mode is enabled. 5 | */ 6 | public class DriveSignal { 7 | private final double mLeftMotor; 8 | private final double mRightMotor; 9 | private final boolean mBrakeMode; 10 | 11 | public DriveSignal(double left, double right) { 12 | this(left, right, false); 13 | } 14 | 15 | public DriveSignal(double left, double right, boolean brakeMode) { 16 | mLeftMotor = left; 17 | mRightMotor = right; 18 | mBrakeMode = brakeMode; 19 | } 20 | 21 | public static DriveSignal fromControls(double throttle, double turn) { 22 | return new DriveSignal(throttle - turn, throttle + turn); 23 | } 24 | 25 | public static final DriveSignal NEUTRAL = new DriveSignal(0, 0); 26 | public static final DriveSignal BRAKE = new DriveSignal(0, 0, true); 27 | 28 | public double getLeft() { 29 | return mLeftMotor; 30 | } 31 | 32 | public double getRight() { 33 | return mRightMotor; 34 | } 35 | 36 | public boolean getBrakeMode() { 37 | return mBrakeMode; 38 | } 39 | 40 | @Override 41 | public String toString() { 42 | return "L: " + mLeftMotor + ", R: " + mRightMotor + (mBrakeMode ? ", BRAKE" : ""); 43 | } 44 | } -------------------------------------------------------------------------------- /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/frc2019/auto/actions/SeriesAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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 | } 53 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/controlboard/GamepadDriveControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.controlboard; 2 | 3 | import com.team254.frc2019.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 false; 35 | } 36 | 37 | @Override 38 | public boolean getThrust() { 39 | return false; 40 | } 41 | 42 | @Override 43 | public boolean getQuickTurn() { 44 | return mController.getTrigger(XboxController.Side.LEFT); 45 | } 46 | 47 | @Override 48 | public boolean getShoot() { 49 | return false; 50 | } 51 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/subsystems/Subsystem.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.team254.frc2019.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/frc2019/states/SuperstructureGoal.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.states; 2 | 3 | /** 4 | * Represents a goal for the superstructure 5 | */ 6 | public class SuperstructureGoal { 7 | public final SuperstructureState state; 8 | 9 | public SuperstructureGoal(double turret, double elevator, double shoulder, double wrist) { 10 | this(new SuperstructureState(turret, elevator, shoulder, wrist)); 11 | } 12 | 13 | public SuperstructureGoal(SuperstructureState state) { 14 | this.state = new SuperstructureState(state); 15 | } 16 | 17 | public boolean equals(SuperstructureGoal other) { 18 | return this.state.turret == other.state.turret && 19 | this.state.elevator == other.state.elevator && 20 | this.state.shoulder == other.state.shoulder && 21 | this.state.wrist == other.state.wrist; 22 | } 23 | 24 | public boolean isAtDesiredState(SuperstructureState currentState) { 25 | double[] distances = { 26 | currentState.turret - state.turret, 27 | currentState.elevator - state.elevator, 28 | currentState.shoulder - state.shoulder, 29 | currentState.wrist - state.wrist 30 | }; 31 | 32 | for (int i = 0; i < distances.length; i++) { 33 | if (Math.abs(distances[i]) > SuperstructureConstants.kPadding[i]) { 34 | return false; 35 | } 36 | } 37 | 38 | return true; 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/test/java/com/team254/frc2019/planners/PlannerTestUtil.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.planners; 2 | 3 | import com.team254.frc2019.states.SuperstructureState; 4 | 5 | import org.hamcrest.Description; 6 | import org.hamcrest.Matcher; 7 | import org.hamcrest.Matchers; 8 | import org.hamcrest.TypeSafeMatcher; 9 | 10 | public class PlannerTestUtil { 11 | public static Matcher near(final double expected) { 12 | return new TypeSafeMatcher<>() { 13 | private static final double EPS = 0.0001; 14 | 15 | @Override 16 | public void describeTo(Description description) { 17 | description.appendValue(expected); 18 | } 19 | 20 | @Override 21 | protected boolean matchesSafely(Double item) { 22 | return Math.abs(item - expected) < EPS; 23 | } 24 | }; 25 | } 26 | 27 | public static Matcher near(SuperstructureState state) { 28 | return near(state.turret, state.elevator, state.shoulder, state.wrist); 29 | } 30 | 31 | public static Matcher near(double turret, double elevator, double shoulder, double wrist) { 32 | return stateIs(near(turret), near(elevator), near(shoulder), near(wrist)); 33 | } 34 | 35 | public static Matcher stateIs(Matcher turret, Matcher elevator, Matcher shoulder, Matcher wrist) { 36 | return Matchers.arrayContaining(turret, elevator, shoulder, wrist); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /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/frc2019/auto/actions/DrivePathAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.paths.PathContainer; 4 | import com.team254.frc2019.subsystems.Drive; 5 | import com.team254.lib.control.Path; 6 | import com.team254.lib.util.DriveSignal; 7 | 8 | /** 9 | * Drives the robot along the Path defined in the PathContainer object. The action finishes once the robot reaches the 10 | * end of the path. 11 | * 12 | * @see PathContainer 13 | * @see Path 14 | * @see Action 15 | */ 16 | public class DrivePathAction implements Action { 17 | 18 | private PathContainer mPathContainer; 19 | private Path mPath; 20 | private Drive mDrive = Drive.getInstance(); 21 | private boolean mStopWhenDone; 22 | 23 | public DrivePathAction(PathContainer p, boolean stopWhenDone) { 24 | mPathContainer = p; 25 | mPath = mPathContainer.buildPath(); 26 | mStopWhenDone = stopWhenDone; 27 | } 28 | 29 | public DrivePathAction(PathContainer p) { 30 | this(p, false); 31 | } 32 | 33 | @Override 34 | public void start() { 35 | mDrive.setWantDrivePath(mPath, mPathContainer.isReversed()); 36 | } 37 | 38 | @Override 39 | public void update() {} 40 | 41 | @Override 42 | public boolean isFinished() { 43 | return mDrive.isDoneWithPath(); 44 | } 45 | 46 | @Override 47 | public void done() { 48 | if (mStopWhenDone) { 49 | mDrive.setVelocity(new DriveSignal(0, 0), new DriveSignal(0, 0)); 50 | } 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/controlboard/MainDriveControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.controlboard; 2 | 3 | import com.team254.frc2019.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 getShoot() { 42 | return mTurnStick.getRawButton(2); 43 | } 44 | 45 | @Override 46 | public boolean getWantsLowGear() { 47 | return mThrottleStick.getRawButton(2); 48 | } 49 | 50 | public boolean getThrust() { 51 | return mThrottleStick.getRawButton(1); 52 | } 53 | } -------------------------------------------------------------------------------- /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/frc2019/auto/modes/CharacterizeDrivebaseMode.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.modes; 2 | 3 | import com.team254.frc2019.auto.AutoModeEndedException; 4 | import com.team254.frc2019.auto.actions.CollectAccelerationDataAction; 5 | import com.team254.frc2019.auto.actions.CollectVelocityDataAction; 6 | import com.team254.frc2019.auto.actions.WaitAction; 7 | import com.team254.lib.physics.DriveCharacterization; 8 | 9 | import java.util.ArrayList; 10 | import java.util.List; 11 | 12 | public class CharacterizeDrivebaseMode extends AutoModeBase { 13 | private final boolean highGear; 14 | private final boolean reverse; 15 | private final boolean turn; 16 | 17 | public CharacterizeDrivebaseMode(boolean highGear, boolean reverse, boolean turn) { 18 | this.highGear = highGear; 19 | this.reverse = reverse; 20 | this.turn = turn; 21 | } 22 | 23 | @Override 24 | protected void routine() throws AutoModeEndedException { 25 | List velocityData = new ArrayList<>(); 26 | List accelerationData = new ArrayList<>(); 27 | 28 | runAction(new CollectVelocityDataAction(velocityData, highGear, reverse, turn)); 29 | runAction(new WaitAction(10)); 30 | runAction(new CollectAccelerationDataAction(accelerationData, highGear, reverse, turn)); 31 | 32 | DriveCharacterization.CharacterizationConstants constants = DriveCharacterization.characterizeDrive(velocityData, accelerationData); 33 | 34 | System.out.println("ks: " + constants.ks); 35 | System.out.println("kv: " + constants.kv); 36 | System.out.println("ka: " + constants.ka); 37 | } 38 | } -------------------------------------------------------------------------------- /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/test/java/com/team254/lib/physics/DriveCharacterizationTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.physics; 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 | import java.util.ArrayList; 9 | import java.util.List; 10 | 11 | @RunWith(JUnit4.class) 12 | public class DriveCharacterizationTest { 13 | public static final double kTestEpsilon = 1e-2; 14 | 15 | @Test 16 | public void test() { 17 | final double ks = 0.75; //Math.random(); 18 | final double kv = 0.2; //Math.random(); 19 | final double ka = 0.15; //Math.random(); 20 | 21 | List velocityData = new ArrayList<>(); 22 | // generate velocity data points 23 | for (double v = 0; v < 1.0; v += 0.01) { 24 | velocityData.add(new DriveCharacterization.DataPoint(Math.max(0.0, (v - ks) / kv), v, v)); 25 | } 26 | 27 | List accelerationData = new ArrayList<>(); 28 | double v, a; 29 | v = 0; 30 | // generate acceleration data points 31 | for (int i = 0; i < 1000; ++i) { 32 | a = Math.max(0.0, 6.0 - kv * v - ks) / ka; 33 | v += a * kTestEpsilon; 34 | accelerationData.add(new DriveCharacterization.DataPoint(v, 6.0, kTestEpsilon * i)); 35 | } 36 | 37 | DriveCharacterization.CharacterizationConstants driveConstants = DriveCharacterization.characterizeDrive(velocityData, accelerationData); 38 | 39 | Assert.assertEquals(driveConstants.ks, ks, kTestEpsilon); 40 | Assert.assertEquals(driveConstants.kv, kv, kTestEpsilon); 41 | Assert.assertEquals(driveConstants.ka, ka, kTestEpsilon); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /vendordeps/REVRobotics.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "REVRobotics.json", 3 | "name": "REVRobotics", 4 | "version": "1.1.9", 5 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 6 | "mavenUrls": [ 7 | "http://www.revrobotics.com/content/sw/max/sdk/maven/" 8 | ], 9 | "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.revrobotics.frc", 13 | "artifactId": "SparkMax-java", 14 | "version": "1.1.9" 15 | } 16 | ], 17 | "jniDependencies": [ 18 | { 19 | "groupId": "com.revrobotics.frc", 20 | "artifactId": "SparkMax-driver", 21 | "version": "1.1.9", 22 | "skipInvalidPlatforms": true, 23 | "isJar": false, 24 | "validPlatforms": [ 25 | "linuxathena" 26 | ] 27 | } 28 | ], 29 | "cppDependencies": [ 30 | { 31 | "groupId": "com.revrobotics.frc", 32 | "artifactId": "SparkMax-cpp", 33 | "version": "1.1.9", 34 | "libName": "libSparkMax", 35 | "headerClassifier": "headers", 36 | "sharedLibrary": false, 37 | "skipInvalidPlatforms": true, 38 | "binaryPlatforms": [ 39 | "linuxathena" 40 | ] 41 | }, 42 | { 43 | "groupId": "com.revrobotics.frc", 44 | "artifactId": "SparkMax-driver", 45 | "version": "1.1.9", 46 | "libName": "libSparkMaxDriver", 47 | "headerClassifier": "headers", 48 | "sharedLibrary": false, 49 | "skipInvalidPlatforms": true, 50 | "binaryPlatforms": [ 51 | "linuxathena" 52 | ] 53 | } 54 | ] 55 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/controlboard/XboxController.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.controlboard; 2 | 3 | import com.team254.frc2019.Constants; 4 | import edu.wpi.first.wpilibj.GenericHID.RumbleType; 5 | import edu.wpi.first.wpilibj.Joystick; 6 | 7 | public class XboxController { 8 | private final Joystick mController; 9 | 10 | public enum Side { 11 | LEFT, RIGHT 12 | } 13 | 14 | public enum Axis { 15 | X, Y 16 | } 17 | 18 | public enum Button { 19 | A(1), B(2), X(3), Y(4), LB(5), RB(6), BACK(7), START(8), L_JOYSTICK(9), R_JOYSTICK(10); 20 | 21 | public final int id; 22 | 23 | Button(int id) { 24 | this.id = id; 25 | } 26 | } 27 | 28 | XboxController(int port) { 29 | mController = new Joystick(port); 30 | } 31 | 32 | double getJoystick(Side side, Axis axis) { 33 | double deadband = Constants.kJoystickThreshold; 34 | 35 | boolean left = side == Side.LEFT; 36 | boolean y = axis == Axis.Y; 37 | // multiplies by -1 if y-axis (inverted normally) 38 | return handleDeadband((y ? -1 : 1) * mController.getRawAxis((left ? 0 : 4) + (y ? 1 : 0)), deadband); 39 | } 40 | 41 | boolean getTrigger(Side side) { 42 | return mController.getRawAxis(side == Side.LEFT ? 2 : 3) > Constants.kJoystickThreshold; 43 | } 44 | 45 | boolean getButton(Button button) { 46 | return mController.getRawButton(button.id); 47 | } 48 | 49 | int getDPad() { 50 | return mController.getPOV(); 51 | } 52 | 53 | public void setRumble(boolean on) { 54 | mController.setRumble(RumbleType.kRightRumble, on ? 1 : 0); 55 | } 56 | 57 | private double handleDeadband(double value, double deadband) { 58 | return (Math.abs(value) > Math.abs(deadband)) ? value : 0; 59 | } 60 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/planners/AvoidDriveBasePlanner.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.planners; 2 | 3 | import com.team254.frc2019.statemachines.SuperstructureCommands; 4 | import com.team254.frc2019.states.SuperstructureConstants; 5 | import com.team254.frc2019.states.SuperstructureGoal; 6 | import com.team254.lib.util.Util; 7 | 8 | /** 9 | * Superstructure motion planner meant to avoid colliding with the drivebase 10 | */ 11 | public class AvoidDriveBasePlanner implements ISuperstructureMotionPlanner { 12 | public boolean isValidGoal(SuperstructureGoal goal) { 13 | return goal.state.isOverBumper() || goal.state.isTurretSafeForWristBelowBumper(); 14 | } 15 | 16 | @Override 17 | public SuperstructureGoal plan(SuperstructureGoal prevSetpoint, SuperstructureGoal goal) { 18 | boolean turretWantsMove = !Util.epsilonEquals(prevSetpoint.state.turret, goal.state.turret, 19 | SuperstructureConstants.kTurretPaddingDegrees); 20 | 21 | if (turretWantsMove) { 22 | if (!prevSetpoint.state.isOverBumper()) { 23 | // Move towards a stowed position until I'm safe to turn. 24 | SuperstructureGoal result = new SuperstructureGoal(SuperstructureCommands.tuckedPosition); 25 | result.state.turret = prevSetpoint.state.turret; 26 | return result; 27 | } 28 | 29 | // Used at the Silicon Valley Regional; not applicable for champs robot 30 | // else if (!goal.state.isOverBumper()) { 31 | // // Do not move down towards the setpoint until the turret is done 32 | // SuperstructureGoal result = new SuperstructureGoal(SuperstructureCommands.tuckedPosition); 33 | // result.state.turret = goal.state.turret; 34 | // return result; 35 | // } 36 | } 37 | 38 | // No restrictions needed. 39 | return new SuperstructureGoal(goal.state); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /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 | } -------------------------------------------------------------------------------- /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 robot_to_goal; 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 Rotation2d field_to_vision_target_normal; 14 | private final int track_id; 15 | 16 | public AimingParameters(Pose2d robot_to_goal, 17 | Pose2d field_to_goal, 18 | Rotation2d field_to_vision_target_normal, double last_seen_timestamp, 19 | double stability, int track_id) { 20 | this.robot_to_goal = robot_to_goal; 21 | this.field_to_vision_target_normal = field_to_vision_target_normal; 22 | this.field_to_goal = field_to_goal; 23 | this.range = robot_to_goal.getTranslation().norm(); 24 | this.robot_to_goal_rotation = robot_to_goal.getTranslation().direction(); 25 | this.last_seen_timestamp = last_seen_timestamp; 26 | this.stability = stability; 27 | this.track_id = track_id; 28 | } 29 | 30 | public Pose2d getRobotToGoal() { 31 | return robot_to_goal; 32 | } 33 | 34 | public Pose2d getFieldToGoal() { 35 | return field_to_goal; 36 | } 37 | 38 | public double getRange() { 39 | return range; 40 | } 41 | 42 | public Rotation2d getRobotToGoalRotation() { 43 | return robot_to_goal_rotation; 44 | } 45 | 46 | public double getLastSeenTimestamp() { 47 | return last_seen_timestamp; 48 | } 49 | 50 | public double getStability() { 51 | return stability; 52 | } 53 | 54 | public Rotation2d getFieldToVisionTargetNormal() { 55 | return field_to_vision_target_normal; 56 | } 57 | 58 | public int getTrackId() { 59 | return track_id; 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/SparkMaxChecker.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.revrobotics.CANSparkMax; 4 | import com.revrobotics.ControlType; 5 | import com.team254.frc2019.subsystems.Subsystem; 6 | 7 | import java.util.ArrayList; 8 | 9 | public class SparkMaxChecker extends MotorChecker { 10 | private static class StoredSparkConfiguration { 11 | CANSparkMax leader = null; 12 | } 13 | 14 | protected ArrayList mStoredConfigurations = new ArrayList<>(); 15 | 16 | public static boolean checkMotors(Subsystem subsystem, 17 | ArrayList> motorsToCheck, 18 | CheckerConfig checkerConfig) { 19 | SparkMaxChecker checker = new SparkMaxChecker(); 20 | return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig); 21 | } 22 | 23 | @Override 24 | protected void storeConfiguration() { 25 | // record previous configuration for all talons 26 | for (MotorConfig config : mMotorsToCheck) { 27 | LazySparkMax spark = (LazySparkMax) config.mMotor; 28 | 29 | StoredSparkConfiguration configuration = new StoredSparkConfiguration(); 30 | configuration.leader = spark.getLeader(); 31 | 32 | mStoredConfigurations.add(configuration); 33 | spark.restoreFactoryDefaults(); 34 | } 35 | } 36 | 37 | @Override 38 | protected void restoreConfiguration() { 39 | for (int i = 0; i < mMotorsToCheck.size(); ++i) { 40 | if (mStoredConfigurations.get(i).leader != null) { 41 | mMotorsToCheck.get(i).mMotor.follow(mStoredConfigurations.get(i).leader); 42 | } 43 | } 44 | } 45 | 46 | @Override 47 | protected void setMotorOutput(CANSparkMax motor, double output) { 48 | motor.getPIDController().setReference(output, ControlType.kDutyCycle); 49 | } 50 | 51 | @Override 52 | protected double getMotorCurrent(CANSparkMax motor) { 53 | return motor.getOutputCurrent(); 54 | } 55 | } 56 | 57 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/TalonSRXChecker.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 5 | import com.team254.frc2019.subsystems.Subsystem; 6 | 7 | import java.util.ArrayList; 8 | 9 | public class TalonSRXChecker 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 | TalonSRXChecker checker = new TalonSRXChecker(); 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(TalonSRX motor, double output) { 48 | motor.set(ControlMode.PercentOutput, output); 49 | } 50 | 51 | @Override 52 | protected double getMotorCurrent(TalonSRX motor) { 53 | return motor.getOutputCurrent(); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/AutoModeExecutor.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto; 2 | 3 | import com.team254.frc2019.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 | } 88 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/subsystems/Wrist.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice; 4 | import com.ctre.phoenix.motorcontrol.RemoteSensorSource; 5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 6 | 7 | import com.team254.frc2019.Constants; 8 | import com.team254.lib.drivers.MotorChecker; 9 | import com.team254.lib.drivers.TalonSRXChecker; 10 | import com.team254.lib.drivers.TalonSRXUtil; 11 | 12 | import java.util.ArrayList; 13 | 14 | public class Wrist extends ServoMotorSubsystem { 15 | private static Wrist mInstance; 16 | 17 | public synchronized static Wrist getInstance() { 18 | if (mInstance == null) { 19 | mInstance = new Wrist(Constants.kWristConstants); 20 | } 21 | 22 | return mInstance; 23 | } 24 | 25 | private Wrist(final ServoMotorSubsystemConstants constants) { 26 | super(constants); 27 | 28 | TalonSRXUtil.checkError(mMaster.configRemoteFeedbackFilter(Constants.kCanifierWristId, RemoteSensorSource.CANifier_Quadrature, 29 | 0, Constants.kLongCANTimeoutMs), 30 | "Could not set wrist encoder!!!: "); 31 | 32 | TalonSRXUtil.checkError(mMaster.configSelectedFeedbackSensor( 33 | RemoteFeedbackDevice.RemoteSensor0, 0, Constants.kLongCANTimeoutMs), 34 | "Could not detect wrist encoder: "); 35 | } 36 | 37 | // Syntactic sugar. 38 | public synchronized double getAngle() { 39 | return getPosition(); 40 | } 41 | 42 | @Override 43 | public boolean checkSystem() { 44 | return TalonSRXChecker.checkMotors(this, 45 | new ArrayList>() { 46 | private static final long serialVersionUID = -716113039054569446L; 47 | 48 | { 49 | add(new MotorChecker.MotorConfig<>("master", mMaster)); 50 | } 51 | }, new MotorChecker.CheckerConfig() { 52 | { 53 | mRunOutputPercentage = 0.5; 54 | mRunTimeSec = 1.0; 55 | mCurrentFloor = 0.1; 56 | mRPMFloor = 90; 57 | mCurrentEpsilon = 2.0; 58 | mRPMEpsilon = 200; 59 | mRPMSupplier = () -> mMaster.getSelectedSensorVelocity(); 60 | } 61 | }); 62 | } 63 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/subsystems/Infrastructure.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.team254.frc2019.Constants; 4 | import com.team254.frc2019.loops.ILooper; 5 | import com.team254.frc2019.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 mIsManualControl = 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.isAtDesiredState(); 39 | 40 | if (superstructureMoving || !mIsManualControl) { 41 | stopCompressor(); 42 | } else { 43 | startCompressor(); 44 | } 45 | } 46 | } 47 | 48 | @Override 49 | public void onStop(double timestamp) {} 50 | }); 51 | } 52 | 53 | public synchronized void setIsManualControl(boolean isManualControl) { 54 | mIsManualControl = isManualControl; 55 | 56 | if (mIsManualControl) { 57 | startCompressor(); 58 | } 59 | } 60 | 61 | public synchronized boolean isManualControl() { 62 | return mIsManualControl; 63 | } 64 | 65 | private void startCompressor() { 66 | mCompressor.start(); 67 | } 68 | 69 | private void stopCompressor() { 70 | mCompressor.stop(); 71 | } 72 | 73 | @Override 74 | public void stop() {} 75 | 76 | @Override 77 | public boolean checkSystem() { 78 | return false; 79 | } 80 | 81 | @Override 82 | public void outputTelemetry() {} 83 | } 84 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/AutoSteerAndIntakeAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.statemachines.EndEffectorStateMachine; 4 | import com.team254.frc2019.statemachines.SuperstructureCommands; 5 | import com.team254.frc2019.subsystems.Drive; 6 | import com.team254.frc2019.subsystems.EndEffector; 7 | import com.team254.frc2019.subsystems.Superstructure; 8 | import com.team254.lib.geometry.Rotation2d; 9 | import com.team254.lib.util.DelayedBoolean; 10 | import com.team254.lib.util.DriveSignal; 11 | import com.team254.lib.vision.AimingParameters; 12 | 13 | import edu.wpi.first.wpilibj.Timer; 14 | 15 | import java.util.Optional; 16 | 17 | public class AutoSteerAndIntakeAction implements Action { 18 | private static final Drive mDrive = Drive.getInstance(); 19 | 20 | private DelayedBoolean mHasDisk = null; 21 | private boolean mFinished = false; 22 | private boolean mIdleWhenDone = false; 23 | private boolean mReverse = false; 24 | 25 | private final double kThrottle = 0.3; 26 | 27 | public AutoSteerAndIntakeAction(boolean reverse, boolean idleWhenDone) { 28 | mIdleWhenDone = idleWhenDone; 29 | mReverse = reverse; 30 | } 31 | 32 | @Override 33 | public void start() { 34 | mHasDisk = new DelayedBoolean(Timer.getFPGATimestamp(), 0.05); 35 | SuperstructureCommands.goToPickupDiskFromWallFront(); 36 | EndEffector.getInstance().setWantedAction(EndEffectorStateMachine.WantedAction.INTAKE_DISK); 37 | Superstructure.getInstance().setWantAutoAim(Rotation2d.fromDegrees(180.0), 38 | true, 50); 39 | } 40 | 41 | @Override 42 | public void update() { 43 | Optional aimParams = Superstructure.getInstance().getLatestAimingParameters(); 44 | if (aimParams.isEmpty() || EndEffector.getInstance().hasDisk()) { 45 | mDrive.setOpenLoop(DriveSignal.BRAKE); 46 | } else { 47 | mDrive.autoSteer(kThrottle * (mReverse ? -1.0 : 1.0), aimParams.get()); 48 | } 49 | mFinished = mHasDisk.update(Timer.getFPGATimestamp(), EndEffector.getInstance().hasDisk()); 50 | } 51 | 52 | @Override 53 | public boolean isFinished() { 54 | return mFinished; 55 | } 56 | 57 | @Override 58 | public void done() { 59 | if (mIdleWhenDone) { 60 | EndEffector.getInstance().setWantedAction(EndEffectorStateMachine.WantedAction.IDLE); 61 | } 62 | Superstructure.getInstance().setWantRobotRelativeTurret(); 63 | } 64 | } -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS="-Xmx64m" 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 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/frc2019/Kinematics.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019; 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/frc2019/controlboard/IButtonControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.controlboard; 2 | 3 | import com.team254.lib.geometry.Rotation2d; 4 | 5 | public interface IButtonControlBoard { 6 | void reset(); 7 | 8 | double getJogTurret(); 9 | 10 | boolean getScorePresetLow(); 11 | boolean getScorePresetMiddle(); 12 | boolean getScorePresetHigh(); 13 | boolean getScorePresetCargo(); 14 | 15 | boolean getPresetStow(); 16 | boolean getPickupDiskWall(); 17 | boolean getPickupBallGround(); 18 | 19 | void setRumble(boolean on); 20 | 21 | // Climbing 22 | boolean getToggleHangMode(); 23 | boolean getToggleHangModeLow(); 24 | double getElevatorThrottle(); 25 | 26 | double getJoggingX(); 27 | double getJoggingZ(); 28 | 29 | // Turret 30 | enum TurretCardinal { 31 | BACK(180), 32 | FRONT(0), 33 | LEFT(90), 34 | RIGHT(-90), 35 | NONE(0), 36 | FRONT_LEFT(30, 45), 37 | FRONT_RIGHT(-30, -45), 38 | BACK_LEFT(150, 135), 39 | BACK_RIGHT(210, 235); 40 | 41 | public final Rotation2d rotation; 42 | private final Rotation2d inputDirection; 43 | 44 | TurretCardinal(double degrees) { 45 | this(degrees, degrees); 46 | } 47 | 48 | TurretCardinal(double degrees, double inputDirectionDegrees) { 49 | rotation = Rotation2d.fromDegrees(degrees); 50 | inputDirection = Rotation2d.fromDegrees(inputDirectionDegrees); 51 | } 52 | 53 | public static TurretCardinal findClosest(double xAxis, double yAxis) { 54 | return findClosest(new Rotation2d(yAxis, -xAxis, true)); 55 | } 56 | 57 | public static TurretCardinal findClosest(Rotation2d stickDirection) { 58 | var values = TurretCardinal.values(); 59 | 60 | TurretCardinal closest = null; 61 | double closestDistance = Double.MAX_VALUE; 62 | for (int i = 0; i < values.length; i++) { 63 | var checkDirection = values[i]; 64 | var distance = Math.abs(stickDirection.distance(checkDirection.inputDirection)); 65 | if (distance < closestDistance) { 66 | closestDistance = distance; 67 | closest = checkDirection; 68 | } 69 | } 70 | return closest; 71 | } 72 | 73 | public static boolean isDiagonal(TurretCardinal cardinal) { 74 | return cardinal == FRONT_LEFT || cardinal == FRONT_RIGHT || cardinal == BACK_LEFT || cardinal == BACK_RIGHT; 75 | } 76 | } 77 | 78 | TurretCardinal getTurretCardinal(); 79 | 80 | boolean getAutoAim(); 81 | } -------------------------------------------------------------------------------- /src/test/java/com/team254/frc2019/subsystems/LimelightTest.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.team254.lib.util.Util; 4 | 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 | /** 13 | * Class that tests the system test 14 | */ 15 | @RunWith(JUnit4.class) 16 | public class LimelightTest { 17 | 18 | @Test 19 | public void testExtractTopCornersFromBoundingBoxes() { 20 | List topCorners1 = Limelight.extractTopCornersFromBoundingBoxes( 21 | new double[]{94.0, 91.0, 98.0, 101.0, 136.0, 140.0, 147.0, 142.0}, 22 | new double[]{65.0, 85.0, 87.0, 66.0, 66.0, 86.0, 85.0, 65.0}); 23 | Assert.assertArrayEquals(new double[]{94.0, 65.0}, topCorners1.get(0), Util.kEpsilon); 24 | Assert.assertArrayEquals(new double[]{142.0, 65.0}, topCorners1.get(1), Util.kEpsilon); 25 | 26 | List topCorners2 = Limelight.extractTopCornersFromBoundingBoxes( 27 | new double[]{118.0, 108.0, 127.0, 135.0, 208.0, 215.0, 229.0, 223.0}, 28 | new double[]{151.0, 193.0, 191.0, 149.0, 140.0, 179.0, 174.0, 136.0}); 29 | Assert.assertArrayEquals(new double[]{118.0, 151.0}, topCorners2.get(0), Util.kEpsilon); 30 | Assert.assertArrayEquals(new double[]{223.0, 136.0}, topCorners2.get(1), Util.kEpsilon); 31 | 32 | List topCorners3 = Limelight.extractTopCornersFromBoundingBoxes( 33 | new double[]{118.0, 108.0, 127.0, 135.0, 208.0, 215.0, 229.0, 223.0}, 34 | new double[]{151.0, 193.0, 191.0, 149.0, 140.0, 179.0, 174.0, 136.0}); 35 | Assert.assertArrayEquals(new double[]{118.0, 151.0}, topCorners3.get(0), Util.kEpsilon); 36 | Assert.assertArrayEquals(new double[]{223.0, 136.0}, topCorners3.get(1), Util.kEpsilon); 37 | 38 | List topCorners4 = Limelight.extractTopCornersFromBoundingBoxes( 39 | new double[]{114.0, 102.0, 108.0, 121.0, 186.0, 200.0, 209.0, 194.0}, 40 | new double[]{161.0, 156.0, 123.0, 126.0, 135.0, 134.0, 175.0, 175.0}); 41 | Assert.assertArrayEquals(new double[]{108.0, 123.0}, topCorners4.get(0), Util.kEpsilon); 42 | Assert.assertArrayEquals(new double[]{200.0, 134.0}, topCorners4.get(1), Util.kEpsilon); 43 | 44 | List topCorners5 = Limelight.extractTopCornersFromBoundingBoxes( 45 | new double[]{88, 86, 91, 94, 123, 128, 134, 129}, 46 | new double[]{42, 61, 62, 44, 46, 67, 67, 46}); 47 | Assert.assertArrayEquals(new double[]{88, 42.0}, topCorners5.get(0), Util.kEpsilon); 48 | Assert.assertArrayEquals(new double[]{129, 46.0}, topCorners5.get(1), Util.kEpsilon); 49 | } 50 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/util/Util.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | import com.team254.lib.geometry.Rotation2d; 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 | public static double toTurretSafeAngleDegrees(Rotation2d rotation2d) { 77 | double result = rotation2d.getDegrees() % 360.0; 78 | if (result > 270) { 79 | result -= 360; 80 | } else if (result < -90) { 81 | result += 360; 82 | } 83 | return result; 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /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/frc2019/subsystems/Kickstand.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.team254.frc2019.Constants; 4 | 5 | import edu.wpi.first.wpilibj.DoubleSolenoid; 6 | 7 | /** 8 | * Deploys and retracts the climbing mechanism, through the kickstand solenoid, 9 | * and the rachet system, which makes sure the robot does not fall down while 10 | * climbing 11 | */ 12 | public class Kickstand extends Subsystem { 13 | private final DoubleSolenoid.Value kSolenoidForKickstandEngaged = DoubleSolenoid.Value.kForward; 14 | private final DoubleSolenoid.Value kSolenoidForKickstandDisengaged = DoubleSolenoid.Value.kReverse; 15 | 16 | private static Kickstand mInstance; 17 | private DoubleSolenoid mKickstandSolenoid; 18 | private DoubleSolenoid mRachetSolenoid; 19 | private boolean mEngaged; 20 | private boolean mRachetEngaged; 21 | 22 | public synchronized static Kickstand getInstance() { 23 | if (mInstance == null) { 24 | mInstance = new Kickstand(); 25 | } 26 | 27 | return mInstance; 28 | } 29 | 30 | private Kickstand() { 31 | mKickstandSolenoid = new DoubleSolenoid(Constants.kPCMId, Constants.kKickstandForwardId, 32 | Constants.kKickstandReverseId); 33 | mRachetSolenoid = new DoubleSolenoid(Constants.kPCMId, Constants.kRatchetForwardId, 34 | Constants.kRatchetReverseId); 35 | mEngaged = true; 36 | mRachetEngaged = true; 37 | setDisengaged(); 38 | setRachetDisengaged(); 39 | } 40 | 41 | public synchronized void setRachetEngaged() { 42 | if (!mRachetEngaged) { 43 | mRachetEngaged = true; 44 | mRachetSolenoid.set(kSolenoidForKickstandEngaged); 45 | } 46 | } 47 | 48 | public synchronized void setRachetDisengaged() { 49 | if (mRachetEngaged) { 50 | mRachetEngaged = false; 51 | mRachetSolenoid.set(kSolenoidForKickstandDisengaged); 52 | } 53 | } 54 | 55 | /** 56 | * Deploys the climbing mechanism 57 | */ 58 | public synchronized void setEngaged() { 59 | if (!mEngaged) { 60 | mEngaged = true; 61 | mKickstandSolenoid.set(kSolenoidForKickstandEngaged); 62 | } 63 | } 64 | 65 | /** 66 | * Retracts the climbing mechanism 67 | */ 68 | public synchronized void setDisengaged() { 69 | if (mEngaged) { 70 | mEngaged = false; 71 | mKickstandSolenoid.set(kSolenoidForKickstandDisengaged); 72 | } 73 | } 74 | 75 | public synchronized boolean isEngaged() { 76 | return mEngaged; 77 | } 78 | 79 | @Override 80 | public void stop() {} 81 | 82 | @Override 83 | public boolean checkSystem() { 84 | return false; 85 | } 86 | 87 | @Override 88 | public void outputTelemetry() {} 89 | } 90 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/states/TimedLEDState.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.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 kJustZeroed = new BlinkingLEDState( 10 | LEDState.kOff, LEDState.kRobotZeroed, 0.250); 11 | public static BlinkingLEDState kBlinkingIntakeCargo = new BlinkingLEDState( 12 | LEDState.kOff, LEDState.kIntakeIntakingCargo, 0.1); 13 | public static BlinkingLEDState kBlinkingIntakingDisk = new BlinkingLEDState( 14 | LEDState.kOff, LEDState.kIntakeIntakingDisk, 0.1); 15 | 16 | public static BlinkingLEDState kHangNoPressure = new BlinkingLEDState( 17 | LEDState.kOff, LEDState.kHanging, 0.5); 18 | 19 | LEDState mStateOne = new LEDState(0.0, 0.0, 0.0); 20 | LEDState mStateTwo = new LEDState(0.0, 0.0, 0.0); 21 | double mDuration; 22 | 23 | public BlinkingLEDState(LEDState stateOne, LEDState stateTwo, double duration) { 24 | mStateOne.copyFrom(stateOne); 25 | mStateTwo.copyFrom(stateTwo); 26 | mDuration = duration; 27 | } 28 | 29 | @Override 30 | public void getCurrentLEDState(LEDState desiredState, double timestamp) { 31 | if ((int) (timestamp / mDuration) % 2 == 0) { 32 | desiredState.copyFrom(mStateOne); 33 | } else { 34 | desiredState.copyFrom(mStateTwo); 35 | } 36 | } 37 | } 38 | 39 | class StaticLEDState implements TimedLEDState { 40 | public static StaticLEDState kStaticOff = new StaticLEDState(LEDState.kOff); 41 | public static StaticLEDState kIntakingDisk = new StaticLEDState(LEDState.kIntakeIntakingDisk); 42 | public static StaticLEDState kIntakingCargo = new StaticLEDState(LEDState.kIntakeIntakingCargo); 43 | public static StaticLEDState kExhausting = new StaticLEDState(LEDState.kIntakeExhuasting); 44 | 45 | public static StaticLEDState kRobotZeroed = new StaticLEDState(LEDState.kRobotZeroed); 46 | 47 | public static StaticLEDState kHangMinimalPressure = new StaticLEDState(LEDState.kMinimalPressure); 48 | public static StaticLEDState kHangOptimalPressure = new StaticLEDState(LEDState.kOptimalPressure); 49 | 50 | LEDState mStaticState = new LEDState(0.0, 0.0, 0.0); 51 | 52 | public StaticLEDState(LEDState staticState) { 53 | mStaticState.copyFrom(staticState); 54 | } 55 | 56 | @Override 57 | public void getCurrentLEDState(LEDState desiredState, double timestamp) { 58 | desiredState.copyFrom(mStaticState); 59 | } 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/motion/MotionSegment.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.motion; 2 | 3 | import static com.team254.lib.util.Util.epsilonEquals; 4 | import static com.team254.lib.motion.MotionUtil.kEpsilon; 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/frc2019/loops/Looper.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.loops; 2 | 3 | import com.team254.frc2019.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/frc2019/auto/actions/AutoAimAndScoreAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.RobotState; 4 | import com.team254.frc2019.statemachines.EndEffectorStateMachine; 5 | import com.team254.frc2019.statemachines.SuperstructureCommands; 6 | import com.team254.frc2019.subsystems.EndEffector; 7 | import com.team254.frc2019.subsystems.Superstructure; 8 | import com.team254.lib.geometry.Rotation2d; 9 | import com.team254.lib.util.DelayedBoolean; 10 | import com.team254.lib.vision.AimingParameters; 11 | 12 | import edu.wpi.first.wpilibj.Timer; 13 | 14 | import java.util.Optional; 15 | 16 | public class AutoAimAndScoreAction implements Action { 17 | 18 | private boolean mHasShot = false; 19 | private DelayedBoolean mHasExtendedLongEnough = null; 20 | private DelayedBoolean mHasShotLongEnough = null; 21 | private boolean mFinishedExtending = false; 22 | private boolean mFinished = false; 23 | 24 | private Rotation2d mHint; 25 | private DelayedBoolean mTimedOutExtending = null; 26 | 27 | public AutoAimAndScoreAction(Rotation2d hint) { 28 | mHint = hint; 29 | } 30 | 31 | @Override 32 | public void start() { 33 | double t = Timer.getFPGATimestamp(); 34 | mHasExtendedLongEnough = new DelayedBoolean(t, 0.25); 35 | mHasShotLongEnough = new DelayedBoolean(t, 0.3); 36 | mTimedOutExtending = new DelayedBoolean(t, 0.75); 37 | if (mHint == null) { 38 | Superstructure.getInstance().setWantAutoAim( 39 | RobotState.getInstance().getLatestVehicleToTurret().getValue()); 40 | } else { 41 | Superstructure.getInstance().setWantAutoAim(mHint); 42 | } 43 | } 44 | 45 | @Override 46 | public void update() { 47 | double t = Timer.getFPGATimestamp(); 48 | Superstructure superstructure = Superstructure.getInstance(); 49 | Optional aimParams = superstructure.getLatestAimingParameters(); 50 | if (!aimParams.isEmpty() && !mFinishedExtending) { 51 | if (superstructure.isOnTarget()) { 52 | SuperstructureCommands.goToAutoScore(); 53 | if (superstructure.isAtDesiredState() || mTimedOutExtending.update(t, true)) { 54 | EndEffector.getInstance().setWantedAction(EndEffectorStateMachine.WantedAction.EXHAUST); 55 | mHasShot = true; 56 | } 57 | } 58 | } 59 | mFinishedExtending = mHasExtendedLongEnough.update(t, mHasShot); 60 | if (mFinishedExtending) { 61 | SuperstructureCommands.goToPreAutoThrust(); 62 | mFinished = mHasShotLongEnough.update(t, true); 63 | } 64 | } 65 | 66 | @Override 67 | public boolean isFinished() { 68 | return mFinished && Superstructure.getInstance().isAtDesiredState(); 69 | } 70 | 71 | @Override 72 | public void done() { 73 | EndEffector.getInstance().setWantedAction(EndEffectorStateMachine.WantedAction.IDLE); 74 | Superstructure.getInstance().setWantRobotRelativeTurret(); 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/modes/AutoModeBase.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.modes; 2 | 3 | import com.team254.frc2019.auto.AutoModeEndedException; 4 | import com.team254.frc2019.auto.actions.Action; 5 | import com.team254.frc2019.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/frc2019/subsystems/Arm.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice; 4 | import com.ctre.phoenix.motorcontrol.RemoteSensorSource; 5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 6 | import com.team254.frc2019.Constants; 7 | import com.team254.lib.drivers.MotorChecker; 8 | import com.team254.lib.drivers.TalonSRXChecker; 9 | import com.team254.lib.drivers.TalonSRXUtil; 10 | 11 | import java.util.ArrayList; 12 | 13 | public class Arm extends ServoMotorSubsystem { 14 | private static Arm mInstance; 15 | 16 | public synchronized static Arm getInstance() { 17 | if (mInstance == null) { 18 | mInstance = new Arm(Constants.kArmConstants); 19 | } 20 | 21 | return mInstance; 22 | } 23 | 24 | private Arm(final ServoMotorSubsystemConstants constants) { 25 | super(constants); 26 | 27 | TalonSRXUtil.checkError(mMaster.configRemoteFeedbackFilter(Constants.kCanifierArmId, 28 | RemoteSensorSource.CANifier_Quadrature, 29 | 0, Constants.kLongCANTimeoutMs), 30 | "Could not set arm encoder!!!: "); 31 | 32 | TalonSRXUtil.checkError(mMaster.configSelectedFeedbackSensor( 33 | RemoteFeedbackDevice.RemoteSensor0, 0, Constants.kLongCANTimeoutMs), 34 | "Could not detect arm encoder: "); 35 | } 36 | 37 | // Syntactic sugar. 38 | public synchronized double getAngle() { 39 | return getPosition(); 40 | } 41 | 42 | public synchronized void setSetpointThrust(double units) { 43 | mMaster.configMotionCruiseVelocity(Constants.kArmCruiseVelocityForThrust); 44 | super.setSetpointMotionMagic(units); 45 | } 46 | 47 | @Override 48 | public synchronized void setSetpointMotionMagic(double units) { 49 | mMaster.configMotionCruiseVelocity(Constants.kArmConstants.kCruiseVelocity); 50 | super.setSetpointMotionMagic(units); 51 | } 52 | 53 | @Override 54 | public synchronized void setSetpointMotionMagic(double units, double feedforward_v) { 55 | mMaster.configMotionCruiseVelocity(Constants.kArmConstants.kCruiseVelocity); 56 | super.setSetpointMotionMagic(units, feedforward_v); 57 | } 58 | 59 | @Override 60 | public boolean checkSystem() { 61 | return TalonSRXChecker.checkMotors(this, 62 | new ArrayList>() { 63 | private static final long serialVersionUID = 3069865439600365807L; 64 | 65 | { 66 | add(new MotorChecker.MotorConfig<>("master", mMaster)); 67 | } 68 | }, new MotorChecker.CheckerConfig() { 69 | { 70 | mRunOutputPercentage = 0.5; 71 | mRunTimeSec = 0.5; 72 | mCurrentFloor = 0.1; 73 | mRPMFloor = 90; 74 | mCurrentEpsilon = 2.0; 75 | mRPMEpsilon = 200; 76 | mRPMSupplier = () -> mMaster.getSelectedSensorVelocity(); 77 | } 78 | }); 79 | } 80 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/SubsystemManager.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019; 2 | 3 | import com.team254.frc2019.loops.ILooper; 4 | import com.team254.frc2019.loops.Loop; 5 | import com.team254.frc2019.loops.Looper; 6 | import com.team254.frc2019.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/main/java/com/team254/frc2019/auto/actions/CollectVelocityDataAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.Constants; 4 | import com.team254.frc2019.subsystems.Drive; 5 | import com.team254.lib.physics.DriveCharacterization; 6 | import com.team254.lib.util.DriveSignal; 7 | import com.team254.lib.util.ReflectingCSVWriter; 8 | import edu.wpi.first.wpilibj.Timer; 9 | 10 | import java.util.List; 11 | 12 | public class CollectVelocityDataAction implements Action { 13 | private static final double kMaxPower = 0.25; 14 | private static final double kRampRate = 0.02; 15 | private static final Drive mDrive = Drive.getInstance(); 16 | 17 | private final ReflectingCSVWriter mCSVWriter; 18 | private final List mVelocityData; 19 | private final boolean mTurn; 20 | private final boolean mReverse; 21 | private final boolean mHighGear; 22 | 23 | private boolean isFinished = false; 24 | private double mStartTime = 0.0; 25 | 26 | /** 27 | * @param data reference to the list where data points should be stored 28 | * @param highGear use high gear or low 29 | * @param reverse if true drive in reverse, if false drive normally 30 | * @param turn if true turn, if false drive straight 31 | */ 32 | public CollectVelocityDataAction(List data, boolean highGear, boolean reverse, boolean turn) { 33 | mVelocityData = data; 34 | mHighGear = highGear; 35 | mReverse = reverse; 36 | mTurn = turn; 37 | mCSVWriter = new ReflectingCSVWriter<>("/home/lvuser/VELOCITY_DATA.csv", DriveCharacterization.DataPoint.class); 38 | 39 | } 40 | 41 | @Override 42 | public void start() { 43 | mDrive.setHighGear(mHighGear); 44 | mStartTime = Timer.getFPGATimestamp(); 45 | } 46 | 47 | @Override 48 | public void update() { 49 | synchronized (mDrive) { 50 | double dt = Timer.getFPGATimestamp() - mStartTime; 51 | double percentPower = kRampRate * dt; 52 | if (percentPower > kMaxPower) { 53 | isFinished = true; 54 | return; 55 | } 56 | mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * percentPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * percentPower)); 57 | double velocity = mDrive.getAverageDriveVelocityMagnitude() / Constants.kDriveWheelRadiusInches; // rad/s 58 | if (velocity < 0.5) { 59 | // Small velocities tend to be untrustworthy. 60 | return; 61 | } 62 | mVelocityData.add(new DriveCharacterization.DataPoint( 63 | velocity, // rad/s 64 | percentPower * 12.0, // convert to volts 65 | dt 66 | )); 67 | } 68 | mCSVWriter.add(mVelocityData.get(mVelocityData.size() - 1)); 69 | } 70 | 71 | @Override 72 | public boolean isFinished() { 73 | return isFinished; 74 | } 75 | 76 | @Override 77 | public void done() { 78 | mDrive.setOpenLoop(DriveSignal.BRAKE); 79 | mCSVWriter.flush(); 80 | } 81 | } -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/states/SuperstructureState.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.states; 2 | 3 | import com.team254.frc2019.Constants; 4 | import com.team254.lib.geometry.Translation2d; 5 | import com.team254.lib.util.Util; 6 | 7 | public class SuperstructureState { 8 | public double turret; // degrees 9 | public double elevator; // inches 10 | public double shoulder; // degrees 11 | public double wrist; // degrees 12 | 13 | public SuperstructureState(double turret, double elevator, double shoulder, double wrist) { 14 | this.turret = turret; 15 | this.elevator = elevator; 16 | this.shoulder = shoulder; 17 | this.wrist = wrist; 18 | } 19 | 20 | public SuperstructureState(SuperstructureState other) { 21 | this.turret = other.turret; 22 | this.elevator = other.elevator; 23 | this.shoulder = other.shoulder; 24 | this.wrist = other.wrist; 25 | } 26 | 27 | // default robot position 28 | public SuperstructureState() { 29 | this(0, 0, 0, 0); 30 | } 31 | 32 | public void setFrom(SuperstructureState source) { 33 | turret = source.turret; 34 | elevator = source.elevator; 35 | shoulder = source.shoulder; 36 | wrist = source.wrist; 37 | } 38 | 39 | /** 40 | * @return height of the bottom roller of the end effector; only applicable for SFR and SVR end effectors 41 | */ 42 | public double getBottomEndEffectorHeight() { 43 | double z = Constants.kElevatorHeightToFloor; // z will represent the height of the bottom of the end effector to the ground in inches 44 | z += this.elevator; 45 | z += Constants.kArmLength * Math.sin(Math.toRadians(this.shoulder)); 46 | z += Constants.kWristToBottomEndEffectorLength * Math.sin(Math.toRadians(this.wrist + Constants.kEndEffectorBottomAngle)); 47 | return z; 48 | } 49 | 50 | /** 51 | * @return Translation2d where x maps to x position and y maps to z position 52 | */ 53 | public Translation2d getPlanarWristJointLocation() { 54 | double z = this.elevator; 55 | z += Constants.kArmLength * Math.sin(Math.toRadians(this.shoulder)); 56 | 57 | double x = Constants.kArmLength * Math.cos(Math.toRadians(this.shoulder)); 58 | 59 | return new Translation2d(x, z); 60 | } 61 | 62 | /** 63 | * @return is bottom roller of the end effector above the bumper; only applicable for SFR and SVR end effectors 64 | */ 65 | public boolean isOverBumper() { 66 | return getBottomEndEffectorHeight() > Constants.kBumperHeight + SuperstructureConstants.kElevatorPaddingInches; 67 | } 68 | 69 | public boolean isTurretSafeForWristBelowBumper() { 70 | return Util.epsilonEquals(turret, 0, SuperstructureConstants.kTurretPaddingDegrees) 71 | || Util.epsilonEquals(turret, 180, SuperstructureConstants.kTurretPaddingDegrees); 72 | } 73 | 74 | @Override 75 | public String toString() { 76 | return "SuperstructureState{" + 77 | "turret=" + turret + 78 | ", elevator=" + elevator + 79 | ", shoulder=" + shoulder + 80 | ", wrist=" + wrist + 81 | '}'; 82 | } 83 | 84 | public Double[] asVector() { 85 | return new Double[]{turret, elevator, shoulder, wrist}; 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/wpilib/TimedRobot.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.wpilib; 2 | 3 | /*----------------------------------------------------------------------------*/ 4 | /* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ 5 | /* Open Source Software - may be modified and shared by FRC teams. The code */ 6 | /* must be accompanied by the FIRST BSD license file in the root directory of */ 7 | /* the project. */ 8 | /*----------------------------------------------------------------------------*/ 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 | *

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

periodic() functions from the base class are called on an interval by a Notifier instance. 22 | */ 23 | public class TimedRobot extends IterativeRobotBase { 24 | public static final double kDefaultPeriod = 0.02; 25 | 26 | // The C pointer to the notifier object. We don't use it directly, it is 27 | // just passed to the JNI bindings. 28 | private final int m_notifier = NotifierJNI.initializeNotifier(); 29 | 30 | // The absolute expiration time 31 | private double m_expirationTime; 32 | 33 | /** 34 | * Constructor for TimedRobot. 35 | */ 36 | protected TimedRobot() { 37 | this(kDefaultPeriod); 38 | } 39 | 40 | /** 41 | * Constructor for TimedRobot. 42 | * 43 | * @param period Period in seconds. 44 | */ 45 | protected TimedRobot(double period) { 46 | super(period); 47 | 48 | HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed); 49 | } 50 | 51 | @Override 52 | @SuppressWarnings("NoFinalizer") 53 | protected void finalize() { 54 | NotifierJNI.stopNotifier(m_notifier); 55 | NotifierJNI.cleanNotifier(m_notifier); 56 | } 57 | 58 | /** 59 | * Provide an alternate "main loop" via startCompetition(). 60 | */ 61 | @Override 62 | @SuppressWarnings("UnsafeFinalization") 63 | public void startCompetition() { 64 | robotInit(); 65 | 66 | // Tell the DS that the robot is ready to be enabled 67 | HAL.observeUserProgramStarting(); 68 | 69 | m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period; 70 | updateAlarm(); 71 | 72 | // Loop forever, calling the appropriate mode-dependent function 73 | while (true) { 74 | long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); 75 | if (curTime == 0) { 76 | break; 77 | } 78 | 79 | m_expirationTime += m_period; 80 | updateAlarm(); 81 | 82 | loopFunc(); 83 | } 84 | } 85 | 86 | /** 87 | * Get time period between calls to Periodic() functions. 88 | */ 89 | public double getPeriod() { 90 | return m_period; 91 | } 92 | 93 | /** 94 | * Update the alarm hardware to reflect the next alarm. 95 | */ 96 | @SuppressWarnings("UnsafeFinalization") 97 | private void updateAlarm() { 98 | NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6)); 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/subsystems/RobotStateEstimator.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.subsystems; 2 | 3 | import com.team254.frc2019.Kinematics; 4 | import com.team254.frc2019.RobotState; 5 | import com.team254.frc2019.loops.ILooper; 6 | import com.team254.frc2019.loops.Loop; 7 | import com.team254.lib.geometry.Pose2d; 8 | import com.team254.lib.geometry.Rotation2d; 9 | import com.team254.lib.geometry.Twist2d; 10 | 11 | public class RobotStateEstimator extends Subsystem { 12 | static RobotStateEstimator mInstance = new RobotStateEstimator(); 13 | private RobotState mRobotState = RobotState.getInstance(); 14 | private Drive mDrive = Drive.getInstance(); 15 | private double left_encoder_prev_distance_ = 0.0; 16 | private double right_encoder_prev_distance_ = 0.0; 17 | private double prev_timestamp_ = -1.0; 18 | private Rotation2d prev_heading_ = null; 19 | 20 | public static RobotStateEstimator getInstance() { 21 | if (mInstance == null) { 22 | mInstance = new RobotStateEstimator(); 23 | } 24 | 25 | return mInstance; 26 | } 27 | 28 | private RobotStateEstimator() {} 29 | 30 | @Override 31 | public void registerEnabledLoops(ILooper looper) { 32 | looper.register(new EnabledLoop()); 33 | } 34 | 35 | private class EnabledLoop implements Loop { 36 | @Override 37 | public synchronized void onStart(double timestamp) { 38 | left_encoder_prev_distance_ = mDrive.getLeftEncoderDistance(); 39 | right_encoder_prev_distance_ = mDrive.getRightEncoderDistance(); 40 | prev_timestamp_ = timestamp; 41 | } 42 | 43 | @Override 44 | public synchronized void onLoop(double timestamp) { 45 | if (prev_heading_ == null) { 46 | prev_heading_ = mRobotState.getLatestFieldToVehicle().getValue().getRotation(); 47 | } 48 | final double dt = timestamp - prev_timestamp_; 49 | final double left_distance = mDrive.getLeftEncoderDistance(); 50 | final double right_distance = mDrive.getRightEncoderDistance(); 51 | final double delta_left = left_distance - left_encoder_prev_distance_; 52 | final double delta_right = right_distance - right_encoder_prev_distance_; 53 | final Rotation2d gyro_angle = mDrive.getHeading(); 54 | Twist2d odometry_twist; 55 | synchronized (mRobotState) { 56 | final Pose2d last_measurement = mRobotState.getLatestFieldToVehicle().getValue(); 57 | odometry_twist = Kinematics.forwardKinematics(last_measurement.getRotation(), delta_left, 58 | delta_right, gyro_angle); 59 | } 60 | final Twist2d measured_velocity = Kinematics.forwardKinematics( 61 | delta_left, delta_right, prev_heading_.inverse().rotateBy(gyro_angle).getRadians()).scaled(1.0 / dt); 62 | final Twist2d predicted_velocity = Kinematics.forwardKinematics(mDrive.getLeftLinearVelocity(), 63 | mDrive.getRightLinearVelocity()).scaled(dt); 64 | mRobotState.addVehicleToTurretObservation(timestamp, 65 | Rotation2d.fromDegrees(Turret.getInstance().getAngle())); 66 | mRobotState.addObservations(timestamp, odometry_twist, measured_velocity, 67 | predicted_velocity); 68 | left_encoder_prev_distance_ = left_distance; 69 | right_encoder_prev_distance_ = right_distance; 70 | prev_heading_ = gyro_angle; 71 | prev_timestamp_ = timestamp; 72 | } 73 | 74 | @Override 75 | public void onStop(double timestamp) {} 76 | } 77 | 78 | @Override 79 | public void stop() {} 80 | 81 | @Override 82 | public boolean checkSystem() { 83 | return true; 84 | } 85 | 86 | @Override 87 | public void outputTelemetry() { 88 | mRobotState.outputToSmartDashboard(); 89 | } 90 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/controlboard/ControlBoard.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.controlboard; 2 | 3 | import com.team254.frc2019.Constants; 4 | 5 | public class ControlBoard implements IControlBoard { 6 | private static ControlBoard mInstance = null; 7 | 8 | public static ControlBoard getInstance() { 9 | if (mInstance == null) { 10 | mInstance = new ControlBoard(); 11 | } 12 | 13 | return mInstance; 14 | } 15 | 16 | private final IDriveControlBoard mDriveControlBoard; 17 | private final IButtonControlBoard mButtonControlBoard; 18 | 19 | private ControlBoard() { 20 | mDriveControlBoard = Constants.kUseDriveGamepad ? GamepadDriveControlBoard.getInstance() 21 | : MainDriveControlBoard.getInstance(); 22 | mButtonControlBoard = GamepadButtonControlBoard.getInstance(); 23 | } 24 | 25 | @Override 26 | public void reset() {} 27 | 28 | @Override 29 | public double getThrottle() { 30 | return mDriveControlBoard.getThrottle(); 31 | } 32 | 33 | @Override 34 | public double getTurn() { 35 | return mDriveControlBoard.getTurn(); 36 | } 37 | 38 | @Override 39 | public boolean getQuickTurn() { 40 | return mDriveControlBoard.getQuickTurn(); 41 | } 42 | 43 | @Override 44 | public boolean getWantsLowGear() { 45 | return mDriveControlBoard.getWantsLowGear(); 46 | } 47 | 48 | @Override 49 | public boolean getShoot() { 50 | return mDriveControlBoard.getShoot(); 51 | } 52 | 53 | @Override 54 | public boolean getThrust() { 55 | return mDriveControlBoard.getThrust(); 56 | } 57 | 58 | @Override 59 | public double getJogTurret() { 60 | return mButtonControlBoard.getJogTurret(); 61 | } 62 | 63 | @Override 64 | public boolean getScorePresetLow() { 65 | return mButtonControlBoard.getScorePresetLow(); 66 | } 67 | 68 | @Override 69 | public boolean getScorePresetMiddle() { 70 | return mButtonControlBoard.getScorePresetMiddle(); 71 | } 72 | 73 | @Override 74 | public boolean getScorePresetHigh() { 75 | return mButtonControlBoard.getScorePresetHigh(); 76 | } 77 | 78 | @Override 79 | public boolean getScorePresetCargo() { 80 | return mButtonControlBoard.getScorePresetCargo(); 81 | } 82 | 83 | @Override 84 | public boolean getPresetStow() { 85 | return mButtonControlBoard.getPresetStow(); 86 | } 87 | 88 | @Override 89 | public boolean getPickupDiskWall() { 90 | return mButtonControlBoard.getPickupDiskWall(); 91 | } 92 | 93 | @Override 94 | public boolean getPickupBallGround() { 95 | return mButtonControlBoard.getPickupBallGround(); 96 | } 97 | 98 | @Override 99 | public void setRumble(boolean on) { 100 | mButtonControlBoard.setRumble(on); 101 | } 102 | 103 | @Override 104 | public boolean getToggleHangMode() { 105 | return mButtonControlBoard.getToggleHangMode(); 106 | } 107 | 108 | @Override 109 | public boolean getToggleHangModeLow() { 110 | return mButtonControlBoard.getToggleHangModeLow(); 111 | } 112 | 113 | @Override 114 | public double getElevatorThrottle() { 115 | return mButtonControlBoard.getElevatorThrottle(); 116 | } 117 | 118 | @Override 119 | public TurretCardinal getTurretCardinal() { 120 | return mButtonControlBoard.getTurretCardinal(); 121 | } 122 | 123 | @Override 124 | public boolean getAutoAim() { 125 | return mButtonControlBoard.getAutoAim(); 126 | } 127 | 128 | @Override 129 | public double getJoggingX() { 130 | return mButtonControlBoard.getJoggingX(); 131 | } 132 | 133 | @Override 134 | public double getJoggingZ() { 135 | return mButtonControlBoard.getJoggingZ(); 136 | } 137 | } 138 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/drivers/SparkMaxFactory.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.drivers; 2 | 3 | import com.revrobotics.CANError; 4 | import com.revrobotics.CANSparkMax; 5 | import com.revrobotics.CANSparkMax.IdleMode; 6 | import com.revrobotics.CANSparkMaxLowLevel; 7 | import com.revrobotics.ControlType; 8 | import edu.wpi.first.wpilibj.DriverStation; 9 | import edu.wpi.first.wpilibj.Timer; 10 | 11 | /** 12 | * Creates CANTalon objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor 13 | * parameters are not set, as these are expected to be set by the application. 14 | */ 15 | public class SparkMaxFactory { 16 | public static class Configuration { 17 | public boolean BURN_FACTORY_DEFAULT_FLASH = false; 18 | public IdleMode NEUTRAL_MODE = IdleMode.kCoast; 19 | public boolean INVERTED = false; 20 | 21 | public int STATUS_FRAME_0_RATE_MS = 10; 22 | public int STATUS_FRAME_1_RATE_MS = 1000; 23 | public int STATUS_FRAME_2_RATE_MS = 1000; 24 | 25 | public double OPEN_LOOP_RAMP_RATE = 0.0; 26 | public double CLOSED_LOOP_RAMP_RATE = 0.0; 27 | 28 | public boolean ENABLE_VOLTAGE_COMPENSATION = false; 29 | public double NOMINAL_VOLTAGE = 12.0; 30 | } 31 | 32 | private static final Configuration kDefaultConfiguration = new Configuration(); 33 | private static final Configuration kSlaveConfiguration = new Configuration(); 34 | 35 | static { 36 | kSlaveConfiguration.STATUS_FRAME_0_RATE_MS = 1000; 37 | kSlaveConfiguration.STATUS_FRAME_1_RATE_MS = 1000; 38 | kSlaveConfiguration.STATUS_FRAME_2_RATE_MS = 1000; 39 | } 40 | 41 | // Create a CANTalon with the default (out of the box) configuration. 42 | public static LazySparkMax createDefaultSparkMax(int id) { 43 | return createSparkMax(id, kDefaultConfiguration); 44 | } 45 | 46 | private static void handleCANError(int id, CANError error, String message) { 47 | if (error != CANError.kOK) { 48 | DriverStation.reportError( 49 | "Could not configure spark id: " + id + " error: " + error.toString() + " " + message, false); 50 | } 51 | } 52 | 53 | public static LazySparkMax createPermanentSlaveSparkMax(int id, CANSparkMax master) { 54 | final LazySparkMax sparkMax = createSparkMax(id, kSlaveConfiguration); 55 | handleCANError(id, sparkMax.follow(master), "setting follower"); 56 | return sparkMax; 57 | } 58 | 59 | public static LazySparkMax createSparkMax(int id, Configuration config) { 60 | // Delay for CAN bus bandwidth to clear up. 61 | Timer.delay(0.25); 62 | LazySparkMax sparkMax = new LazySparkMax(id); 63 | handleCANError(id, sparkMax.setCANTimeout(200), "set timeout"); 64 | 65 | //sparkMax.restoreFactoryDefaults(config.BURN_FACTORY_DEFAULT_FLASH); 66 | 67 | sparkMax.set(ControlType.kDutyCycle, 0.0); 68 | 69 | handleCANError(id, sparkMax.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, config.STATUS_FRAME_0_RATE_MS), "set status0 rate"); 70 | handleCANError(id, sparkMax.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, config.STATUS_FRAME_1_RATE_MS), "set status1 rate"); 71 | handleCANError(id, sparkMax.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, config.STATUS_FRAME_2_RATE_MS), "set status2 rate"); 72 | 73 | sparkMax.clearFaults(); 74 | 75 | handleCANError(id, sparkMax.setIdleMode(config.NEUTRAL_MODE), "set neutrual"); 76 | sparkMax.setInverted(config.INVERTED); 77 | handleCANError(id, sparkMax.setOpenLoopRampRate(config.OPEN_LOOP_RAMP_RATE), "set open loop ramp"); 78 | handleCANError(id, sparkMax.setClosedLoopRampRate(config.CLOSED_LOOP_RAMP_RATE), "set closed loop ramp"); 79 | 80 | if (config.ENABLE_VOLTAGE_COMPENSATION) { 81 | handleCANError(id, sparkMax.enableVoltageCompensation(config.NOMINAL_VOLTAGE), "voltage compensation"); 82 | } else { 83 | handleCANError(id, sparkMax.disableVoltageCompensation(), "voltage compensation"); 84 | } 85 | 86 | return sparkMax; 87 | } 88 | } -------------------------------------------------------------------------------- /src/main/java/com/team254/lib/geometry/Pose2dWithCurvature.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 Pose2dWithCurvature implements IPose2d, ICurvature { 8 | protected static final Pose2dWithCurvature kIdentity = new Pose2dWithCurvature(); 9 | 10 | public static Pose2dWithCurvature identity() { 11 | return kIdentity; 12 | } 13 | 14 | protected final Pose2d pose_; 15 | protected final double curvature_; 16 | protected final double dcurvature_ds_; 17 | 18 | public Pose2dWithCurvature() { 19 | pose_ = new Pose2d(); 20 | curvature_ = 0.0; 21 | dcurvature_ds_ = 0.0; 22 | } 23 | 24 | public Pose2dWithCurvature(final Pose2d pose, double curvature) { 25 | pose_ = pose; 26 | curvature_ = curvature; 27 | dcurvature_ds_ = 0.0; 28 | } 29 | 30 | public Pose2dWithCurvature(final Pose2d pose, double curvature, double dcurvature_ds) { 31 | pose_ = pose; 32 | curvature_ = curvature; 33 | dcurvature_ds_ = dcurvature_ds; 34 | } 35 | 36 | public Pose2dWithCurvature(final Translation2d translation, final Rotation2d rotation, double curvature) { 37 | pose_ = new Pose2d(translation, rotation); 38 | curvature_ = curvature; 39 | dcurvature_ds_ = 0.0; 40 | } 41 | 42 | public Pose2dWithCurvature(final Translation2d translation, final Rotation2d rotation, double curvature, double dcurvature_ds) { 43 | pose_ = new Pose2d(translation, rotation); 44 | curvature_ = curvature; 45 | dcurvature_ds_ = dcurvature_ds; 46 | } 47 | 48 | @Override 49 | public final Pose2d getPose() { 50 | return pose_; 51 | } 52 | 53 | @Override 54 | public Pose2dWithCurvature transformBy(Pose2d transform) { 55 | return new Pose2dWithCurvature(getPose().transformBy(transform), getCurvature(), getDCurvatureDs()); 56 | } 57 | 58 | @Override 59 | public Pose2dWithCurvature mirror() { 60 | return new Pose2dWithCurvature(getPose().mirror().getPose(), -getCurvature(), -getDCurvatureDs()); 61 | } 62 | 63 | @Override 64 | public double getCurvature() { 65 | return curvature_; 66 | } 67 | 68 | @Override 69 | public double getDCurvatureDs() { 70 | return dcurvature_ds_; 71 | } 72 | 73 | @Override 74 | public final Translation2d getTranslation() { 75 | return getPose().getTranslation(); 76 | } 77 | 78 | @Override 79 | public final Rotation2d getRotation() { 80 | return getPose().getRotation(); 81 | } 82 | 83 | @Override 84 | public Pose2dWithCurvature interpolate(final Pose2dWithCurvature other, double x) { 85 | return new Pose2dWithCurvature(getPose().interpolate(other.getPose(), x), 86 | Util.interpolate(getCurvature(), other.getCurvature(), x), 87 | Util.interpolate(getDCurvatureDs(), other.getDCurvatureDs(), x)); 88 | } 89 | 90 | @Override 91 | public double distance(final Pose2dWithCurvature other) { 92 | return getPose().distance(other.getPose()); 93 | } 94 | 95 | @Override 96 | public boolean equals(final Object other) { 97 | if (!(other instanceof Pose2dWithCurvature)) { 98 | return false; 99 | } 100 | 101 | Pose2dWithCurvature p2dwc = (Pose2dWithCurvature) other; 102 | return getPose().equals(p2dwc.getPose()) && Util.epsilonEquals(getCurvature(), p2dwc.getCurvature()) && Util.epsilonEquals(getDCurvatureDs(), p2dwc.getDCurvatureDs()); 103 | } 104 | 105 | @Override 106 | public String toString() { 107 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 108 | return getPose().toString() + ", curvature: " + fmt.format(getCurvature()) + ", dcurvature_ds: " + fmt.format(getDCurvatureDs()); 109 | } 110 | 111 | @Override 112 | public String toCSV() { 113 | final DecimalFormat fmt = new DecimalFormat("#0.000"); 114 | return getPose().toCSV() + "," + fmt.format(getCurvature()) + "," + fmt.format(getDCurvatureDs()); 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /src/main/java/com/team254/frc2019/auto/actions/CollectAccelerationDataAction.java: -------------------------------------------------------------------------------- 1 | package com.team254.frc2019.auto.actions; 2 | 3 | import com.team254.frc2019.Constants; 4 | import com.team254.frc2019.subsystems.Drive; 5 | import com.team254.lib.physics.DriveCharacterization; 6 | import com.team254.lib.util.DriveSignal; 7 | import com.team254.lib.util.ReflectingCSVWriter; 8 | import com.team254.lib.util.Util; 9 | import edu.wpi.first.wpilibj.Timer; 10 | 11 | import java.util.List; 12 | 13 | public class CollectAccelerationDataAction implements Action { 14 | private static final double kStartPower = 0.1; 15 | private static final double kPower = 0.8; 16 | private static final double kTotalTime = 2.0; // how long to run the test for 17 | private static final double kStartTime = 1.0; 18 | private static final Drive mDrive = Drive.getInstance(); 19 | 20 | private final ReflectingCSVWriter mCSVWriter; 21 | private final List mAccelerationData; 22 | private final boolean mTurn; 23 | private final boolean mReverse; 24 | private final boolean mHighGear; 25 | 26 | private double mStartTime = 0.0; 27 | private double mPrevVelocity = 0.0; 28 | private double mPrevTime = 0.0; 29 | 30 | /** 31 | * @param data reference to the list where data points should be stored 32 | * @param highGear use high gear or low 33 | * @param reverse if true drive in reverse, if false drive normally 34 | * @param turn if true turn, if false drive straight 35 | */ 36 | public CollectAccelerationDataAction(List data, boolean highGear, boolean reverse, boolean turn) { 37 | mAccelerationData = data; 38 | mHighGear = highGear; 39 | mReverse = reverse; 40 | mTurn = turn; 41 | mCSVWriter = new ReflectingCSVWriter<>("/home/lvuser/ACCEL_DATA.csv", DriveCharacterization.DataPoint.class); 42 | } 43 | 44 | @Override 45 | public void start() { 46 | mDrive.setHighGear(mHighGear); 47 | mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * kStartPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * kStartPower)); 48 | mStartTime = Timer.getFPGATimestamp(); 49 | mPrevTime = mStartTime; 50 | } 51 | 52 | @Override 53 | public void update() { 54 | double currentVelocity; 55 | double currentTime; 56 | synchronized (mDrive) { 57 | currentVelocity = mDrive.getAverageDriveVelocityMagnitude() / Constants.kDriveWheelRadiusInches; // rad/s 58 | currentTime = mDrive.getTimestamp(); 59 | } 60 | 61 | // don't calculate acceleration until we've populated prevTime and prevVelocity 62 | if (mPrevTime == mStartTime) { 63 | mPrevTime = currentTime; 64 | mPrevVelocity = currentVelocity; 65 | return; 66 | } 67 | 68 | if (currentTime - mStartTime > kStartTime) { 69 | mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * kPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * kPower)); 70 | } else { 71 | return; 72 | } 73 | 74 | double acceleration = (currentVelocity - mPrevVelocity) / (currentTime - mPrevTime); 75 | 76 | // ignore accelerations that are too small 77 | if (acceleration < Util.kEpsilon) { 78 | mPrevTime = currentTime; 79 | mPrevVelocity = currentVelocity; 80 | return; 81 | } 82 | 83 | mAccelerationData.add(new DriveCharacterization.DataPoint( 84 | currentVelocity, // convert to radians per second 85 | kPower * 12.0, // convert to volts 86 | currentTime - mStartTime 87 | )); 88 | 89 | mCSVWriter.add(mAccelerationData.get(mAccelerationData.size() - 1)); 90 | 91 | mPrevTime = currentTime; 92 | mPrevVelocity = currentVelocity; 93 | } 94 | 95 | @Override 96 | public boolean isFinished() { 97 | return Timer.getFPGATimestamp() - mStartTime > kTotalTime + kStartTime; 98 | } 99 | 100 | @Override 101 | public void done() { 102 | mDrive.setOpenLoop(DriveSignal.BRAKE); 103 | mCSVWriter.flush(); 104 | } 105 | } --------------------------------------------------------------------------------