diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index 23f93cd697..4635d05057 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -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 diff --git a/styleguide/spotbugs-exclude.xml b/styleguide/spotbugs-exclude.xml index c5e2d20898..0e8e41ffc6 100644 --- a/styleguide/spotbugs-exclude.xml +++ b/styleguide/spotbugs-exclude.xml @@ -108,6 +108,10 @@ + + + + diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index d74ef838c7..139b6130d7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -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.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Constants.java new file mode 100644 index 0000000000..026efdd4c1 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Constants.java @@ -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. + * + *

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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Main.java new file mode 100644 index 0000000000..125996d191 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Main.java @@ -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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Robot.java new file mode 100644 index 0000000000..daf5d32d19 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/Robot.java @@ -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. + * + *

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() {} +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/RobotContainer.java new file mode 100644 index 0000000000..593e5a77dc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/RobotContainer.java @@ -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 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)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/ArcadeDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/ArcadeDrive.java new file mode 100644 index 0000000000..0269817aef --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/ArcadeDrive.java @@ -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 m_xaxisSpeedSupplier; + private final Supplier 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 xaxisSpeedSupplier, + Supplier 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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousDistance.java new file mode 100644 index 0000000000..8b8af83e7b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousDistance.java @@ -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)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousTime.java new file mode 100644 index 0000000000..4fb20d9607 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/AutonomousTime.java @@ -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)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/DriveDistance.java new file mode 100644 index 0000000000..b7d9640b4a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/DriveDistance.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/DriveTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/DriveTime.java new file mode 100644 index 0000000000..5608c0f8f3 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/DriveTime.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnDegrees.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnDegrees.java new file mode 100644 index 0000000000..dfee189260 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnDegrees.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnTime.java new file mode 100644 index 0000000000..030981ec3c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/commands/TurnTime.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPMotor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPMotor.java new file mode 100644 index 0000000000..be84e6b5ea --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPMotor.java @@ -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. + * + *

A SimDevice based motor controller representing the motors on an XRP robot + */ +public class XRPMotor implements MotorController { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPServo.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPServo.java new file mode 100644 index 0000000000..064e20911a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/devices/XRPServo.java @@ -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. + * + *

A SimDevice based servo + */ +public class XRPServo { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/sensors/XRPGyro.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/sensors/XRPGyro.java new file mode 100644 index 0000000000..101006f57b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/sensors/XRPGyro.java @@ -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(); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Arm.java new file mode 100644 index 0000000000..e3082ba573 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Arm.java @@ -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); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java new file mode 100644 index 0000000000..a5082c016d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/Drivetrain.java @@ -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 + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/XRPOnBoardIO.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/XRPOnBoardIO.java new file mode 100644 index 0000000000..f647728009 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrpreference/subsystems/XRPOnBoardIO.java @@ -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 + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json index 41db61a8a8..8f797d5690 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json @@ -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 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Constants.java new file mode 100644 index 0000000000..cd72c67ef5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Constants.java @@ -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. + * + *

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 {} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Main.java new file mode 100644 index 0000000000..babb187530 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Main.java @@ -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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Robot.java new file mode 100644 index 0000000000..f7950f559e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Robot.java @@ -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. + * + *

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() {} +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/RobotContainer.java new file mode 100644 index 0000000000..e6059ca473 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/RobotContainer.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/commands/ExampleCommand.java new file mode 100644 index 0000000000..27f759a8c7 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/commands/ExampleCommand.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPMotor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPMotor.java new file mode 100644 index 0000000000..a5c71465cd --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPMotor.java @@ -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. + * + *

A SimDevice based motor controller representing the motors on an XRP robot + */ +public class XRPMotor implements MotorController { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPServo.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPServo.java new file mode 100644 index 0000000000..b88fa50c5b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPServo.java @@ -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. + * + *

A SimDevice based servo + */ +public class XRPServo { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java new file mode 100644 index 0000000000..ed8b0f6c0f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java @@ -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 + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/EducationalRobot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/EducationalRobot.java new file mode 100644 index 0000000000..4b44707d7d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/EducationalRobot.java @@ -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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Main.java new file mode 100644 index 0000000000..dbb4d81e24 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Main.java @@ -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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Robot.java new file mode 100644 index 0000000000..66c2806952 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Robot.java @@ -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() {} +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java new file mode 100644 index 0000000000..54fff39523 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.java @@ -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(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPMotor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPMotor.java new file mode 100644 index 0000000000..c11f08153c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPMotor.java @@ -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. + * + *

A SimDevice based motor controller representing the motors on an XRP robot + */ +public class XRPMotor implements MotorController { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPServo.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPServo.java new file mode 100644 index 0000000000..9f727797b0 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPServo.java @@ -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. + * + *

A SimDevice based servo + */ +public class XRPServo { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Main.java new file mode 100644 index 0000000000..6405cdc843 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Main.java @@ -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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Robot.java new file mode 100644 index 0000000000..f3b6d57d43 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Robot.java @@ -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 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. + * + *

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 + * + *

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() {} +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java new file mode 100644 index 0000000000..3d8721b8fc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.java @@ -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(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPMotor.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPMotor.java new file mode 100644 index 0000000000..8dab7022b9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPMotor.java @@ -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. + * + *

A SimDevice based motor controller representing the motors on an XRP robot + */ +public class XRPMotor implements MotorController { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPServo.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPServo.java new file mode 100644 index 0000000000..b3c4e637ef --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPServo.java @@ -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. + * + *

A SimDevice based servo + */ +public class XRPServo { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet 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: + 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; + } +}