diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java new file mode 100644 index 00000000000..45f2ccf17d2 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java @@ -0,0 +1,235 @@ +// 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.wpilibj2.command.sysid; + +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; +import static java.util.Map.entry; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.sysid.MotorLog; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.Map; +import java.util.function.Consumer; + +/** + * A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors. + * + *

A single subsystem may have multiple mechanisms, but mechanisms should not share test + * routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the + * log name of the recorded data is determined by the mechanism name. + */ +public class SysIdRoutine extends SysIdRoutineLog { + private final Config m_config; + private final Mechanism m_mechanism; + private final MutableMeasure m_outputVolts = mutable(Volts.of(0)); + private final Consumer m_recordState; + + /** + * Create a new SysId characterization routine. + * + * @param config Hardware-independent parameters for the SysId routine. + * @param mechanism Hardware interface for the SysId routine. + */ + public SysIdRoutine(Config config, Mechanism mechanism) { + super(mechanism.m_subsystem.getName()); + m_config = config; + m_mechanism = mechanism; + m_recordState = config.m_recordState != null ? config.m_recordState : this::recordState; + } + + /** Hardware-independent configuration for a SysId test routine. */ + public static class Config { + public final Measure> m_rampRate; + public final Measure m_stepVoltage; + public final Consumer m_recordState; + + /** + * Create a new configuration for a SysId test routine. + * + * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt + * per second if left null. + * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7 + * volts if left null. + * @param recordState Optional handle for recording test state in a third-party logging + * solution. If provided, the test routine state will be passed to this callback instead of + * logged in WPILog. + */ + public Config( + Measure> rampRate, + Measure stepVoltage, + Consumer recordState) { + m_rampRate = rampRate != null ? rampRate : Volts.of(1).per(Seconds.of(1)); + m_stepVoltage = stepVoltage != null ? stepVoltage : Volts.of(7); + m_recordState = recordState; + } + + /** + * Create a new configuration for a SysId test routine. + * + * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt + * per second if left null. + * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7 + * volts if left null. + */ + public Config(Measure> rampRate, Measure stepVoltage) { + this(rampRate, stepVoltage, null); + } + + /** + * Create a default configuration for a SysId test routine with a default ramp rate of 1 + * volt/sec and a default step voltage of 7 volts. + */ + public Config() { + this(null, null, null); + } + } + + /** + * A mechanism to be characterized by a SysId routine. Defines callbacks needed for the SysId test + * routine to control and record data from the mechanism. + */ + public static class Mechanism { + public final Consumer> m_drive; + public final Consumer m_log; + public final Subsystem m_subsystem; + public final String m_name; + + /** + * Create a new mechanism specification for a SysId routine. + * + * @param drive Sends the SysId-specified drive signal to the mechanism motors during test + * routines. + * @param log Returns measured data (voltages, positions, velocities) of the mechanism motors + * during test routines. To return data, call `recordFrame` on the supplied `MotorLog` + * instance. Multiple motors can return data within a single `log` callback by calling + * `recordFrame` multiple times. + * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized. + * Will be declared as a requirement for the returned test commands. + * @param name The name of the mechanism being tested. Will be appended to the log entry * title + * for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of + * the subsystem if left null. + */ + public Mechanism( + Consumer> drive, + Consumer log, + Subsystem subsystem, + String name) { + m_drive = drive; + m_log = log != null ? log : l -> {}; + m_subsystem = subsystem; + m_name = name != null ? name : subsystem.getName(); + } + + /** + * Create a new mechanism specification for a SysId routine. Defaults the mechanism name to the + * subsystem name. + * + * @param drive Sends the SysId-specified drive signal to the mechanism motors during test + * routines. + * @param log Returns measured data (voltages, positions, velocities) of the mechanism motors + * during test routines. To return data, call `recordFrame` on the supplied `MotorLog` + * instance. Multiple motors can return data within a single `log` callback by calling + * `recordFrame` multiple times. + * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized. + * Will be declared as a requirement for the returned test commands. The subsystem's `name` + * will be appended to the log entry title for the routine's test state, e.g. + * "sysid-test-state-subsystem". + */ + public Mechanism( + Consumer> drive, Consumer log, Subsystem subsystem) { + this(drive, log, subsystem, null); + } + } + + /** Motor direction for a SysId test. */ + public enum Direction { + kForward, + kReverse + } + + /** + * Returns a command to run a quasistatic test in the specified direction. + * + *

The command will call the `drive` and `log` callbacks supplied at routine construction once + * per iteration. Upon command end or interruption, the `drive` callback is called with a value of + * 0 volts. + * + * @param direction The direction in which to run the test. + * @return A command to run the test. + */ + public Command quasistatic(Direction direction) { + Timer timer = new Timer(); + double outputSign = direction == Direction.kForward ? 1.0 : -1.0; + State state = + Map.ofEntries( + entry(Direction.kForward, State.kQuasistaticForward), + entry(Direction.kReverse, State.kQuasistaticReverse)) + .get(direction); + + return m_mechanism + .m_subsystem + .runOnce(timer::start) + .andThen( + m_mechanism.m_subsystem.run( + () -> { + m_outputVolts.mut_replace( + outputSign * timer.get() * m_config.m_rampRate.in(Volts.per(Second)), Volts); + m_mechanism.m_drive.accept(m_outputVolts); + m_mechanism.m_log.accept(this); + m_recordState.accept(state); + })) + .finallyDo( + () -> { + m_mechanism.m_drive.accept(Volts.of(0)); + m_recordState.accept(State.kNone); + timer.stop(); + }) + .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name); + } + + /** + * Returns a command to run a dynamic test in the specified direction. + * + *

The command will call the `drive` and `log` callbacks supplied at routine construction once + * per iteration. Upon command end or interruption, the `drive` callback is called with a value of + * 0 volts. + * + * @param direction The direction in which to run the test. + * @return A command to run the test. + */ + public Command dynamic(Direction direction) { + State state = + Map.ofEntries( + entry(Direction.kForward, State.kDynamicForward), + entry(Direction.kReverse, State.kDynamicReverse)) + .get(direction); + + return m_mechanism + .m_subsystem + .runOnce(() -> m_outputVolts.mut_replace(m_config.m_stepVoltage)) + .andThen( + m_mechanism.m_subsystem.run( + () -> { + m_mechanism.m_drive.accept(m_outputVolts); + m_mechanism.m_log.accept(this); + m_recordState.accept(state); + })) + .finallyDo( + () -> { + m_mechanism.m_drive.accept(Volts.of(0)); + m_recordState.accept(State.kNone); + }) + .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java index 4cd50da11d8..ca61cae00bf 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java @@ -30,6 +30,16 @@ default void setVoltage(double outputVolts) { set(outputVolts / RobotController.getBatteryVoltage()); } + /** + * Returns the currently-applied voltage at the motor. + * + * @return The voltage applied to the motor, in volts. Equal to the current battery voltage times + * the duty cycle output. + */ + default double getVoltage() { + return get() * RobotController.getBatteryVoltage(); + } + /** * Common interface for getting the current set speed of a motor controller. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/MotorLog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/MotorLog.java new file mode 100644 index 00000000000..9993ee1727e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/MotorLog.java @@ -0,0 +1,90 @@ +// 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.sysid; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; + +/** Handle for recording SysId motor data to WPILog. */ +@FunctionalInterface +public interface MotorLog { + /** + * Records a frame of motor data during a SysId routine. The data are recorded as a struct under + * the string topic specified in motorName. + * + * @param voltage Voltage applied to the motor in volts. + * @param distance Total displacement of the motor, in units of distanceUnit. + * @param velocity Rate of change of displacement of the motor, in units of distanceUnit per + * second. + * @param motorName Name of the motor, used as the topic name for the WPILog entry for this data + * frame. + * @param distanceUnit String name of the unit used to measure distance, e.g. "meters". + */ + void recordFrame( + double voltage, double distance, double velocity, String motorName, String distanceUnit); + + /** + * Records a frame of motor data during a SysId routine. The data are recorded as a struct under + * the string topic specified in motorName. Assumes a linear distance dimension, and records all + * data to WPILog in meters. + * + *

For best performance, MutableMeasure should be used by calling code to avoid the need to + * allocate during each call. + * + * @param voltage Voltage applied to the motor. + * @param distance Total linear displacement of the motor. + * @param velocity Rate of change of linear displacement of the motor. + * @param motorName Name of the motor, used as the topic name for the WPILog entry for this data + * frame. + */ + default void recordFrameLinear( + Measure voltage, + Measure distance, + Measure> velocity, + String motorName) { + recordFrame( + voltage.in(Volts), + distance.in(Meters), + velocity.in(MetersPerSecond), + motorName, + Meters.name()); + } + + /** + * Records a frame of motor data during a SysId routine. The data are recorded as a struct under + * the string topic specified in motorName. Assumes an angular distance dimension, and records all + * data to WPILog in rotations. + * + *

For best performance, MutableMeasure should be used by calling code to avoid the need to + * allocate during each call. + * + * @param voltage Voltage applied to the motor. + * @param angle Total angular displacement of the motor. + * @param velocity Rate of change of angular displacement of the motor. + * @param motorName Name of the motor, used as the topic name for the WPILog entry for this data + * frame. + */ + default void recordFrameAngular( + Measure voltage, + Measure angle, + Measure> velocity, + String motorName) { + recordFrame( + voltage.in(Volts), + angle.in(Rotations), + velocity.in(RotationsPerSecond), + motorName, + Rotations.name()); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java new file mode 100644 index 00000000000..92b5ce12146 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java @@ -0,0 +1,140 @@ +// 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.sysid; + +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.DataLogManager; +import java.nio.ByteBuffer; +import java.util.HashMap; +import java.util.Map; + +/** + * Utility for logging data from a SysId test routine. Each complete routine (quasistatic and + * dynamic, forward and reverse) should have its own SysIdRoutineLog instance, with a unique log + * name. + */ +public class SysIdRoutineLog implements MotorLog { + private final Map> m_logEntries = new HashMap<>(); + private final StringLogEntry m_state; + private final DataFrame m_dataFrame = new DataFrame(); + + /** + * Create a new logging utility for a SysId test routine. + * + * @param logName The name for the test routine in the log. Should be unique between complete test + * routines (quasistatic and dynamic, forward and reverse). The current state of this test + * (e.g. "quasistatic-forward") will appear in WPILog under the "sysid-test-state-logName" + * entry. + */ + public SysIdRoutineLog(String logName) { + m_state = new StringLogEntry(DataLogManager.getLog(), "sysid-test-state-" + logName); + m_state.append(State.kNone.toString()); + } + + /** Possible state of a SysId routine. */ + public enum State { + kQuasistaticForward("quasistatic-forward"), + kQuasistaticReverse("quasistatic-reverse"), + kDynamicForward("dynamic-forward"), + kDynamicReverse("dynamic-reverse"), + kNone("none"); + + private final String m_state; + + State(String state) { + m_state = state; + } + + @Override + public String toString() { + return m_state; + } + } + + static class DataFrame { + public double m_voltage; + public double m_distance; + public double m_velocity; + + DataFrame(double voltage, double distance, double velocity) { + m_voltage = voltage; + m_distance = distance; + m_velocity = velocity; + } + + DataFrame() { + this(0, 0, 0); + } + + static class DataFrameStruct implements Struct { + @Override + public Class getTypeClass() { + return DataFrame.class; + } + + @Override + public String getTypeString() { + return "SysIdDataFrame"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double voltage; double distance; double velocity"; + } + + @Override + public DataFrame unpack(ByteBuffer bb) { + double voltage = bb.getDouble(); + double distance = bb.getDouble(); + double velocity = bb.getDouble(); + return new DataFrame(voltage, distance, velocity); + } + + @Override + public void pack(ByteBuffer bb, DataFrame value) { + bb.putDouble(value.m_voltage); + bb.putDouble(value.m_distance); + bb.putDouble(value.m_velocity); + } + } + + static final DataFrameStruct struct = new DataFrameStruct(); + } + + @Override + public void recordFrame( + double voltage, double distance, double velocity, String motorName, String distanceUnit) { + var entry = m_logEntries.get(motorName); + + if (entry == null) { + entry = + StructLogEntry.create(DataLogManager.getLog(), motorName, DataFrame.struct, distanceUnit); + m_logEntries.put(motorName, entry); + } + + m_dataFrame.m_voltage = voltage; + m_dataFrame.m_distance = distance; + m_dataFrame.m_velocity = velocity; + + entry.append(m_dataFrame); + } + + /** + * Records the current state of the SysId test routine. Should be called once per iteration during + * tests with the type of the current test, and once upon test end with state `none`. + * + * @param state The current state of the SysId test routine. + */ + public void recordState(State state) { + m_state.append(state.toString()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java new file mode 100644 index 00000000000..285b98f4070 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java @@ -0,0 +1,83 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.sysid; + +import edu.wpi.first.math.util.Units; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static 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 = {0, 1}; + public static final int[] kRightEncoderPorts = {2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterMeters = Units.inchesToMeters(6); + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; + } + + public static final class ShooterConstants { + public static final int[] kEncoderPorts = {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 / (double) kEncoderCPR; + + public static final int kShooterMotorPort = 4; + public static final int kFeederMotorPort = 5; + + public static final double kShooterFreeRPS = 5300; + public static final double kShooterTargetRPS = 4000; + public static final double kShooterToleranceRPS = 50; + + // These are not real PID gains, and will have to be tuned for your specific robot. + public static final double kP = 1; + + // 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 IntakeConstants { + public static final int kMotorPort = 6; + public static final int[] kSolenoidPorts = {2, 3}; + } + + public static final class StorageConstants { + public static final int kMotorPort = 7; + public static final int kBallSensorPort = 6; + } + + public static final class AutoConstants { + public static final double kTimeoutSeconds = 3; + public static final double kDriveDistanceMeters = 2; + public static final double kDriveSpeed = 0.5; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java new file mode 100644 index 00000000000..1fdd4b7374c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.sysid; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java new file mode 100644 index 00000000000..0b0f1428913 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java @@ -0,0 +1,92 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.sysid; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private final SysIdRoutineBot m_robot = new SysIdRoutineBot(); + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Configure default commands and condition bindings on robot startup + m_robot.configureBindings(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This 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() {} + + @Override + public void autonomousInit() { + m_autonomousCommand = m_robot.getAutonomousCommand(); + + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java new file mode 100644 index 00000000000..6837ffc8f21 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.sysid; + +import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants; + +import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +/** + * 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 SysIdRoutineBot { + // The robot's subsystems + private final Drive m_drive = new Drive(); + + // The driver's controller + CommandXboxController m_driverController = + new CommandXboxController(OIConstants.kDriverControllerPort); + + /** + * Use this method to define bindings between conditions and commands. These are useful for + * automating robot behaviors based on button and sensor input. + * + *

Should be called during {@link Robot#robotInit()}. + * + *

Event binding methods are available on the {@link Trigger} class. + */ + public void configureBindings() { + // Control the drive with split-stick arcade controls + m_drive.setDefaultCommand( + m_drive.arcadeDriveCommand( + () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX())); + + m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + /** + * Use this to define the command that runs during autonomous. + * + *

Scheduled during {@link Robot#autonomousInit()}. + */ + public Command getAutonomousCommand() { + // Do nothing + return m_drive.run(() -> {}); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java new file mode 100644 index 00000000000..f387a133fc6 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java @@ -0,0 +1,128 @@ +// 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.sysid.subsystems; + +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants; +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.sysid.MotorLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.function.DoubleSupplier; + +public class Drive extends SubsystemBase { + // The motors on the left side of the drive. + private final MotorControllerGroup m_leftMotors = + new MotorControllerGroup( + new PWMSparkMax(DriveConstants.kLeftMotor1Port), + new PWMSparkMax(DriveConstants.kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final MotorControllerGroup m_rightMotors = + new MotorControllerGroup( + new PWMSparkMax(DriveConstants.kRightMotor1Port), + new PWMSparkMax(DriveConstants.kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder( + 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); + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutableMeasure m_distance = mutable(Meters.of(0)); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutableMeasure> m_velocity = mutable(MetersPerSecond.of(0)); + + // Create a new SysId routine for characterizing the drive. + private final SysIdRoutine m_sysIdRoutine = + new SysIdRoutine( + // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + // Tell SysId how to plumb the driving voltage to the motors. + (Measure volts) -> { + m_leftMotors.setVoltage(volts.in(Volts)); + m_rightMotors.setVoltage(volts.in(Volts)); + }, + // Tell SysId how to record a frame of data for each motor on the mechanism being + // characterized. + (MotorLog log) -> { + // Record a frame for the left motors. Since these share an encoder, we consider + // the entire group to be one motor. + log.recordFrameLinear( + m_appliedVoltage.mut_replace(m_leftMotors.getVoltage(), Volts), + m_distance.mut_replace(m_leftEncoder.getDistance(), Meters), + m_velocity.mut_replace(m_leftEncoder.getRate(), MetersPerSecond), + "drive-left"); + // Record a frame for the right motors. Since these share an encoder, we consider + // the entire group to be one motor. + log.recordFrameLinear( + m_appliedVoltage.mut_replace(m_rightMotors.getVoltage(), Volts), + m_distance.mut_replace(m_rightEncoder.getDistance(), Meters), + m_velocity.mut_replace(m_rightEncoder.getRate(), MetersPerSecond), + "drive-right"); + }, + this)); + + /** Creates a new Drive subsystem. */ + public Drive() { + // 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_rightMotors.setInverted(true); + + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + } + + /** + * Returns a command that drives the robot with arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble())) + .withName("arcadeDrive"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction).withTimeout(10); // include a timeout for safety + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction).withTimeout(10); // include a timeout for safety + } +}