diff --git a/src/main/java/com/spartronics4915/frc2020/ButtonFactory.java b/src/main/java/com/spartronics4915/frc2020/ButtonFactory.java index d6f0fc5..65c902e 100644 --- a/src/main/java/com/spartronics4915/frc2020/ButtonFactory.java +++ b/src/main/java/com/spartronics4915/frc2020/ButtonFactory.java @@ -14,7 +14,6 @@ public class ButtonFactory { - private Constants.OI.DeviceSpec[] mDeviceList; private HashSet mInUse; @@ -23,10 +22,10 @@ public class ButtonFactory /* our job is to configure Constants.OI.deviceList according * to runtime configuration inputs */ - String config; + String config; mDeviceList = Constants.OI.deviceList; mInUse = new HashSet<>(); - if(!RobotBase.isReal()) + if (!RobotBase.isReal()) { config = "noOI"; Logger.notice("ButtonFactory: configuring buttons for no OI"); @@ -50,7 +49,7 @@ public class ButtonFactory case "robot2020": case "default": // here we trust in Constants.OI values - for(int i=0;i kStateStdDevs = new MatBuilder<>(Nat.N3(), Nat.N1()) .fill(0.02, 0.02, 0.01); public static final Matrix measurementStdDevs = new MatBuilder<>(Nat.N6(), Nat.N1()) - .fill(0.1, 0.1, 0.1, 0.005, - 0.005, 0.002); + .fill(0.1, 0.1, 0.1, 0.005, 0.005, 0.002); public static final double kSlamStdDevsPerMeter = 3; - public static final Pose2d kApproximateStartingPose = new Pose2d(Units.inchesToMeters(508), 0, Rotation2d.fromDegrees(180)); + public static final Pose2d kApproximateStartingPose = new Pose2d(Units.inchesToMeters(508), 0, + Rotation2d.fromDegrees(180)); } public static final class Vision diff --git a/src/main/java/com/spartronics4915/frc2020/Robot.java b/src/main/java/com/spartronics4915/frc2020/Robot.java index 9cc5559..7d2999a 100644 --- a/src/main/java/com/spartronics4915/frc2020/Robot.java +++ b/src/main/java/com/spartronics4915/frc2020/Robot.java @@ -28,7 +28,7 @@ public class Robot extends TimedRobot // PDP is used to detect total-current-draw, in 2019 we had spurious // CAN errors. If this happens in 2020, we can live without it. // See more notes in robotPeriodic below. - private PowerDistributionPanel mPDP; + private PowerDistributionPanel mPDP; private static final String kRobotLogVerbosity = "Robot/Verbosity"; @@ -36,9 +36,8 @@ public class Robot extends TimedRobot public void robotInit() { Logger.logRobotInit(); - - try (InputStream manifest = - getClass().getClassLoader().getResourceAsStream("META-INF/MANIFEST.MF")) + + try (InputStream manifest = getClass().getClassLoader().getResourceAsStream("META-INF/MANIFEST.MF")) { // build a version string Attributes attributes = new Manifest(manifest).getMainAttributes(); @@ -67,7 +66,7 @@ public void robotInit() shed.onCommandInterrupt((c) -> Logger.info(c.getName() + " interrupted")); // if CAN bus spews, delete (see notes at top) - this.mPDP = new PowerDistributionPanel(); + this.mPDP = new PowerDistributionPanel(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. @@ -87,14 +86,14 @@ public void robotInit() @Override public void robotPeriodic() { - // robotPeriodic runs in all "match epochs". - // Oddly, the scheduler is *not* operational during "disabled epoch" - // because it follows the LiveWindow disabled state. + // robotPeriodic runs in all "match epochs". + // Oddly, the scheduler is *not* operational during "disabled epoch" + // because it follows the LiveWindow disabled state. // The scheduler is responsible for invoking all Subsystem's periodic // method so we don't expect dashboard updates without this running. - // IterativeRobotBase is the one that controls the LiveWindow state - // and it explicitly disables LiveWindow traffic when the robot is - // disabled. Contrast this with the "test epoch". In this mode, the + // IterativeRobotBase is the one that controls the LiveWindow state + // and it explicitly disables LiveWindow traffic when the robot is + // disabled. Contrast this with the "test epoch". In this mode, the // scheduler does run as do all LiveWindow functions. CommandScheduler.getInstance().run(); @@ -105,7 +104,7 @@ public void robotPeriodic() // Dashboard can rely on LiveWindow but then we don't receive // updates when robot is disabled. SmartDashboard.putNumber("Robot/TotalCurrent", this.mPDP.getTotalCurrent()); - } + } /** * This function is called once each time the robot enters Disabled mode. @@ -168,7 +167,7 @@ public void teleopPeriodic() public void testInit() { CommandScheduler.getInstance().cancelAll(); - LED.getInstance().setBlingState(Bling.kDriveSlow); + LED.getInstance().setBlingState(Bling.kDriveSlow); // TODO: cycle through bling animations? } /** diff --git a/src/main/java/com/spartronics4915/frc2020/RobotContainer.java b/src/main/java/com/spartronics4915/frc2020/RobotContainer.java index 25cefe6..6c9390a 100644 --- a/src/main/java/com/spartronics4915/frc2020/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc2020/RobotContainer.java @@ -23,17 +23,13 @@ import com.spartronics4915.lib.hardware.motors.SpartronicsSimulatedMotor; import com.spartronics4915.lib.hardware.sensors.T265Camera; import com.spartronics4915.lib.hardware.sensors.T265Camera.CameraJNIException; -import com.spartronics4915.lib.math.twodim.control.FeedForwardTracker; import com.spartronics4915.lib.math.twodim.control.RamseteTracker; import com.spartronics4915.lib.math.twodim.control.TrajectoryTracker; -import com.spartronics4915.lib.math.twodim.geometry.Pose2d; -import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; import com.spartronics4915.lib.subsystems.estimator.DrivetrainEstimator; import com.spartronics4915.lib.subsystems.estimator.RobotStateEstimator; import com.spartronics4915.lib.subsystems.estimator.RobotStateEstimator.EstimatorSource; import com.spartronics4915.lib.util.Kinematics; import com.spartronics4915.lib.util.Logger; -import com.spartronics4915.lib.util.Units; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -46,7 +42,6 @@ public class RobotContainer { private static final String kAutoOptionsKey = "AutoStrategyOptions"; - public final NetworkTableEntry mAutoModeEntry = NetworkTableInstance.getDefault() .getTable("SmartDashboard").getEntry("AutoStrategy"); @@ -119,7 +114,7 @@ public RobotContainer() mStateEstimator); mStateEstimator.setDefaultCommand(slamraCmd); mStateEstimator.resetRobotStateMaps(Constants.Trajectory.kStartPointRight); - mVision = new Vision(mStateEstimator, mLauncher); + mVision = new Vision(mStateEstimator); if (!RobotBase.isReal()) // we're unit testing SpartronicsSimulatedMotor.resetGlobalState(); @@ -138,120 +133,70 @@ public RobotContainer() mVision.registerTargetListener(mStateEstimator.getVisionListener()); - // Default Commands are established in each commands class - // NB: ButtonFactory handles the !RobotBase.isReal case. configureJoystickBindings(); configureButtonBoardBindings(); /* publish our automodes to the dashboard -----------------*/ mAutoModes = TrajectoryContainer.getAutoModes(mStateEstimator, mDrive, mRamseteController, mSuperstructureCommands); - String autoModeList = Arrays.stream(mAutoModes).map((m) -> m.name) - .collect(Collectors.joining(",")); + String autoModeList = Arrays.stream(mAutoModes).map((m) -> m.name).collect(Collectors.joining(",")); SmartDashboard.putString(kAutoOptionsKey, autoModeList); - } + // Joystick buttons are labelled on the joystick! what a concept private void configureJoystickBindings() { - /* toggle animation to indicate SLOW vs NORMAL drive speeds */ - mButtons.create(mJoystick, 1).whenPressed(mDriveCommands.new SetSlow() - .alongWith(mLEDCommands.new SetBlingState(Bling.kDriveSlow))) + // toggle animation to indicate SLOW vs NORMAL drive speeds + mButtons.create(mJoystick, 1) + .whenPressed(mDriveCommands.new SetSlow() + .alongWith(mLEDCommands.new SetBlingState(Bling.kDriveSlow))) .whenReleased(mDriveCommands.new ToggleSlow() .alongWith(mLEDCommands.new SetBlingState(Bling.kTeleop))); - mButtons.create(mJoystick, 8).whenPressed(new InstantCommand(() -> mIndexer.setZero())); - mButtons.create(mJoystick, 10) - .whenPressed(new InstantCommand(() -> mIndexer.setZero())); - // .whenPressed(mIndexerCommands.new ZeroSpinnerCommand(true)); - mButtons.create(mJoystick, 11) - .whenPressed(new InstantCommand(() -> mLauncher.zeroTurret())); // Chris has expressed he doesn't want functionality on buttons 2, 4, and 5 - mButtons.create(mJoystick, 3).whenPressed(mDriveCommands.new ToggleInverted()); // TODO: - // alongWith - // Vision + mButtons.create(mJoystick, 3).whenPressed(mDriveCommands.new ToggleInverted()); - // Both JoystickButton 6 and 7 have the same functionality - they're close - // together + on passive hand side - /* animation for drive SLOW */ - mButtons.create(mJoystick, 6).whenPressed(mDriveCommands.new ToggleSlow() // TODO: alongWith - // Vision - .alongWith(mLEDCommands.new SetBlingState(Bling.kDriveSlow))); - mButtons.create(mJoystick, 7).whenPressed(mDriveCommands.new ToggleSlow() // TODO: alongWith - // Vision + // JoystickButton 6 / 7 have the same functionality - they're close together + on passive hand side + mButtons.create(mJoystick, 6).whenPressed(mDriveCommands.new ToggleSlow() + .alongWith(mLEDCommands.new SetBlingState(Bling.kDriveSlow))); // FIXME: this will not go back to kTeleop + mButtons.create(mJoystick, 7).whenPressed(mDriveCommands.new ToggleSlow() .alongWith(mLEDCommands.new SetBlingState(Bling.kDriveSlow))); - mButtons.create(mJoystick, 2).whenPressed(mIndexerCommands.new SpinIndexer(5)); - mButtons.create(mJoystick, 4).whenPressed(mIndexerCommands.new StartTransfer()) - .whenReleased(mIndexerCommands.new EndTransfer()); - mButtons.create(mJoystick, 5).whenPressed(mIndexerCommands.new StartKicker()) - .whenReleased(mIndexerCommands.new EndKicker()); - /* - mButtons.create(mJoystick, 1).whenPressed(mIndexerCommands.new ZeroSpinnerCommand(true)); - mButtons.create(mJoystick, 2).whenPressed(mIndexerCommands.new SpinIndexer(5)); - mButtons.create(mJoystick, 4).whenPressed(mIndexerCommands.new StartTransfer()) - .whenReleased(mIndexerCommands.new EndTransfer()); - mButtons.create(mJoystick, 5).whenPressed(mIndexerCommands.new StartKicker()) - .whenReleased(mIndexerCommands.new EndKicker()); - mButtons.create(mJoystick, 6).whenPressed(mSuperstructureCommands.new LaunchSequence()); - */ - - /* - mButtons.create(mJoystick, 1).toggleWhenPressed(mLauncherCommands.new ShootBallTest()); - mButtons.create(mJoystick, 2).toggleWhenPressed(mLauncherCommands.new Zero()); - mButtons.create(mJoystick, 3).toggleWhenPressed(mLauncherCommands.new HoodTest()); - mButtons.create(mJoystick, 4).toggleWhenPressed(mPanelRotatorCommands.new Raise()); - mButtons.create(mJoystick, 5).toggleWhenPressed(mPanelRotatorCommands.new Lower()); - mButtons.create(mJoystick, 6).toggleWhenPressed(mPanelRotatorCommands.new SpinToColor()); - */ - - /* Test Command that fires all balls after setting Flywheel/Hood values from SmartDashboard - mButtons.create(mJoystick, 4).toggleWhenPressed(new SequentialCommandGroup( - new ParallelRaceGroup( - new ParallelCommandGroup(mIndexerCommands.new SpinUpKicker(mIndexer), - mLauncherCommands.new ShootBallTest(mLauncher)), - mLauncherCommands.new WaitForFlywheel(mLauncher)), - new ParallelCommandGroup(mIndexerCommands.new LoadToLauncher(mIndexer, 5), - mLauncherCommands.new ShootBallTest(mLauncher)))); - mButtons.create(mJoystick, 5).toggleWhenPressed(new SequentialCommandGroup( - new ParallelRaceGroup( - new ParallelCommandGroup(mIndexerCommands.new SpinUpKicker(mIndexer), - mLauncherCommands.new ShootBallTestWithDistance(mLauncher)), - mLauncherCommands.new WaitForFlywheel(mLauncher)), - new ParallelCommandGroup(mIndexerCommands.new LoadToLauncher(mIndexer, 5), - mLauncherCommands.new ShootBallTestWithDistance(mLauncher)))); - */ - - /* - mButtons.create(mJoystick, 7).whileHeld(new TrajectoryTrackerCommand(mDrive, mDrive, - this::throughTrench, mRamseteController, mStateEstimator.getEncoderRobotStateMap())); - mButtons.create(mJoystick, 7).whileHeld(new TrajectoryTrackerCommand(mDrive, mDrive, - this::toControlPanel, mRamseteController, mStateEstimator.getEncoderRobotStateMap())); - mButtons.create(mJoystick, 3).toggleWhenPressed(mLauncherCommands.new AutoAimTurret( - mLauncher,Constants.Launcher.goalLocation,mStateEstimator.getEncoderRobotStateMap())); - */ + mButtons.create(mJoystick, 10).whenPressed(new InstantCommand(() -> mIndexer.setZero())); + mButtons.create(mJoystick, 11).whenPressed(new InstantCommand(() -> mLauncher.zeroTurret())); } + + /** TODO: verify + * +--------------------------------------+ + * | (9) (7)(8) (10)(11) | + * | | + * | (x) (3) (4) | + * | (x)++(x) (5) (6) | + * | (x) (1) (2) | + * | (x) (x) | + * | | + * +--------------------------------------+ + */ private void configureButtonBoardBindings() { - /* animate launch */ + // launch buttons mButtons.create(mButtonBoard, 4).whenPressed(mSuperstructureCommands.new LaunchSequence(1) .alongWith(mLEDCommands.new SetBlingState(Bling.kLaunch))); - /* TODO: validate multiple launch animations */ mButtons.create(mButtonBoard, 3).whenPressed(mSuperstructureCommands.new LaunchSequence(5)) - .whileActiveContinuous(mLEDCommands.new SetBlingState(Bling.kLaunch)); - /* animation for pickup: change bling state when command active/inactive */ - // TODO: validate pickup animation + .whileActiveContinuous(mLEDCommands.new SetBlingState(Bling.kLaunch)); // TODO: validate multiple launch animations + + // toggle pickup (command group) mButtons.create(mButtonBoard, 2).toggleWhenPressed(mSuperstructureCommands.new IntakeFive()) - .whenActive(mLEDCommands.new SetBlingState(Bling.kIntake)) + .whenActive(mLEDCommands.new SetBlingState(Bling.kIntake)) // TODO: validate pickup animation .whenInactive(mLEDCommands.new SetBlingState(Bling.kTeleop)); - /* animation for eject: change bling state when command active/inactive */ - // TODO: validate eject animation + + // toggle eject mButtons.create(mButtonBoard, 1).toggleWhenPressed(mIntakeCommands.new Eject()) - .whenActive(mLEDCommands.new SetBlingState(Bling.kEject)) + .whenActive(mLEDCommands.new SetBlingState(Bling.kEject)) // TODO: validate eject animation .whenInactive(mLEDCommands.new SetBlingState(Bling.kTeleop)); - /* animation for climb -- note, we are not differentiating different climb states */ + // climb buttons -- note, we are not differentiating different climb states for animations mButtons.create(mButtonBoard, 8).whenHeld(mClimberCommands.new Winch() .alongWith(mLEDCommands.new SetBlingState(Bling.kClimb))); mButtons.create(mButtonBoard, 9).whileHeld(mClimberCommands.new Retract() @@ -259,37 +204,18 @@ private void configureButtonBoardBindings() mButtons.create(mButtonBoard, 10).whileHeld(mClimberCommands.new Extend() .alongWith(mLEDCommands.new SetBlingState(Bling.kClimb))); - // new JoystickButton(mButtonBoard, 6).toggleWhenPressed(new - // ConditionalCommand(mLauncherCommands.new Target()); - // new JoystickButton(mButtonBoard, 7).whenPressed(LauncherCommands.new - // Launch()); - - /* turning off LEDs for control panel actions to minimize interference */ - // mButtons.create(mButtonBoard, 5).whenPressed(mPanelRotatorCommands.new - // Lower() - // .alongWith(mLEDCommands.new SetBlingState(Bling.kTeleop))); - // mButtons.create(mButtonBoard, 6).whenPressed(mPanelRotatorCommands.new - // Raise() - // .alongWith(mLEDCommands.new SetBlingState(Bling.kOff))); - // mButtons.create(mButtonBoard, 7).whenPressed(mPanelRotatorCommands.new - // SpinToColor()); - - // TODO: interface with the button board "joystick" potentially through - // GenericHID - // mButtons.create(mButtonBoard, 12).whenPressed(mClimberCommands.new - // ExtendMin()); - // mButtons.create(mButtonBoard, 13).whenPressed(mClimberCommands.new - // ExtendMax()); - // mButtons.create(mButtonBoard, 14).whenPressed(mPanelRotatorCommands.new - // AutoSpinRotation()); - // mButtons.create(mButtonBoard, 15).whenPressed(mPanelRotatorCommands.new - // AutoSpinToColor()); - - /* Four-way Joystick - mButtons.create(mButtonBoard, 15).whenHeld(new TurretRaiseCommand()); - mButtons.create(mButtonBoard, 16).whenHeld(new TurretLowerCommand()); - mButtons.create(mButtonBoard, 17).whenHeld(new TurretLeftCommand()); - mButtons.create(mButtonBoard, 18).whenHeld(new TurretRightCommand()); + // control panel buttons - turning off LEDs to minimize interference + mButtons.create(mButtonBoard, 5).whenPressed(mPanelRotatorCommands.new Lower() + .alongWith(mLEDCommands.new SetBlingState(Bling.kTeleop))); + mButtons.create(mButtonBoard, 6).whenPressed(mPanelRotatorCommands.new Raise() + .alongWith(mLEDCommands.new SetBlingState(Bling.kOff))); + mButtons.create(mButtonBoard, 7).whenPressed(mPanelRotatorCommands.new SpinToColor()); + + /* TODO: interface with the button board "joystick" potentially through GenericHID + mButtons.create(mButtonBoard, 12).whenPressed(mClimberCommands.new ExtendMin()); + mButtons.create(mButtonBoard, 13).whenPressed(mClimberCommands.new ExtendMax()); + mButtons.create(mButtonBoard, 14).whenPressed(mPanelRotatorCommands.new AutoSpinRotation()); + mButtons.create(mButtonBoard, 15).whenPressed(mPanelRotatorCommands.new AutoSpinToColor()); */ } diff --git a/src/main/java/com/spartronics4915/frc2020/commands/ClimberCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/ClimberCommands.java index 827414f..e83270b 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/ClimberCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/ClimberCommands.java @@ -21,15 +21,6 @@ public ClimberCommands(Climber climber) mClimber.setDefaultCommand(new Stop()); } - /** - * A {@link StartEndCommand} allows us to specify an execute() and end() - * condition, and runs until interrupted. - * - * @param Runnable onInit Runs over and over until interrupted - * @param Runnable onEnd (boolean interrupted) Method to call once when ended - * @param Subsystem requirement For both the CommandScheduler and the above method references. - */ - /** * This {@link StartEndCommand} extends the "lightsaber" while held down. *

@@ -62,6 +53,10 @@ public Retract() } } + /** + * This {@link ParallelRaceGroup} extends the climber to a "minimum" distance - + * ideal for climbing when the bar is low-to-horizontal - by use of a timer. + */ public class ExtendMin extends ParallelRaceGroup { public ExtendMin() @@ -70,6 +65,11 @@ public ExtendMin() } } + /** + * This {@link ParallelRaceGroup} extends the climber to a "maximum" distance - + * ideal for climbing when the bar high (likely another robot on the other end) - + * by use of a timer. + */ public class ExtendMax extends ParallelRaceGroup { public ExtendMax() @@ -137,6 +137,8 @@ public void execute() mClimber.winch(Constants.Climber.kStalled); } + // TODO: Should there be an additional stall check? + @Override public void end(boolean interrupted) { @@ -155,7 +157,7 @@ public class Winch extends SequentialCommandGroup { public Winch() { - super(/*new WinchPrimary(), */new WinchSecondary()); + super(/*new WinchPrimary(),*/ new WinchSecondary()); // FIXME: test this! } } diff --git a/src/main/java/com/spartronics4915/frc2020/commands/DriveCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/DriveCommands.java index 38dcfa1..86de062 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/DriveCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/DriveCommands.java @@ -47,9 +47,9 @@ public void execute() x *= Constants.Drive.kSlowModeMultiplier; } if (mInverted) - y *= -1; + y *= -1; // intentionally only invert y - y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); + y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); // apply response curve mDrive.arcadeDrive(applyDeadzone(y), applyDeadzone(x)); } @@ -64,8 +64,7 @@ private double applyDeadzone(double val) } /** - * Written exclusively for the trigger, this command sets - * the drivetrain to slow mode. + * Written exclusively for the trigger, this command sets the drivetrain to slow mode. * A corresponding UnsetSlow does not exist - ToggleSlow functions fine. */ public class SetSlow extends CommandBase diff --git a/src/main/java/com/spartronics4915/frc2020/commands/ExampleCommandFactory.java b/src/main/java/com/spartronics4915/frc2020/commands/ExampleCommandFactory.java index 6a65033..84979fc 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/ExampleCommandFactory.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/ExampleCommandFactory.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; /** @@ -17,15 +16,15 @@ * to be represented in a sourcecode file, we utilize java "nested classes" to * define multiple classes in a single file. More detail can be found * here: https://docs.oracle.com/javase/tutorial/java/javaOO/nested.html. - * + * * There are two kinds of inner classes supported by java: - * + * * 1. static public class Foo - * A static inner class instance can be constructed without access to an - * instance of the outer class. Commands generally require access to one + * A static inner class instance can be constructed without access to an + * instance of the outer class. Commands generally require access to one * or more subystems so minimimally: * CommandBase b = new Outerclass.InnerStaticClass(mMySubsystem); - * + * * 2. public class Bar * A regular inner class must be constructed via the 'new' method * associated with the outer class *instance*. Now we can implicity @@ -33,15 +32,15 @@ * Sharing data amongst many Commands might be accomplished via this * pattern. Minimally: * CommandBase b = mMySubsystem.new InnerClass(); - * + * * This approach suffers when your InnerClass wants to inherit behavior * from certain wpilibj commands that are tailored for inline * construction. One example of this is InstantCommand as extended * in Test5 below. In order to construct an instance of InstantCommand - * we must pass a Runnable as well as one or more subsystems via super(). - * The only problem is that we can't access OuterClass instance variables - * until after InnerClass is constructed. To work around this, we must - * pass in references to the required subsystem as well as any other + * we must pass a Runnable as well as one or more subsystems via super(). + * The only problem is that we can't access OuterClass instance variables + * until after InnerClass is constructed. To work around this, we must + * pass in references to the required subsystem as well as any other * parameters to express the construction, like so: * CommandBase b = mMySubsystem.new InnerClass(mMySubsystem); * This feels a little weird. In the case of InstantCommand there @@ -49,51 +48,51 @@ * simply override the minimal behavior. See Test5a, below. * This is not possible with FunctionalCommand and you should consider * these comments from FunctionalCommand: - * A command that allows the user to pass in functions for each of - * the basic command methods through the constructor. Useful for + * A command that allows the user to pass in functions for each of + * the basic command methods through the constructor. Useful for * inline definitions of complex commands - note, however, that if a - * command is beyond a certain complexity it is usually better practice + * command is beyond a certain complexity it is usually better practice * to write a proper class for it than to inline it. * * Above we discuss two innerclass solutions. There is also a third * approach available to us as exemplified in our MakeCmd method, below. * Pick the best for your application, though option #1 is likely preferred. - * + * * Executive summary: * - * 1. Contextualized inner class + * 1. Contextualized inner class * - access to outerclass member variables, only not as args to super. * - requires unusual, but legal, java syntax for construction - * - if you wish to subclass simple base classes (ie: InstantCommand, + * - if you wish to subclass simple base classes (ie: InstantCommand, * StartStopCommand, FunctionalCommand) either follow example Test5a * or simply bail on the use of FunctionalCommand in favor of CommandBase. * - odd-looking constructor syntax: mCommandFactory.new Test5a(); - * + * * 2. Independant inner class * - no access to outerclass member variables * - yes access to outerclass static variables * - standard constructor syntax: new ExampleCommandFactory.Test7(mCameraSys) - * + * * 3. Enumerated MakeCmd method * - easy to read and use, but the dispatch is switch-based * * Usage: * This examples operates on a single subsystem. Note that some commands may - * require or operate on more than one subsystem. This example could be - * extended to accept an ArrayList or - * Set. Or explicitly require, say, the DriveTrain + * require or operate on more than one subsystem. This example could be + * extended to accept an ArrayList or + * Set. Or explicitly require, say, the DriveTrain * subsystem. * * in RobotContainer(): * this.mCamera = new CameraSubsystem(); - * + * * // ExampleCommandFactory constructor accepts subsystem and * // misc parameters to store in the instance. These will be * // be available to methods of inner classes unless the methods * // are defined in the constructor or passed to the super during - * // construction. - * this.mCamCmds = new ExampleCommandFactory(mCamera, ...parameters...); - * + * // construction. + * this.mCamCmds = new ExampleCommandFactory(mCamera, ...parameters...); + * * See RobotContainer::configureTestCommands(), for instantiatioon examples. * */ @@ -170,8 +169,8 @@ public Test4() } } - // Example InstantCommand. Question: why not just place the lambda - // expression in the caller? Answer: to encapsulate the details of the + // Example InstantCommand. Question: why not just place the lambda + // expression in the caller? Answer: to encapsulate the details of the // expression in this file and not the caller (ownership of files is clearer) public class Test5 extends InstantCommand { @@ -181,7 +180,7 @@ public Test5(SpartronicsSubsystem subsys) // to super since it may run it before we're fully constructed. // We require that the subsystem be passed in if this InstantCommand // 'requires' it. Error message: - // cannot refer to 'this' nor 'super' while explicitly invoking a + // cannot refer to 'this' nor 'super' while explicitly invoking a // constructorJava(134217866) super(() -> { subsys.logInfo("Run Test5"); }, subsys); } @@ -257,7 +256,7 @@ public void end(boolean interrupted) // A public static (inner) class allows us to reference // this class without contructing an instance of our outer - // class. ie: we can be instantiated via: + // class. ie: we can be instantiated via: // new ExampleCommandFactory.Test7(mySubsys) public static class Test7 extends StartEndCommand { diff --git a/src/main/java/com/spartronics4915/frc2020/commands/IndexerCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/IndexerCommands.java index 9e3c30f..b0dbedf 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/IndexerCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/IndexerCommands.java @@ -23,11 +23,6 @@ public IndexerCommands(Indexer indexer) // mIndexer.setDefaultCommand(mIndexerCommands.new ZeroAndStopGroup(mIndexer)); } - public Indexer getIndexer() - { - return mIndexer; - } - /** * Waits until a ball is held, then ends. */ @@ -37,13 +32,6 @@ public WaitForBallHeld() { super(mIndexer::getIntakeBallLoaded); } - - @Override - public void initialize() - { - super.initialize(); - System.out.println("WaitForBallHeld"); - } } /** @@ -77,7 +65,6 @@ public LoadBallToSlot(double spinCount) @Override public void initialize() { - System.out.println("LoadBallToSlot"); mIndexer.rotateN(mSpinCount); } @@ -115,10 +102,7 @@ public class ZeroSpinnerCommand extends CommandBase public ZeroSpinnerCommand(boolean unzero) { if (unzero) - { mIndexer.unzero(); - } - addRequirements(mIndexer); } @@ -191,13 +175,6 @@ public EndKicker() { super(mIndexer::endLaunch, mIndexer); } - - @Override - public void initialize() - { - super.initialize(); - System.out.println("EndKicker"); - } } /** @@ -384,6 +361,5 @@ private void sharedInit(int ballsToShoot) new InstantCommand(() -> mIndexer.addBalls(-ballsToShoot), mIndexer) ); } - } } diff --git a/src/main/java/com/spartronics4915/frc2020/commands/IntakeCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/IntakeCommands.java index 25d92e7..f3da554 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/IntakeCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/IntakeCommands.java @@ -6,7 +6,6 @@ import com.spartronics4915.frc2020.subsystems.Intake; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -70,7 +69,7 @@ public Set getRequirements() * Note that this is not an Unjam command. The {@link Intake} subsystem only * controls the mechanical vector roller. */ - public class Eject extends StartEndCommand // TODO: Does this execute(), or initialize()? + public class Eject extends StartEndCommand { public Eject() { diff --git a/src/main/java/com/spartronics4915/frc2020/commands/LEDCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/LEDCommands.java index e4965a8..c4caa8e 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/LEDCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/LEDCommands.java @@ -30,17 +30,18 @@ public LEDCommands(LED led) public class SetBlingState extends InstantCommand { Bling mState; + public SetBlingState(Bling s) { super(); this.addRequirements(mLED); this.mState = s; } + @Override public void initialize() { mLED.setBlingState(this.mState); } } - } diff --git a/src/main/java/com/spartronics4915/frc2020/commands/PanelRotatorCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/PanelRotatorCommands.java index 4c7f84b..3b73e94 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/PanelRotatorCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/PanelRotatorCommands.java @@ -4,7 +4,6 @@ import com.spartronics4915.frc2020.subsystems.PanelRotator; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -18,21 +17,6 @@ public PanelRotatorCommands(PanelRotator panelRotator) mPanelRotator.setDefaultCommand(new Stop()); } - /** - * Commands with simple logic statements should be implemented as a - * {@link FunctionalCommand}. This saves the overhead of a full - * {@link CommandBase}, but still allows us to deal with isFinished. - *

- * A FunctionalCommand takes five inputs: - * @param Runnable onInit - * @param Runnable onExecute - * @param Consumer onEnd (boolean interrupted) - * @param BooleanSupplier isFinished - * @param Subsystem requirement For both the CommandScheduler and the above method references. - *

- * Each of these parameters corresponds with a method in the CommandBase class. - */ - /** * This Raise {@link CommandBase} calls {@link PanelRotator}.raise * repeatedly, until the upper optical flag is broken, at which point the @@ -99,12 +83,6 @@ public void end(boolean interrupted) } } - /** - * Commands that are "complex", or have > simple logic within them, - * should be put here. They simply extend {@link CommandBase} and - * are written as such. - */ - /** * This {@link CommandBase} will spin the color wheel to the correct color as broadcast * by the FMS. @@ -152,6 +130,11 @@ else if (mPanelRotator.getRotatedColor().equals("Error")) mPanelRotator.logError("Color Sensor: No data provided"); return true; } + else if (mPanelRotator.getLimitSwitchDown()) + { + mPanelRotator.logError("Arm is down, so the wheel is stopped"); + return true; + } else return false; @@ -213,6 +196,16 @@ public boolean isFinished() mPanelRotator.logError("Confidence too low!"); return true; } + else if (mPanelRotator.getRotatedColor().equals("Error")) + { + mPanelRotator.logError("Color Sensor: No data provided"); + return true; + } + else if (mPanelRotator.getLimitSwitchDown()) + { + mPanelRotator.logError("Arm is down, so the wheel is stopped"); + return true; + } // If the detected color has changed, iterate the eighths counter. currentColor = mPanelRotator.getRotatedColor(); @@ -250,11 +243,7 @@ public Stop() } } - /** - * These names are frustratingly vague. TODO: determine a better prefix than "auto" - *

- * FIXME: Will the act of lowering / raising the wheel spin the control panel? - */ + /* These won't work without backing up the robot. public class AutoSpinRotation extends SequentialCommandGroup { public AutoSpinRotation() @@ -264,10 +253,6 @@ public AutoSpinRotation() } } - /** - * Should be a one-press stop for automatically spinning to the correct color. - * FIXME: won't work because of the wheel hangover - */ public class AutoSpinToColor extends SequentialCommandGroup { public AutoSpinToColor() @@ -275,4 +260,5 @@ public AutoSpinToColor() super(new Raise(), new SpinToColor(), new Lower()); } } + */ } diff --git a/src/main/java/com/spartronics4915/frc2020/commands/StateMapResetCommand.java b/src/main/java/com/spartronics4915/frc2020/commands/StateMapResetCommand.java index fced6d9..0053504 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/StateMapResetCommand.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/StateMapResetCommand.java @@ -8,11 +8,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; -public class StateMapResetCommand extends InstantCommand { - public StateMapResetCommand(RobotStateEstimator stateEstimator, Pose2d start) { - super(() -> { - stateEstimator.resetRobotStateMaps(start); - }); +public class StateMapResetCommand extends InstantCommand +{ + public StateMapResetCommand(RobotStateEstimator stateEstimator, Pose2d start) + { + super(() -> {stateEstimator.resetRobotStateMaps(start);}); } @Override @@ -20,9 +20,4 @@ public Set getRequirements() { return Set.of(); } - - @Override - public void execute() { - - } -} \ No newline at end of file +} diff --git a/src/main/java/com/spartronics4915/frc2020/commands/SuperstructureCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/SuperstructureCommands.java index 84fd263..640cf92 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/SuperstructureCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/SuperstructureCommands.java @@ -1,7 +1,6 @@ package com.spartronics4915.frc2020.commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class SuperstructureCommands @@ -24,10 +23,11 @@ public LaunchSequence(int ballsToShoot) { addCommands( mLauncherCommands.new WaitForFlywheel(), - mIndexerCommands.new LoadToLauncher(ballsToShoot)); + mIndexerCommands.new LoadToLauncher(ballsToShoot) + ); } - /** XXX: should this shoot > 1? */ - public LaunchSequence() + + public LaunchSequence() // XXX: should this shoot > 1? { this(1); } @@ -57,5 +57,4 @@ public IntakeFive() ); } } - } diff --git a/src/main/java/com/spartronics4915/frc2020/commands/VisionCommands.java b/src/main/java/com/spartronics4915/frc2020/commands/VisionCommands.java index 86ca92f..2b28e0d 100644 --- a/src/main/java/com/spartronics4915/frc2020/commands/VisionCommands.java +++ b/src/main/java/com/spartronics4915/frc2020/commands/VisionCommands.java @@ -10,7 +10,7 @@ * Very little going on here currently: * 1. (driver or autonomous) control over VisionLED releay * 2. default behavior listening for dashboard request to change LED state. - * + * * NB: turret angle updates are under the control of LauncherCommands. * we assume that the updated turret angle is correctly delivered * to the CoordSysMgr. @@ -36,16 +36,16 @@ public static enum LEDStateChange * An instant command that can turn on, off or toggle the VisionLED relay. * This probably should be invoked automatically when odometry determines * that we're in a place where vision targeting is enabled. - * + * * XXX: perhaps this should be the central control for acquisition: - * ie: light is off, we could notify raspi to rest + * ie: light is off, we could notify raspi to rest */ public class SetLEDRelay extends InstantCommand { LEDStateChange mStateChange; public SetLEDRelay(LEDStateChange c) { - super(); + super(); // NB: I think it's legit to say 'no subsystem requirements'. // since we're the only one who cares about this relay. // Now we won't unschedule our default command on each toggle. @@ -57,7 +57,7 @@ public void initialize() { // this is where InstantCommand does its thing boolean oldstate = mVision.isLEDOn(), newstate; - switch(mStateChange) + switch (mStateChange) { case kOff: newstate = false; @@ -65,7 +65,7 @@ public void initialize() case kOn: newstate = true; break; - case kToggle: + case kToggle: // jumping to default is intended behavior default: newstate = !oldstate; break; @@ -92,7 +92,7 @@ public void execute() // synchronize mLEDRelay with LEDRelay network table value. boolean newstate = mVision.dashboardGetBoolean(Constants.Vision.kLEDRelayKey, true); boolean oldstate = mVision.isLEDOn(); - if(newstate != oldstate) + if (newstate != oldstate) mVision.setLED(newstate); } @@ -102,5 +102,4 @@ public boolean isFinished() return false; // for clarity, we're always in this mode } } - -} \ No newline at end of file +} diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/Climber.java b/src/main/java/com/spartronics4915/frc2020/subsystems/Climber.java index 54c5160..9f82f8d 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/Climber.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/Climber.java @@ -2,7 +2,6 @@ import com.spartronics4915.frc2020.Constants; import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; -import com.spartronics4915.lib.hardware.motors.SensorModel; import com.spartronics4915.lib.hardware.motors.SpartronicsMax; import com.spartronics4915.lib.hardware.motors.SpartronicsMotor; import com.spartronics4915.lib.hardware.motors.SpartronicsSimulatedMotor; @@ -36,7 +35,7 @@ * 2) Winch * * USE CASE 4: RETRACT - * 1) Press the retract button + * 1) Hold the retract button */ public class Climber extends SpartronicsSubsystem { @@ -49,8 +48,6 @@ public Climber() mLiftMotor = SpartronicsSRX.makeMotor(Constants.Climber.kLiftMotorId); mWinchMotor = SpartronicsMax.makeMotor(Constants.Climber.kWinchMotorId); - stop(); - if (mLiftMotor.hadStartupError() || mWinchMotor.hadStartupError()) { mLiftMotor = new SpartronicsSimulatedMotor(Constants.Climber.kLiftMotorId); @@ -61,6 +58,8 @@ public Climber() { logInitialized(true); } + + stop(); } /** @@ -119,18 +118,4 @@ public boolean secondaryIsStalled() { return mWinchMotor.getOutputCurrent() >= Constants.Climber.kSecondaryStallThreshold; } - - private double mMaxAmps = Double.NEGATIVE_INFINITY; - - @Override - public void periodic() - { - double amps = mWinchMotor.getOutputCurrent(); - if (amps > mMaxAmps) - { - mMaxAmps = amps; - dashboardPutNumber("winchCurrentMax", mMaxAmps); - } - dashboardPutNumber("winchCurrent", mMaxAmps); - } } diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/Drive.java b/src/main/java/com/spartronics4915/frc2020/subsystems/Drive.java index 5973981..8cf1107 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/Drive.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/Drive.java @@ -67,7 +67,7 @@ public Drive(Launcher launcher) } @Override - public double getTurretAngle() + public double getTurretAngle() // XXX: why is getTurretAngle in Drive, of all places? { return mLauncher.getTurretDirection().getDegrees(); } diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/Indexer.java b/src/main/java/com/spartronics4915/frc2020/subsystems/Indexer.java index 850eac5..5349045 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/Indexer.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/Indexer.java @@ -24,7 +24,6 @@ public class Indexer extends SpartronicsSubsystem private SensorModel mIndexerModel; private SensorModel mKickerModel; - private SensorModel mTransferModel; private DigitalInput mLimitSwitch; private DigitalInput mOpticalProxSensor; diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/Intake.java b/src/main/java/com/spartronics4915/frc2020/subsystems/Intake.java index 86c1a52..0faae8a 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/Intake.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/Intake.java @@ -29,7 +29,6 @@ public Intake() } stop(); - mHarvestMotor.setOutputInverted(true); } @@ -51,16 +50,6 @@ public void reverse() mHarvestMotor.setPercentOutput(Constants.Intake.kEjectSpeed); } - /** - * Checks to see if a ball is held in the intake chamber - * with a proximity sensor returning a digital value. - *

- * The style of proximity sensor we use requires MANUAL calibration. - * - * @return Whether a ball is held - */ - - /** * Universal stop method */ diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/Launcher.java b/src/main/java/com/spartronics4915/frc2020/subsystems/Launcher.java index 5802a26..6e70ec2 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/Launcher.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/Launcher.java @@ -1,9 +1,7 @@ package com.spartronics4915.frc2020.subsystems; import com.spartronics4915.frc2020.Constants; -import com.spartronics4915.frc2020.commands.LauncherCommands; import com.spartronics4915.lib.hardware.motors.SensorModel; -import com.spartronics4915.lib.hardware.motors.SpartronicsAnalogEncoder; import com.spartronics4915.lib.hardware.motors.SpartronicsEncoder; import com.spartronics4915.lib.hardware.motors.SpartronicsMax; import com.spartronics4915.lib.hardware.motors.SpartronicsMotor; @@ -12,13 +10,10 @@ import com.spartronics4915.lib.math.Util; import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; -import com.spartronics4915.lib.subsystems.estimator.RobotStateEstimator; import com.spartronics4915.lib.util.Interpolable; import com.spartronics4915.lib.util.InterpolatingDouble; import com.spartronics4915.lib.util.InterpolatingTreeMap; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; @@ -74,8 +69,7 @@ public Launcher() Constants.Launcher.kFlywheelMasterId); initSuccess = false; } - mFlywheelMasterMotor.setVelocityGains(Constants.Launcher.kP, 0, 0, 0); // ref value is - // 0.00036 + mFlywheelMasterMotor.setVelocityGains(Constants.Launcher.kP, 0, 0, 0); // ref value is 0.00036 mFeedforwardCalculator = new SimpleMotorFeedforward(Constants.Launcher.kS, Constants.Launcher.kV, Constants.Launcher.kA); mFlywheelMasterMotor.setOutputInverted(false); @@ -89,7 +83,6 @@ public Launcher() mTurretMotor = SpartronicsSRX.makeMotor(Constants.Launcher.kTurretId, SensorModel.fromMultiplier(Math.toDegrees(1.0 / 1024.0 / 11.75 / 20.0) * 2.0)); - if (mTurretMotor.hadStartupError()) { logError("Turret Motor Startup Failed"); @@ -122,7 +115,7 @@ public Launcher() reset(); mTurretMotor.setNeutral(); mFlywheelMasterMotor.setNeutral(); - + logInitialized(initSuccess); } @@ -245,8 +238,7 @@ public double calcRPS(double distance) */ public boolean inRange(Double distance) { - // TODO figure out actual bounds of the range and make a check for the turret - // rotation + // TODO figure out actual bounds of the range and make a check for the turret rotation boolean inRange = (distance < Units.feetToMeters(Constants.Launcher.MaxShootingDistance) || distance > Units.feetToMeters(Constants.Launcher.MinShootingDistance)); return inRange; @@ -285,7 +277,8 @@ public void setUpLookupTable(int size, double[] distances, double[] angles, doub public void zeroTurret() { - /*if (mTurretMotor.getOutputCurrent() > Constants.Launcher.kTurretStallAmps) + /* + if (mTurretMotor.getOutputCurrent() > Constants.Launcher.kTurretStallAmps) { mTurretEncoder.setPosition(45.0); mTurretMotor.setPercentOutput(0.0); @@ -294,7 +287,8 @@ public void zeroTurret() else if (!zeroed) { mTurretMotor.setPercentOutput(0.1); - }*/ + } + */ mTurretEncoder.setPosition(0.0); } diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/PanelRotator.java b/src/main/java/com/spartronics4915/frc2020/subsystems/PanelRotator.java index 84a202a..953ee66 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/PanelRotator.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/PanelRotator.java @@ -1,7 +1,6 @@ package com.spartronics4915.frc2020.subsystems; import com.spartronics4915.frc2020.Constants; -import com.spartronics4915.lib.hardware.motors.SensorModel; import com.spartronics4915.lib.hardware.motors.SpartronicsMax; import com.spartronics4915.lib.hardware.motors.SpartronicsMotor; import com.spartronics4915.lib.hardware.motors.SpartronicsSRX; @@ -35,7 +34,10 @@ public PanelRotator() mOpticalFlagUp = new DigitalInput(Constants.PanelRotator.kOpticalFlagUpId); mLimitSwitchDown = new DigitalInput(Constants.PanelRotator.kLimitSwitchDownId); mSpinMotor = SpartronicsMax.makeMotor(Constants.PanelRotator.kSpinMotorId); - mRaiseMotor = new SpartronicsSimulatedMotor(42);//SpartronicsSRX.makeMotor(Constants.PanelRotator.kRaiseMotorId); + mSpinMotor.setBrakeMode(true); + // mRaiseMotor = new SpartronicsSRX.makeMotor(Constants.PanelRotator.kRaiseMotorId); + // mRaiseMotor.setBrakeMode(true); + mRaiseMotor = new SpartronicsSimulatedMotor(42); // FIXME: we have enough pdp ports - add back before competitions mColorSensor = new ColorSensorV3(I2C.Port.kOnboard); mColorMatcher.addColorMatch(Constants.PanelRotator.kRedTarget); @@ -79,8 +81,7 @@ public void spin() mSpinMotor.setPercentOutput(Constants.PanelRotator.kSpinSpeed); } - // TODO: What will this return before Stage Two? - /** + /** TODO: what will happen before stage two? * Gets the color the robot needs to spin to through game specific messages * * @return A String color - either Red, Blue, Yellow, or Green @@ -90,38 +91,6 @@ public String getTargetColor() return DriverStation.getInstance().getGameSpecificMessage(); } - /** - * This gets the 18-bit output (max is 2^18 - 1, I think) - * - * @return a comma-separated String of raw RGB values - */ - public String get18BitRGB() - { - int red = mColorSensor.getRed(); - int green = mColorSensor.getGreen(); - int blue = mColorSensor.getBlue(); - - String RGB = red + ", " + green + ", " + blue; - - return RGB; - } - - /** - * This gets the 18-bit output but divided by 262143 to make a fraction between 0 & 1 - * - * @return a comma-separated String of RGB values, as a percentage - */ - public String getFloatRGB() - { - int redFloat = mColorSensor.getRed() / 262143; - int greenFloat = mColorSensor.getGreen() / 262143; - int blueFloat = mColorSensor.getBlue() / 262143; - - String RGB = redFloat + ", " + greenFloat + ", " + blueFloat; - - return RGB; - } - /** * Finds what actual color the color sensor is seeing. * @@ -194,6 +163,47 @@ public double getColorConfidence() return match.confidence; } + /** + * Finds the distance between the sensor and what it is looking at + * @return 11-bit (0-2047) value + */ + public int getDistance() + { + return mColorSensor.getProximity(); + } + + /** + * This gets the 18-bit output (max is 2^18 - 1, I think) + * + * @return a comma-separated String of raw RGB values + */ + public String get18BitRGB() + { + int red = mColorSensor.getRed(); + int green = mColorSensor.getGreen(); + int blue = mColorSensor.getBlue(); + + String RGB = red + ", " + green + ", " + blue; + + return RGB; + } + + /** + * This gets the 18-bit output but divided by 262143 to make a fraction between 0 & 1 + * + * @return a comma-separated String of RGB values, as a percentage + */ + public String getFloatRGB() + { + int redFloat = mColorSensor.getRed() / 262143; + int greenFloat = mColorSensor.getGreen() / 262143; + int blueFloat = mColorSensor.getBlue() / 262143; + + String RGB = redFloat + ", " + greenFloat + ", " + blueFloat; + + return RGB; + } + /** * Checks if the top optical flag is broken * @@ -201,7 +211,6 @@ public double getColorConfidence() */ public boolean getOpticalFlagUp() { - // TODO: Double-check this return mOpticalFlagUp.get(); } @@ -212,7 +221,6 @@ public boolean getOpticalFlagUp() */ public boolean getLimitSwitchDown() { - // TODO: Double-check this return mLimitSwitchDown.get(); } @@ -224,4 +232,14 @@ public void stop() mSpinMotor.setPercentOutput(0); mRaiseMotor.setPercentOutput(0); } + + @Override + public void periodic() + { + dashboardPutNumber("IR distance: ", getDistance()); + dashboardPutString("Color seen by robot: ", getActualColor()); + dashboardPutString("Color seen by FMS: ", getRotatedColor()); + dashboardPutNumber("Color match confidence: ", getColorConfidence()); + dashboardPutString("Color sensor target (what the FIELD wants to see): ", getTargetColor()); + } } diff --git a/src/main/java/com/spartronics4915/frc2020/subsystems/Vision.java b/src/main/java/com/spartronics4915/frc2020/subsystems/Vision.java index 8815f3a..350370f 100644 --- a/src/main/java/com/spartronics4915/frc2020/subsystems/Vision.java +++ b/src/main/java/com/spartronics4915/frc2020/subsystems/Vision.java @@ -27,16 +27,16 @@ import edu.wpi.first.wpilibj.Timer; /** - * The Vision subsystem has these responsibilities - * + * The Vision subsystem has these responsibilities + * * - to listen for vision coprocessor results and use it to estimate * robot pose. NB: this estimate is for a moment in the past associated * with the timestamp delivered with the vision target position. * - to turn on & off the vision LED - * - * We estimate the robot pose whenever we receive a vision target update from - * raspi. This pose should be delivered to the CoordSysManager. - * NB: there's an alternate operating mode where we the raspi-Vision + * + * We estimate the robot pose whenever we receive a vision target update from + * raspi. This pose should be delivered to the CoordSysManager. + * NB: there's an alternate operating mode where we the raspi-Vision * code computes/estimates robot pose. In that case our job would be * to ensure that all required robot state would be presented to * the network tables. @@ -50,8 +50,7 @@ public class Vision extends SpartronicsSubsystem private final NetworkTableInstance mNetTab; private List mListeners; private Deque mVisionEstimates; - private final Relay mLEDRelay; - private final Launcher mLauncher; + private final Relay mLEDRelay; /* NB: our targets are measured in inches, while RobotStateMap * operates in meters! @@ -65,16 +64,14 @@ public class Vision extends SpartronicsSubsystem * @param rse * @param launcherSubsys */ - public Vision(RobotStateEstimator rse, Launcher launcher) + public Vision(RobotStateEstimator rse) { this.mOfficialRSM = rse.getBestRobotStateMap(); - this.mLauncher = launcher; this.mCoordSysMgr = new CoordSysMgr2020(); // our private copy this.mNetTab = NetworkTableInstance.getDefault(); - this.mNetTab.addEntryListener(Constants.Vision.kTargetResultKey, - this::visionTargetUpdate, - EntryListenerFlags.kUpdate); + this.mNetTab.addEntryListener(Constants.Vision.kTargetResultKey, + this::visionTargetUpdate, EntryListenerFlags.kUpdate); this.mListeners = new ArrayList(); this.mVisionEstimates = new ArrayDeque(); this.dashboardPutString(Constants.Vision.kStatusKey, "ready+waiting"); @@ -82,8 +79,8 @@ public Vision(RobotStateEstimator rse, Launcher launcher) this.mLEDRelay = new Relay(Constants.Vision.kLEDRelayPin); mLEDRelay.set(Relay.Value.kOn); /// XXX: set the relay into a known/desired state! - this.dashboardPutString(Constants.Vision.kLEDRelayKey, - this.mLEDRelay.get().toString()); + this.dashboardPutString(Constants.Vision.kLEDRelayKey, + this.mLEDRelay.get().toString()); } /* VisionEvents -----------------------------------------------*/ @@ -91,7 +88,7 @@ public Vision(RobotStateEstimator rse, Launcher launcher) * register interest in vision events - caller may subclass * VisionEvent. Alternative is just to request access to * the mVisionEstimates queue. - * @param l - + * @param l - */ public void registerTargetListener(VisionEvent l) { @@ -146,7 +143,7 @@ private void visionTargetUpdate(EntryNotification event) double timestamp = Double.parseDouble(vals[3]); RobotStateMap.State officialState = mOfficialRSM.get(timestamp); double turretAngle; - if(vals.length == 5) + if (vals.length == 5) turretAngle = Double.parseDouble(vals[4]); else { @@ -177,7 +174,7 @@ private void visionTargetUpdate(EntryNotification event) double robotHeading = r2d.getDegrees(); Vec3 fieldTarget; // Our target is at field heading == -180 since the turret is - // mounted on back.. + // mounted on back.. if ((robotHeading < 90 && robotHeading > -90) || robotHeading > 270) { // We're more likely to see theirs than ours. @@ -193,11 +190,10 @@ private void visionTargetUpdate(EntryNotification event) this.mCoordSysMgr.updateTurretAngle(turretAngle); this.mCoordSysMgr.updateRobotPose(robotHeading, tgtInRobot, fieldTarget); Vec3 robotPos = mCoordSysMgr.robotPointToField(Vec3.ZeroPt); - // Use robot's heading in our poseEstimate - remember to convert + // Use robot's heading in our poseEstimate - remember to convert // from inches to meters before commiting to RSM. - Pose2d poseEstimate = new Pose2d(Units.inchesToMeters(robotPos.getX()), - Units.inchesToMeters(robotPos.getY()), - r2d); + Pose2d poseEstimate = new Pose2d(Units.inchesToMeters(robotPos.getX()), + Units.inchesToMeters(robotPos.getY()), r2d); Iterator it = this.mListeners.iterator(); while (it.hasNext()) { @@ -211,14 +207,13 @@ private void visionTargetUpdate(EntryNotification event) // now measure the distance between our estimate and the // official robot estimate (at timestamp). - double derror = robotPos.subtract(Units.metersToInches(t2d.getX()), - Units.metersToInches(t2d.getY()), - 0).length(); + double derror = robotPos.subtract(Units.metersToInches(t2d.getX()), + Units.metersToInches(t2d.getY()), 0).length(); this.dashboardPutNumber(Constants.Vision.kPoseErrorKey, derror); // NB: robotPos is in inches! (dashboard too!) - String pstr = String.format("%g %g %g", - robotPos.getX(), robotPos.getY(), robotHeading); + String pstr = String.format("%g %g %g", + robotPos.getX(), robotPos.getY(), robotHeading); this.dashboardPutString(Constants.Vision.kPoseEstimateKey, pstr); double delay = Timer.getFPGATimestamp() - timestamp; @@ -228,17 +223,14 @@ private void visionTargetUpdate(EntryNotification event) // offset. This is a "forward" estimate of our well-known // landmarks. Hopefully these will produce nearly constant // and correct (!) results. - mCoordSysMgr.updateRobotPose(Units.metersToInches(t2d.getX()), - Units.metersToInches(t2d.getY()), - r2d.getDegrees()); + mCoordSysMgr.updateRobotPose(Units.metersToInches(t2d.getX()), + Units.metersToInches(t2d.getY()), r2d.getDegrees()); Vec3 tgtInField = mCoordSysMgr.camPointToField(tgtInCam); - String key = (fieldTarget == this.mOurTarget) ? - Constants.Vision.kOurGoalEstimateKey : - Constants.Vision.kTheirGoalEstimateKey; + String key = (fieldTarget == this.mOurTarget) ? + Constants.Vision.kOurGoalEstimateKey : Constants.Vision.kTheirGoalEstimateKey; this.dashboardPutString(key, tgtInField.asPointString()); } else this.logError("Vision target value must be a string"); } - }