Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add new Java Command framework (#1682)

Browse Source 
The old command framework is still available, but will be deprecated.

Due to name conflicts, the new framework is in the wpilibj2 package.
Eventually (after the old command framework is removed in a future year)
it will be moved into the main wpilibj package.
This commit is contained in:
Oblarg
2019-08-25 17:47:07 -04:00
committed by Peter Johnson
parent 1379735aff
commit 558c383088
126 changed files with 9510 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -230,6 +230,16 @@
"gradlebase": "java",
"mainclass": "Main"
},
{
"name": "GearsBot (New)",
"description": "A fully functional example CommandBased program for WPIs GearsBot robot, ported to the new CommandBased library. This code can run on your computer if it supports simulation.",
"tags": [
"Complete Robot"
],
"foldername": "gearsbotnew",
"gradlebase": "java",
"mainclass": "Main"
},
{
"name": "PacGoat",
"description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.",
@@ -282,5 +292,66 @@
"foldername": "shuffleboard",
"gradlebase": "java",
"mainclass": "Main"
},
{
"name": "'Traditional' Hatchbot",
"description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'traditional' style, i.e. commands are given their own classes.",
"tags": [
"Complete robot",
"Command-based"
],
"foldername": "hatchbottraditional",
"mainclass": "Main"
},
{
"name": "'Inlined' Hatchbot",
"description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
"tags": [
"Complete robot",
"Command-based",
"Lambdas"
],
"foldername": "hatchbotinlined",
"mainclass": "Main"
},
{
"name": "Select Command Example",
"description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
"tags": [
"Command-based"
],
"foldername": "selectcommand",
"mainclass": "Main"
},
{
"name": "Scheduler Event Logging",
"description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
"tags": [
"Command-based",
"Shuffleboard"
],
"foldername": "schedulereventlogging",
"mainclass": "Main"
},
{
"name": "Frisbeebot",
"description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework",
"tags": [
"Command-based",
"PID"
],
"foldername": "frisbeebot",
"mainclass": "Main"
},
{
"name": "Gyro Drive Commands",
"description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.",
"tags": [
"Command-based",
"PID",
"Gyro"
],
"foldername": "gyrodrivecommands",
"mainclass": "Main"
}
]

74
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java Normal file
View File

@@ -0,0 +1,74 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.frisbeebot;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[]{0, 1};
public static final int[] kRightEncoderPorts = new int[]{2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
}
public static final class ShooterConstants {
public static final int[] kEncoderPorts = new int[]{4, 5};
public static final boolean kEncoderReversed = false;
public static final int kEncoderCPR = 1024;
public static final double kEncoderDistancePerPulse =
// Distance units will be rotations
1. / (double) kEncoderCPR;
public static final int kShooterMotorPort = 4;
public static final int kFeederMotorPort = 5;
public static final double kShooterFreeRPS = 5300;
public static final double kShooterTargetRPS = 4000;
public static final double kShooterToleranceRPS = 50;
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
public static final double kSFractional = .05;
public static final double kVFractional =
// Should have value 1 at free speed...
1. / kShooterFreeRPS;
public static final double kFeederSpeed = .5;
}
public static final class AutoConstants {
public static final double kAutoTimeoutSeconds = 12;
public static final double kAutoShootTimeSeconds = 7;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 1;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

121
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

119
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java Normal file
View File

@@ -0,0 +1,119 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoShootTimeSeconds;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoTimeoutSeconds;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants.kDriverControllerPort;
/**
* 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 m_robotDrive = new DriveSubsystem();
private final ShooterSubsystem m_shooter = new ShooterSubsystem();
// A simple autonomous routine that shoots the loaded frisbees
private final Command m_autoCommand =
// Start the command by spinning up the shooter...
new InstantCommand(m_shooter::enable, m_shooter).andThen(
// Wait until the shooter is at speed before feeding the frisbees
new WaitUntilCommand(m_shooter::atSetpoint),
// Start running the feeder
new InstantCommand(m_shooter::runFeeder, m_shooter),
// Shoot for the specified time
new WaitCommand(kAutoShootTimeSeconds))
// Add a timeout (will end the command if, for instance, the shooter never gets up to
// speed)
.withTimeout(kAutoTimeoutSeconds)
// When the command ends, turn off the shooter and the feeder
.whenFinished(() -> {
m_shooter.disable();
m_shooter.stopFeeder();
});
// The driver's controller
XboxController m_driverController = new XboxController(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
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new RunCommand(() -> m_robotDrive
.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Spin up the shooter when the 'A' button is pressed
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(new InstantCommand(m_shooter::enable, m_shooter));
// Turn off the shooter when the 'B' button is pressed
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(new InstantCommand(m_shooter::disable, m_shooter));
// Run the feeder when the 'X' button is held, but only if the shooter is at speed
new JoystickButton(m_driverController, Button.kX.value).whenPressed(new ConditionalCommand(
// Run the feeder
new InstantCommand(m_shooter::runFeeder, m_shooter),
// Do nothing
new InstantCommand(),
// Determine which of the above to do based on whether the shooter has reached the
// desired speed
m_shooter::atSetpoint)).whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
// Drive at half speed when the bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_autoCommand;
}
}

110
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor2Port;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
new PWMVictorSPX(kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
new PWMVictorSPX(kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Resets the drive encoders to currently read a position of 0.
*/
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

79
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java Normal file
View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderPorts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderReversed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederMotorPort;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional;
public class ShooterSubsystem extends PIDSubsystem {
private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
private final Encoder m_shooterEncoder =
new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
/**
* The shooter subsystem for the robot.
*/
public ShooterSubsystem() {
super(new PIDController(kP, kI, kD));
getController().setAbsoluteTolerance(kShooterToleranceRPS);
m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
@Override
public void useOutput(double output) {
// Use a feedforward of the form kS + kV * velocity
m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS);
}
@Override
public double getSetpoint() {
return kShooterTargetRPS;
}
@Override
public double getMeasurement() {
return m_shooterEncoder.getRate();
}
public boolean atSetpoint() {
return m_controller.atSetpoint();
}
public void runFeeder() {
m_feederMotor.set(kFeederSpeed);
}
public void stopFeeder() {
m_feederMotor.set(0);
}
@Override
public void disable() {
super.disable();
// Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our
// feedforward
m_shooterMotor.set(0);
}
}

31
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Main.java Normal file
View File

@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

113
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Robot.java Normal file
View File

@@ -0,0 +1,113 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

127
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/RobotContainer.java Normal file
View File

@@ -0,0 +1,127 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Autonomous;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.CloseClaw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.OpenClaw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Pickup;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Place;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.PrepareToPickup;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.SetElevatorSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.SetWristSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.TankDrive;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
/**
* 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 and commands are defined here...
private final DriveTrain m_drivetrain = new DriveTrain();
private final Elevator m_elevator = new Elevator();
private final Wrist m_wrist = new Wrist();
private final Claw m_claw = new Claw();
private final Joystick m_joystick = new Joystick(0);
private final CommandBase m_autonomousCommand =
new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2, m_elevator));
SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3, m_elevator));
SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
SmartDashboard
.putData("Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
// Assign default commands
m_drivetrain.setDefaultCommand(new TankDrive(m_drivetrain, () -> m_joystick.getY(Hand.kLeft),
() -> m_joystick.getY(Hand.kRight)));
// Show what command your subsystem is running on the SmartDashboard
SmartDashboard.putData(m_drivetrain);
SmartDashboard.putData(m_elevator);
SmartDashboard.putData(m_wrist);
SmartDashboard.putData(m_claw);
// Call log method on all subsystems
m_wrist.log();
m_elevator.log();
m_drivetrain.log();
m_claw.log();
// Configure the button bindings
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Create some buttons
final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
final JoystickButton l2 = new JoystickButton(m_joystick, 9);
final JoystickButton r2 = new JoystickButton(m_joystick, 10);
final JoystickButton l1 = new JoystickButton(m_joystick, 11);
final JoystickButton r1 = new JoystickButton(m_joystick, 12);
// Connect the buttons to commands
dpadUp.whenPressed(new SetElevatorSetpoint(0.2, m_elevator));
dpadDown.whenPressed(new SetElevatorSetpoint(-0.2, m_elevator));
dpadRight.whenPressed(new CloseClaw(m_claw));
dpadLeft.whenPressed(new OpenClaw(m_claw));
r1.whenPressed(new PrepareToPickup(m_claw, m_wrist, m_elevator));
r2.whenPressed(new Pickup(m_claw, m_wrist, m_elevator));
l1.whenPressed(new Place(m_claw, m_wrist, m_elevator));
l2.whenPressed(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_autonomousCommand;
}
}

39
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Autonomous.java Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
/**
* The main autonomous command to pickup and deliver the soda to the box.
*/
public class Autonomous extends SequentialCommandGroup {
/**
* Create a new autonomous command.
*/
public Autonomous(DriveTrain drive, Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new PrepareToPickup(claw, wrist, elevator),
new Pickup(claw, wrist, elevator),
new SetDistanceToBox(0.10, drive),
// new DriveStraight(4), // Use encoders if ultrasonic is broken
new Place(claw, wrist, elevator),
new SetDistanceToBox(0.60, drive),
// new DriveStraight(-2), // Use Encoders if ultrasonic is broken
parallel(
new SetWristSetpoint(-45, wrist),
new CloseClaw(claw)
)
);
}
}

49
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/CloseClaw.java Normal file
View File

@@ -0,0 +1,49 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.gearsbotnew.Robot;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
/**
* Closes the claw for one second. Real robots should use sensors, stalling motors is BAD!
*/
public class CloseClaw extends CommandBase {
private final Claw m_claw;
public CloseClaw(Claw claw) {
m_claw = claw;
addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_claw.close();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_claw.isGrabbing();
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
// NOTE: Doesn't stop in simulation due to lower friction causing the
// can to fall out
// + there is no need to worry about stalling the motor or crushing the
// can.
if (!Robot.isSimulation()) {
m_claw.stop();
}
}
}

53
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/DriveStraight.java Normal file
View File

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
/**
* Drive the given distance straight (negative values go backwards). Uses a
* local PID controller to run a simple PID loop that is only enabled while this
* command is running. The input is the averaged values of the left and right
* encoders.
*/
public class DriveStraight extends PIDCommand {
private final DriveTrain m_drivetrain;
/**
* Create a new DriveStraight command.
* @param distance The distance to drive
*/
public DriveStraight(double distance, DriveTrain drivetrain) {
super(new PIDController(4, 0, 0),
drivetrain::getDistance,
distance,
d -> drivetrain.drive(d, d));
m_drivetrain = drivetrain;
addRequirements(m_drivetrain);
getController().setAbsoluteTolerance(0.01);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
// Get everything in a safe starting state.
m_drivetrain.reset();
super.initialize();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return getController().atSetpoint();
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/OpenClaw.java Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
/**
* Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!
*/
public class OpenClaw extends WaitCommand {
private final Claw m_claw;
/**
* Creates a new OpenClaw command.
*
* @param claw The claw to use
*/
public OpenClaw(Claw claw) {
super(1);
m_claw = claw;
addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_claw.open();
super.initialize();
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
m_claw.stop();
}
}

35
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Pickup.java Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
/**
* Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around.
*/
public class Pickup extends SequentialCommandGroup {
/**
* Create a new pickup command.
*
* @param claw The claw subsystem to use
* @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public Pickup(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new CloseClaw(claw),
parallel(
new SetWristSetpoint(-45, wrist),
new SetElevatorSetpoint(0.25, elevator)));
}
}

34
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Place.java Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
/**
* Place a held soda can onto the platform.
*/
public class Place extends SequentialCommandGroup {
/**
* Create a new place command.
*
* @param claw The claw subsystem to use
* @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public Place(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new SetElevatorSetpoint(0.25, elevator),
new SetWristSetpoint(0, wrist),
new OpenClaw(claw));
}
}

34
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/PrepareToPickup.java Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
/**
* Make sure the robot is in a state to pickup soda cans.
*/
public class PrepareToPickup extends SequentialCommandGroup {
/**
* Create a new prepare to pickup command.
*
* @param claw The claw subsystem to use
* @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new OpenClaw(claw),
parallel(
new SetWristSetpoint(0, wrist),
new SetElevatorSetpoint(0, elevator)));
}
}

54
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetDistanceToBox.java Normal file
View File

@@ -0,0 +1,54 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
/**
* Drive until the robot is the given distance away from the box. Uses a local
* PID controller to run a simple PID loop that is only enabled while this
* command is running. The input is the averaged values of the left and right
* encoders.
*/
public class SetDistanceToBox extends PIDCommand {
private final DriveTrain m_drivetrain;
/**
* Create a new set distance to box command.
*
* @param distance The distance away from the box to drive to
*/
public SetDistanceToBox(double distance, DriveTrain drivetrain) {
super(new PIDController(-2, 0, 0),
drivetrain::getDistanceToObstacle, distance,
d -> drivetrain.drive(d, d));
m_drivetrain = drivetrain;
addRequirements(m_drivetrain);
getController().setAbsoluteTolerance(0.01);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
// Get everything in a safe starting state.
m_drivetrain.reset();
super.initialize();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return getController().atSetpoint();
}
}

48
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetElevatorSetpoint.java Normal file
View File

@@ -0,0 +1,48 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
/**
* Move the elevator to a given location. This command finishes when it is within the tolerance, but
* leaves the PID loop running to maintain the position. Other commands using the elevator should
* make sure they disable PID!
*/
public class SetElevatorSetpoint extends CommandBase {
private final Elevator m_elevator;
private final double m_setpoint;
/**
* Create a new SetElevatorSetpoint command.
*
* @param setpoint The setpoint to set the elevator to
* @param elevator The elevator to use
*/
public SetElevatorSetpoint(double setpoint, Elevator elevator) {
m_elevator = elevator;
m_setpoint = setpoint;
addRequirements(m_elevator);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_elevator.setSetpoint(m_setpoint);
m_elevator.enable();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_elevator.getController().atSetpoint();
}
}

48
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetWristSetpoint.java Normal file
View File

@@ -0,0 +1,48 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
/**
* Move the wrist to a given angle. This command finishes when it is within the tolerance, but
* leaves the PID loop running to maintain the position. Other commands using the wrist should make
* sure they disable PID!
*/
public class SetWristSetpoint extends CommandBase {
private final Wrist m_wrist;
private final double m_setpoint;
/**
* Create a new SetWristSetpoint command.
*
* @param setpoint The setpoint to set the wrist to
* @param wrist The wrist to use
*/
public SetWristSetpoint(double setpoint, Wrist wrist) {
m_wrist = wrist;
m_setpoint = setpoint;
addRequirements(m_wrist);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_wrist.enable();
m_wrist.setSetpoint(m_setpoint);
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_wrist.getController().atSetpoint();
}
}

56
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/TankDrive.java Normal file
View File

@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
import java.util.function.DoubleSupplier;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
/**
* Have the robot drive tank style.
*/
public class TankDrive extends CommandBase {
private final DriveTrain m_drivetrain;
private final DoubleSupplier m_left;
private final DoubleSupplier m_right;
/**
* Creates a new TankDrive command.
*
* @param drivetrain The drivetrain subsystem to drive
* @param left The control input for the left side of the drive
* @param right The control input for the right sight of the drive
*/
public TankDrive(DriveTrain drivetrain, DoubleSupplier left, DoubleSupplier right) {
m_drivetrain = drivetrain;
m_left = left;
m_right = right;
addRequirements(m_drivetrain);
}
// Called repeatedly when this Command is scheduled to run
@Override
public void execute() {
m_drivetrain.drive(m_left.getAsDouble(), m_right.getAsDouble());
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return false; // Runs until interrupted
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
m_drivetrain.drive(0, 0);
}
}

63
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Claw.java Normal file
View File

@@ -0,0 +1,63 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
/**
* The claw subsystem is a simple system with a motor for opening and closing. If using stronger
* motors, you should probably use a sensor so that the motors don't stall.
*/
public class Claw extends SubsystemBase {
private final Victor m_motor = new Victor(7);
private final DigitalInput m_contact = new DigitalInput(5);
/**
* Create a new claw subsystem.
*/
public Claw() {
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Limit Switch", m_contact);
}
public void log() {
SmartDashboard.putData("Claw switch", m_contact);
}
/**
* Set the claw motor to move in the open direction.
*/
public void open() {
m_motor.set(-1);
}
/**
* Set the claw motor to move in the close direction.
*/
public void close() {
m_motor.set(1);
}
/**
* Stops the claw motor from moving.
*/
public void stop() {
m_motor.set(0);
}
/**
* Return true when the robot is grabbing an object hard enough to trigger the limit switch.
*/
public boolean isGrabbing() {
return m_contact.get();
}
}

124
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/DriveTrain.java Normal file
View File

@@ -0,0 +1,124 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
public class DriveTrain extends SubsystemBase {
/**
* The DriveTrain subsystem incorporates the sensors and actuators attached to the robots chassis.
* These include four drive motors, a left and right encoder and a gyro.
*/
private final SpeedController m_leftMotor =
new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
private final SpeedController m_rightMotor =
new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
private final Encoder m_leftEncoder = new Encoder(1, 2);
private final Encoder m_rightEncoder = new Encoder(3, 4);
private final AnalogInput m_rangefinder = new AnalogInput(6);
private final AnalogGyro m_gyro = new AnalogGyro(1);
/**
* Create a new drive train subsystem.
*/
public DriveTrain() {
super();
// Encoders may measure differently in the real world and in
// simulation. In this example the robot moves 0.042 barleycorns
// per tick in the real world, but the simulated encoders
// simulate 360 tick encoders. This if statement allows for the
// real robot to handle this difference in devices.
if (Robot.isReal()) {
m_leftEncoder.setDistancePerPulse(0.042);
m_rightEncoder.setDistancePerPulse(0.042);
} else {
// Circumference in ft = 4in/12(in/ft)*PI
m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
}
// Let's name the sensors on the LiveWindow
addChild("Drive", m_drive);
addChild("Left Encoder", m_leftEncoder);
addChild("Right Encoder", m_rightEncoder);
addChild("Rangefinder", m_rangefinder);
addChild("Gyro", m_gyro);
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
}
/**
* Tank style driving for the DriveTrain.
*
* @param left Speed in range [-1,1]
* @param right Speed in range [-1,1]
*/
public void drive(double left, double right) {
m_drive.tankDrive(left, right);
}
/**
* Get the robot's heading.
*
* @return The robots heading in degrees.
*/
public double getHeading() {
return m_gyro.getAngle();
}
/**
* Reset the robots sensors to the zero states.
*/
public void reset() {
m_gyro.reset();
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Get the average distance of the encoders since the last reset.
*
* @return The distance driven (average of left and right encoders).
*/
public double getDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
}
/**
* Get the distance to the obstacle.
*
* @return The distance to the obstacle detected by the rangefinder.
*/
public double getDistanceToObstacle() {
// Really meters in simulation since it's a rangefinder...
return m_rangefinder.getAverageVoltage();
}
}

98
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Elevator.java Normal file
View File

@@ -0,0 +1,98 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID
* values for simulation are different than in the real world do to minor differences.
*/
public class Elevator extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
private double m_setpoint;
private static final double kP_real = 4;
private static final double kI_real = 0.07;
private static final double kP_simulation = 18;
private static final double kI_simulation = 0.2;
/**
* Create a new elevator subsystem.
*/
public Elevator() {
super(new PIDController(kP_real, kI_real, 0));
if (Robot.isSimulation()) { // Check for simulation and update PID values
getController().setPID(kP_simulation, kI_simulation, 0);
}
getController().setAbsoluteTolerance(0.005);
m_motor = new Victor(5);
// Conversion value of potentiometer varies between the real world and
// simulation
if (Robot.isReal()) {
m_pot = new AnalogPotentiometer(2, -2.0 / 5);
} else {
m_pot = new AnalogPotentiometer(2); // Defaults to meters
}
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Pot", m_pot);
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
SmartDashboard.putData("Elevator Pot", m_pot);
}
/**
* Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
*/
@Override
public double getMeasurement() {
return m_pot.get();
}
/**
* Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
public void useOutput(double output) {
m_motor.set(output);
}
/**
* Returns the setpoint used by the PIDController.
*
* @return The setpoint for the PIDController.
*/
@Override
public double getSetpoint() {
return m_setpoint;
}
/**
* Sets the setpoint used by the PIDController.
*
* @param setpoint The setpoint for the PIDController.
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
}
}

95
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Wrist.java Normal file
View File

@@ -0,0 +1,95 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
*/
public class Wrist extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
private double m_setpoint;
private static final double kP_real = 1;
private static final double kP_simulation = 0.05;
/**
* Create a new wrist subsystem.
*/
public Wrist() {
super(new PIDController(kP_real, 0, 0));
if (Robot.isSimulation()) { // Check for simulation and update PID values
getController().setPID(kP_simulation, 0, 0);
}
getController().setAbsoluteTolerance(2.5);
m_motor = new Victor(6);
// Conversion value of potentiometer varies between the real world and
// simulation
if (Robot.isReal()) {
m_pot = new AnalogPotentiometer(3, -270.0 / 5);
} else {
m_pot = new AnalogPotentiometer(3); // Defaults to degrees
}
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Pot", m_pot);
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
SmartDashboard.putData("Wrist Angle", m_pot);
}
/**
* Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
*/
@Override
public double getMeasurement() {
return m_pot.get();
}
/**
* Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
public void useOutput(double output) {
m_motor.set(output);
}
/**
* Returns the setpoint used by the PIDController.
*
* @return The setpoint for the PIDController.
*/
@Override
public double getSetpoint() {
return m_setpoint;
}
/**
* Sets the setpoint used by the PIDController.
*
* @param setpoint The setpoint for the PIDController.
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
}
}

53
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java Normal file
View File

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[]{0, 1};
public static final int[] kRightEncoderPorts = new int[]{2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
public static final boolean kGyroReversed = false;
public static final double kStabilizationP = 1;
public static final double kStabilizationI = .5;
public static final double kStabilizationD = 0;
public static final double kTurnP = 1;
public static final double kTurnI = 0;
public static final double kTurnD = 0;
public static final double kTurnToleranceDeg = 5;
public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
}
public static final class OIConstants {
public static final int kDriverControllerPort = 1;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

121
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java Normal file
View File

@@ -0,0 +1,100 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationD;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationI;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationP;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants.kDriverControllerPort;
/**
* 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 m_robotDrive = new DriveSubsystem();
// The driver's controller
XboxController m_driverController = new XboxController(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
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new RunCommand(() -> m_robotDrive
.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
// Stabilize robot to drive straight with gyro when left bumper is held
new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(
new PIDCommand(
new PIDController(kStabilizationP, kStabilizationI, kStabilizationD),
// Close the loop on the turn rate
m_robotDrive::getTurnRate,
// Setpoint is 0
0,
// Pipe the output to the turning controls
output -> m_robotDrive
.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
// Require the robot drive
m_robotDrive));
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kX.value)
.whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
}
}

54
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java Normal file
View File

@@ -0,0 +1,54 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg;
/**
* A command that will turn the robot to the specified angle.
*/
public class TurnToAngle extends PIDCommand {
/**
* Turns to robot to the specified angle.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
super(new PIDController(kTurnP, kTurnI, kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
output -> drive.arcadeDrive(0, output),
// Require the drive
drive);
// Set the controller to be continuous (because it is an angle controller)
getController().enableContinuousInput(-180, 180);
// Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
// setpoint before it is considered as having reached the reference
getController().setAbsoluteTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
}
@Override
public boolean isFinished() {
// End when the controller is at the reference.
return getController().atSetpoint();
}
}

141
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,141 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
new PWMVictorSPX(kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
new PWMVictorSPX(kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
// The gyro sensor
private final Gyro m_gyro = new ADXRS450_Gyro();
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Resets the drive encoders to currently read a position of 0.
*/
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
/**
* Zeroes the heading of the robot.
*/
public void zeroHeading() {
m_gyro.reset();
}
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
}
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
}
}

51
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java Normal file
View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[]{0, 1};
public static final int[] kRightEncoderPorts = new int[]{2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
}
public static final class HatchConstants {
public static final int kHatchSolenoidModule = 0;
public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
}
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
public static final double kAutoDriveSpeed = .5;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 1;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

113
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java Normal file
View File

@@ -0,0 +1,113 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

120
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants.kDriverControllerPort;
/**
* 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 m_robotDrive = new DriveSubsystem();
private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
// The autonomous routines
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto =
new StartEndCommand(
// Start driving forward at the start of the command
() -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0),
// Stop driving at the end of the command
() -> m_robotDrive.arcadeDrive(0, 0),
// Requires the drive subsystem
m_robotDrive
)
// Reset the encoders before starting
.beforeStarting(m_robotDrive::resetEncoders)
// End the command when the robot's driven distance exceeds the desired value
.interruptOn(() -> m_robotDrive.getAverageEncoderDistance() >= kAutoDriveDistanceInches);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);
// A chooser for autonomous commands
SendableChooser<Command> m_chooser = new SendableChooser<>();
// The driver's controller
XboxController m_driverController = new XboxController(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
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new RunCommand(() -> m_robotDrive.arcadeDrive(
m_driverController.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight)),
m_robotDrive)
);
// Add commands to the autonomous command chooser
m_chooser.addOption("Simple Auto", m_simpleAuto);
m_chooser.addOption("Complex Auto", m_complexAuto);
// Put the chooser on the dashboard
Shuffleboard.getTab("Autonomous").add(m_chooser);
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Grab the hatch when the 'A' button is pressed.
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem));
// Release the hatch when the 'B' button is pressed.
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
// While holding the shoulder button, drive at half speed
new JoystickButton(m_driverController, Button.kBumperRight.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
}
}

61
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java Normal file
View File

@@ -0,0 +1,61 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoBackupDistanceInches;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
/**
* A complex auto command that drives forward, releases a hatch, and then drives backward.
*/
public class ComplexAutoCommand extends SequentialCommandGroup {
/**
* Creates a new ComplexAutoCommand.
*
* @param driveSubsystem The drive subsystem this command will run on
* @param hatchSubsystem The hatch subsystem this command will run on
*/
public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
addCommands(
// Drive forward up to the front of the cargo ship
new StartEndCommand(
// Start driving forward at the start of the command
() -> driveSubsystem.arcadeDrive(kAutoDriveSpeed, 0),
// Stop driving at the end of the command
() -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem
)
// Reset the encoders before starting
.beforeStarting(driveSubsystem::resetEncoders)
// End the command when the robot's driven distance exceeds the desired value
.interruptOn(
() -> driveSubsystem.getAverageEncoderDistance() >= kAutoDriveDistanceInches),
// Release the hatch
new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
// Drive backward the specified distance
new StartEndCommand(
() -> driveSubsystem.arcadeDrive(-kAutoDriveSpeed, 0),
() -> driveSubsystem.arcadeDrive(0, 0),
driveSubsystem
)
.beforeStarting(driveSubsystem::resetEncoders)
.interruptOn(
() -> driveSubsystem.getAverageEncoderDistance() <= -kAutoBackupDistanceInches)
);
}
}

110
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor2Port;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
new PWMVictorSPX(kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
new PWMVictorSPX(kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Resets the drive encoders to currently read a position of 0.
*/
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the TWO encoders.
*
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

38
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java Normal file
View File

@@ -0,0 +1,38 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidModule;
import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidPorts;
/**
* A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}.
*/
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
/**
* Grabs the hatch.
*/
public void grabHatch() {
m_hatchSolenoid.set(kForward);
}
/**
* Releases the hatch.
*/
public void releaseHatch() {
m_hatchSolenoid.set(kReverse);
}
}

51
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java Normal file
View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[]{0, 1};
public static final int[] kRightEncoderPorts = new int[]{2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
}
public static final class HatchConstants {
public static final int kHatchSolenoidModule = 0;
public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
}
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
public static final double kAutoDriveSpeed = .5;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 1;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

120
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

110
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.GrabHatch;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.HalveDriveSpeed;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants.kDriverControllerPort;
/**
* 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 m_robotDrive = new DriveSubsystem();
private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
// The autonomous routines
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto =
new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, m_robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
// A chooser for autonomous commands
SendableChooser<Command> m_chooser = new SendableChooser<>();
// The driver's controller
XboxController m_driverController = new XboxController(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
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new DefaultDrive(
m_robotDrive,
() -> m_driverController.getY(GenericHID.Hand.kLeft),
() -> m_driverController.getX(GenericHID.Hand.kRight))
);
// Add commands to the autonomous command chooser
m_chooser.addOption("Simple Auto", m_simpleAuto);
m_chooser.addOption("Complex Auto", m_complexAuto);
// Put the chooser on the dashboard
Shuffleboard.getTab("Autonomous").add(m_chooser);
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Grab the hatch when the 'A' button is pressed.
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(new GrabHatch(m_hatchSubsystem));
// Release the hatch when the 'B' button is pressed.
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(new ReleaseHatch(m_hatchSubsystem));
// While holding the shoulder button, drive at half speed
new JoystickButton(m_driverController, Button.kBumperRight.value)
.whenHeld(new HalveDriveSpeed(m_robotDrive));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
}
}

42
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java Normal file
View File

@@ -0,0 +1,42 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoBackupDistanceInches;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
/**
* A complex auto command that drives forward, releases a hatch, and then drives backward.
*/
public class ComplexAuto extends SequentialCommandGroup {
/**
* Creates a new ComplexAuto.
*
* @param drive The drive subsystem this command will run on
* @param hatch The hatch subsystem this command will run on
*/
public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) {
addCommands(
// Drive forward the specified distance
new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive),
// Release the hatch
new ReleaseHatch(hatch),
// Drive backward the specified distance
new DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive)
);
}
}

44
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java Normal file
View File

@@ -0,0 +1,44 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
import java.util.function.DoubleSupplier;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
/**
* A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written
* explicitly for pedagogical purposes - actual code should inline a command this simple with {@link
* edu.wpi.first.wpilibj2.command.RunCommand}.
*/
public class DefaultDrive extends CommandBase {
private final DriveSubsystem m_drive;
private final DoubleSupplier m_forward;
private final DoubleSupplier m_rotation;
/**
* Creates a new DefaultDrive.
*
* @param subsystem The drive subsystem this command wil run on.
* @param forward The control input for driving forwards/backwards
* @param rotation The control input for turning
*/
public DefaultDrive(DriveSubsystem subsystem, DoubleSupplier forward, DoubleSupplier rotation) {
m_drive = subsystem;
m_forward = forward;
m_rotation = rotation;
addRequirements(m_drive);
}
@Override
public void execute() {
m_drive.arcadeDrive(m_forward.getAsDouble(), m_rotation.getAsDouble());
}
}

47
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java Normal file
View File

@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
public class DriveDistance extends CommandBase {
private final DriveSubsystem m_drive;
private final double m_distance;
private final double m_speed;
/**
* Creates a new DriveDistance.
*
* @param inches The number of inches the robot will drive
* @param speed The speed at which the robot will drive
* @param drive The drive subsystem on which this command will run
*/
public DriveDistance(double inches, double speed, DriveSubsystem drive) {
m_distance = inches;
m_speed = speed;
m_drive = drive;
}
@Override
public void initialize() {
m_drive.resetEncoders();
m_drive.arcadeDrive(m_speed, 0);
}
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
}
@Override
public boolean isFinished() {
return Math.abs(m_drive.getAverageEncoderDistance()) >= m_distance;
}
}

37
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java Normal file
View File

@@ -0,0 +1,37 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
/**
* A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
* pedagogical purposes. Actual code should inline a command this simple with {@link
* edu.wpi.first.wpilibj2.command.InstantCommand}.
*/
public class GrabHatch extends CommandBase {
// The subsystem the command runs on
private final HatchSubsystem m_hatchSubsystem;
public GrabHatch(HatchSubsystem subsystem) {
m_hatchSubsystem = subsystem;
addRequirements(m_hatchSubsystem);
}
@Override
public void initialize() {
m_hatchSubsystem.grabHatch();
}
@Override
public boolean isFinished() {
return true;
}
}

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
public class HalveDriveSpeed extends CommandBase {
private final DriveSubsystem m_drive;
public HalveDriveSpeed(DriveSubsystem drive) {
m_drive = drive;
}
@Override
public void initialize() {
m_drive.setMaxOutput(.5);
}
@Override
public void end(boolean interrupted) {
m_drive.setMaxOutput(1);
}
}

21
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java Normal file
View File

@@ -0,0 +1,21 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
/**
* A command that releases the hatch.
*/
public class ReleaseHatch extends InstantCommand {
public ReleaseHatch(HatchSubsystem subsystem) {
super(subsystem::releaseHatch, subsystem);
}
}

110
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor2Port;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
new PWMVictorSPX(kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
new PWMVictorSPX(kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/**
* Resets the drive encoders to currently read a position of 0.
*/
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the TWO encoders.
*
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

38
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java Normal file
View File

@@ -0,0 +1,38 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidModule;
import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidPorts;
/**
* A hatch mechanism actuated by a single {@link DoubleSolenoid}.
*/
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
/**
* Grabs the hatch.
*/
public void grabHatch() {
m_hatchSolenoid.set(kForward);
}
/**
* Releases the hatch.
*/
public void releaseHatch() {
m_hatchSolenoid.set(kReverse);
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Trigger;
/**
* A custom button that is triggered when two buttons on a Joystick are
* A custom button that is triggered when TWO buttons on a Joystick are
* simultaneously pressed.
*/
public class DoubleButton extends Trigger {

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java Normal file
View File

@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.schedulereventlogging;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
/**
* Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain
* access to the constants contained within without having to preface the names with the class,
* greatly reducing the amount of text required.
*/
public static final class OIConstants {
// Example: the port of the driver's controller
public static final int kDriverControllerPort = 1;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.schedulereventlogging;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

120
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.schedulereventlogging;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

83
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java Normal file
View File

@@ -0,0 +1,83 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.schedulereventlogging;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants.kDriverControllerPort;
/**
* 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 driver's controller
private final XboxController m_driverController = new XboxController(kDriverControllerPort);
// A few commands that do nothing, but will demonstrate the scheduler functionality
private final CommandBase m_instantCommand1 = new InstantCommand();
private final CommandBase m_instantCommand2 = new InstantCommand();
private final CommandBase m_waitCommand = new WaitCommand(5);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Set names of commands
m_instantCommand1.setName("Instant Command 1");
m_instantCommand2.setName("Instant Command 2");
m_waitCommand.setName("Wait 5 Seconds Command");
// Set the scheduler to log Shuffleboard events for command initialize, interrupt, finish
CommandScheduler.getInstance().onCommandInitialize(command -> Shuffleboard.addEventMarker(
"Command initialized", command.getName(), EventImportance.kNormal));
CommandScheduler.getInstance().onCommandInterrupt(command -> Shuffleboard.addEventMarker(
"Command interrupted", command.getName(), EventImportance.kNormal));
CommandScheduler.getInstance().onCommandFinish(command -> Shuffleboard.addEventMarker(
"Command finished", command.getName(), EventImportance.kNormal));
// Configure the button bindings
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Run instant command 1 when the 'A' button is pressed
new JoystickButton(m_driverController, Button.kA.value).whenPressed(m_instantCommand1);
// Run instant command 2 when the 'X' button is pressed
new JoystickButton(m_driverController, Button.kX.value).whenPressed(m_instantCommand2);
// Run instant command 3 when the 'Y' button is held; release early to interrupt
new JoystickButton(m_driverController, Button.kY.value).whenHeld(m_waitCommand);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return new InstantCommand();
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java Normal file
View File

@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.selectcommand;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
/**
* Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain
* access to the constants contained within without having to preface the names with the class,
* greatly reducing the amount of text required.
*/
public static final class OIConstants {
// Example: the port of the driver's controller
public static final int kDriverControllerPort = 1;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.selectcommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

120
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.selectcommand;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

75
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java Normal file
View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.selectcommand;
import java.util.Map;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SelectCommand;
import static java.util.Map.entry;
/**
* 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 enum used as keys for selecting the command to run.
private enum CommandSelector {
ONE, TWO, THREE
}
// An example selector method for the selectcommand. Returns the selector that will select
// which command to run. Can base this choice on logical conditions evaluated at runtime.
private CommandSelector select() {
return CommandSelector.ONE;
}
// An example selectcommand. Will select from the three commands based on the value returned
// by the selector method at runtime. Note that selectcommand works on Object(), so the
// selector does not have to be an enum; it could be any desired type (string, integer,
// boolean, double...)
private final Command m_exampleSelectCommand =
new SelectCommand(
// Maps selector values to commands
Map.ofEntries(
entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")),
entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")),
entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))
),
this::select
);
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_exampleSelectCommand;
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -30,7 +30,7 @@ public class Robot extends TimedRobot {
@Override
public void robotInit() {
// Add a 'max speed' widget to a tab named 'Configuration', using a number slider
// The widget will be placed in the second column and row and will be two columns wide
// The widget will be placed in the second column and row and will be TWO columns wide
m_maxSpeed = Shuffleboard.getTab("Configuration")
.add("Max Speed", 1)
.withWidget("Number Slider")

19
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java Normal file
View File

@@ -0,0 +1,19 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates.commandbased;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
}

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,9 +10,9 @@ package edu.wpi.first.wpilibj.templates.commandbased;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all.
* Unless you know what you are doing, do not modify this file except to
* change the parameter class to the startRobot call.
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {

42
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java
View File

@@ -1,42 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates.commandbased;
/**
* This class is the glue that binds the controls on the physical operator
* interface to the commands and command groups that allow control of the robot.
*/
public class OI {
//// CREATING BUTTONS
// One type of button is a joystick button which is any button on a
//// joystick.
// You create one by telling it which joystick it's on and which button
// number it is.
// Joystick stick = new Joystick(port);
// Button button = new JoystickButton(stick, buttonNumber);
// There are a few additional built in buttons you can use. Additionally,
// by subclassing Button you can create custom triggers and bind those to
// commands the same as any other Button.
//// TRIGGERING COMMANDS WITH BUTTONS
// Once you have a button, it's trivial to bind it to a button in one of
// three ways:
// Start the command when the button is pressed and let it run the command
// until it is finished as determined by it's isFinished method.
// button.whenPressed(new ExampleCommand());
// Run the command while the button is being held down and interrupt it once
// the button is released.
// button.whileHeld(new ExampleCommand());
// Start the command when the button is released and let it run the command
// until it is finished as determined by it's isFinished method.
// button.whenReleased(new ExampleCommand());
}

76
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,55 +8,49 @@
package edu.wpi.first.wpilibj.templates.commandbased;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand;
import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the TimedRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the build.gradle file in the
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
public static OI m_oi;
private Command m_autonomousCommand;
Command m_autonomousCommand;
SendableChooser<Command> m_chooser = new SendableChooser<>();
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
m_oi = new OI();
m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
// chooser.addOption("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", m_chooser);
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@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.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
@Override
public void disabledInit() {
@@ -64,34 +58,18 @@ public class Robot extends TimedRobot {
@Override
public void disabledPeriodic() {
Scheduler.getInstance().run();
}
/**
* This autonomous (along with the chooser code above) shows how to select
* between different autonomous modes using the dashboard. The sendable
* chooser code works with the Java SmartDashboard. If you prefer the
* LabVIEW Dashboard, remove all of the chooser code and uncomment the
* getString code to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional commands to the
* chooser code above (like the commented example) or additional comparisons
* to the switch structure below with additional strings & commands.
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_chooser.getSelected();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.start();
m_autonomousCommand.schedule();
}
}
@@ -100,7 +78,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();
}
@Override
@@ -119,7 +96,12 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/**

57
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java Normal file
View File

@@ -0,0 +1,57 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates.commandbased;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand;
import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
/**
* 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 and commands are defined here...
private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
return m_autoCommand;
}
}

26
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java
View File

@@ -1,26 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates.commandbased;
/**
* The RobotMap is a mapping from the ports sensors and actuators are wired into
* to a variable name. This provides flexibility changing wiring, makes checking
* the wiring easier and significantly reduces the number of magic numbers
* floating around.
*/
public class RobotMap {
// For example to map the left and right motors, you could define the
// following variables to use with your drivetrain subsystem.
// public static int leftMotor = 1;
// public static int rightMotor = 2;
// If you are using multiple modules, make sure to define both the port
// number and the module. For example you with a rangefinder:
// public static int rangefinderPort = 1;
// public static int rangefinderModule = 1;
}

49
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,42 +7,23 @@
package edu.wpi.first.wpilibj.templates.commandbased.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.templates.commandbased.Robot;
import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.CommandBase;
/**
* An example command. You can replace me with your own command.
* An example command that uses an example subsystem.
*/
public class ExampleCommand extends Command {
public ExampleCommand() {
// Use requires() here to declare subsystem dependencies
requires(Robot.m_subsystem);
}
public class ExampleCommand extends CommandBase {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final ExampleSubsystem m_subsystem;
// Called just before this Command runs the first time
@Override
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return false;
}
// Called once after isFinished returns true
@Override
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ExampleCommand(ExampleSubsystem subsystem) {
m_subsystem = subsystem;
addRequirements(subsystem);
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,18 +7,21 @@
package edu.wpi.first.wpilibj.templates.commandbased.subsystems;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
/**
* An example subsystem. You can replace me with your own Subsystem.
*/
public class ExampleSubsystem extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
public class ExampleSubsystem extends SubsystemBase {
/**
* Creates a new ExampleSubsystem.
*/
public ExampleSubsystem() {
}
/**
* Will be called periodically whenever the CommandScheduler runs.
*/
@Override
public void initDefaultCommand() {
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());
public void periodic() {
}
}