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

[documentation] Remove more outdated commands examples (#7054)

Browse Source 
There are still some examples we'd like to remove here (eg Hatchbot
traditional) but this is a good start with not too many changes required
in frc-docs.
This commit is contained in:
Jade
2024-10-11 14:09:11 +08:00
committed by GitHub
parent 37e7bfe4f9
commit 8f57e4c566
120 changed files with 0 additions and 6615 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
View File

@@ -1,65 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbot;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ArmConstants {
public static final int kMotorPort = 4;
public static final double kP = 1;
// These are fake gains; in actuality these must be determined individually for each robot
public static final double kSVolts = 1;
public static final double kGVolts = 1;
public static final double kVVoltSecondPerRad = 0.5;
public static final double kAVoltSecondSquaredPerRad = 0.1;
public static final double kMaxVelocityRadPerSecond = 3;
public static final double kMaxAccelerationRadPerSecSquared = 10;
public static final int[] kEncoderPorts = new int[] {4, 5};
public static final int kEncoderPPR = 256;
public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
// The offset of the arm from the horizontal in its neutral position,
// measured from the horizontal
public static final double kArmOffsetRads = 0.5;
}
public static final class AutoConstants {
public static final double kAutoTimeoutSeconds = 12;
public static final double kAutoShootTimeSeconds = 7;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

102
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
View File

@@ -1,102 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically 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 Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {
m_robotContainer.disablePIDSubsystems();
}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

103
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
View File

@@ -1,103 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final ArmSubsystem m_robotArm = new ArmSubsystem();
// The driver's controller
CommandXboxController m_driverController =
new CommandXboxController(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
Commands.run(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
}
/**
* 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
* JoystickButton}.
*/
private void configureButtonBindings() {
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
m_driverController
.a()
.onTrue(
Commands.runOnce(
() -> {
m_robotArm.setGoal(2);
m_robotArm.enable();
},
m_robotArm));
// Move the arm to neutral position when the 'B' button is pressed.
m_driverController
.b()
.onTrue(
Commands.runOnce(
() -> {
m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
m_robotArm.enable();
},
m_robotArm));
// Disable the arm controller when Y is pressed.
m_driverController.y().onTrue(Commands.runOnce(m_robotArm::disable));
// Drive at half speed when the bumper is held
m_driverController
.rightBumper()
.onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))
.onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1.0)));
}
/**
* Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This should be called on robot
* disable to prevent integral windup.
*/
public void disablePIDSubsystems() {
m_robotArm.disable();
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return Commands.none();
}
}

60
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
View File

@@ -1,60 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbot.subsystems;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends ProfiledPIDSubsystem {
private final PWMSparkMax m_motor = new PWMSparkMax(ArmConstants.kMotorPort);
private final Encoder m_encoder =
new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(
ArmConstants.kSVolts, ArmConstants.kGVolts,
ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/** Create a new ArmSubsystem. */
public ArmSubsystem() {
super(
new ProfiledPIDController(
ArmConstants.kP,
0,
0,
new TrapezoidProfile.Constraints(
ArmConstants.kMaxVelocityRadPerSecond,
ArmConstants.kMaxAccelerationRadPerSecSquared)),
0);
m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
// Start arm at rest in neutral position
setGoal(ArmConstants.kArmOffsetRads);
}
@Override
public void useOutput(double output, TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double feedforward =
m_feedforward
.calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
.in(Volts);
// Add the feedforward to the PID output to get the motor output
m_motor.setVoltage(output + feedforward);
}
@Override
public double getMeasurement() {
return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
}
}

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

@@ -1,110 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbot.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

56
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
View File

@@ -1,56 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ArmConstants {
public static final int kMotorPort = 4;
public static final double kP = 1;
// These are fake gains; in actuality these must be determined individually for each robot
public static final double kSVolts = 1;
public static final double kGVolts = 1;
public static final double kVVoltSecondPerRad = 0.5;
public static final double kAVoltSecondSquaredPerRad = 0.1;
public static final double kMaxVelocityRadPerSecond = 3;
public static final double kMaxAccelerationRadPerSecSquared = 10;
// The offset of the arm from the horizontal in its neutral position,
// measured from the horizontal
public static final double kArmOffsetRads = 0.5;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
}

88
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
View File

@@ -1,88 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
public class ExampleSmartMotorController {
public enum PIDMode {
kPosition,
kVelocity,
kMovementWitchcraft
}
/**
* Creates a new ExampleSmartMotorController.
*
* @param port The port for the controller.
*/
@SuppressWarnings("PMD.UnusedFormalParameter")
public ExampleSmartMotorController(int port) {}
/**
* Example method for setting the PID gains of the smart controller.
*
* @param kp The proportional gain.
* @param ki The integral gain.
* @param kd The derivative gain.
*/
public void setPID(double kp, double ki, double kd) {}
/**
* Example method for setting the setpoint of the smart controller in PID mode.
*
* @param mode The mode of the PID controller.
* @param setpoint The controller setpoint.
* @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
*/
public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
/**
* Places this motor controller in follower mode.
*
* @param leader The leader to follow.
*/
public void follow(ExampleSmartMotorController leader) {}
/**
* Returns the encoder distance.
*
* @return The current encoder distance.
*/
public double getEncoderDistance() {
return 0;
}
/**
* Returns the encoder rate.
*
* @return The current encoder rate.
*/
public double getEncoderRate() {
return 0;
}
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
public void set(double speed) {}
public double get() {
return 0;
}
public void setInverted(boolean isInverted) {}
public boolean getInverted() {
return false;
}
public void disable() {}
public void stopMotor() {}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
View File

@@ -1,100 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically 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 Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

75
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
View File

@@ -1,75 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final ArmSubsystem m_robotArm = new ArmSubsystem();
// The driver's controller
CommandXboxController m_driverController =
new CommandXboxController(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
m_robotDrive.arcadeDriveCommand(
() -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
}
/**
* 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
* JoystickButton}.
*/
private void configureButtonBindings() {
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2));
// Move the arm to neutral position when the 'B' button is pressed.
m_driverController
.b()
.onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads));
// Drive at half speed when the bumper is held
m_driverController
.rightBumper()
.onTrue(m_robotDrive.limitOutputCommand(0.5))
.onFalse(m_robotDrive.limitOutputCommand(1));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return Commands.none();
}
}

52
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
View File

@@ -1,52 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends TrapezoidProfileSubsystem {
private final ExampleSmartMotorController m_motor =
new ExampleSmartMotorController(ArmConstants.kMotorPort);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(
ArmConstants.kSVolts, ArmConstants.kGVolts,
ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/** Create a new ArmSubsystem. */
public ArmSubsystem() {
super(
new TrapezoidProfile.Constraints(
ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
ArmConstants.kArmOffsetRads);
m_motor.setPID(ArmConstants.kP, 0, 0);
}
@Override
public void useState(TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double feedforward =
m_feedforward
.calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
.in(Volts);
// Add the feedforward to the PID output to get the motor output
m_motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
}
public Command setArmGoalCommand(double kArmOffsetRads) {
return Commands.runOnce(() -> setGoal(kArmOffsetRads), this);
}
}

114
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
View File

@@ -1,114 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.function.DoubleSupplier;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
}
/**
* A split-stick arcade command, with forward/backward controlled by the left hand, and turning
* controlled by the right.
*
* @param fwd supplier for the commanded forward movement
* @param rot supplier for the commanded rotation
*/
public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
return Commands.run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()), this);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public Command limitOutputCommand(double maxOutput) {
return Commands.runOnce(() -> m_drive.setMaxOutput(maxOutput));
}
}

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

@@ -276,25 +276,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "GearsBot",
"description": "A fully functional Command-Based program for WPI's GearsBot robot.",
"tags": [
"Complete Robot",
"Command-based",
"Differential Drive",
"Elevator",
"Arm",
"Analog",
"Digital Input",
"SmartDashboard",
"XboxController"
],
"foldername": "gearsbot",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Simple Vision",
"description": "Use the CameraServer class to stream from a USB Webcam without processing the images.",
@@ -425,40 +406,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Frisbeebot",
"description": "A simple frisbee shooter for the 2013 game, demonstrating use of PIDSubsystem.",
"tags": [
"Complete Robot",
"Command-based",
"Differential Drive",
"Flywheel",
"Encoder",
"XboxController",
"PID"
],
"foldername": "frisbeebot",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Gyro Drive Commands",
"description": "Control a robot's angle with PID and a gyro, in command-based.",
"tags": [
"Command-based",
"Differential Drive",
"Encoder",
"PS4Controller",
"PID",
"Profiled PID",
"Gyro"
],
"foldername": "gyrodrivecommands",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "SwerveBot",
"description": "Use kinematics and odometry with a swerve drive.",
@@ -583,38 +530,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "ArmBot",
"description": "Control an arm with ProfiledPIDSubsystem.",
"tags": [
"Command-based",
"Arm",
"Encoder",
"Profiled PID",
"XboxController",
"Differential Drive"
],
"foldername": "armbot",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "ArmBotOffboard",
"description": "Control an arm with TrapezoidProfileSubsystem and smart motor controller PID.",
"tags": [
"Command-based",
"Arm",
"Smart Motor Controller",
"Trapezoid Profile",
"XboxController",
"Differential Drive"
],
"foldername": "armbotoffboard",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "DriveDistanceOffboard",
"description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.",

72
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
View File

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

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
View File

@@ -1,100 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically 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 Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

124
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
View File

@@ -1,124 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final ShooterSubsystem m_shooter = new ShooterSubsystem();
private final Command m_spinUpShooter = Commands.runOnce(m_shooter::enable, m_shooter);
private final Command m_stopShooter = Commands.runOnce(m_shooter::disable, m_shooter);
// An autonomous routine that shoots the loaded frisbees
Command m_autonomousCommand =
Commands.sequence(
// Start the command by spinning up the shooter...
Commands.runOnce(m_shooter::enable, m_shooter),
// Wait until the shooter is at speed before feeding the frisbees
Commands.waitUntil(m_shooter::atSetpoint),
// Start running the feeder
Commands.runOnce(m_shooter::runFeeder, m_shooter),
// Shoot for the specified time
Commands.waitSeconds(AutoConstants.kAutoShootTimeSeconds))
// Add a timeout (will end the command if, for instance, the shooter
// never gets up to speed)
.withTimeout(AutoConstants.kAutoTimeoutSeconds)
// When the command ends, turn off the shooter and the feeder
.andThen(
Commands.runOnce(
() -> {
m_shooter.disable();
m_shooter.stopFeeder();
}));
// The driver's controller
CommandXboxController m_driverController =
new CommandXboxController(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
Commands.run(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created via the button
* factories on {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID} or one of its
* subclasses ({@link edu.wpi.first.wpilibj2.command.button.CommandJoystick} or {@link
* CommandXboxController}).
*/
private void configureButtonBindings() {
// Configure your button bindings here
// We can bind commands while retaining references to them in RobotContainer
// Spin up the shooter when the 'A' button is pressed
m_driverController.a().onTrue(m_spinUpShooter);
// Turn off the shooter when the 'B' button is pressed
m_driverController.b().onTrue(m_stopShooter);
// We can also write them as temporary variables outside the bindings
// Shoots if the shooter wheel has reached the target speed
Command shoot =
Commands.either(
// Run the feeder
Commands.runOnce(m_shooter::runFeeder, m_shooter),
// Do nothing
Commands.none(),
// Determine which of the above to do based on whether the shooter has reached the
// desired speed
m_shooter::atSetpoint);
Command stopFeeder = Commands.runOnce(m_shooter::stopFeeder, m_shooter);
// Shoot when the 'X' button is pressed
m_driverController.x().onTrue(shoot).onFalse(stopFeeder);
// We can also define commands inline at the binding!
// While holding the shoulder button, drive at half speed
m_driverController
.rightBumper()
.onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))
.onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1)));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_autonomousCommand;
}
}

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

@@ -1,110 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

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

@@ -1,62 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
public class ShooterSubsystem extends PIDSubsystem {
private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
private final Encoder m_shooterEncoder =
new Encoder(
ShooterConstants.kEncoderPorts[0],
ShooterConstants.kEncoderPorts[1],
ShooterConstants.kEncoderReversed);
private final SimpleMotorFeedforward m_shooterFeedforward =
new SimpleMotorFeedforward(
ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
/** The shooter subsystem for the robot. */
public ShooterSubsystem() {
super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
setSetpoint(ShooterConstants.kShooterTargetRPS);
}
@Override
public void useOutput(double output, double setpoint) {
m_shooterMotor.setVoltage(
output
+ m_shooterFeedforward
.calculate(RadiansPerSecond.of(ShooterConstants.kShooterTargetRPS))
.in(Volts));
}
@Override
public double getMeasurement() {
return m_shooterEncoder.getRate();
}
public boolean atSetpoint() {
return m_controller.atSetpoint();
}
public void runFeeder() {
m_feederMotor.set(ShooterConstants.kFeederSpeed);
}
public void stopFeeder() {
m_feederMotor.set(0);
}
}

94
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java
View File

@@ -1,94 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot;
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotorPort1 = 0;
public static final int kLeftMotorPort2 = 1;
public static final int kRightMotorPort1 = 2;
public static final int kRightMotorPort2 = 3;
public static final int[] kLeftEncoderPorts = {0, 1};
public static final int[] kRightEncoderPorts = {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = false;
public static final int kRangeFinderPort = 6;
public static final int kAnalogGyroPort = 1;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ClawConstants {
public static final int kMotorPort = 7;
public static final int kContactPort = 5;
}
public static final class WristConstants {
public static final int kMotorPort = 6;
// these pid constants are not real, and will need to be tuned
public static final double kP = 0.1;
public static final double kI = 0.0;
public static final double kD = 0.0;
public static final double kTolerance = 2.5;
public static final int kPotentiometerPort = 3;
}
public static final class ElevatorConstants {
public static final int kMotorPort = 5;
public static final int kPotentiometerPort = 2;
// these pid constants are not real, and will need to be tuned
public static final double kP_real = 4;
public static final double kI_real = 0.007;
public static final double kP_simulation = 18;
public static final double kI_simulation = 0.2;
public static final double kD = 0.0;
public static final double kTolerance = 0.005;
}
public static final class AutoConstants {
public static final double kDistToBox1 = 0.10;
public static final double kDistToBox2 = 0.60;
public static final double kWristSetpoint = -45.0;
}
public static final class DriveStraightConstants {
// these pid constants are not real, and will need to be tuned
public static final double kP = 4.0;
public static final double kI = 0.0;
public static final double kD = 0.0;
}
public static final class Positions {
public static final class Pickup {
public static final double kWristSetpoint = -45.0;
public static final double kElevatorSetpoint = 0.25;
}
public static final class Place {
public static final double kWristSetpoint = 0.0;
public static final double kElevatorSetpoint = 0.25;
}
public static final class PrepareToPickup {
public static final double kWristSetpoint = 0.0;
public static final double kElevatorSetpoint = 0.0;
}
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

93
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
View File

@@ -1,93 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically 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 Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

109
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
View File

@@ -1,109 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDrive;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final Drivetrain m_drivetrain = new Drivetrain();
private final Elevator m_elevator = new Elevator();
private final Wrist m_wrist = new Wrist();
private final Claw m_claw = new Claw();
private final XboxController m_joystick = new XboxController(0);
private final Command m_autonomousCommand =
new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.25, m_elevator));
SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
SmartDashboard.putData(
"Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
// Assign default commands
m_drivetrain.setDefaultCommand(
new TankDrive(() -> -m_joystick.getLeftY(), () -> -m_joystick.getRightY(), m_drivetrain));
// Show what command your subsystem is running on the SmartDashboard
SmartDashboard.putData(m_drivetrain);
SmartDashboard.putData(m_elevator);
SmartDashboard.putData(m_wrist);
SmartDashboard.putData(m_claw);
// 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() {
// Create some buttons
final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
final JoystickButton l2 = new JoystickButton(m_joystick, 9);
final JoystickButton r2 = new JoystickButton(m_joystick, 10);
final JoystickButton l1 = new JoystickButton(m_joystick, 11);
final JoystickButton r1 = new JoystickButton(m_joystick, 12);
// Connect the buttons to commands
dpadUp.onTrue(new SetElevatorSetpoint(0.25, m_elevator));
dpadDown.onTrue(new SetElevatorSetpoint(0.0, m_elevator));
dpadRight.onTrue(new CloseClaw(m_claw));
dpadLeft.onTrue(new OpenClaw(m_claw));
r1.onTrue(new PrepareToPickup(m_claw, m_wrist, m_elevator));
r2.onTrue(new Pickup(m_claw, m_wrist, m_elevator));
l1.onTrue(new Place(m_claw, m_wrist, m_elevator));
l2.onTrue(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_autonomousCommand;
}
}

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
View File

@@ -1,30 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
/** The main autonomous command to pickup and deliver the soda to the box. */
public class Autonomous extends SequentialCommandGroup {
/** Create a new autonomous command. */
public Autonomous(Drivetrain drive, Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new PrepareToPickup(claw, wrist, elevator),
new Pickup(claw, wrist, elevator),
new SetDistanceToBox(AutoConstants.kDistToBox1, drive),
// new DriveStraight(4), // Use encoders if ultrasonic is broken
new Place(claw, wrist, elevator),
new SetDistanceToBox(AutoConstants.kDistToBox2, drive),
// new DriveStraight(-2), // Use Encoders if ultrasonic is broken
Commands.parallel(
new SetWristSetpoint(AutoConstants.kWristSetpoint, wrist), new CloseClaw(claw)));
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
View File

@@ -1,43 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj2.command.Command;
/** Closes the claw until the limit switch is tripped. */
public class CloseClaw extends Command {
private final Claw m_claw;
public CloseClaw(Claw claw) {
m_claw = claw;
addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_claw.close();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_claw.isGrabbing();
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
// NOTE: Doesn't stop in simulation due to lower friction causing the
// can to fall out
// + there is no need to worry about stalling the motor or crushing the
// can.
if (!Robot.isSimulation()) {
m_claw.stop();
}
}
}

52
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
View File

@@ -1,52 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveStraightConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.PIDCommand;
/**
* Drive the given distance straight (negative values go backwards). Uses a local PID controller to
* run a simple PID loop that is only enabled while this command is running. The input is the
* averaged values of the left and right encoders.
*/
public class DriveStraight extends PIDCommand {
private final Drivetrain m_drivetrain;
/**
* Create a new DriveStraight command.
*
* @param distance The distance to drive
*/
public DriveStraight(double distance, Drivetrain drivetrain) {
super(
new PIDController(
DriveStraightConstants.kP, DriveStraightConstants.kI, DriveStraightConstants.kD),
drivetrain::getDistance,
distance,
d -> drivetrain.drive(d, d));
m_drivetrain = drivetrain;
addRequirements(m_drivetrain);
getController().setTolerance(0.01);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
// Get everything in a safe starting state.
m_drivetrain.reset();
super.initialize();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return getController().atSetpoint();
}
}

37
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
View File

@@ -1,37 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj2.command.WaitCommand;
/** Opens the claw for one second. Real robots should use sensors, stalling motors is BAD! */
public class OpenClaw extends WaitCommand {
private final Claw m_claw;
/**
* Creates a new OpenClaw command.
*
* @param claw The claw to use
*/
public OpenClaw(Claw claw) {
super(1);
m_claw = claw;
addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_claw.open();
super.initialize();
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
m_claw.stop();
}
}

32
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
/**
* Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around.
*/
public class Pickup extends SequentialCommandGroup {
/**
* Create a new pickup command.
*
* @param claw The claw subsystem to use
* @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public Pickup(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new CloseClaw(claw),
Commands.parallel(
new SetWristSetpoint(Positions.Pickup.kWristSetpoint, wrist),
new SetElevatorSetpoint(Positions.Pickup.kElevatorSetpoint, elevator)));
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
View File

@@ -1,28 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
/** Place a held soda can onto the platform. */
public class Place extends SequentialCommandGroup {
/**
* Create a new place command.
*
* @param claw The claw subsystem to use
* @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public Place(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new SetElevatorSetpoint(Positions.Place.kElevatorSetpoint, elevator),
new SetWristSetpoint(Positions.Place.kWristSetpoint, wrist),
new OpenClaw(claw));
}
}

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
View File

@@ -1,30 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
/** Make sure the robot is in a state to pickup soda cans. */
public class PrepareToPickup extends SequentialCommandGroup {
/**
* Create a new prepare to pickup command.
*
* @param claw The claw subsystem to use
* @param wrist The wrist subsystem to use
* @param elevator The elevator subsystem to use
*/
public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new OpenClaw(claw),
Commands.parallel(
new SetWristSetpoint(Positions.PrepareToPickup.kWristSetpoint, wrist),
new SetElevatorSetpoint(Positions.PrepareToPickup.kElevatorSetpoint, elevator)));
}
}

50
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
View File

@@ -1,50 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.PIDCommand;
/**
* Drive until the robot is the given distance away from the box. Uses a local PID controller to run
* a simple PID loop that is only enabled while this command is running. The input is the averaged
* values of the left and right encoders.
*/
public class SetDistanceToBox extends PIDCommand {
private final Drivetrain m_drivetrain;
/**
* Create a new set distance to box command.
*
* @param distance The distance away from the box to drive to
*/
public SetDistanceToBox(double distance, Drivetrain drivetrain) {
super(
new PIDController(-2, 0, 0),
drivetrain::getDistanceToObstacle,
distance,
d -> drivetrain.drive(d, d));
m_drivetrain = drivetrain;
addRequirements(m_drivetrain);
getController().setTolerance(0.01);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
// Get everything in a safe starting state.
m_drivetrain.reset();
super.initialize();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return getController().atSetpoint();
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
View File

@@ -1,43 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj2.command.Command;
/**
* Move the elevator to a given location. This command finishes when it is within the tolerance, but
* leaves the PID loop running to maintain the position. Other commands using the elevator should
* make sure they disable PID!
*/
public class SetElevatorSetpoint extends Command {
private final Elevator m_elevator;
private final double m_setpoint;
/**
* Create a new SetElevatorSetpoint command.
*
* @param setpoint The setpoint to set the elevator to
* @param elevator The elevator to use
*/
public SetElevatorSetpoint(double setpoint, Elevator elevator) {
m_elevator = elevator;
m_setpoint = setpoint;
addRequirements(m_elevator);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_elevator.setSetpoint(m_setpoint);
m_elevator.enable();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_elevator.getController().atSetpoint();
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
View File

@@ -1,43 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj2.command.Command;
/**
* Move the wrist to a given angle. This command finishes when it is within the tolerance, but
* leaves the PID loop running to maintain the position. Other commands using the wrist should make
* sure they disable PID!
*/
public class SetWristSetpoint extends Command {
private final Wrist m_wrist;
private final double m_setpoint;
/**
* Create a new SetWristSetpoint command.
*
* @param setpoint The setpoint to set the wrist to
* @param wrist The wrist to use
*/
public SetWristSetpoint(double setpoint, Wrist wrist) {
m_wrist = wrist;
m_setpoint = setpoint;
addRequirements(m_wrist);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_wrist.enable();
m_wrist.setSetpoint(m_setpoint);
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_wrist.getController().atSetpoint();
}
}

48
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
View File

@@ -1,48 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
import java.util.function.DoubleSupplier;
/** Have the robot drive tank style. */
public class TankDrive extends Command {
private final Drivetrain m_drivetrain;
private final DoubleSupplier m_left;
private final DoubleSupplier m_right;
/**
* Creates a new TankDrive command.
*
* @param left The control input for the left side of the drive
* @param right The control input for the right sight of the drive
* @param drivetrain The drivetrain subsystem to drive
*/
public TankDrive(DoubleSupplier left, DoubleSupplier right, Drivetrain drivetrain) {
m_drivetrain = drivetrain;
m_left = left;
m_right = right;
addRequirements(m_drivetrain);
}
// Called repeatedly when this Command is scheduled to run
@Override
public void execute() {
m_drivetrain.drive(m_left.getAsDouble(), m_right.getAsDouble());
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return false; // Runs until interrupted
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
m_drivetrain.drive(0, 0);
}
}

57
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
View File

@@ -1,57 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.ClawConstants;
import edu.wpi.first.wpilibj.motorcontrol.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
/**
* The claw subsystem is a simple system with a motor for opening and closing. If using stronger
* motors, you should probably use a sensor so that the motors don't stall.
*/
public class Claw extends SubsystemBase {
private final Victor m_motor = new Victor(ClawConstants.kMotorPort);
private final DigitalInput m_contact = new DigitalInput(ClawConstants.kContactPort);
/** Create a new claw subsystem. */
public Claw() {
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Limit Switch", m_contact);
}
public void log() {
SmartDashboard.putData("Claw switch", m_contact);
}
/** Set the claw motor to move in the open direction. */
public void open() {
m_motor.set(-1);
}
/** Set the claw motor to move in the close direction. */
public void close() {
m_motor.set(1);
}
/** Stops the claw motor from moving. */
public void stop() {
m_motor.set(0);
}
/** Return true when the robot is grabbing an object hard enough to trigger the limit switch. */
public boolean isGrabbing() {
return m_contact.get();
}
/** Call log method every loop. */
@Override
public void periodic() {
log();
}
}

138
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
View File

@@ -1,138 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Drivetrain extends SubsystemBase {
// The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
// These include four drive motors, a left and right encoder and a gyro.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotorPort1);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotorPort2);
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotorPort1);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotorPort2);
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
private final AnalogInput m_rangefinder = new AnalogInput(DriveConstants.kRangeFinderPort);
private final AnalogGyro m_gyro = new AnalogGyro(DriveConstants.kAnalogGyroPort);
/** Create a new drivetrain subsystem. */
public Drivetrain() {
super();
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Encoders may measure differently in the real world and in
// simulation. In this example the robot moves 0.042 barleycorns
// per tick in the real world, but the simulated encoders
// simulate 360 tick encoders. This if statement allows for the
// real robot to handle this difference in devices.
if (Robot.isReal()) {
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
} else {
// Circumference = diameter in feet * pi. 360 tick simulated encoders.
m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
}
// Let's name the sensors on the LiveWindow
addChild("Drive", m_drive);
addChild("Left Encoder", m_leftEncoder);
addChild("Right Encoder", m_rightEncoder);
addChild("Rangefinder", m_rangefinder);
addChild("Gyro", m_gyro);
}
/** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
}
/**
* Tank style driving for the Drivetrain.
*
* @param left Speed in range [-1,1]
* @param right Speed in range [-1,1]
*/
public void drive(double left, double right) {
m_drive.tankDrive(left, right);
}
/**
* Get the robot's heading.
*
* @return The robots heading in degrees.
*/
public double getHeading() {
return m_gyro.getAngle();
}
/** Reset the robots sensors to the zero states. */
public void reset() {
m_gyro.reset();
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Get the average distance of the encoders since the last reset.
*
* @return The distance driven (average of left and right encoders).
*/
public double getDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
}
/**
* Get the distance to the obstacle.
*
* @return The distance to the obstacle detected by the rangefinder.
*/
public double getDistanceToObstacle() {
// Really meters in simulation since it's a rangefinder...
return m_rangefinder.getAverageVoltage();
}
/** Call log method every loop. */
@Override
public void periodic() {
log();
}
}

77
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
View File

@@ -1,77 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.ElevatorConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.motorcontrol.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID
* values for simulation are different than in the real world do to minor differences.
*/
public class Elevator extends PIDSubsystem {
private final Victor m_motor = new Victor(ElevatorConstants.kMotorPort);
private final AnalogPotentiometer m_pot;
/** Create a new elevator subsystem. */
public Elevator() {
super(
new PIDController(
ElevatorConstants.kP_real, ElevatorConstants.kI_real, ElevatorConstants.kD));
if (Robot.isSimulation()) { // Check for simulation and update PID values
getController()
.setPID(
ElevatorConstants.kP_simulation,
ElevatorConstants.kI_simulation,
ElevatorConstants.kD);
}
getController().setTolerance(ElevatorConstants.kTolerance);
// Conversion value of potentiometer varies between the real world and
// simulation
if (Robot.isReal()) {
m_pot = new AnalogPotentiometer(ElevatorConstants.kPotentiometerPort, -2.0 / 5);
} else {
// Defaults to meters
m_pot = new AnalogPotentiometer(ElevatorConstants.kPotentiometerPort);
}
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Pot", m_pot);
}
/** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putData("Elevator Pot", m_pot);
}
/**
* Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
*/
@Override
public double getMeasurement() {
return m_pot.get();
}
/** Use the motor as the PID output. This method is automatically called by the subsystem. */
@Override
public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
/** Call log method every loop. */
@Override
public void periodic() {
log();
}
}

65
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
View File

@@ -1,65 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.WristConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.motorcontrol.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
*/
public class Wrist extends PIDSubsystem {
private final Victor m_motor = new Victor(WristConstants.kMotorPort);
private final AnalogPotentiometer m_pot;
/** Create a new wrist subsystem. */
public Wrist() {
super(new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD));
getController().setTolerance(WristConstants.kTolerance);
// Conversion value of potentiometer varies between the real world and
// simulation
if (Robot.isReal()) {
m_pot = new AnalogPotentiometer(WristConstants.kPotentiometerPort, -270.0 / 5);
} else {
// Defaults to degrees
m_pot = new AnalogPotentiometer(WristConstants.kPotentiometerPort);
}
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Pot", m_pot);
}
/** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putData("Wrist Angle", m_pot);
}
/**
* Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
*/
@Override
public double getMeasurement() {
return m_pot.get();
}
/** Use the motor as the PID output. This method is automatically called by the subsystem. */
@Override
public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
/** Call log method every loop. */
@Override
public void periodic() {
log();
}
}

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

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

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
View File

@@ -1,100 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically 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 Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

99
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
View File

@@ -1,99 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
import static edu.wpi.first.wpilibj.PS4Controller.Button;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.PS4Controller;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// The driver's controller
PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new RunCommand(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
}
/**
* 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 PS4Controller}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
new JoystickButton(m_driverController, Button.kR1.value)
.onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5)))
.onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1)));
// Stabilize robot to drive straight with gyro when left bumper is held
new JoystickButton(m_driverController, Button.kL1.value)
.whileTrue(
new PIDCommand(
new PIDController(
DriveConstants.kStabilizationP,
DriveConstants.kStabilizationI,
DriveConstants.kStabilizationD),
// Close the loop on the turn rate
m_robotDrive::getTurnRate,
// Setpoint is 0
0,
// Pipe the output to the turning controls
output -> m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), output),
// Require the robot drive
m_robotDrive));
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kCross.value)
.onTrue(new TurnToAngle(90, m_robotDrive).withTimeout(5));
// Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kCircle.value)
.onTrue(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
}
}

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

@@ -1,45 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.PIDCommand;
/** A command that will turn the robot to the specified angle. */
public class TurnToAngle extends PIDCommand {
/**
* Turns to robot to the specified angle.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
super(
new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
output -> drive.arcadeDrive(0, output),
// Require the drive
drive);
// Set the controller to be continuous (because it is an angle controller)
getController().enableContinuousInput(-180, 180);
// Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
// setpoint before it is considered as having reached the reference
getController()
.setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
}
@Override
public boolean isFinished() {
// End when the controller is at the reference.
return getController().atSetpoint();
}
}

52
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
View File

@@ -1,52 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
/** A command that will turn the robot to the specified angle using a motion profile. */
public class TurnToAngleProfiled extends ProfiledPIDCommand {
/**
* Turns to robot to the specified angle using a motion profile.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
super(
new ProfiledPIDController(
DriveConstants.kTurnP,
DriveConstants.kTurnI,
DriveConstants.kTurnD,
new TrapezoidProfile.Constraints(
DriveConstants.kMaxTurnRateDegPerS,
DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
(output, setpoint) -> drive.arcadeDrive(0, output),
// Require the drive
drive);
// Set the controller to be continuous (because it is an angle controller)
getController().enableContinuousInput(-180, 180);
// Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
// setpoint before it is considered as having reached the reference
getController()
.setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
}
@Override
public boolean isFinished() {
// End when the controller is at the reference.
return getController().atGoal();
}
}

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

@@ -1,137 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
*
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
/** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
}
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}