mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[cmd3] Add example and template projects for commands v3 and make hatchbot example idiomatic (#8887)
The `command3` example project contains a program that could plausibly play in the 2026 rebuilt game. It includes nested mechanisms (`Intake` has an inner `IntakeWrist` and `IntakeRoller` and is similar to the v2 superstructure concept), swerve drive with localization and path following (albeit stubbed for sake of example), opmodes and opmode-scoped commands, and command-scoped triggers. The template projects are basic skeletons. The larger template includes a basic command that just increments and prints a counter variable every time it runs. The hatchbot v3 example has been refactored to be more idiomatic: - `RobotContainer` removed - "Subsystem" names in packages, comments, and classes has been replaced with "Mechanism" - Some v2-specific comments and structures have been reworded or deleted - The Drive mechanism now provides commands for arcade drive and driving a distance, instead of exposing public methods that write directly to hardware (which broke encapsulation and made it possible to issue conflicting hardware requests)
This commit is contained in:
Sam Carlberg
@@ -10,6 +10,22 @@
|
||||
"robotclass": "Robot",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "Commands v3 - REBUILT Robot",
|
||||
"description": "A robot with a drivetrain, intake, and shooter subsystems controlled by the commands v3 framework",
|
||||
"tags": [
|
||||
"Complete Robot",
|
||||
"Commandv3",
|
||||
"DataLog",
|
||||
"Joystick",
|
||||
"Swerve Drive",
|
||||
"OpMode"
|
||||
],
|
||||
"foldername": "rebuiltcmdv3",
|
||||
"gradlebase": "java",
|
||||
"robotclass": "Robot",
|
||||
"commandversion": 3
|
||||
},
|
||||
{
|
||||
"name": "Encoder",
|
||||
"description": "View values from a quadrature encoder.",
|
||||
|
||||
@@ -38,7 +38,7 @@ public final class Constants {
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final double kAutoDriveDistanceInches = 60;
|
||||
public static final double kAutoBackupDistanceInches = 20;
|
||||
public static final double kAutoBackupDistanceInches = -20;
|
||||
public static final double kAutoDriveSpeed = 0.5;
|
||||
}
|
||||
|
||||
|
||||
@@ -6,8 +6,14 @@ package org.wpilib.examples.hatchbotcmdv3;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Scheduler;
|
||||
import org.wpilib.command3.button.CommandGamepad;
|
||||
import org.wpilib.driverstation.DriverStation;
|
||||
import org.wpilib.examples.hatchbotcmdv3.commands.Autos;
|
||||
import org.wpilib.examples.hatchbotcmdv3.mechanisms.DriveMechanism;
|
||||
import org.wpilib.examples.hatchbotcmdv3.mechanisms.HatchMechanism;
|
||||
import org.wpilib.framework.TimedRobot;
|
||||
import org.wpilib.smartdashboard.SendableChooser;
|
||||
import org.wpilib.smartdashboard.SmartDashboard;
|
||||
import org.wpilib.system.DataLogManager;
|
||||
|
||||
/**
|
||||
@@ -16,18 +22,41 @@ import org.wpilib.system.DataLogManager;
|
||||
* this project, you must also update the Main.java file in the project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private Command autonomousCommand;
|
||||
/**
|
||||
* A chooser for autonomous commands. Drivers can choose between different autonomous modes on a
|
||||
* dashboard before the start of a match.
|
||||
*/
|
||||
private final SendableChooser<Command> autonomousChooser = new SendableChooser<>();
|
||||
|
||||
private final RobotContainer robotContainer;
|
||||
// The driver's controller
|
||||
private final CommandGamepad driverController =
|
||||
new CommandGamepad(Constants.OIConstants.kDriverControllerPort);
|
||||
|
||||
// The robot's mechanisms
|
||||
private final DriveMechanism robotDrive = new DriveMechanism();
|
||||
private final HatchMechanism hatchMechanism = new HatchMechanism();
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be used for any
|
||||
* initialization code.
|
||||
*/
|
||||
public Robot() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
robotContainer = new RobotContainer();
|
||||
// Configure the button bindings
|
||||
configureButtonBindings();
|
||||
|
||||
// Configure default commands
|
||||
// Set the default drive command to split-stick arcade drive with forward/backward controlled by
|
||||
// the left hand, and turning controlled by the right.
|
||||
robotDrive.setDefaultCommand(
|
||||
robotDrive.arcadeDrive(
|
||||
() -> -driverController.getLeftY(), () -> -driverController.getRightX()));
|
||||
|
||||
// Add commands to the autonomous command chooser
|
||||
autonomousChooser.setDefaultOption("Simple Auto", Autos.simpleAuto(robotDrive));
|
||||
autonomousChooser.addOption("Complex Auto", Autos.complexAuto(robotDrive, hatchMechanism));
|
||||
|
||||
// Put the chooser on the dashboard
|
||||
SmartDashboard.putData("Autonomous", autonomousChooser);
|
||||
|
||||
// Start recording to data log
|
||||
DataLogManager.start();
|
||||
@@ -37,6 +66,19 @@ public class Robot extends TimedRobot {
|
||||
DriverStation.startDataLog(DataLogManager.getLog(), true);
|
||||
}
|
||||
|
||||
/** Use this method to define your button->command mappings. */
|
||||
private void configureButtonBindings() {
|
||||
// Grab the hatch when the Circle button is pressed.
|
||||
driverController.faceRight().onTrue(hatchMechanism.grabHatchCommand());
|
||||
// Release the hatch when the Square button is pressed.
|
||||
driverController.faceLeft().onTrue(hatchMechanism.releaseHatchCommand());
|
||||
// While holding R1, drive at half speed
|
||||
driverController
|
||||
.rightBumper()
|
||||
.onTrue(Command.noRequirements(_ -> robotDrive.setMaxOutput(0.5)).named("Set half speed"))
|
||||
.onFalse(Command.noRequirements(_ -> robotDrive.setMaxOutput(1)).named("Set full speed"));
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
@@ -46,10 +88,10 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
// Runs the scheduler. This is responsible for polling buttons, running scheduled commands,
|
||||
// and removing finished or interrupted commands.
|
||||
// This must be called from the robot's periodic block in order for anything in the
|
||||
// Command-based framework to work.
|
||||
Scheduler.getDefault().run();
|
||||
}
|
||||
|
||||
@@ -60,12 +102,14 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
|
||||
/** This autonomous runs the autonomous command selected by the {@link #autonomousChooser}. */
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
autonomousCommand = robotContainer.getAutonomousCommand();
|
||||
Command autonomousCommand = autonomousChooser.getSelected();
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
// Schedule the autonomous command.
|
||||
// Because we schedule this command in the autonomous mode, it will be automatically canceled
|
||||
// when the autonomous mode ends.
|
||||
if (autonomousCommand != null) {
|
||||
Scheduler.getDefault().schedule(autonomousCommand);
|
||||
}
|
||||
@@ -76,15 +120,7 @@ public class Robot extends TimedRobot {
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (autonomousCommand != null) {
|
||||
Scheduler.getDefault().cancel(autonomousCommand);
|
||||
}
|
||||
}
|
||||
public void teleopInit() {}
|
||||
|
||||
/** This function is called periodically during operator control. */
|
||||
@Override
|
||||
@@ -92,7 +128,7 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
// Cancel all running commands at the start of utility mode.
|
||||
Scheduler.getDefault().cancelAll();
|
||||
}
|
||||
|
||||
|
||||
@@ -1,96 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.hatchbotcmdv3;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.button.CommandGamepad;
|
||||
import org.wpilib.driverstation.Gamepad;
|
||||
import org.wpilib.examples.hatchbotcmdv3.Constants.OIConstants;
|
||||
import org.wpilib.examples.hatchbotcmdv3.commands.Autos;
|
||||
import org.wpilib.examples.hatchbotcmdv3.subsystems.DriveSubsystem;
|
||||
import org.wpilib.examples.hatchbotcmdv3.subsystems.HatchSubsystem;
|
||||
import org.wpilib.smartdashboard.SendableChooser;
|
||||
import org.wpilib.smartdashboard.SmartDashboard;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
|
||||
* subsystems, commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
// The robot's subsystems
|
||||
private final DriveSubsystem robotDrive = new DriveSubsystem();
|
||||
private final HatchSubsystem hatchSubsystem = new HatchSubsystem();
|
||||
|
||||
// Retained command handles
|
||||
|
||||
// The autonomous routines
|
||||
// A simple auto routine that drives forward a specified distance, and then stops.
|
||||
private final Command simpleAuto = Autos.simpleAuto(robotDrive);
|
||||
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
|
||||
private final Command complexAuto = Autos.complexAuto(robotDrive, hatchSubsystem);
|
||||
|
||||
// A chooser for autonomous commands
|
||||
SendableChooser<Command> chooser = new SendableChooser<>();
|
||||
|
||||
// The driver's controller
|
||||
CommandGamepad driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
|
||||
|
||||
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||
public RobotContainer() {
|
||||
// Configure the button bindings
|
||||
configureButtonBindings();
|
||||
|
||||
// Configure default commands
|
||||
// Set the default drive command to split-stick arcade drive
|
||||
robotDrive.setDefaultCommand(
|
||||
// A split-stick arcade command, with forward/backward controlled by the left
|
||||
// hand, and turning controlled by the right.
|
||||
robotDrive
|
||||
.runRepeatedly(
|
||||
() ->
|
||||
robotDrive.arcadeDrive(
|
||||
-driverController.getLeftY(), -driverController.getRightX()))
|
||||
.withPriority(Command.LOWEST_PRIORITY)
|
||||
.named("Split-Stick Arcade Drive (Default Command)"));
|
||||
|
||||
// Add commands to the autonomous command chooser
|
||||
chooser.setDefaultOption("Simple Auto", simpleAuto);
|
||||
chooser.addOption("Complex Auto", complexAuto);
|
||||
|
||||
// Put the chooser on the dashboard
|
||||
SmartDashboard.putData("Autonomous", chooser);
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be created by
|
||||
* instantiating a {@link org.wpilib.driverstation.GenericHID} or one of its subclasses ({@link
|
||||
* org.wpilib.driverstation.Joystick} or {@link Gamepad}), and then passing it to a {@link
|
||||
* org.wpilib.command3.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
// Grab the hatch when the Circle button is pressed.
|
||||
driverController.faceRight().onTrue(hatchSubsystem.grabHatchCommand());
|
||||
// Release the hatch when the Square button is pressed.
|
||||
driverController.faceLeft().onTrue(hatchSubsystem.releaseHatchCommand());
|
||||
// While holding R1, drive at half speed
|
||||
driverController
|
||||
.rightBumper()
|
||||
.onTrue(
|
||||
Command.noRequirements(coro -> robotDrive.setMaxOutput(0.5)).named("Set half speed"))
|
||||
.onFalse(
|
||||
Command.noRequirements(coro -> robotDrive.setMaxOutput(1)).named("Set full speed"));
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
return chooser.getSelected();
|
||||
}
|
||||
}
|
||||
@@ -6,81 +6,36 @@ package org.wpilib.examples.hatchbotcmdv3.commands;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.examples.hatchbotcmdv3.Constants.AutoConstants;
|
||||
import org.wpilib.examples.hatchbotcmdv3.subsystems.DriveSubsystem;
|
||||
import org.wpilib.examples.hatchbotcmdv3.subsystems.HatchSubsystem;
|
||||
import org.wpilib.examples.hatchbotcmdv3.mechanisms.DriveMechanism;
|
||||
import org.wpilib.examples.hatchbotcmdv3.mechanisms.HatchMechanism;
|
||||
|
||||
/** Container for auto command factories. */
|
||||
public final class Autos {
|
||||
/** A simple auto routine that drives forward a specified distance, and then stops. */
|
||||
public static Command simpleAuto(DriveSubsystem drive) {
|
||||
return drive
|
||||
.run(
|
||||
coro -> {
|
||||
drive.resetEncoders();
|
||||
while (drive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches) {
|
||||
drive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0);
|
||||
coro.yield();
|
||||
}
|
||||
drive.arcadeDrive(0, 0);
|
||||
})
|
||||
.whenCanceled(
|
||||
() -> {
|
||||
drive.arcadeDrive(0, 0);
|
||||
})
|
||||
.named("Simple Auto");
|
||||
/** A simple auto routine that drives forward a specified distance and then stops. */
|
||||
public static Command simpleAuto(DriveMechanism drive) {
|
||||
return drive.driveDistance(
|
||||
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed);
|
||||
}
|
||||
|
||||
/** A complex auto routine that drives forward, drops a hatch, and then drives backward. */
|
||||
public static Command complexAuto(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
|
||||
public static Command complexAuto(DriveMechanism driveMechanism, HatchMechanism hatchMechanism) {
|
||||
// NOTE: requirement behavior.
|
||||
// To require each mechanism for while it's active, replace `requiring` with `noRequirements`.
|
||||
return Command.requiring(driveSubsystem, hatchSubsystem)
|
||||
return Command.requiring(driveMechanism, hatchMechanism)
|
||||
.executing(
|
||||
coro -> {
|
||||
// Drive forward up to the front of the cargo ship
|
||||
coro.await(
|
||||
driveSubsystem
|
||||
.run(
|
||||
coro2 -> {
|
||||
// Reset encoders on command start
|
||||
driveSubsystem.resetEncoders();
|
||||
// End the command when the robot's driven distance exceeds the desired
|
||||
// value
|
||||
while (driveSubsystem.getAverageEncoderDistance()
|
||||
>= AutoConstants.kAutoDriveDistanceInches) {
|
||||
// Drive forward while the command is executing
|
||||
driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0);
|
||||
coro2.yield();
|
||||
}
|
||||
// Stop driving at the end of the command
|
||||
driveSubsystem.arcadeDrive(0, 0);
|
||||
})
|
||||
.whenCanceled(() -> driveSubsystem.arcadeDrive(0, 0))
|
||||
.named("Part 1"));
|
||||
driveMechanism.driveDistance(
|
||||
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed));
|
||||
|
||||
// Release the hatch
|
||||
coro.await(hatchSubsystem.releaseHatchCommand());
|
||||
coro.await(hatchMechanism.releaseHatchCommand());
|
||||
|
||||
// Drive backward the specified distance
|
||||
coro.await(
|
||||
driveSubsystem
|
||||
.run(
|
||||
coro2 -> {
|
||||
// Reset encoders on command start
|
||||
driveSubsystem.resetEncoders();
|
||||
// End the command when the robot's driven distance exceeds the desired
|
||||
// value
|
||||
while (driveSubsystem.getAverageEncoderDistance()
|
||||
>= AutoConstants.kAutoDriveDistanceInches) {
|
||||
// Drive backward while the command is executing
|
||||
driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0);
|
||||
coro2.yield();
|
||||
}
|
||||
// Stop driving at the end of the command
|
||||
driveSubsystem.arcadeDrive(0, 0);
|
||||
})
|
||||
.whenCanceled(() -> driveSubsystem.arcadeDrive(0, 0))
|
||||
.named("Part 3"));
|
||||
driveMechanism.driveDistance(
|
||||
AutoConstants.kAutoBackupDistanceInches, AutoConstants.kAutoDriveSpeed));
|
||||
})
|
||||
.named("Complex Auto");
|
||||
}
|
||||
|
||||
@@ -2,8 +2,10 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.hatchbotcmdv3.subsystems;
|
||||
package org.wpilib.examples.hatchbotcmdv3.mechanisms;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.drive.DifferentialDrive;
|
||||
import org.wpilib.examples.hatchbotcmdv3.Constants.DriveConstants;
|
||||
@@ -11,7 +13,7 @@ import org.wpilib.hardware.motor.PWMSparkMax;
|
||||
import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.util.sendable.SendableRegistry;
|
||||
|
||||
public class DriveSubsystem implements Mechanism {
|
||||
public class DriveMechanism implements Mechanism {
|
||||
// The motors on the left side of the drive.
|
||||
private final PWMSparkMax leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
|
||||
private final PWMSparkMax leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
|
||||
@@ -38,8 +40,8 @@ public class DriveSubsystem implements Mechanism {
|
||||
DriveConstants.kRightEncoderPorts[1],
|
||||
DriveConstants.kRightEncoderReversed);
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
/** Creates a new DriveMechanism. */
|
||||
public DriveMechanism() {
|
||||
SendableRegistry.addChild(drive, leftLeader);
|
||||
SendableRegistry.addChild(drive, rightLeader);
|
||||
|
||||
@@ -59,15 +61,50 @@ public class DriveSubsystem implements Mechanism {
|
||||
/**
|
||||
* Drives the robot using arcade controls.
|
||||
*
|
||||
* @param fwd the commanded forward movement
|
||||
* @param rot the commanded rotation
|
||||
* @param speed The speed to drive at, from -1 (full reverse) to +1 (full forward)
|
||||
* @param turn The turn rate, from -1 (full counterclockwise) to +1 (full clockwise)
|
||||
* @return A command that drives the robot using arcade controls
|
||||
*/
|
||||
public void arcadeDrive(double fwd, double rot) {
|
||||
drive.arcadeDrive(fwd, rot);
|
||||
public Command arcadeDrive(DoubleSupplier speed, DoubleSupplier turn) {
|
||||
return runRepeatedly(() -> drive.arcadeDrive(speed.getAsDouble(), turn.getAsDouble()))
|
||||
.named("Arcade Drive");
|
||||
}
|
||||
|
||||
/**
|
||||
* Drives a set distance, in inches, at a set speed ratio from 0-1. The command will exit when the
|
||||
* distance has been reached.
|
||||
*
|
||||
* @param distance How many inches to travel. This may be negative to drive backwards.
|
||||
* @param speed Speed ratio from 0 (off) to 1 (full speed). Negative values will make the robot
|
||||
* drive the wrong way.
|
||||
* @return A command that drives a specified distance.
|
||||
*/
|
||||
public Command driveDistance(double distance, double speed) {
|
||||
return run(coroutine -> {
|
||||
resetEncoders();
|
||||
|
||||
if (distance >= 0) {
|
||||
// drive forward
|
||||
while (getAverageEncoderDistance() < distance) {
|
||||
drive.tankDrive(speed, 0);
|
||||
coroutine.yield();
|
||||
}
|
||||
} else {
|
||||
// drive backward
|
||||
while (getAverageEncoderDistance() > distance) {
|
||||
drive.tankDrive(-speed, 0);
|
||||
coroutine.yield();
|
||||
}
|
||||
}
|
||||
|
||||
// Finally, stop
|
||||
drive.stopMotor();
|
||||
})
|
||||
.named("Drive Distance[" + distance + "@" + speed + "]");
|
||||
}
|
||||
|
||||
/** Resets the drive encoders to currently read a position of 0. */
|
||||
public void resetEncoders() {
|
||||
private void resetEncoders() {
|
||||
leftEncoder.reset();
|
||||
rightEncoder.reset();
|
||||
}
|
||||
@@ -2,7 +2,7 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.hatchbotcmdv3.subsystems;
|
||||
package org.wpilib.examples.hatchbotcmdv3.mechanisms;
|
||||
|
||||
import static org.wpilib.hardware.pneumatic.DoubleSolenoid.Value.FORWARD;
|
||||
import static org.wpilib.hardware.pneumatic.DoubleSolenoid.Value.REVERSE;
|
||||
@@ -14,7 +14,7 @@ import org.wpilib.hardware.pneumatic.DoubleSolenoid;
|
||||
import org.wpilib.hardware.pneumatic.PneumaticsModuleType;
|
||||
|
||||
/** A hatch mechanism actuated by a single {@link org.wpilib.hardware.pneumatic.DoubleSolenoid}. */
|
||||
public class HatchSubsystem implements Mechanism {
|
||||
public class HatchMechanism implements Mechanism {
|
||||
private final DoubleSolenoid hatchSolenoid =
|
||||
new DoubleSolenoid(
|
||||
0,
|
||||
@@ -0,0 +1,67 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3;
|
||||
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.DriveConstants;
|
||||
import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
|
||||
import org.wpilib.math.geometry.Pose2d;
|
||||
import org.wpilib.math.geometry.Rectangle2d;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
import org.wpilib.math.kinematics.SwerveModulePosition;
|
||||
|
||||
@Logged
|
||||
public class PoseEstimator {
|
||||
private final SwerveDrivePoseEstimator poseEstimator =
|
||||
new SwerveDrivePoseEstimator(
|
||||
DriveConstants.KINEMATICS,
|
||||
Rotation2d.kZero,
|
||||
new SwerveModulePosition[] {
|
||||
new SwerveModulePosition(),
|
||||
new SwerveModulePosition(),
|
||||
new SwerveModulePosition(),
|
||||
new SwerveModulePosition()
|
||||
},
|
||||
Pose2d.kZero);
|
||||
|
||||
/**
|
||||
* Updates the pose estimator with the current gyro heading and swerve module positions.
|
||||
*
|
||||
* @param gyroHeading The current gyro heading
|
||||
* @param modulePositions The current module positions
|
||||
*/
|
||||
public void odometryUpdate(Rotation2d gyroHeading, SwerveModulePosition... modulePositions) {
|
||||
poseEstimator.update(gyroHeading, modulePositions);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the pose estimator with a vision measurement.
|
||||
*
|
||||
* @param estimatedPose The estimated pose from vision
|
||||
* @param timestamp The timestamp of the vision measurement
|
||||
*/
|
||||
public void visionUpdate(Pose2d estimatedPose, double timestamp) {
|
||||
poseEstimator.addVisionMeasurement(estimatedPose, timestamp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current estimated pose of the robot.
|
||||
*
|
||||
* @return The current estimated pose
|
||||
*/
|
||||
public Pose2d getEstimatedPose() {
|
||||
return poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the current estimated pose is within a given rectangular zone.
|
||||
*
|
||||
* @param bounds The rectangular zone to check against
|
||||
* @return True if the pose is within the zone, false otherwise
|
||||
*/
|
||||
public boolean inZone(Rectangle2d bounds) {
|
||||
return bounds.contains(poseEstimator.getEstimatedPosition().getTranslation());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3;
|
||||
|
||||
import static org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants.NEUTRAL_ZONE;
|
||||
import static org.wpilib.units.Units.Meters;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Scheduler;
|
||||
import org.wpilib.command3.Trigger;
|
||||
import org.wpilib.command3.button.CommandGamepad;
|
||||
import org.wpilib.epilogue.Epilogue;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.mechanisms.Intake;
|
||||
import org.wpilib.examples.rebuiltcmdv3.mechanisms.Shooter;
|
||||
import org.wpilib.examples.rebuiltcmdv3.mechanisms.SwerveDrive;
|
||||
import org.wpilib.framework.OpModeRobot;
|
||||
import org.wpilib.math.geometry.Pose2d;
|
||||
|
||||
@Logged
|
||||
public class Robot extends OpModeRobot {
|
||||
public final SwerveDrive swerveDrive = new SwerveDrive();
|
||||
public final Shooter shooter = new Shooter();
|
||||
public final Intake intake = new Intake();
|
||||
public final PoseEstimator poseEstimator = new PoseEstimator();
|
||||
public final CommandGamepad controller = new CommandGamepad(1);
|
||||
|
||||
public final Trigger inNeutralZone = new Trigger(() -> poseEstimator.inZone(NEUTRAL_ZONE));
|
||||
|
||||
/** Initializes the robot class and sets safe default commands for all its mechanisms. */
|
||||
public Robot() {
|
||||
swerveDrive.setDefaultCommand(swerveDrive.idle());
|
||||
shooter.setDefaultCommand(shooter.idle());
|
||||
intake.setDefaultCommand(intake.idle());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// 1. Update odometry so commands will be working with up-to-date information.
|
||||
poseEstimator.odometryUpdate(swerveDrive.getGyroHeading(), swerveDrive.getModulePositions());
|
||||
|
||||
// 2. Run the scheduler to poll triggers and execute our commands.
|
||||
Scheduler.getDefault().run();
|
||||
|
||||
// 3. Update telemetry.
|
||||
Epilogue.update(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* A command that uses all the mechanisms on the robot to aim and shoot at a specific target.
|
||||
*
|
||||
* @param targetPose The target position to shoot at.
|
||||
* @return A command to shoot at a target
|
||||
*/
|
||||
public Command shootAt(Pose2d targetPose) {
|
||||
return Command.noRequirements(
|
||||
coroutine -> {
|
||||
coroutine.await(swerveDrive.aimAt(poseEstimator::getEstimatedPose, targetPose));
|
||||
|
||||
coroutine.awaitAll(
|
||||
intake.agitate(),
|
||||
shooter.shootAtHub(
|
||||
() -> {
|
||||
return Meters.of(
|
||||
targetPose
|
||||
.minus(poseEstimator.getEstimatedPose())
|
||||
.getTranslation()
|
||||
.getNorm());
|
||||
}));
|
||||
})
|
||||
.named("Robot.ShootAt[" + targetPose + "]");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.constants;
|
||||
|
||||
import static org.wpilib.units.Units.FeetPerSecond;
|
||||
import static org.wpilib.units.Units.RotationsPerSecond;
|
||||
|
||||
import org.wpilib.math.kinematics.SwerveDriveKinematics;
|
||||
import org.wpilib.units.measure.AngularVelocity;
|
||||
import org.wpilib.units.measure.LinearVelocity;
|
||||
|
||||
public final class DriveConstants {
|
||||
public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics();
|
||||
public static final LinearVelocity MAX_VELOCITY = FeetPerSecond.of(15);
|
||||
public static final AngularVelocity MAX_TURN_RATE = RotationsPerSecond.of(3);
|
||||
|
||||
// All drive IDs are odd, all turn IDs are even
|
||||
public static final int FRONT_LEFT_DRIVE_ID = 1;
|
||||
public static final int FRONT_LEFT_TURN_ID = 2;
|
||||
public static final int FRONT_RIGHT_DRIVE_ID = 3;
|
||||
public static final int FRONT_RIGHT_TURN_ID = 4;
|
||||
public static final int REAR_LEFT_DRIVE_ID = 5;
|
||||
public static final int REAR_LEFT_TURN_ID = 6;
|
||||
public static final int REAR_RIGHT_DRIVE_ID = 7;
|
||||
public static final int REAR_RIGHT_TURN_ID = 8;
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.constants;
|
||||
|
||||
import java.util.Optional;
|
||||
import org.wpilib.driverstation.internal.DriverStationBackend;
|
||||
import org.wpilib.math.geometry.Pose2d;
|
||||
import org.wpilib.math.geometry.Rectangle2d;
|
||||
import org.wpilib.math.geometry.Translation2d;
|
||||
|
||||
public final class FieldConstants {
|
||||
// TODO: Measurements
|
||||
public static final Pose2d RED_HUB_CENTER = new Pose2d();
|
||||
public static final Pose2d BLUE_HUB_CENTER = new Pose2d();
|
||||
public static final Rectangle2d NEUTRAL_ZONE =
|
||||
new Rectangle2d(new Translation2d(), new Translation2d());
|
||||
public static final Rectangle2d RED_ZONE =
|
||||
new Rectangle2d(new Translation2d(), new Translation2d());
|
||||
public static final Rectangle2d BLUE_ZONE =
|
||||
new Rectangle2d(new Translation2d(), new Translation2d());
|
||||
|
||||
/**
|
||||
* Gets location of our hub. Returns an empty optional if we don't have an alliance yet.
|
||||
*
|
||||
* @return The location of our hub.
|
||||
*/
|
||||
public static Optional<Pose2d> targetHub() {
|
||||
return DriverStationBackend.getAlliance()
|
||||
.map(
|
||||
a -> {
|
||||
return switch (a) {
|
||||
case RED -> RED_HUB_CENTER;
|
||||
case BLUE -> BLUE_HUB_CENTER;
|
||||
};
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.constants;
|
||||
|
||||
import static org.wpilib.units.Units.Degrees;
|
||||
import static org.wpilib.units.Units.RPM;
|
||||
|
||||
import org.wpilib.units.measure.Angle;
|
||||
import org.wpilib.units.measure.AngularVelocity;
|
||||
|
||||
public final class IntakeConstants {
|
||||
public static final int ROLLER_MOTOR_ID = 20;
|
||||
public static final int WRIST_MOTOR_ID = 21;
|
||||
|
||||
public static final Angle WRIST_STOW_ANGLE = Degrees.of(90);
|
||||
public static final Angle WRIST_DOWN_ANGLE = Degrees.of(-10);
|
||||
|
||||
public static final Angle WRIST_AGITATE_UP = Degrees.of(10);
|
||||
public static final Angle WRIST_AGITATE_DOWN = WRIST_DOWN_ANGLE;
|
||||
|
||||
public static final AngularVelocity ROLLER_INTAKE_SPEED = RPM.of(4000);
|
||||
public static final AngularVelocity ROLLER_EXPULSION_SPEED = RPM.of(-3500);
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.constants;
|
||||
|
||||
public final class ShooterConstants {
|
||||
public static final int HOOD_MOTOR_ID = 10;
|
||||
public static final int PRIMARY_SHOOTER_MOTOR_ID = 11;
|
||||
public static final int SECONDARY_SHOOTER_MOTOR_ID = 12;
|
||||
public static final int TERTIARY_SHOOTER_MOTOR_ID = 13;
|
||||
public static final int QUATERNARY_SHOOTER_MOTOR_ID = 14;
|
||||
|
||||
public static final double HOOD_KP = 80;
|
||||
|
||||
/** Example motor velocity feedforward constant, in volts per RPM. */
|
||||
public static final double EXAMPLE_MOTOR_KV = 12d / 6000d;
|
||||
|
||||
/** Voltage necessary to break static friction to get the flywheel moving. */
|
||||
public static final double FLYWHEEL_KS = 1.25;
|
||||
|
||||
public static final double FLYWHEEL_KP = 100;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.lookup;
|
||||
|
||||
import static org.wpilib.units.Units.Value;
|
||||
|
||||
import org.wpilib.math.interpolation.Interpolator;
|
||||
import org.wpilib.math.interpolation.InverseInterpolator;
|
||||
import org.wpilib.units.Measure;
|
||||
import org.wpilib.units.Unit;
|
||||
import org.wpilib.units.measure.Dimensionless;
|
||||
|
||||
/** Utility class for working with unit-based interpolating tree maps. */
|
||||
public final class LookupTables {
|
||||
private LookupTables() {
|
||||
throw new UnsupportedOperationException("This is a utility class!");
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an interpolator operating on Measure objects.
|
||||
*
|
||||
* @param <U> The type of the unit for the measure
|
||||
* @param <M> The type of the measure
|
||||
* @return The interpolator for the measure
|
||||
*/
|
||||
@SuppressWarnings("unchecked")
|
||||
public static <U extends Unit, M extends Measure<U>> Interpolator<M> unitInterpolator() {
|
||||
return (startValue, endValue, t) ->
|
||||
(M) endValue.minus(startValue).times(Math.clamp(t, 0, 1)).plus(startValue);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an inverse interpolator operating on Measure objects.
|
||||
*
|
||||
* @param <U> The type of the unit for the measure
|
||||
* @param <M> The type of the measure
|
||||
* @return The inverse interpolator for the measure
|
||||
*/
|
||||
public static <U extends Unit, M extends Measure<U>>
|
||||
InverseInterpolator<M> inverseUnitInterpolator() {
|
||||
return (startValue, endValue, q) -> {
|
||||
Measure<U> totalRange = endValue.minus(startValue);
|
||||
if (totalRange.baseUnitMagnitude() <= 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
Measure<U> queryToStart = q.minus(startValue);
|
||||
if (queryToStart.baseUnitMagnitude() <= 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return ((Dimensionless) queryToStart.div(totalRange)).in(Value);
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.mechanisms;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants;
|
||||
|
||||
@Logged
|
||||
public class Intake implements Mechanism {
|
||||
private final IntakeWrist wrist = new IntakeWrist();
|
||||
private final IntakeRoller roller = new IntakeRoller();
|
||||
|
||||
/**
|
||||
* Intakes fuel off the ground. This command will run until canceled or interrupted by another
|
||||
* command.
|
||||
*
|
||||
* @return An intake command
|
||||
*/
|
||||
public Command intake() {
|
||||
return run(coroutine -> {
|
||||
coroutine.awaitAll(wrist.down(), roller.intake());
|
||||
})
|
||||
.named("Intake.Intake");
|
||||
}
|
||||
|
||||
/**
|
||||
* Stops the intake mechanism. This stops the roller and depowers the wrist, which may sag under
|
||||
* gravity.
|
||||
*
|
||||
* @return A command that stops the intake mechanism
|
||||
*/
|
||||
public Command stop() {
|
||||
return run(coroutine -> {
|
||||
coroutine.awaitAll(wrist.stop(), roller.stop());
|
||||
})
|
||||
.withPriority(Command.LOWEST_PRIORITY)
|
||||
.named("Intake.Idle");
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command idle() {
|
||||
return stop();
|
||||
}
|
||||
|
||||
/**
|
||||
* Stows the intake mechanism. This raises the wrist and stops the roller.
|
||||
*
|
||||
* @return A command that stows the intake mechanism
|
||||
*/
|
||||
public Command stow() {
|
||||
return run(coroutine -> {
|
||||
coroutine.awaitAll(wrist.stow(), roller.stop());
|
||||
})
|
||||
.named("Intake.Stow");
|
||||
}
|
||||
|
||||
/**
|
||||
* Uses the intake mechanism to agitate fuel inside the hopper. The roller will spin to encourage
|
||||
* fuel to move around and help unblock jams.
|
||||
*
|
||||
* @return A command that agitates the intake mechanism
|
||||
*/
|
||||
public Command agitate() {
|
||||
return run(coroutine -> {
|
||||
coroutine.fork(roller.intake());
|
||||
|
||||
while (true) {
|
||||
// await() will yield internally until the wrist has moved to the desired position,
|
||||
// so we don't need to explicitly yield here
|
||||
coroutine.await(wrist.moveToAngle(IntakeConstants.WRIST_AGITATE_UP));
|
||||
coroutine.await(wrist.moveToAngle(IntakeConstants.WRIST_AGITATE_DOWN));
|
||||
}
|
||||
})
|
||||
.named("Intake.Agitate");
|
||||
}
|
||||
|
||||
/**
|
||||
* Uses the intake mechanism to expel fuel from the hopper.
|
||||
*
|
||||
* @return A command that expels fuel from the intake mechanism
|
||||
*/
|
||||
public Command outtake() {
|
||||
return run(coroutine -> {
|
||||
coroutine.awaitAll(wrist.down(), roller.expel());
|
||||
})
|
||||
.named("Intake.Outtake");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.mechanisms;
|
||||
|
||||
import static org.wpilib.units.Units.RadiansPerSecond;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants;
|
||||
import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController;
|
||||
|
||||
@Logged
|
||||
public class IntakeRoller implements Mechanism {
|
||||
private final ExampleSmartMotorController motor =
|
||||
new ExampleSmartMotorController(IntakeConstants.ROLLER_MOTOR_ID);
|
||||
|
||||
/**
|
||||
* Stops the roller motor. This command keeps the roller off until interrupted by another command.
|
||||
*
|
||||
* @return Command to stop the roller
|
||||
*/
|
||||
public Command stop() {
|
||||
return runRepeatedly(motor::stopMotor).named("Intake.Roller.Stop");
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command idle() {
|
||||
return stop();
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the roller motor in intake mode. This command keeps the roller running until interrupted
|
||||
* by another command.
|
||||
*
|
||||
* @return Command to start the roller in intake mode
|
||||
*/
|
||||
public Command intake() {
|
||||
return run(coroutine -> {
|
||||
motor.setSetpoint(
|
||||
ExampleSmartMotorController.PIDMode.kVelocity,
|
||||
IntakeConstants.ROLLER_INTAKE_SPEED.in(RadiansPerSecond));
|
||||
|
||||
coroutine.park();
|
||||
})
|
||||
.named("Intake.Roller.Intake");
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the roller motor in expel mode. This command keeps the roller running until interrupted
|
||||
* by another command.
|
||||
*
|
||||
* @return Command to start the roller in expel mode
|
||||
*/
|
||||
public Command expel() {
|
||||
return run(coroutine -> {
|
||||
motor.setSetpoint(
|
||||
ExampleSmartMotorController.PIDMode.kVelocity,
|
||||
IntakeConstants.ROLLER_EXPULSION_SPEED.in(RadiansPerSecond));
|
||||
|
||||
coroutine.park();
|
||||
})
|
||||
.named("Intake.Roller.Expel");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.mechanisms;
|
||||
|
||||
import static org.wpilib.units.Units.Degrees;
|
||||
import static org.wpilib.units.Units.Radians;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants;
|
||||
import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController;
|
||||
import org.wpilib.units.measure.Angle;
|
||||
|
||||
@Logged
|
||||
public class IntakeWrist implements Mechanism {
|
||||
private final ExampleSmartMotorController motor =
|
||||
new ExampleSmartMotorController(IntakeConstants.WRIST_MOTOR_ID);
|
||||
|
||||
public Command stop() {
|
||||
return runRepeatedly(motor::stopMotor).named("Intake.Wrist.Stop");
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command idle() {
|
||||
return stop();
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the wrist to a specific angle. The command will exit when the target angle is reached.
|
||||
*
|
||||
* @param target The target angle to move to
|
||||
* @return A command that moves the wrist to the target angle
|
||||
*/
|
||||
public Command moveToAngle(Angle target) {
|
||||
double targetRads = target.in(Radians);
|
||||
double tolerance = Radians.convertFrom(0.5, Degrees);
|
||||
|
||||
return run(coroutine -> {
|
||||
motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, targetRads);
|
||||
|
||||
coroutine.waitUntil(() -> Math.abs(motor.getEncoderDistance() - targetRads) <= tolerance);
|
||||
})
|
||||
.named("Intake.Wrist.MoveToAngle[" + target.toLongString() + "]");
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the wrist to a specific angle. The command will hold the wrist at the target angle until
|
||||
* interrupted.
|
||||
*
|
||||
* @param target The target angle to hold
|
||||
* @return A command that holds the wrist at the target angle
|
||||
*/
|
||||
public Command holdAngle(Angle target) {
|
||||
double targetRads = target.in(Radians);
|
||||
|
||||
return run(coroutine -> {
|
||||
motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, targetRads);
|
||||
coroutine.park();
|
||||
})
|
||||
.named("Intake.Wrist.HoldAngle[" + target.toLongString() + "]");
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the wrist to the stowed position and holds it there until interrupted.
|
||||
*
|
||||
* @return A command that stows the wrist
|
||||
*/
|
||||
public Command stow() {
|
||||
return holdAngle(IntakeConstants.WRIST_STOW_ANGLE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the wrist to the down position and holds it there until interrupted.
|
||||
*
|
||||
* @return A command that moves the wrist to the down position
|
||||
*/
|
||||
public Command down() {
|
||||
return holdAngle(IntakeConstants.WRIST_DOWN_ANGLE);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.mechanisms;
|
||||
|
||||
import static org.wpilib.units.Units.Degrees;
|
||||
import static org.wpilib.units.Units.Meters;
|
||||
import static org.wpilib.units.Units.RPM;
|
||||
import static org.wpilib.units.Units.Radians;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.ShooterConstants;
|
||||
import org.wpilib.examples.rebuiltcmdv3.lookup.LookupTables;
|
||||
import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController;
|
||||
import org.wpilib.math.interpolation.InterpolatingTreeMap;
|
||||
import org.wpilib.units.measure.Angle;
|
||||
import org.wpilib.units.measure.AngularVelocity;
|
||||
import org.wpilib.units.measure.Distance;
|
||||
|
||||
@Logged
|
||||
public class Shooter implements Mechanism {
|
||||
private static final InterpolatingTreeMap<Distance, Angle> HOOD_LUT =
|
||||
new InterpolatingTreeMap<>(
|
||||
LookupTables.inverseUnitInterpolator(), LookupTables.unitInterpolator());
|
||||
private static final InterpolatingTreeMap<Distance, AngularVelocity> FLYWHEEL_LUT =
|
||||
new InterpolatingTreeMap<>(
|
||||
LookupTables.inverseUnitInterpolator(), LookupTables.unitInterpolator());
|
||||
|
||||
static {
|
||||
// Populate the lookup tables
|
||||
HOOD_LUT.put(Meters.of(0), Degrees.of(0));
|
||||
HOOD_LUT.put(Meters.of(1), Degrees.of(5));
|
||||
HOOD_LUT.put(Meters.of(2), Degrees.of(10));
|
||||
HOOD_LUT.put(Meters.of(3), Degrees.of(20));
|
||||
HOOD_LUT.put(Meters.of(4), Degrees.of(30));
|
||||
|
||||
FLYWHEEL_LUT.put(Meters.of(0), RPM.of(1000));
|
||||
FLYWHEEL_LUT.put(Meters.of(1), RPM.of(1000));
|
||||
FLYWHEEL_LUT.put(Meters.of(2), RPM.of(2000));
|
||||
FLYWHEEL_LUT.put(Meters.of(3), RPM.of(3000));
|
||||
FLYWHEEL_LUT.put(Meters.of(4), RPM.of(4000));
|
||||
}
|
||||
|
||||
private final ExampleSmartMotorController hoodMotor;
|
||||
|
||||
private final ExampleSmartMotorController flywheelMotorPrimary;
|
||||
private final ExampleSmartMotorController flywheelMotorSecondary;
|
||||
private final ExampleSmartMotorController flywheelMotorTertiary;
|
||||
private final ExampleSmartMotorController flywheelMotorQuatenary;
|
||||
|
||||
/**
|
||||
* Creates a new shooter mechanism object and initializes its motors. Only one shooter mechanism
|
||||
* should exist at a time.
|
||||
*/
|
||||
public Shooter() {
|
||||
hoodMotor = new ExampleSmartMotorController(ShooterConstants.HOOD_MOTOR_ID);
|
||||
flywheelMotorPrimary =
|
||||
new ExampleSmartMotorController(ShooterConstants.PRIMARY_SHOOTER_MOTOR_ID);
|
||||
flywheelMotorSecondary =
|
||||
new ExampleSmartMotorController(ShooterConstants.SECONDARY_SHOOTER_MOTOR_ID);
|
||||
flywheelMotorTertiary =
|
||||
new ExampleSmartMotorController(ShooterConstants.TERTIARY_SHOOTER_MOTOR_ID);
|
||||
flywheelMotorQuatenary =
|
||||
new ExampleSmartMotorController(ShooterConstants.QUATERNARY_SHOOTER_MOTOR_ID);
|
||||
|
||||
flywheelMotorSecondary.follow(flywheelMotorPrimary);
|
||||
flywheelMotorTertiary.follow(flywheelMotorPrimary);
|
||||
flywheelMotorQuatenary.follow(flywheelMotorPrimary);
|
||||
|
||||
hoodMotor.setPID(ShooterConstants.HOOD_KP, 0, 0);
|
||||
flywheelMotorPrimary.setPID(ShooterConstants.FLYWHEEL_KP, 0, 0);
|
||||
|
||||
setDefaultCommand(idle());
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command idle() {
|
||||
return run(coroutine -> {
|
||||
coroutine.awaitAll(runHoodAngle(Degrees::zero), runFlywheelSpeed(RPM::zero));
|
||||
})
|
||||
.withPriority(Command.LOWEST_PRIORITY)
|
||||
.named("Shooter.Idle");
|
||||
}
|
||||
|
||||
/**
|
||||
* Shoots at the hub. The flywheel and hood are automatically adjusted based on the distance to
|
||||
* the center of the hub. This could potentially be used for passing as well; but, because the
|
||||
* center of the hub is six feet above the ground, the distance would need to be fudged to account
|
||||
* for the height difference.
|
||||
*
|
||||
* @param targetDistance The distance to the target, measured from the center of the robot to the
|
||||
* center of the target.
|
||||
* @return A command that shoots at the hub
|
||||
*/
|
||||
public Command shootAtHub(Supplier<Distance> targetDistance) {
|
||||
// Because this command is used when only the distance is known,
|
||||
// we need to do a little bit of work to convert from a target distance to
|
||||
// individual hood angle and flywheel speed values.
|
||||
Supplier<Angle> hoodAngle = () -> HOOD_LUT.get(targetDistance.get());
|
||||
Supplier<AngularVelocity> flywheelSpeed = () -> FLYWHEEL_LUT.get(targetDistance.get());
|
||||
|
||||
return run(coroutine -> {
|
||||
coroutine.awaitAll(runHoodAngle(hoodAngle), runFlywheelSpeed(flywheelSpeed));
|
||||
})
|
||||
.named("Shooter.ShootAtHub");
|
||||
}
|
||||
|
||||
// Private commands to be used by compositions
|
||||
|
||||
/**
|
||||
* Moves the hood to the specified angle and holds it. This command will run forever unless
|
||||
* interrupted.
|
||||
*
|
||||
* @param angle A dynamic supplier for the desired hood angle.
|
||||
* @return The hood angle command
|
||||
*/
|
||||
private Command runHoodAngle(Supplier<Angle> angle) {
|
||||
return runRepeatedly(
|
||||
() -> {
|
||||
hoodMotor.setSetpoint(
|
||||
ExampleSmartMotorController.PIDMode.kPosition, angle.get().in(Radians));
|
||||
})
|
||||
.named("Shooter.RunHoodAngle");
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the flywheel at the specified speed and holds it. This command will run forever unless
|
||||
* interrupted.
|
||||
*
|
||||
* @param speed A dynamic supplier for the desired flywheel speed.
|
||||
* @return The flywheel speed command
|
||||
*/
|
||||
private Command runFlywheelSpeed(Supplier<AngularVelocity> speed) {
|
||||
return runRepeatedly(
|
||||
() -> {
|
||||
flywheelMotorPrimary.setSetpoint(
|
||||
ExampleSmartMotorController.PIDMode.kVelocity, speed.get().in(RPM));
|
||||
})
|
||||
.named("Shooter.RunFlywheelSpeed");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,242 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.mechanisms;
|
||||
|
||||
import static org.wpilib.units.Units.MetersPerSecond;
|
||||
import static org.wpilib.units.Units.Radians;
|
||||
import static org.wpilib.units.Units.RadiansPerSecond;
|
||||
|
||||
import java.util.function.Function;
|
||||
import java.util.function.Supplier;
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.command3.button.CommandGamepad;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.DriveConstants;
|
||||
import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController;
|
||||
import org.wpilib.examples.rebuiltcmdv3.stubs.PathFollower;
|
||||
import org.wpilib.hardware.imu.OnboardIMU;
|
||||
import org.wpilib.math.controller.PIDController;
|
||||
import org.wpilib.math.geometry.Pose2d;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
import org.wpilib.math.kinematics.ChassisVelocities;
|
||||
import org.wpilib.math.kinematics.SwerveDriveKinematics;
|
||||
import org.wpilib.math.kinematics.SwerveModulePosition;
|
||||
import org.wpilib.math.kinematics.SwerveModuleVelocity;
|
||||
import org.wpilib.units.measure.Angle;
|
||||
import org.wpilib.units.measure.AngularVelocity;
|
||||
|
||||
@Logged
|
||||
public class SwerveDrive implements Mechanism {
|
||||
public final SwerveDriveKinematics kinematics = DriveConstants.KINEMATICS;
|
||||
private final OnboardIMU gyro = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
|
||||
private final SwerveModule frontLeft =
|
||||
new SwerveModule(
|
||||
new ExampleSmartMotorController(DriveConstants.FRONT_LEFT_DRIVE_ID),
|
||||
new ExampleSmartMotorController(DriveConstants.FRONT_LEFT_TURN_ID));
|
||||
private final SwerveModule frontRight =
|
||||
new SwerveModule(
|
||||
new ExampleSmartMotorController(DriveConstants.FRONT_RIGHT_DRIVE_ID),
|
||||
new ExampleSmartMotorController(DriveConstants.FRONT_RIGHT_TURN_ID));
|
||||
private final SwerveModule rearLeft =
|
||||
new SwerveModule(
|
||||
new ExampleSmartMotorController(DriveConstants.REAR_LEFT_DRIVE_ID),
|
||||
new ExampleSmartMotorController(DriveConstants.REAR_LEFT_TURN_ID));
|
||||
private final SwerveModule rearRight =
|
||||
new SwerveModule(
|
||||
new ExampleSmartMotorController(DriveConstants.REAR_RIGHT_DRIVE_ID),
|
||||
new ExampleSmartMotorController(DriveConstants.REAR_RIGHT_TURN_ID));
|
||||
|
||||
// PID controllers for XY field position and heading
|
||||
// These accept position (meters for XY, radians for heading) and output velocities
|
||||
// (meters per second for XY, radians per second for heading)
|
||||
private final PIDController xController = new PIDController(0.05, 0, 0);
|
||||
private final PIDController yController = new PIDController(0.05, 0, 0);
|
||||
private final PIDController headingController = new PIDController(0.05, 0, 0);
|
||||
|
||||
public SwerveDrive() {
|
||||
// Enable continuous input on the heading PID controller to handle wraparound.
|
||||
headingController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the heading of the drivebase. Used for localization.
|
||||
*
|
||||
* @return The heading of the drivebase.
|
||||
*/
|
||||
public Rotation2d getGyroHeading() {
|
||||
return gyro.getRotation2d();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the positions of all swerve modules. Used for localization.
|
||||
*
|
||||
* @return An array of module positions.
|
||||
*/
|
||||
public SwerveModulePosition[] getModulePositions() {
|
||||
return getSwerveData(SwerveModule::getPosition);
|
||||
}
|
||||
|
||||
public SwerveModuleVelocity[] getModuleVelocities() {
|
||||
return getSwerveData(SwerveModule::getVelocity);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command idle() {
|
||||
return runRepeatedly(
|
||||
() -> {
|
||||
frontLeft.stop();
|
||||
frontRight.stop();
|
||||
rearLeft.stop();
|
||||
rearRight.stop();
|
||||
})
|
||||
.named("Drive.Idle");
|
||||
}
|
||||
|
||||
/**
|
||||
* A command that gives the driver full control of the drive.
|
||||
*
|
||||
* @param controller The driver controller.
|
||||
* @return a command that gives the driver full control of the drive
|
||||
*/
|
||||
public Command driverControl(CommandGamepad controller) {
|
||||
return driveFieldRelative(
|
||||
() -> {
|
||||
double x = -controller.getLeftY();
|
||||
double y = -controller.getLeftX();
|
||||
double omega = -controller.getRightX();
|
||||
|
||||
return new ChassisVelocities(
|
||||
DriveConstants.MAX_VELOCITY.times(x),
|
||||
DriveConstants.MAX_VELOCITY.times(y),
|
||||
DriveConstants.MAX_TURN_RATE.times(omega));
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* A command that aims the drivebase at a target. The command will exit when the necessary heading
|
||||
* is reached.
|
||||
*
|
||||
* @param robotPose A supplier for the current pose of the robot.
|
||||
* @param targetPose A supplier for the target pose to aim at.
|
||||
* @return The aiming command
|
||||
*/
|
||||
public Command aimAt(Supplier<Pose2d> robotPose, Pose2d targetPose) {
|
||||
return driveFieldRelative(
|
||||
() -> {
|
||||
Pose2d robot = robotPose.get();
|
||||
Angle targetHeading = angleBetweenPoses(robot, targetPose);
|
||||
AngularVelocity omega =
|
||||
RadiansPerSecond.of(
|
||||
headingController.calculate(
|
||||
robot.getRotation().getRadians(), targetHeading.in(Radians)));
|
||||
|
||||
return new ChassisVelocities(MetersPerSecond.of(0), MetersPerSecond.of(0), omega);
|
||||
})
|
||||
.until(headingController::atSetpoint)
|
||||
.named("Drive.AimAt");
|
||||
}
|
||||
|
||||
/**
|
||||
* A command that assists the driver in aiming the robot at a target pose. The driver still has
|
||||
* full control over the X and Y speeds, but heading is controlled automatically based on the
|
||||
* relative position of the robot and the target.
|
||||
*
|
||||
* @param controller The driver controller.
|
||||
* @param robotPose A supplier for the current robot pose.
|
||||
* @param targetPose A supplier for the target pose.
|
||||
* @return The aim-assist command.
|
||||
*/
|
||||
public Command aimAssist(
|
||||
CommandGamepad controller, Supplier<Pose2d> robotPose, Supplier<Pose2d> targetPose) {
|
||||
return driveFieldRelative(
|
||||
() -> {
|
||||
// We use the driver's input to control X and Y speed as normal...
|
||||
double x = -controller.getLeftY();
|
||||
double y = -controller.getLeftX();
|
||||
|
||||
// ... but heading is controlled automatically based on the relative position of the robot
|
||||
// and
|
||||
// the target.
|
||||
Pose2d robot = robotPose.get();
|
||||
Pose2d target = targetPose.get();
|
||||
Angle targetHeading = angleBetweenPoses(robot, target);
|
||||
AngularVelocity omega =
|
||||
RadiansPerSecond.of(
|
||||
headingController.calculate(
|
||||
robot.getRotation().getRadians(), targetHeading.in(Radians)));
|
||||
|
||||
return new ChassisVelocities(
|
||||
DriveConstants.MAX_VELOCITY.times(x), DriveConstants.MAX_VELOCITY.times(y), omega);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the angle from one pose to another. Useful for aiming the robot (positioned at the
|
||||
* source pose) at some target point on the field.
|
||||
*
|
||||
* @param source The source pose
|
||||
* @param target The target pose
|
||||
* @return The angle between the two poses
|
||||
*/
|
||||
private static Angle angleBetweenPoses(Pose2d source, Pose2d target) {
|
||||
// atan2 conveniently returns a value in radians between -pi and pi, so we don't need to do
|
||||
// any additional calculations to normalize the angle
|
||||
return Radians.of(Math.atan2(target.getY() - source.getY(), target.getX() - source.getX()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Drives with a dynamic velocity. This command will never exit.
|
||||
*
|
||||
* @param velocities A supplier of chassis velocities. These velocities are expected to be in the
|
||||
* field coordinate system.
|
||||
* @return a command that drives with the given velocities
|
||||
*/
|
||||
public Command driveFieldRelative(Supplier<ChassisVelocities> velocities) {
|
||||
return runRepeatedly(
|
||||
() -> {
|
||||
ChassisVelocities velocity = velocities.get().toRobotRelative(getGyroHeading());
|
||||
SwerveModuleVelocity[] swerveModuleVelocities =
|
||||
kinematics.toSwerveModuleVelocities(velocity);
|
||||
|
||||
frontLeft.setTargetVelocity(swerveModuleVelocities[0]);
|
||||
frontRight.setTargetVelocity(swerveModuleVelocities[1]);
|
||||
rearLeft.setTargetVelocity(swerveModuleVelocities[2]);
|
||||
rearRight.setTargetVelocity(swerveModuleVelocities[3]);
|
||||
})
|
||||
.named("Drive.DriveSpeeds");
|
||||
}
|
||||
|
||||
/**
|
||||
* Follows a path loaded from a file. The command will exit when the robot reaches the end of the
|
||||
* path.
|
||||
*
|
||||
* @param pathName the path to follow
|
||||
* @return a command that follows the path
|
||||
*/
|
||||
public Command followPath(String pathName) {
|
||||
return run(coroutine -> {
|
||||
// Load the path
|
||||
PathFollower follower = PathFollower.load(pathName);
|
||||
|
||||
// Run path follower in the background
|
||||
coroutine.fork(driveFieldRelative(follower::next));
|
||||
|
||||
// Wait until we reach the end of the path, then exit
|
||||
coroutine.waitUntil(follower::isDone);
|
||||
})
|
||||
.named("Drive.FollowPath[" + pathName + "]");
|
||||
}
|
||||
|
||||
@SuppressWarnings("unchecked")
|
||||
private <T> T[] getSwerveData(Function<SwerveModule, ? extends T> extractor) {
|
||||
T[] data = (T[]) new Object[4];
|
||||
data[0] = extractor.apply(frontLeft);
|
||||
data[1] = extractor.apply(frontRight);
|
||||
data[2] = extractor.apply(rearLeft);
|
||||
data[3] = extractor.apply(rearRight);
|
||||
return data;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.mechanisms;
|
||||
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
import org.wpilib.math.kinematics.SwerveModulePosition;
|
||||
import org.wpilib.math.kinematics.SwerveModuleVelocity;
|
||||
|
||||
@Logged
|
||||
public final class SwerveModule {
|
||||
private final ExampleSmartMotorController driveMotor;
|
||||
private final ExampleSmartMotorController turnMotor;
|
||||
|
||||
/**
|
||||
* Creates a new swerve module with the given drive and turn motors. This constructor is
|
||||
* package-private and should only be called by the SwerveDrive class.
|
||||
*
|
||||
* @param driveMotor The motor that drives the module.
|
||||
* @param turnMotor The motor that turns the module.
|
||||
*/
|
||||
SwerveModule(ExampleSmartMotorController driveMotor, ExampleSmartMotorController turnMotor) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.turnMotor = turnMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the target velocity of this module.
|
||||
*
|
||||
* @param targetVelocity The target velocity of the module.
|
||||
*/
|
||||
public void setTargetVelocity(SwerveModuleVelocity targetVelocity) {
|
||||
driveMotor.setSetpoint(ExampleSmartMotorController.PIDMode.kVelocity, targetVelocity.velocity);
|
||||
turnMotor.setSetpoint(
|
||||
ExampleSmartMotorController.PIDMode.kPosition, targetVelocity.angle.getRadians());
|
||||
}
|
||||
|
||||
/** Stops the module by turning off the drive and turn motors. */
|
||||
public void stop() {
|
||||
driveMotor.stopMotor();
|
||||
turnMotor.stopMotor();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current position of the module.
|
||||
*
|
||||
* @return The current position of the module.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(
|
||||
driveMotor.getEncoderDistance(), Rotation2d.fromRadians(turnMotor.getEncoderDistance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current velocity of the module.
|
||||
*
|
||||
* @return The current velocity of the module.
|
||||
*/
|
||||
public SwerveModuleVelocity getVelocity() {
|
||||
return new SwerveModuleVelocity(
|
||||
driveMotor.getEncoderRate(), Rotation2d.fromRadians(turnMotor.getEncoderDistance()));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.opmodes.auto;
|
||||
|
||||
import org.wpilib.examples.rebuiltcmdv3.Robot;
|
||||
import org.wpilib.opmode.Autonomous;
|
||||
import org.wpilib.opmode.OpMode;
|
||||
|
||||
@Autonomous
|
||||
public class DoNothingAuto implements OpMode {
|
||||
/**
|
||||
* Creates a new autonomous opmode that does nothing. This constructor is called automatically by
|
||||
* the OpModeRobot framework when the program starts up.
|
||||
*
|
||||
* @param robot The robot instance to control.
|
||||
*/
|
||||
public DoNothingAuto(Robot robot) {
|
||||
// Robot mechanisms have idle default commands, so we don't need to do anything special
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.opmodes.auto;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Trigger;
|
||||
import org.wpilib.driverstation.RobotState;
|
||||
import org.wpilib.examples.rebuiltcmdv3.Robot;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants;
|
||||
import org.wpilib.opmode.Autonomous;
|
||||
import org.wpilib.opmode.OpMode;
|
||||
|
||||
@Autonomous
|
||||
public class SweepAuto implements OpMode {
|
||||
private final Trigger enabled = new Trigger(RobotState::isEnabled);
|
||||
|
||||
/**
|
||||
* Creates a new autonomous mode that sweeps the left trench and scores on the hub. This
|
||||
* constructor is called automatically by the OpModeRobot framework when the program starts up.
|
||||
*
|
||||
* @param robot The robot instance to control.
|
||||
*/
|
||||
public SweepAuto(Robot robot) {
|
||||
// Start the intake stowed
|
||||
robot.intake.setDefaultCommand(robot.intake.stow());
|
||||
|
||||
// Extend the intake once we cross into the neutral zone and leave it extended for the entire
|
||||
// opmode
|
||||
robot.inNeutralZone.onTrue(robot.intake.intake());
|
||||
|
||||
// Once the robot is enabled, start following a sweep path through the left trench,
|
||||
// into the neutral zone, then back over the bump.
|
||||
// When we return to the alliance zone, aim at the hub and start shooting.
|
||||
enabled.onTrue(sweepAndScore(robot));
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a command that drives the robot in a path that sweeps through the neutral zone to
|
||||
* gather fuel, returns to the alliance zone, and then shoots at the hub until the end of the
|
||||
* autonomous period.
|
||||
*
|
||||
* @param robot The robot instance to control
|
||||
* @return A sweep-and-score command
|
||||
*/
|
||||
private Command sweepAndScore(Robot robot) {
|
||||
return Command.noRequirements(
|
||||
coroutine -> {
|
||||
coroutine.await(robot.swerveDrive.followPath("nz-sweep-left-trench"));
|
||||
|
||||
FieldConstants.targetHub()
|
||||
.ifPresent(
|
||||
hub -> {
|
||||
coroutine.await(robot.shootAt(hub));
|
||||
});
|
||||
})
|
||||
.named("Sweep and Score");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.opmodes.teleop;
|
||||
|
||||
import org.wpilib.examples.rebuiltcmdv3.Robot;
|
||||
import org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants;
|
||||
import org.wpilib.opmode.OpMode;
|
||||
import org.wpilib.opmode.Teleop;
|
||||
|
||||
@Teleop
|
||||
public class DefaultTeleop implements OpMode {
|
||||
/**
|
||||
* Creates a new default teleop mode. This constructor is called automatically by the OpModeRobot
|
||||
* framework when the program starts up.
|
||||
*
|
||||
* @param robot The robot instance to control.
|
||||
*/
|
||||
public DefaultTeleop(Robot robot) {
|
||||
robot.swerveDrive.setDefaultCommand(robot.swerveDrive.driverControl(robot.controller));
|
||||
|
||||
robot.controller.faceUp().onTrue(robot.intake.intake());
|
||||
robot.controller.faceDown().onTrue(robot.intake.stow());
|
||||
robot.controller.faceRight().whileTrue(robot.intake.agitate());
|
||||
|
||||
// The right stick is used for turning, but switch to aim assist when the driver presses down
|
||||
// on the stick. Un-pressing the stick will return to normal driver control.
|
||||
// If, for some reason, no target hub is found, the robot's current pose will be used as the
|
||||
// target location.
|
||||
robot
|
||||
.controller
|
||||
.rightStick()
|
||||
.whileTrue(
|
||||
robot.swerveDrive.aimAssist(
|
||||
robot.controller,
|
||||
robot.poseEstimator::getEstimatedPose,
|
||||
() -> FieldConstants.targetHub().orElseGet(robot.poseEstimator::getEstimatedPose)));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.stubs;
|
||||
|
||||
/**
|
||||
* A simplified stub class that simulates the API of a common "smart" motor controller.
|
||||
*
|
||||
* <p>Has no actual functionality.
|
||||
*/
|
||||
public class ExampleSmartMotorController {
|
||||
public enum PIDMode {
|
||||
kPosition,
|
||||
kVelocity,
|
||||
kMovementWitchcraft
|
||||
}
|
||||
|
||||
double value;
|
||||
|
||||
/**
|
||||
* Creates a new ExampleSmartMotorController.
|
||||
*
|
||||
* @param port The port for the controller.
|
||||
*/
|
||||
public ExampleSmartMotorController(int port) {}
|
||||
|
||||
/**
|
||||
* Example method for setting the PID gains of the smart controller.
|
||||
*
|
||||
* @param kp The proportional gain.
|
||||
* @param ki The integral gain.
|
||||
* @param kd The derivative gain.
|
||||
*/
|
||||
public void setPID(double kp, double ki, double kd) {}
|
||||
|
||||
/**
|
||||
* Example method for setting the setpoint of the smart controller in PID mode.
|
||||
*
|
||||
* @param mode The mode of the PID controller.
|
||||
* @param setpoint The controller setpoint.
|
||||
*/
|
||||
public void setSetpoint(PIDMode mode, double setpoint) {}
|
||||
|
||||
/**
|
||||
* Example method for setting the voltage supplied to the motor.
|
||||
*
|
||||
* @param volts How many volts to apply to the motor.
|
||||
*/
|
||||
public void setVoltage(int volts) {}
|
||||
|
||||
/**
|
||||
* Places this motor controller in follower mode.
|
||||
*
|
||||
* @param leader The leader to follow.
|
||||
*/
|
||||
public void follow(ExampleSmartMotorController leader) {}
|
||||
|
||||
/**
|
||||
* Returns the encoder distance.
|
||||
*
|
||||
* @return The current encoder distance.
|
||||
*/
|
||||
public double getEncoderDistance() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the encoder rate.
|
||||
*
|
||||
* @return The current encoder rate.
|
||||
*/
|
||||
public double getEncoderRate() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** Resets the encoder to zero distance. */
|
||||
public void resetEncoder() {}
|
||||
|
||||
public void setThrottle(double throttle) {
|
||||
value = throttle;
|
||||
}
|
||||
|
||||
public double getThrottle() {
|
||||
return value;
|
||||
}
|
||||
|
||||
public void setInverted(boolean isInverted) {}
|
||||
|
||||
public boolean getInverted() {
|
||||
return false;
|
||||
}
|
||||
|
||||
public void disable() {}
|
||||
|
||||
public void stopMotor() {}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.examples.rebuiltcmdv3.stubs;
|
||||
|
||||
import org.wpilib.math.kinematics.ChassisVelocities;
|
||||
|
||||
/**
|
||||
* A stub interface for an API that could plausibly follow a path. No WPILib code exists for this;
|
||||
* if you want to follow paths on a real robot, use a third-party library provided by the FRC
|
||||
* programming community.
|
||||
*/
|
||||
public interface PathFollower {
|
||||
/**
|
||||
* Gets the next chassis velocities from the path.
|
||||
*
|
||||
* @return the next chassis velocities
|
||||
*/
|
||||
ChassisVelocities next();
|
||||
|
||||
/**
|
||||
* Checks if the robot has reached the end of the path.
|
||||
*
|
||||
* @return true if the robot has reached the end of the path, false otherwise
|
||||
*/
|
||||
boolean isDone();
|
||||
|
||||
/**
|
||||
* Loads a path from a file in the deploy directory. This is a stub implementation as an example;
|
||||
* a real robot program would use a third-party path following library.
|
||||
*
|
||||
* @param pathName The name of the path file to load.
|
||||
* @return A PathFollower instance that can follow the loaded path.
|
||||
*/
|
||||
static PathFollower load(String pathName) {
|
||||
return new PathFollower() {
|
||||
@Override
|
||||
public ChassisVelocities next() {
|
||||
return new ChassisVelocities();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isDone() {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3;
|
||||
|
||||
import org.wpilib.opmode.Autonomous;
|
||||
import org.wpilib.opmode.OpMode;
|
||||
|
||||
@Autonomous
|
||||
public class ExampleAuto implements OpMode {
|
||||
public ExampleAuto(Robot robot) {
|
||||
robot.exampleMechanism.exampleCondition.whileTrue(robot.exampleMechanism.exampleCommand());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3;
|
||||
|
||||
import org.wpilib.opmode.OpMode;
|
||||
import org.wpilib.opmode.Teleop;
|
||||
|
||||
@Teleop
|
||||
public class ExampleTeleop implements OpMode {
|
||||
public ExampleTeleop(Robot robot) {
|
||||
robot.exampleController.rightStick().whileTrue(robot.exampleMechanism.exampleCommand());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3;
|
||||
|
||||
import org.wpilib.command3.Scheduler;
|
||||
import org.wpilib.command3.button.CommandGamepad;
|
||||
import org.wpilib.framework.OpModeRobot;
|
||||
import org.wpilib.templates.commandv3.constants.DriverConstants;
|
||||
import org.wpilib.templates.commandv3.mechanisms.ExampleMechanism;
|
||||
|
||||
public class Robot extends OpModeRobot {
|
||||
final ExampleMechanism exampleMechanism = new ExampleMechanism();
|
||||
final CommandGamepad exampleController =
|
||||
new CommandGamepad(DriverConstants.DRIVER_CONTROLLER_PORT);
|
||||
|
||||
public Robot() {}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
Scheduler.getDefault().run();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3.constants;
|
||||
|
||||
public final class DriverConstants {
|
||||
public static final int DRIVER_CONTROLLER_PORT = 1;
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3.constants;
|
||||
|
||||
import org.wpilib.templates.commandv3.mechanisms.ExampleMechanism;
|
||||
|
||||
/** Constants for the {@link ExampleMechanism}. */
|
||||
public final class ExampleConstants {}
|
||||
@@ -0,0 +1,34 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3.mechanisms;
|
||||
|
||||
import org.wpilib.command3.Command;
|
||||
import org.wpilib.command3.Mechanism;
|
||||
import org.wpilib.command3.Trigger;
|
||||
|
||||
/** An example class demonstrating basic use of mechanisms and commands. */
|
||||
public class ExampleMechanism implements Mechanism {
|
||||
public final Trigger exampleCondition = new Trigger(() -> false);
|
||||
|
||||
/**
|
||||
* An example command that requires this mechanism. Only one command that requires this mechanism
|
||||
* may run at a time - if a command is already running, this command will interrupt it.
|
||||
*
|
||||
* <p>This example runs indefinitely and prints a message every time it executes.
|
||||
*
|
||||
* @return An example command.
|
||||
*/
|
||||
public Command exampleCommand() {
|
||||
return run(coroutine -> {
|
||||
int runCount = 0;
|
||||
while (true) {
|
||||
runCount++;
|
||||
System.out.println("Example command run #" + runCount);
|
||||
coroutine.yield();
|
||||
}
|
||||
})
|
||||
.named("Example Command");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.templates.commandv3skeleton;
|
||||
|
||||
import org.wpilib.command3.Scheduler;
|
||||
import org.wpilib.epilogue.Epilogue;
|
||||
import org.wpilib.epilogue.Logged;
|
||||
import org.wpilib.framework.OpModeRobot;
|
||||
|
||||
@Logged
|
||||
public class Robot extends OpModeRobot {
|
||||
public Robot() {}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
Scheduler.getDefault().run();
|
||||
Epilogue.update(this);
|
||||
}
|
||||
}
|
||||
@@ -22,6 +22,29 @@
|
||||
"robotclass": "Robot",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "Command v3 Robot",
|
||||
"description": "Command v3, with explanatory comments and example code.",
|
||||
"tags": [
|
||||
"Commandv3"
|
||||
],
|
||||
"foldername": "commandv3",
|
||||
"gradlebase": "java",
|
||||
"robotclass": "Robot",
|
||||
"commandversion": 3
|
||||
},
|
||||
{
|
||||
"name": "Command v3 Robot Skeleton (Advanced)",
|
||||
"description": "Skeleton (stub) code for Command v3, without explanatory comments and example code.",
|
||||
"tags": [
|
||||
"Commandv3",
|
||||
"Skeleton"
|
||||
],
|
||||
"foldername": "commandv3skeleton",
|
||||
"gradlebase": "java",
|
||||
"robotclass": "Robot",
|
||||
"commandversion": 3
|
||||
},
|
||||
{
|
||||
"name": "OpMode Robot",
|
||||
"description": "OpMode style, with explanatory comments and example code.",
|
||||
|
||||
Reference in New Issue
Block a user