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

[wpilib] Add SysIdRoutine logging utility and command factory (#6033)

Browse Source 
Co-authored-by: Drew Williams <williams.r.drew@gmail.com>
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Eli Barnett
2024-01-05 14:50:23 -05:00
committed by GitHub
parent 3e40b9e5da
commit 707cb06105
22 changed files with 1936 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -983,5 +983,18 @@
"gradlebase": "java",
"commandversion": 2,
"mainclass": "Main"
},
{
"name": "SysIdRoutine",
"description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
"tags": [
"SysId",
"Command-based",
"DataLog"
],
"foldername": "sysid",
"gradlebase": "java",
"commandversion": 2,
"mainclass": "Main"
}
]

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

@@ -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.
*
* <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 = {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;
}
}

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

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.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.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

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

@@ -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.
*
* <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() {}
@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() {}
}

60
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java Normal file
View File

@@ -0,0 +1,60 @@
// 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.
*
* <p>Should be called during {@link Robot#robotInit()}.
*
* <p>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()));
// Bind full set of SysId routine tests to buttons; a complete routine should run each of these
// once.
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.
*
* <p>Scheduled during {@link Robot#autonomousInit()}.
*/
public Command getAutonomousCommand() {
// Do nothing
return m_drive.run(() -> {});
}
}

132
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java Normal file
View File

@@ -0,0 +1,132 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.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.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
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 PWMSparkMax m_leftMotor = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightMotor = new PWMSparkMax(DriveConstants.kRightMotor1Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::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);
// Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
private final MutableMeasure<Velocity<Distance>> 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<Voltage> volts) -> {
m_leftMotor.setVoltage(volts.in(Volts));
m_rightMotor.setVoltage(volts.in(Volts));
},
// Tell SysId how to record a frame of data for each motor on the mechanism being
// characterized.
log -> {
// Record a frame for the left motors. Since these share an encoder, we consider
// the entire group to be one motor.
log.motor("drive-left")
.voltage(
m_appliedVoltage.mut_replace(
m_leftMotor.get() * RobotController.getBatteryVoltage(), Volts))
.linearPosition(m_distance.mut_replace(m_leftEncoder.getDistance(), Meters))
.linearVelocity(
m_velocity.mut_replace(m_leftEncoder.getRate(), MetersPerSecond));
// Record a frame for the right motors. Since these share an encoder, we consider
// the entire group to be one motor.
log.motor("drive-right")
.voltage(
m_appliedVoltage.mut_replace(
m_rightMotor.get() * RobotController.getBatteryVoltage(), Volts))
.linearPosition(m_distance.mut_replace(m_rightEncoder.getDistance(), Meters))
.linearVelocity(
m_velocity.mut_replace(m_rightEncoder.getRate(), MetersPerSecond));
},
// Tell SysId to make generated commands require this subsystem, suffix test state in
// WPILog with this subsystem's name ("drive")
this));
/** Creates a new Drive subsystem. */
public Drive() {
// Add the second motors on each side of the drivetrain
m_leftMotor.addFollower(new PWMSparkMax(DriveConstants.kLeftMotor2Port));
m_rightMotor.addFollower(new PWMSparkMax(DriveConstants.kRightMotor2Port));
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
// 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);
}
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.dynamic(direction);
}
}