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

[examples] Add angular subsystem to SysIdRoutine example (#6297)

Browse Source 
Co-authored-by: Tim Winters <twinters@wpi.edu>
This commit is contained in:
DeltaDizzy
2024-02-10 12:44:57 -06:00
committed by GitHub
parent 62cba9a4d3
commit da3abade83
9 changed files with 304 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp
View File

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

37
wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp Normal file
View File

@@ -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 <frc2/command/Commands.h>
#include <units/angle.h>
#include <units/voltage.h>
Shooter::Shooter() {
m_shooterEncoder.SetDistancePerPulse(
constants::shooter::kEncoderDistancePerPulse.value());
}
frc2::CommandPtr Shooter::RunShooterCommand(
std::function<double()> 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);
}

9
wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
View File

@@ -8,6 +8,7 @@
#include <numbers>
#include <units/angle.h>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/time.h>
@@ -38,8 +39,13 @@ using kv_unit =
units::inverse<units::turns>>;
using kv_unit_t = units::unit_t<kv_unit>;
using ka_unit =
units::compound_unit<units::volts,
units::inverse<units::turns_per_second_squared>>;
using ka_unit_t = units::unit_t<ka_unit>;
inline constexpr std::array<int, 2> 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<double>(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

2
wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h
View File

@@ -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;
};

54
wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h Normal file
View File

@@ -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 <functional>
#include <frc/Encoder.h>
#include <frc/RobotController.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc2/command/SubsystemBase.h>
#include <frc2/command/sysid/SysIdRoutine.h>
#include "Constants.h"
class Shooter : public frc2::SubsystemBase {
public:
Shooter();
frc2::CommandPtr RunShooterCommand(std::function<double()> 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<units::turns> m_shooterFeedforward{
constants::shooter::kS, constants::shooter::kV, constants::shooter::kA};
};

1
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
View File

@@ -57,6 +57,7 @@ public final class Constants {
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;
}

47
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
View File

@@ -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 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
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 class SysIdRoutineBot {
// 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));
}
/**

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

@@ -122,10 +122,20 @@ public class Drive extends SubsystemBase {
.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);
}

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

@@ -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<Voltage> m_appliedVoltage = mutable(Volts.of(0));
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
private final MutableMeasure<Velocity<Angle>> 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<Voltage> 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);
}
}