Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add angular subsystem to SysIdRoutine example #6297

Merged
merged 20 commits into from
Feb 10, 2024
Merged
Show file tree
Hide file tree
Changes from 15 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,28 @@ void SysIdRoutineBot::ConfigureBindings() {
[this] { return -m_driverController.GetLeftY(); },
[this] { return -m_driverController.GetRightX(); }));

m_driverController.A().WhileTrue(
// 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().WhileTrue(
(m_driverController.B() && m_driverController.RightBumper()).WhileTrue(
m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
m_driverController.X().WhileTrue(
(m_driverController.X() && m_driverController.RightBumper()).WhileTrue(
m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
m_driverController.Y().WhileTrue(
(m_driverController.Y() && m_driverController.RightBumper()).WhileTrue(
m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));

m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand(
[] { return constants::shooter::kShooterTargetSpeed.value(); }));
DeltaDizzy marked this conversation as resolved.
Show resolved Hide resolved


(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() {
Expand Down
Original file line number Diff line number Diff line change
@@ -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);
}
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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);
Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@

#include "Constants.h"
#include "subsystems/Drive.h"
#include "subsystems/Shooter.h"

class SysIdRoutineBot {
public:
Expand All @@ -21,4 +22,5 @@ class SysIdRoutineBot {
frc2::CommandXboxController m_driverController{
constants::oi::kDriverControllerPort};
Drive m_drive;
Shooter m_shooter;
};
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
// 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_unit_t{0}};
DeltaDizzy marked this conversation as resolved.
Show resolved Hide resolved
};
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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 =
Expand All @@ -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));
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
// 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, 0);
DeltaDizzy marked this conversation as resolved.
Show resolved Hide resolved

/** Creates a new Drive subsystem. */
DeltaDizzy marked this conversation as resolved.
Show resolved Hide resolved
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);
}
}
Loading