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

[examples] Add Java Examples and Templates for the XRP (#5529)

Browse Source
This commit is contained in:
Zhiquan Yeo
2023-08-12 02:31:35 -04:00
committed by GitHub
parent e8b5d44752
commit 86d7bbc4e4
40 changed files with 2613 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
shared/examplecheck.gradle
View File

@@ -50,6 +50,8 @@ def tagList = [
"Command-based",
// Romi
"Romi",
// XRP
"XRP",
// Extremely simple programs showcasing a single hardware API
"Hardware",
// Full robot, with multiple mechanisms

4
styleguide/spotbugs-exclude.xml
View File

@@ -108,6 +108,10 @@
<Bug pattern="UC_USELESS_VOID_METHOD" />
<Class name="edu.wpi.first.wpilibj.templates.romitimed.Robot" />
</Match>
<Match>
<Bug pattern="UC_USELESS_VOID_METHOD" />
<Class name="edu.wpi.first.wpilibj.templates.xrptimed.Robot" />
</Match>
<Match>
<Bug pattern="UC_USELESS_VOID_METHOD" />
<Class name="edu.wpi.first.wpilibj.templates.timed.Robot" />

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

@@ -893,6 +893,21 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "XRP Reference",
"description": "An example command-based robot program that can be used with the XRP reference robot design.",
"tags": [
"XRP",
"Command-based",
"Differential Drive",
"Digital Input",
"Joystick"
],
"foldername": "xrpreference",
"gradlebase": "javaxrp",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Digital Communication Sample",
"description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",

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

@@ -0,0 +1,19 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference;
/**
* 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 class OperatorConstants {
public static final int kDriverControllerPort = 0;
}
}

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

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference;
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);
}
}

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

@@ -0,0 +1,103 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference;
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 20 ms, 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() {}
/** This function is called once when the robot is first started up. */
@Override
public void simulationInit() {}
/** This function is called periodically whilst in simulation. */
@Override
public void simulationPeriodic() {}
}

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

@@ -0,0 +1,99 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.xrpreference.commands.ArcadeDrive;
import edu.wpi.first.wpilibj.examples.xrpreference.commands.AutonomousDistance;
import edu.wpi.first.wpilibj.examples.xrpreference.commands.AutonomousTime;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Arm;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.XRPOnBoardIO;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
/**
* 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 XRPOnBoardIO m_onboardIO = new XRPOnBoardIO();
private final Arm m_arm = new Arm();
// Assumes a gamepad plugged into channel 0
private final Joystick m_controller = new Joystick(0);
// Create SmartDashboard chooser for autonomous routines
private final SendableChooser<Command> m_chooser = new SendableChooser<>();
/** 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 passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Default command is arcade drive. This will run unless another command
// is scheduled over it.
m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
// Example of how to use the onboard IO
Trigger userButton = new Trigger(m_onboardIO::getUserButtonPressed);
userButton
.onTrue(new PrintCommand("USER Button Pressed"))
.onFalse(new PrintCommand("USER Button Released"));
JoystickButton joystickAButton = new JoystickButton(m_controller, 1);
joystickAButton
.onTrue(new InstantCommand(() -> m_arm.setAngle(45.0), m_arm))
.onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm));
JoystickButton joystickBButton = new JoystickButton(m_controller, 2);
joystickBButton
.onTrue(new InstantCommand(() -> m_arm.setAngle(90.0), m_arm))
.onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm));
// Setup SmartDashboard options
m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
SmartDashboard.putData(m_chooser);
}
/**
* 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();
}
/**
* Use this to pass the teleop command to the main {@link Robot} class.
*
* @return the command to run in teleop
*/
public Command getArcadeDriveCommand() {
return new ArcadeDrive(
m_drivetrain, () -> -m_controller.getRawAxis(1), () -> -m_controller.getRawAxis(2));
}
}

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

@@ -0,0 +1,53 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
import java.util.function.Supplier;
public class ArcadeDrive extends Command {
private final Drivetrain m_drivetrain;
private final Supplier<Double> m_xaxisSpeedSupplier;
private final Supplier<Double> m_zaxisRotateSupplier;
/**
* Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
* lambdas. This command does not terminate.
*
* @param drivetrain The drivetrain subsystem on which this command will run
* @param xaxisSpeedSupplier Lambda supplier of forward/backward speed
* @param zaxisRotateSupplier Lambda supplier of rotational speed
*/
public ArcadeDrive(
Drivetrain drivetrain,
Supplier<Double> xaxisSpeedSupplier,
Supplier<Double> zaxisRotateSupplier) {
m_drivetrain = drivetrain;
m_xaxisSpeedSupplier = xaxisSpeedSupplier;
m_zaxisRotateSupplier = zaxisRotateSupplier;
addRequirements(drivetrain);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

24
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousDistance.java Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
public class AutonomousDistance extends SequentialCommandGroup {
/**
* Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
* turn around and drive back.
*
* @param drivetrain The drivetrain subsystem on which this command will run
*/
public AutonomousDistance(Drivetrain drivetrain) {
addCommands(
new DriveDistance(-0.5, 10, drivetrain),
new TurnDegrees(-0.5, 180, drivetrain),
new DriveDistance(-0.5, 10, drivetrain),
new TurnDegrees(0.5, 180, drivetrain));
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousTime.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
public class AutonomousTime extends SequentialCommandGroup {
/**
* Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn
* around for time (equivalent to time to turn around) and drive forward again. This should mimic
* driving out, turning around and driving back.
*
* @param drivetrain The drive subsystem on which this command will run
*/
public AutonomousTime(Drivetrain drivetrain) {
addCommands(
new DriveTime(-0.6, 2.0, drivetrain),
new TurnTime(-0.5, 1.3, drivetrain),
new DriveTime(-0.6, 2.0, drivetrain),
new TurnTime(0.5, 1.3, drivetrain));
}
}

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

@@ -0,0 +1,55 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
public class DriveDistance extends Command {
private final Drivetrain m_drive;
private final double m_distance;
private final double m_speed;
/**
* Creates a new DriveDistance. This command will drive your your robot for a desired distance at
* a desired speed.
*
* @param speed The speed at which the robot will drive
* @param inches The number of inches the robot will drive
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveDistance(double speed, double inches, Drivetrain drive) {
m_distance = inches;
m_speed = speed;
m_drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_drive.arcadeDrive(0, 0);
m_drive.resetEncoders();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// Compare distance travelled from start to desired distance
return Math.abs(m_drive.getAverageDistanceInch()) >= m_distance;
}
}

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

@@ -0,0 +1,54 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
public class DriveTime extends Command {
private final double m_duration;
private final double m_speed;
private final Drivetrain m_drive;
private long m_startTime;
/**
* Creates a new DriveTime. This command will drive your robot for a desired speed and time.
*
* @param speed The speed which the robot will drive. Negative is in reverse.
* @param time How much time to drive in seconds
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveTime(double speed, double time, Drivetrain drive) {
m_speed = speed;
m_duration = time * 1000;
m_drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
m_drive.arcadeDrive(0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (System.currentTimeMillis() - m_startTime) >= m_duration;
}
}

68
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnDegrees.java Normal file
View File

@@ -0,0 +1,68 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
public class TurnDegrees extends Command {
private final Drivetrain m_drive;
private final double m_degrees;
private final double m_speed;
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
* degrees) and rotational speed.
*
* @param speed The speed which the robot will drive. Negative is in reverse.
* @param degrees Degrees to turn. Leverages encoders to compare distance.
* @param drive The drive subsystem on which this command will run
*/
public TurnDegrees(double speed, double degrees, Drivetrain drive) {
m_degrees = degrees;
m_speed = speed;
m_drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
// Set motors to stop, read encoder values for starting point
m_drive.arcadeDrive(0, 0);
m_drive.resetEncoders();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_speed);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
/* Need to convert distance travelled to degrees. The Standard
XRP Chassis found here, https://www.sparkfun.com/products/22230,
has a wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm
or 6.102 inches. We then take into consideration the width of the tires.
*/
double inchPerDegree = Math.PI * 6.102 / 360;
// Compare distance travelled from start to distance based on degree turn
return getAverageTurningDistance() >= (inchPerDegree * m_degrees);
}
private double getAverageTurningDistance() {
double leftDistance = Math.abs(m_drive.getLeftDistanceInch());
double rightDistance = Math.abs(m_drive.getRightDistanceInch());
return (leftDistance + rightDistance) / 2.0;
}
}

58
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnTime.java Normal file
View File

@@ -0,0 +1,58 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.commands;
import edu.wpi.first.wpilibj.examples.xrpreference.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
/*
* Creates a new TurnTime command. This command will turn your robot for a
* desired rotational speed and time.
*/
public class TurnTime extends Command {
private final double m_duration;
private final double m_rotationalSpeed;
private final Drivetrain m_drive;
private long m_startTime;
/**
* Creates a new TurnTime.
*
* @param speed The speed which the robot will turn. Negative is in reverse.
* @param time How much time to turn in seconds
* @param drive The drive subsystem on which this command will run
*/
public TurnTime(double speed, double time, Drivetrain drive) {
m_rotationalSpeed = speed;
m_duration = time * 1000;
m_drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
m_drive.arcadeDrive(0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_rotationalSpeed);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (System.currentTimeMillis() - m_startTime) >= m_duration;
}
}

109
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPMotor.java Normal file
View File

@@ -0,0 +1,109 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.devices;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPMotor.
*
* <p>A SimDevice based motor controller representing the motors on an XRP robot
*/
public class XRPMotor implements MotorController {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(0, "motorL");
s_simDeviceNameMap.put(1, "motorR");
s_simDeviceNameMap.put(2, "motor3");
s_simDeviceNameMap.put(3, "motor4");
}
private final SimDouble m_simSpeed;
private final SimBoolean m_simInverted;
/** XRPMotor. */
public XRPMotor(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
if (xrpMotorSimDevice != null) {
xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
} else {
m_simInverted = null;
m_simSpeed = null;
}
}
@Override
public void set(double speed) {
if (m_simSpeed != null) {
boolean invert = false;
if (m_simInverted != null) {
invert = m_simInverted.get();
}
m_simSpeed.set(invert ? -speed : speed);
}
}
@Override
public double get() {
if (m_simSpeed != null) {
return m_simSpeed.get();
}
return 0.0;
}
@Override
public void setInverted(boolean isInverted) {
if (m_simInverted != null) {
m_simInverted.set(isInverted);
}
}
@Override
public boolean getInverted() {
if (m_simInverted != null) {
return m_simInverted.get();
}
return false;
}
@Override
public void disable() {
set(0.0);
}
@Override
public void stopMotor() {
set(0.0);
}
}

123
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPServo.java Normal file
View File

@@ -0,0 +1,123 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.devices;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPServo.
*
* <p>A SimDevice based servo
*/
public class XRPServo {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(4, "servo1");
s_simDeviceNameMap.put(5, "servo2");
}
private final SimDouble m_simPosition;
/** XRPServo. */
public XRPServo(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on WS as type: "XRPServo", device: <servo name>
String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
if (xrpServoSimDevice != null) {
xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
// This should mimic PWM position [0.0, 1.0]
m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
} else {
m_simPosition = null;
}
}
/**
* Set the servo angle.
*
* @param angle Desired angle in degrees
*/
public void setAngle(double angle) {
if (angle < 0.0) {
angle = 0.0;
}
if (angle > 180.0) {
angle = 180.0;
}
double pos = angle / 180.0;
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo angle.
*
* @return Current servo angle
*/
public double getAngle() {
if (m_simPosition != null) {
return m_simPosition.get() * 180.0;
}
return 90.0;
}
/**
* Set the servo position.
*
* @param pos Desired position (Between 0.0 and 1.0)
*/
public void setPosition(double pos) {
if (pos < 0.0) {
pos = 0.0;
}
if (pos > 1.0) {
pos = 1.0;
}
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo position.
*
* @return Current servo position
*/
public double getPosition() {
if (m_simPosition != null) {
return m_simPosition.get();
}
return 0.5;
}
}

132
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/sensors/XRPGyro.java Normal file
View File

@@ -0,0 +1,132 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.sensors;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
public class XRPGyro {
private final SimDouble m_simRateX;
private final SimDouble m_simRateY;
private final SimDouble m_simRateZ;
private final SimDouble m_simAngleX;
private final SimDouble m_simAngleY;
private final SimDouble m_simAngleZ;
private double m_angleXOffset;
private double m_angleYOffset;
private double m_angleZOffset;
/** Create a new XRPGyro. */
public XRPGyro() {
SimDevice gyroSimDevice = SimDevice.create("Gyro:XRPGyro");
if (gyroSimDevice != null) {
gyroSimDevice.createBoolean("init", Direction.kOutput, true);
m_simRateX = gyroSimDevice.createDouble("rate_x", Direction.kInput, 0.0);
m_simRateY = gyroSimDevice.createDouble("rate_y", Direction.kInput, 0.0);
m_simRateZ = gyroSimDevice.createDouble("rate_z", Direction.kInput, 0.0);
m_simAngleX = gyroSimDevice.createDouble("angle_x", Direction.kInput, 0.0);
m_simAngleY = gyroSimDevice.createDouble("angle_y", Direction.kInput, 0.0);
m_simAngleZ = gyroSimDevice.createDouble("angle_z", Direction.kInput, 0.0);
} else {
m_simRateX = null;
m_simRateY = null;
m_simRateZ = null;
m_simAngleX = null;
m_simAngleY = null;
m_simAngleZ = null;
}
}
/**
* Get the rate of turn in degrees-per-second around the X-axis.
*
* @return rate of turn in degrees-per-second
*/
public double getRateX() {
if (m_simRateX != null) {
return m_simRateX.get();
}
return 0.0;
}
/**
* Get the rate of turn in degrees-per-second around the Y-axis.
*
* @return rate of turn in degrees-per-second
*/
public double getRateY() {
if (m_simRateY != null) {
return m_simRateY.get();
}
return 0.0;
}
/**
* Get the rate of turn in degrees-per-second around the Z-axis.
*
* @return rate of turn in degrees-per-second
*/
public double getRateZ() {
if (m_simRateZ != null) {
return m_simRateZ.get();
}
return 0.0;
}
/**
* Get the currently reported angle around the X-axis.
*
* @return current angle around X-axis in degrees
*/
public double getAngleX() {
if (m_simAngleX != null) {
return m_simAngleX.get() - m_angleXOffset;
}
return 0.0;
}
/**
* Get the currently reported angle around the X-axis.
*
* @return current angle around Y-axis in degrees
*/
public double getAngleY() {
if (m_simAngleY != null) {
return m_simAngleY.get() - m_angleYOffset;
}
return 0.0;
}
/**
* Get the currently reported angle around the Z-axis.
*
* @return current angle around Z-axis in degrees
*/
public double getAngleZ() {
if (m_simAngleZ != null) {
return m_simAngleZ.get() - m_angleZOffset;
}
return 0.0;
}
/** Reset the gyro angles to 0. */
public void reset() {
if (m_simAngleX != null) {
m_angleXOffset = m_simAngleX.get();
m_angleYOffset = m_simAngleY.get();
m_angleZOffset = m_simAngleZ.get();
}
}
}

32
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Arm.java Normal file
View File

@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.subsystems;
import edu.wpi.first.wpilibj.examples.xrpreference.devices.XRPServo;
import edu.wpi.first.wpilibj2.command.Subsystem;
public class Arm extends Subsystem {
private final XRPServo m_armServo;
/** Creates a new Arm. */
public Arm() {
// Device number 4 maps to the physical Servo 1 port on the XRP
m_armServo = new XRPServo(4);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Set the current angle of the arm (0 - 180 degrees).
*
* @param angleDeg Desired arm angle in degrees
*/
public void setAngle(double angleDeg) {
m_armServo.setAngle(angleDeg);
}
}

145
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java Normal file
View File

@@ -0,0 +1,145 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.subsystems;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.xrpreference.devices.XRPMotor;
import edu.wpi.first.wpilibj.examples.xrpreference.sensors.XRPGyro;
import edu.wpi.first.wpilibj2.command.Subsystem;
public class Drivetrain extends Subsystem {
private static final double kGearRatio =
(30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
private static final double kCountsPerMotorShaftRev = 12.0;
private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
private static final double kWheelDiameterInch = 2.3622; // 60 mm
// The XRP has the left and right motors set to
// channels 0 and 1 respectively
private final XRPMotor m_leftMotor = new XRPMotor(0);
private final XRPMotor m_rightMotor = new XRPMotor(1);
// The XRP has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
// Set up the XRPGyro
private final XRPGyro m_gyro = new XRPGyro();
// Set up the BuiltInAccelerometer
private final BuiltInAccelerometer m_accelerometer = new BuiltInAccelerometer();
/** Creates a new Drivetrain. */
public Drivetrain() {
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
}
public double getAverageDistanceInch() {
return (getLeftDistanceInch() + getRightDistanceInch()) / 2.0;
}
/**
* The acceleration in the X-axis.
*
* @return The acceleration of the XRP along the X-axis in Gs
*/
public double getAccelX() {
return m_accelerometer.getX();
}
/**
* The acceleration in the Y-axis.
*
* @return The acceleration of the XRP along the Y-axis in Gs
*/
public double getAccelY() {
return m_accelerometer.getY();
}
/**
* The acceleration in the Z-axis.
*
* @return The acceleration of the XRP along the Z-axis in Gs
*/
public double getAccelZ() {
return m_accelerometer.getZ();
}
/**
* Current angle of the XRP around the X-axis.
*
* @return The current angle of the XRP in degrees
*/
public double getGyroAngleX() {
return m_gyro.getAngleX();
}
/**
* Current angle of the XRP around the Y-axis.
*
* @return The current angle of the XRP in degrees
*/
public double getGyroAngleY() {
return m_gyro.getAngleY();
}
/**
* Current angle of the XRP around the Z-axis.
*
* @return The current angle of the XRP in degrees
*/
public double getGyroAngleZ() {
return m_gyro.getAngleZ();
}
/** Reset the gyro. */
public void resetGyro() {
m_gyro.reset();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

47
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/XRPOnBoardIO.java Normal file
View File

@@ -0,0 +1,47 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.xrpreference.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj2.command.Subsystem;
/**
* This class represents the onboard IO of the XRP Reference Robot. This includes the USER
* pushbutton and LED
*/
public class XRPOnBoardIO extends Subsystem {
private final DigitalInput m_button = new DigitalInput(0);
private final DigitalOutput m_led = new DigitalOutput(1);
/** Constructor. */
public XRPOnBoardIO() {
// No need to do anything else. Unlike the Romi, there are no other configurable
// I/O ports
}
/**
* Gets if the USER button is pressed.
*
* @return True if the USER button is currently pressed.
*/
public boolean getUserButtonPressed() {
return m_button.get();
}
/**
* Sets the onboard LED.
*
* @param value True to activate LED, false otherwise.
*/
public void setLed(boolean value) {
m_led.set(value);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

36
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
View File

@@ -81,6 +81,30 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "XRP - Command Robot",
"description": "XRP - Command style",
"tags": [
"Command",
"XRP"
],
"foldername": "xrpcommandbased",
"gradlebase": "javaxrp",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "XRP - Timed Robot",
"description": "XRP - Timed style",
"tags": [
"Timed",
"XRP"
],
"foldername": "xrptimed",
"gradlebase": "javaxrp",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Educational Robot",
"description": "Educational Robot - Not for competition use",
@@ -103,5 +127,17 @@
"gradlebase": "javaromi",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "XRP - Educational Robot",
"description": "XRP - Educational Robot",
"tags": [
"Educational",
"XRP"
],
"foldername": "xrpeducational",
"gradlebase": "javaxrp",
"mainclass": "Main",
"commandversion": 2
}
]

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

@@ -0,0 +1,15 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased;
/**
* 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 {}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Main.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased;
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);
}
}

95
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Robot.java Normal file
View File

@@ -0,0 +1,95 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased;
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 20 ms, 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() {}
}

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

@@ -0,0 +1,47 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.templates.xrpcommandbased.commands.ExampleCommand;
import edu.wpi.first.wpilibj.templates.xrpcommandbased.subsystems.XRPDrivetrain;
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 XRPDrivetrain m_xrpDrivetrain = new XRPDrivetrain();
private final ExampleCommand m_autoCommand = new ExampleCommand(m_xrpDrivetrain);
/** 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 edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then 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;
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/commands/ExampleCommand.java Normal file
View File

@@ -0,0 +1,43 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased.commands;
import edu.wpi.first.wpilibj.templates.xrpcommandbased.subsystems.XRPDrivetrain;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ExampleCommand extends Command {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final XRPDrivetrain m_subsystem;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ExampleCommand(XRPDrivetrain subsystem) {
m_subsystem = subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

109
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPMotor.java Normal file
View File

@@ -0,0 +1,109 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased.devices;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPMotor.
*
* <p>A SimDevice based motor controller representing the motors on an XRP robot
*/
public class XRPMotor implements MotorController {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(0, "motorL");
s_simDeviceNameMap.put(1, "motorR");
s_simDeviceNameMap.put(2, "motor3");
s_simDeviceNameMap.put(3, "motor4");
}
private final SimDouble m_simSpeed;
private final SimBoolean m_simInverted;
/** XRPMotor. */
public XRPMotor(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
if (xrpMotorSimDevice != null) {
xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
} else {
m_simInverted = null;
m_simSpeed = null;
}
}
@Override
public void set(double speed) {
if (m_simSpeed != null) {
boolean invert = false;
if (m_simInverted != null) {
invert = m_simInverted.get();
}
m_simSpeed.set(invert ? -speed : speed);
}
}
@Override
public double get() {
if (m_simSpeed != null) {
return m_simSpeed.get();
}
return 0.0;
}
@Override
public void setInverted(boolean isInverted) {
if (m_simInverted != null) {
m_simInverted.set(isInverted);
}
}
@Override
public boolean getInverted() {
if (m_simInverted != null) {
return m_simInverted.get();
}
return false;
}
@Override
public void disable() {
set(0.0);
}
@Override
public void stopMotor() {
set(0.0);
}
}

123
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPServo.java Normal file
View File

@@ -0,0 +1,123 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased.devices;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPServo.
*
* <p>A SimDevice based servo
*/
public class XRPServo {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(4, "servo1");
s_simDeviceNameMap.put(5, "servo2");
}
private final SimDouble m_simPosition;
/** XRPServo. */
public XRPServo(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on WS as type: "XRPServo", device: <servo name>
String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
if (xrpServoSimDevice != null) {
xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
// This should mimic PWM position [0.0, 1.0]
m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
} else {
m_simPosition = null;
}
}
/**
* Set the servo angle.
*
* @param angle Desired angle in degrees
*/
public void setAngle(double angle) {
if (angle < 0.0) {
angle = 0.0;
}
if (angle > 180.0) {
angle = 180.0;
}
double pos = angle / 180.0;
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo angle.
*
* @return Current servo angle
*/
public double getAngle() {
if (m_simPosition != null) {
return m_simPosition.get() * 180.0;
}
return 90.0;
}
/**
* Set the servo position.
*
* @param pos Desired position (Between 0.0 and 1.0)
*/
public void setPosition(double pos) {
if (pos < 0.0) {
pos = 0.0;
}
if (pos > 1.0) {
pos = 1.0;
}
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo position.
*
* @return Current servo position
*/
public double getPosition() {
if (m_simPosition != null) {
return m_simPosition.get();
}
return 0.5;
}
}

69
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java Normal file
View File

@@ -0,0 +1,69 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpcommandbased.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.templates.xrpcommandbased.devices.XRPMotor;
import edu.wpi.first.wpilibj2.command.Subsystem;
public class XRPDrivetrain extends Subsystem {
private static final double kGearRatio =
(30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
private static final double kCountsPerMotorShaftRev = 12.0;
private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
private static final double kWheelDiameterInch = 2.3622; // 60 mm
// The XRP has the left and right motors set to
// channels 0 and 1 respectively
private final XRPMotor m_leftMotor = new XRPMotor(0);
private final XRPMotor m_rightMotor = new XRPMotor(1);
// The XRP has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
// Invert right side since motor is flipped
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}

104
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/EducationalRobot.java Normal file
View File

@@ -0,0 +1,104 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpeducational;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.internal.DriverStationModeThread;
/** Educational robot base class. */
public class EducationalRobot extends RobotBase {
public void robotInit() {}
public void disabled() {}
public void run() {}
public void autonomous() {
run();
}
public void teleop() {
run();
}
public void test() {
run();
}
private volatile boolean m_exit;
@Override
public void startCompetition() {
robotInit();
DriverStationModeThread modeThread = new DriverStationModeThread();
int event = WPIUtilJNI.createEvent(false, false);
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
DriverStationJNI.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
modeThread.inDisabled(true);
disabled();
modeThread.inDisabled(false);
while (isDisabled()) {
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isAutonomous()) {
modeThread.inAutonomous(true);
autonomous();
modeThread.inAutonomous(false);
while (isAutonomousEnabled()) {
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isTest()) {
modeThread.inTest(true);
test();
modeThread.inTest(false);
while (isTest() && isEnabled()) {
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else {
modeThread.inTeleop(true);
teleop();
modeThread.inTeleop(false);
while (isTeleopEnabled()) {
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
}
}
DriverStation.removeRefreshedDataEventHandle(event);
modeThread.close();
}
@Override
public void endCompetition() {
m_exit = true;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Main.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpeducational;
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);
}
}

23
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Robot.java Normal file
View File

@@ -0,0 +1,23 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpeducational;
/**
* The VM is configured to automatically run this class, and to call the run() function when the
* robot is enabled. 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 EducationalRobot {
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {}
/** This function is run when the robot is enabled. */
@Override
public void run() {}
}

58
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java Normal file
View File

@@ -0,0 +1,58 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpeducational;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.templates.xrpeducational.devices.XRPMotor;
public class XRPDrivetrain {
private static final double kGearRatio =
(30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
private static final double kCountsPerMotorShaftRev = 12.0;
private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
private static final double kWheelDiameterInch = 2.3622; // 60 mm
// The XRP has the left and right motors set to
// channels 0 and 1 respectively
private final XRPMotor m_leftMotor = new XRPMotor(0);
private final XRPMotor m_rightMotor = new XRPMotor(1);
// The XRP has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
// Invert right side since motor is flipped
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
}
}

109
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPMotor.java Normal file
View File

@@ -0,0 +1,109 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpeducational.devices;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPMotor.
*
* <p>A SimDevice based motor controller representing the motors on an XRP robot
*/
public class XRPMotor implements MotorController {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(0, "motorL");
s_simDeviceNameMap.put(1, "motorR");
s_simDeviceNameMap.put(2, "motor3");
s_simDeviceNameMap.put(3, "motor4");
}
private final SimDouble m_simSpeed;
private final SimBoolean m_simInverted;
/** XRPMotor. */
public XRPMotor(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
if (xrpMotorSimDevice != null) {
xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
} else {
m_simInverted = null;
m_simSpeed = null;
}
}
@Override
public void set(double speed) {
if (m_simSpeed != null) {
boolean invert = false;
if (m_simInverted != null) {
invert = m_simInverted.get();
}
m_simSpeed.set(invert ? -speed : speed);
}
}
@Override
public double get() {
if (m_simSpeed != null) {
return m_simSpeed.get();
}
return 0.0;
}
@Override
public void setInverted(boolean isInverted) {
if (m_simInverted != null) {
m_simInverted.set(isInverted);
}
}
@Override
public boolean getInverted() {
if (m_simInverted != null) {
return m_simInverted.get();
}
return false;
}
@Override
public void disable() {
set(0.0);
}
@Override
public void stopMotor() {
set(0.0);
}
}

123
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPServo.java Normal file
View File

@@ -0,0 +1,123 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrpeducational.devices;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPServo.
*
* <p>A SimDevice based servo
*/
public class XRPServo {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(4, "servo1");
s_simDeviceNameMap.put(5, "servo2");
}
private final SimDouble m_simPosition;
/** XRPServo. */
public XRPServo(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on WS as type: "XRPServo", device: <servo name>
String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
if (xrpServoSimDevice != null) {
xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
// This should mimic PWM position [0.0, 1.0]
m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
} else {
m_simPosition = null;
}
}
/**
* Set the servo angle.
*
* @param angle Desired angle in degrees
*/
public void setAngle(double angle) {
if (angle < 0.0) {
angle = 0.0;
}
if (angle > 180.0) {
angle = 180.0;
}
double pos = angle / 180.0;
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo angle.
*
* @return Current servo angle
*/
public double getAngle() {
if (m_simPosition != null) {
return m_simPosition.get() * 180.0;
}
return 90.0;
}
/**
* Set the servo position.
*
* @param pos Desired position (Between 0.0 and 1.0)
*/
public void setPosition(double pos) {
if (pos < 0.0) {
pos = 0.0;
}
if (pos > 1.0) {
pos = 1.0;
}
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo position.
*
* @return Current servo position
*/
public double getPosition() {
if (m_simPosition != null) {
return m_simPosition.get();
}
return 0.5;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Main.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrptimed;
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);
}
}

102
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Robot.java Normal file
View File

@@ -0,0 +1,102 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrptimed;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* 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 static final String kDefaultAuto = "Default";
private static final String kCustomAuto = "My Auto";
private String m_autoSelected;
private final SendableChooser<String> m_chooser = new SendableChooser<>();
private final XRPDrivetrain m_drivetrain = new XRPDrivetrain();
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
m_chooser.addOption("My Auto", kCustomAuto);
SmartDashboard.putData("Auto choices", m_chooser);
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {}
/**
* 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 line to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional comparisons to the switch structure
* below with additional strings. If using the SendableChooser make sure to add them to the
* chooser code above as well.
*/
@Override
public void autonomousInit() {
m_autoSelected = m_chooser.getSelected();
// m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
System.out.println("Auto selected: " + m_autoSelected);
m_drivetrain.resetEncoders();
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {
switch (m_autoSelected) {
case kCustomAuto:
// Put custom auto code here
break;
case kDefaultAuto:
default:
// Put default auto code here
break;
}
}
/** This function is called once when teleop is enabled. */
@Override
public void teleopInit() {}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
/** This function is called once when the robot is disabled. */
@Override
public void disabledInit() {}
/** This function is called periodically when disabled. */
@Override
public void disabledPeriodic() {}
/** This function is called once when test mode is enabled. */
@Override
public void testInit() {}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

58
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java Normal file
View File

@@ -0,0 +1,58 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrptimed;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.templates.xrptimed.devices.XRPMotor;
public class XRPDrivetrain {
private static final double kGearRatio =
(30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
private static final double kCountsPerMotorShaftRev = 12.0;
private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
private static final double kWheelDiameterInch = 2.3622; // 60 mm
// The XRP has the left and right motors set to
// channels 0 and 1 respectively
private final XRPMotor m_leftMotor = new XRPMotor(0);
private final XRPMotor m_rightMotor = new XRPMotor(1);
// The XRP has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
// Invert right side since motor is flipped
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
}
}

109
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPMotor.java Normal file
View File

@@ -0,0 +1,109 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrptimed.devices;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPMotor.
*
* <p>A SimDevice based motor controller representing the motors on an XRP robot
*/
public class XRPMotor implements MotorController {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(0, "motorL");
s_simDeviceNameMap.put(1, "motorR");
s_simDeviceNameMap.put(2, "motor3");
s_simDeviceNameMap.put(3, "motor4");
}
private final SimDouble m_simSpeed;
private final SimBoolean m_simInverted;
/** XRPMotor. */
public XRPMotor(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
if (xrpMotorSimDevice != null) {
xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
} else {
m_simInverted = null;
m_simSpeed = null;
}
}
@Override
public void set(double speed) {
if (m_simSpeed != null) {
boolean invert = false;
if (m_simInverted != null) {
invert = m_simInverted.get();
}
m_simSpeed.set(invert ? -speed : speed);
}
}
@Override
public double get() {
if (m_simSpeed != null) {
return m_simSpeed.get();
}
return 0.0;
}
@Override
public void setInverted(boolean isInverted) {
if (m_simInverted != null) {
m_simInverted.set(isInverted);
}
}
@Override
public boolean getInverted() {
if (m_simInverted != null) {
return m_simInverted.get();
}
return false;
}
@Override
public void disable() {
set(0.0);
}
@Override
public void stopMotor() {
set(0.0);
}
}

123
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPServo.java Normal file
View File

@@ -0,0 +1,123 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.templates.xrptimed.devices;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import java.util.HashMap;
import java.util.HashSet;
/**
* XRPServo.
*
* <p>A SimDevice based servo
*/
public class XRPServo {
private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
private static HashSet<Integer> s_registeredDevices = new HashSet<>();
private static void checkDeviceAllocation(int deviceNum) {
if (!s_simDeviceNameMap.containsKey(deviceNum)) {
throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
}
if (s_registeredDevices.contains(deviceNum)) {
throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
}
s_registeredDevices.add(deviceNum);
}
static {
s_simDeviceNameMap.put(4, "servo1");
s_simDeviceNameMap.put(5, "servo2");
}
private final SimDouble m_simPosition;
/** XRPServo. */
public XRPServo(int deviceNum) {
checkDeviceAllocation(deviceNum);
// We want this to appear on WS as type: "XRPServo", device: <servo name>
String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
if (xrpServoSimDevice != null) {
xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
// This should mimic PWM position [0.0, 1.0]
m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
} else {
m_simPosition = null;
}
}
/**
* Set the servo angle.
*
* @param angle Desired angle in degrees
*/
public void setAngle(double angle) {
if (angle < 0.0) {
angle = 0.0;
}
if (angle > 180.0) {
angle = 180.0;
}
double pos = angle / 180.0;
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo angle.
*
* @return Current servo angle
*/
public double getAngle() {
if (m_simPosition != null) {
return m_simPosition.get() * 180.0;
}
return 90.0;
}
/**
* Set the servo position.
*
* @param pos Desired position (Between 0.0 and 1.0)
*/
public void setPosition(double pos) {
if (pos < 0.0) {
pos = 0.0;
}
if (pos > 1.0) {
pos = 1.0;
}
if (m_simPosition != null) {
m_simPosition.set(pos);
}
}
/**
* Get the servo position.
*
* @return Current servo position
*/
public double getPosition() {
if (m_simPosition != null) {
return m_simPosition.get();
}
return 0.5;
}
}