From da3abade83ada38d387d7ab0633f0914a44ff9d1 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Sat, 10 Feb 2024 12:44:57 -0600 Subject: [PATCH] [examples] Add angular subsystem to SysIdRoutine example (#6297) Co-authored-by: Tim Winters --- .../examples/SysId/cpp/SysIdRoutineBot.cpp | 30 ++-- .../examples/SysId/cpp/subsystems/Shooter.cpp | 37 +++++ .../cpp/examples/SysId/include/Constants.h | 9 +- .../examples/SysId/include/SysIdRoutineBot.h | 2 + .../SysId/include/subsystems/Shooter.h | 54 ++++++++ .../wpilibj/examples/sysid/Constants.java | 1 + .../examples/sysid/SysIdRoutineBot.java | 47 ++++++- .../examples/sysid/subsystems/Drive.java | 10 ++ .../examples/sysid/subsystems/Shooter.java | 129 ++++++++++++++++++ 9 files changed, 304 insertions(+), 15 deletions(-) create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp index 45d480fe0b6..4e38374bff8 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp @@ -15,14 +15,28 @@ void SysIdRoutineBot::ConfigureBindings() { [this] { return -m_driverController.GetLeftY(); }, [this] { return -m_driverController.GetRightX(); })); - m_driverController.A().WhileTrue( - m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward)); - m_driverController.B().WhileTrue( - m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); - m_driverController.X().WhileTrue( - m_drive.SysIdDynamic(frc2::sysid::Direction::kForward)); - m_driverController.Y().WhileTrue( - m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse)); + // Using bumpers as a modifier and combining it with the buttons so that we + // can have both sets of bindings at once + (m_driverController.A() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward)); + (m_driverController.B() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); + (m_driverController.X() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kForward)); + (m_driverController.Y() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse)); + + m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand( + [this] { return m_driverController.GetLeftTriggerAxis(); })); + + (m_driverController.A() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kForward)); + (m_driverController.B() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); + (m_driverController.X() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kForward)); + (m_driverController.Y() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kReverse)); } frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp new file mode 100644 index 00000000000..8146cdd133b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp @@ -0,0 +1,37 @@ +// 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. + +#include "subsystems/Shooter.h" + +#include +#include +#include + +Shooter::Shooter() { + m_shooterEncoder.SetDistancePerPulse( + constants::shooter::kEncoderDistancePerPulse.value()); +} + +frc2::CommandPtr Shooter::RunShooterCommand( + std::function shooterSpeed) { + return frc2::cmd::Run( + [this, shooterSpeed] { + m_shooterMotor.SetVoltage( + units::volt_t{m_shooterFeedback.Calculate( + m_shooterEncoder.GetRate(), shooterSpeed())} + + m_shooterFeedforward.Calculate( + units::turns_per_second_t{shooterSpeed()})); + m_feederMotor.Set(constants::shooter::kFeederSpeed); + }, + {this}) + .WithName("Set Shooter Speed"); +} + +frc2::CommandPtr Shooter::SysIdQuasistatic(frc2::sysid::Direction direction) { + return m_sysIdRoutine.Quasistatic(direction); +} + +frc2::CommandPtr Shooter::SysIdDynamic(frc2::sysid::Direction direction) { + return m_sysIdRoutine.Dynamic(direction); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h index e01f533afee..a0e7728c101 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -38,8 +39,13 @@ using kv_unit = units::inverse>; using kv_unit_t = units::unit_t; +using ka_unit = + units::compound_unit>; +using ka_unit_t = units::unit_t; + inline constexpr std::array kEncoderPorts = {4, 5}; -inline constexpr bool kLeftEncoderReversed = false; +inline constexpr bool kEncoderReversed = false; inline constexpr int kEncoderCpr = 1024; inline constexpr units::turn_t kEncoderDistancePerPulse = units::turn_t{1.0} / static_cast(kEncoderCpr); @@ -55,6 +61,7 @@ inline constexpr double kP = 1.0; inline constexpr units::volt_t kS = 0.05_V; inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed; +inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / units::turn_t{1}; inline constexpr double kFeederSpeed = 0.5; } // namespace shooter diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h index 31110fc57a6..4363bb30ffc 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h @@ -9,6 +9,7 @@ #include "Constants.h" #include "subsystems/Drive.h" +#include "subsystems/Shooter.h" class SysIdRoutineBot { public: @@ -21,4 +22,5 @@ class SysIdRoutineBot { frc2::CommandXboxController m_driverController{ constants::oi::kDriverControllerPort}; Drive m_drive; + Shooter m_shooter; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h new file mode 100644 index 00000000000..0a33db68ea3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class Shooter : public frc2::SubsystemBase { + public: + Shooter(); + + frc2::CommandPtr RunShooterCommand(std::function shooterSpeed); + frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction); + frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction); + + private: + frc::PWMSparkMax m_shooterMotor{constants::shooter::kShooterMotorPort}; + frc::PWMSparkMax m_feederMotor{constants::shooter::kFeederMotorPort}; + + frc::Encoder m_shooterEncoder{constants::shooter::kEncoderPorts[0], + constants::shooter::kEncoderPorts[1], + constants::shooter::kEncoderReversed}; + + frc2::sysid::SysIdRoutine m_sysIdRoutine{ + frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, + std::nullopt}, + frc2::sysid::Mechanism{ + [this](units::volt_t driveVoltage) { + m_shooterMotor.SetVoltage(driveVoltage); + }, + [this](frc::sysid::SysIdRoutineLog* log) { + log->Motor("shooter-wheel") + .voltage(m_shooterMotor.Get() * + frc::RobotController::GetBatteryVoltage()) + .position(units::turn_t{m_shooterEncoder.GetDistance()}) + .velocity( + units::turns_per_second_t{m_shooterEncoder.GetRate()}); + }, + this}}; + frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0}; + frc::SimpleMotorFeedforward m_shooterFeedforward{ + constants::shooter::kS, constants::shooter::kV, constants::shooter::kA}; +}; 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 index 285b98f4070..f7731317dec 100644 --- 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 @@ -57,6 +57,7 @@ public static final class ShooterConstants { public static final double kVVoltSecondsPerRotation = // Should have value 12V at free speed... 12.0 / kShooterFreeRPS; + public static final double kAVoltSecondsSquaredPerRotation = 0; public static final double kFeederSpeed = 0.5; } 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 index 6665f3f7cb0..93496fdcc37 100644 --- 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 @@ -4,9 +4,9 @@ package edu.wpi.first.wpilibj.examples.sysid; -import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants; - +import edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive; +import edu.wpi.first.wpilibj.examples.sysid.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -21,6 +21,7 @@ public class SysIdRoutineBot { // The robot's subsystems private final Drive m_drive = new Drive(); + private final Shooter m_shooter = new Shooter(); // The driver's controller CommandXboxController m_driverController = @@ -42,10 +43,44 @@ public void configureBindings() { // 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)); + // Using bumpers as a modifier and combining it with the buttons so that we can have both sets + // of bindings at once + m_driverController + .a() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_driverController + .b() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_driverController + .x() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_driverController + .y() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Control the shooter wheel with the left trigger + m_shooter.setDefaultCommand(m_shooter.runShooter(m_driverController::getLeftTriggerAxis)); + + m_driverController + .a() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_driverController + .b() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_driverController + .x() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_driverController + .y() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } /** 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 index 3c9610897c5..9cad2950c0d 100644 --- 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 @@ -122,10 +122,20 @@ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { .withName("arcadeDrive"); } + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutine.quasistatic(direction); } + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutine.dynamic(direction); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java new file mode 100644 index 00000000000..322df8e1390 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java @@ -0,0 +1,129 @@ +// 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.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +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.units.Angle; +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.examples.sysid.Constants.ShooterConstants; +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 Shooter extends SubsystemBase { + // The motor on the shooter wheel . + private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort); + + // The motor on the feeder wheels. + private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort); + + // The shooter wheel encoder + private final Encoder m_shooterEncoder = + new Encoder( + ShooterConstants.kEncoderPorts[0], + ShooterConstants.kEncoderPorts[1], + ShooterConstants.kEncoderReversed); + + // 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_angle = mutable(Rotations.of(0)); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutableMeasure> m_velocity = mutable(RotationsPerSecond.of(0)); + + // Create a new SysId routine for characterizing the shooter. + 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 motor(s). + (Measure volts) -> { + m_shooterMotor.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 shooter motor. + log.motor("shooter-wheel") + .voltage( + m_appliedVoltage.mut_replace( + m_shooterMotor.get() * RobotController.getBatteryVoltage(), Volts)) + .angularPosition(m_angle.mut_replace(m_shooterEncoder.getDistance(), Rotations)) + .angularVelocity( + m_velocity.mut_replace(m_shooterEncoder.getRate(), RotationsPerSecond)); + }, + // Tell SysId to make generated commands require this subsystem, suffix test state in + // WPILog with this subsystem's name ("shooter") + this)); + // PID controller to run the shooter wheel in closed-loop, set the constants equal to those + // calculated by SysId + private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0, 0); + // Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to + // those calculated by SysId + private final SimpleMotorFeedforward m_shooterFeedforward = + new SimpleMotorFeedforward( + ShooterConstants.kSVolts, + ShooterConstants.kVVoltSecondsPerRotation, + ShooterConstants.kAVoltSecondsSquaredPerRotation); + + /** Creates a new Shooter subsystem. */ + public Shooter() { + // Sets the distance per pulse for the encoders + m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse); + } + + /** + * Returns a command that runs the shooter at a specifc velocity. + * + * @param shooterSpeed The commanded shooter wheel speed in rotations per second + */ + public Command runShooter(DoubleSupplier shooterSpeed) { + // Run shooter wheel at the desired speed using a PID controller and feedforward. + return run(() -> { + m_shooterMotor.setVoltage( + m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble()) + + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble())); + m_feederMotor.set(ShooterConstants.kFeederSpeed); + }) + .finallyDo( + () -> { + m_shooterMotor.stopMotor(); + m_feederMotor.stopMotor(); + }) + .withName("runShooter"); + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction); + } +}