Skip to content

Commit

Permalink
working shooter control
Browse files Browse the repository at this point in the history
  • Loading branch information
@r4stered
r4stered committed Aug 30, 2024
1 parent d7b9781 commit f5a5c55
Show file tree
Hide file tree
Showing 6 changed files with 229 additions and 45 deletions.
29 changes: 21 additions & 8 deletions src/main/cpp/RobotContainer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,14 +49,27 @@ void RobotContainer::ConfigureBindings() {
// controller.X().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kForward));
// controller.Y().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kReverse));

controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque(
frc2::sysid::Direction::kForward));
controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque(
frc2::sysid::Direction::kReverse));
controller.X().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque(
frc2::sysid::Direction::kForward));
controller.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque(
frc2::sysid::Direction::kReverse));
controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; }));
controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; }));

// controller.A().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kForward));
// controller.B().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse));
// controller.X().WhileTrue(shooterSubsystem.TopWheelSysIdDynamic(frc2::sysid::Direction::kForward));
// controller.Y().WhileTrue(shooterSubsystem.TopWheelSysIdDynamic(frc2::sysid::Direction::kReverse));

// (controller.A() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdQuasistatic(frc2::sysid::Direction::kForward));
// (controller.B() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse));
// (controller.X() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kForward));
// (controller.Y() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kReverse));

// controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque(
// frc2::sysid::Direction::kForward));
// controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque(
// frc2::sysid::Direction::kReverse));
// controller.X().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque(
// frc2::sysid::Direction::kForward));
// controller.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque(
// frc2::sysid::Direction::kReverse));

// controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kForward));
// controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kReverse));
Expand Down
100 changes: 81 additions & 19 deletions src/main/cpp/subsystems/ShooterSubsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include "subsystems/ShooterSubsystem.h"
#include "constants/Constants.h"
#include <frc2/command/Commands.h>

ShooterSubsystem::ShooterSubsystem() {
ConfigureShooterMotors(consts::shooter::physical::BOTTOM_INVERT,
Expand All @@ -14,22 +15,68 @@ ShooterSubsystem::ShooterSubsystem() {
ConfigureMotorSignals();
}

frc2::CommandPtr ShooterSubsystem::RunShooter(std::function<consts::shooter::PRESET_SPEEDS()> preset) {
return frc2::cmd::Run([this, preset] {
switch(preset()) {
case consts::shooter::PRESET_SPEEDS::AMP:
topWheelVelocitySetpoint = consts::shooter::AMP_SPEEDS.topSpeed;
bottomWheelVelocitySetpoint = consts::shooter::AMP_SPEEDS.bottomSpeed;
break;
case consts::shooter::PRESET_SPEEDS::OFF:
topWheelVelocitySetpoint = 0_rpm;
bottomWheelVelocitySetpoint = 0_rpm;
break;
default:
topWheelVelocitySetpoint = 0_rpm;
bottomWheelVelocitySetpoint = 0_rpm;
break;
}
}, {this}).Until([this] { return IsUpToSpeed(); });
}

// This method will be called once per scheduler run
void ShooterSubsystem::Periodic() {
ctre::phoenix::StatusCode shooterWaitResult = ctre::phoenix6::BaseStatusSignal::RefreshAll({&topMotorVelSig, &topMotorVoltageSig, &bottomMotorVelSig, &bottomMotorVoltageSig});
ctre::phoenix::StatusCode shooterWaitResult = ctre::phoenix6::BaseStatusSignal::RefreshAll({
&topMotorPosSig,
&topMotorVelSig,
&topMotorVoltageSig,
&bottomMotorPosSig,
&bottomMotorVelSig,
&bottomMotorVoltageSig
});

if(!shooterWaitResult.IsOK()) {
fmt::print("Error grabbing shooter signals! Details: {}\n", shooterWaitResult.GetName());
}

currentTopWheelPosition = ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(topMotorPosSig, topMotorVelSig);
currentBottomWheelPosition = ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(bottomMotorPosSig, bottomMotorVelSig);

currentBottomWheelVelocity = bottomMotorVelSig.GetValue();
currentTopWheelVelocity = topMotorVelSig.GetValue();

topWheelMotor.SetControl(topMotorSetter.WithOutput(10_V));
bottomWheelMotor.SetControl(bottomMotorSetter.WithOutput(10_V));
units::volt_t topFFVolts = topWheelFF.Calculate(topWheelVelocitySetpoint);
units::volt_t bottomFFVolts = bottomWheelFF.Calculate(bottomWheelVelocitySetpoint);

units::volt_t topPIDOutput = units::volt_t{topWheelPID.Calculate(currentTopWheelVelocity.value(), topWheelVelocitySetpoint.value())};
units::volt_t bottomPIDOutput = units::volt_t{bottomWheelPID.Calculate(currentTopWheelVelocity.value(), topWheelVelocitySetpoint.value())};

if(!runningSysid) {
if(topWheelVelocitySetpoint == 0_rpm) {
topWheelMotor.SetControl(coastSetter);
}
else {
topWheelMotor.SetControl(topMotorVoltageSetter.WithOutput(topFFVolts + topPIDOutput));
}
if(bottomWheelVelocitySetpoint == 0_rpm) {
bottomWheelMotor.SetControl(coastSetter);
}
else {
bottomWheelMotor.SetControl(bottomMotorVoltageSetter.WithOutput(bottomFFVolts + bottomPIDOutput));
}
}

bottomWheelVelocityPub.Set(currentBottomWheelVelocity.convert<units::revolutions_per_minute>().value());
topWheelVelocityPub.Set(currentTopWheelVelocity.convert<units::revolutions_per_minute>().value());
UpdateNTEntries();
}

void ShooterSubsystem::SimulationPeriodic() {
Expand All @@ -46,25 +93,21 @@ void ShooterSubsystem::SimulationPeriodic() {
bottomMotorSim.SetRotorVelocity(bottomFlywheelSim.GetAngularVelocity());
}

bool ShooterSubsystem::IsUpToSpeed() {
return (units::math::abs(topWheelVelocitySetpoint - currentTopWheelVelocity) < consts::shooter::gains::VEL_TOLERANCE) &&
(units::math::abs(bottomWheelVelocitySetpoint - currentBottomWheelVelocity) < consts::shooter::gains::VEL_TOLERANCE);
void ShooterSubsystem::UpdateNTEntries() {
topWheelSetpointPub.Set(topWheelVelocitySetpoint.convert<units::revolutions_per_minute>().value());
bottomWheelSetpointPub.Set(bottomWheelVelocitySetpoint.convert<units::revolutions_per_minute>().value());

bottomWheelVelocityPub.Set(currentBottomWheelVelocity.convert<units::revolutions_per_minute>().value());
topWheelVelocityPub.Set(currentTopWheelVelocity.convert<units::revolutions_per_minute>().value());

isUpToSpeedPub.Set(UpToSpeed().Get());
}

bool ShooterSubsystem::ConfigureShooterMotors(
bool invertBottom, bool invertTop, units::scalar_t shooterGearing,
units::ampere_t supplyCurrentLimit, units::ampere_t statorCurrentLimit) {

ctre::phoenix6::configs::TalonFXConfiguration shooterConfig{};
ctre::phoenix6::configs::Slot0Configs shooterSlotConfig{};

shooterSlotConfig.kV = consts::shooter::gains::SHOOTER_KV.value();
shooterSlotConfig.kA = consts::shooter::gains::SHOOTER_KA.value();
shooterSlotConfig.kS = consts::shooter::gains::SHOOTER_KS.value();
shooterSlotConfig.kP = consts::shooter::gains::SHOOTER_KP.value();
shooterSlotConfig.kI = consts::shooter::gains::SHOOTER_KI.value();
shooterSlotConfig.kD = consts::shooter::gains::SHOOTER_KD.value();
shooterConfig.Slot0 = shooterSlotConfig;

shooterConfig.MotorOutput.NeutralMode =
ctre::phoenix6::signals::NeutralModeValue::Coast;
Expand Down Expand Up @@ -100,14 +143,17 @@ bool ShooterSubsystem::ConfigureShooterMotors(
}

bool ShooterSubsystem::ConfigureMotorSignals() {
topMotorSetter.UpdateFreqHz = 0_Hz;
bottomMotorSetter.UpdateFreqHz = 0_Hz;
topMotorVoltageSetter.UpdateFreqHz = 0_Hz;
bottomMotorVoltageSetter.UpdateFreqHz = 0_Hz;
coastSetter.UpdateFreqHz = 0_Hz;

//Double the rio update rate? Not sure what is optimal here
units::hertz_t updateRate = 1.0 / (consts::LOOP_PERIOD * 2.0);

topMotorPosSig.SetUpdateFrequency(updateRate);
topMotorVelSig.SetUpdateFrequency(updateRate);
topMotorVoltageSig.SetUpdateFrequency(updateRate);
bottomMotorPosSig.SetUpdateFrequency(updateRate);
bottomMotorVelSig.SetUpdateFrequency(updateRate);
bottomMotorVoltageSig.SetUpdateFrequency(updateRate);

Expand All @@ -124,4 +170,20 @@ bool ShooterSubsystem::ConfigureMotorSignals() {
}

return optimizeTopMotor.IsOK() && optimizeBottomMotor.IsOK();
}

frc2::CommandPtr ShooterSubsystem::TopWheelSysIdQuasistatic(frc2::sysid::Direction direction) {
return topWheelSysIdRoutine.Quasistatic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });
}

frc2::CommandPtr ShooterSubsystem::TopWheelSysIdDynamic(frc2::sysid::Direction direction) {
return topWheelSysIdRoutine.Dynamic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });;
}

frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdQuasistatic(frc2::sysid::Direction direction) {
return bottomWheelSysIdRoutine.Quasistatic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });
}

frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdDynamic(frc2::sysid::Direction direction) {
return bottomWheelSysIdRoutine.Dynamic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });;
}
2 changes: 1 addition & 1 deletion src/main/deploy/commit.txt
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
e8855b1
d7b9781
35 changes: 26 additions & 9 deletions src/main/include/constants/ShooterConstants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <frc/system/plant/DCMotor.h>
#include <units/base.h>
#include <units/velocity.h>
#include <wpi/interpolating_map.h>

#include "str/Gains.h"

Expand Down Expand Up @@ -34,18 +35,34 @@ inline constexpr frc::DCMotor SHOOTER_MOTOR = frc::DCMotor::Falcon500(1);
inline constexpr units::meter_t WHEEL_RADIUS = 2_in;

//From onshape doc
static constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 62.564007 * 1_in * 1_in * 1_lb;
inline constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 5.01 * 1_in * 1_in * 1_lb;
} // namespace physical

namespace gains {
inline constexpr units::radians_per_second_t VEL_TOLERANCE = 10_rpm;
inline constexpr str::gains::radial::turn_volt_ka_unit_t SHOOTER_KA{0};
inline constexpr str::gains::radial::turn_amp_kv_unit_t SHOOTER_KV{0};
inline constexpr units::ampere_t SHOOTER_KS{0};
inline constexpr units::volt_t SHOOTER_KS_V{0};
inline constexpr str::gains::radial::turn_amp_kp_unit_t SHOOTER_KP{0};
inline constexpr str::gains::radial::turn_amp_ki_unit_t SHOOTER_KI{0};
inline constexpr str::gains::radial::turn_amp_kd_unit_t SHOOTER_KD{0};
inline constexpr units::turns_per_second_t VEL_TOLERANCE = 2_rpm;
inline constexpr str::gains::radial::turn_volt_ka_unit_t SHOOTER_KA{0.021356};
inline constexpr str::gains::radial::turn_volt_kv_unit_t SHOOTER_KV{0.11227};
inline constexpr units::volt_t SHOOTER_KS{0.02364};
inline constexpr str::gains::radial::turn_volt_kp_unit_t SHOOTER_KP{0.047275};
inline constexpr str::gains::radial::turn_volt_ki_unit_t SHOOTER_KI{0};
inline constexpr str::gains::radial::turn_volt_kd_unit_t SHOOTER_KD{0};
} // namespace gains

struct ShooterSpeeds {
units::turns_per_second_t topSpeed;
units::turns_per_second_t bottomSpeed;
};

inline constexpr ShooterSpeeds AMP_SPEEDS{3000_rpm, 2000_rpm};

static wpi::interpolating_map<units::meter_t, ShooterSpeeds> SHOOTER_LUT;

enum class PRESET_SPEEDS {
OFF,
AMP,
SPEAKER_DIST,
SUBWOOFER,
PASS
};
} // namespace shooter
} // namespace consts
14 changes: 14 additions & 0 deletions src/main/include/str/Gains.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,20 @@ using turn_volt_kv_unit =
units::compound_unit<units::volts, units::inverse<radial_velocity>>;
using turn_volt_kv_unit_t = units::unit_t<turn_volt_kv_unit>;

using turn_volt_kp_unit =
units::compound_unit<units::volts, units::inverse<units::turns>>;
using turn_volt_kp_unit_t = units::unit_t<turn_volt_kp_unit>;

using turn_volt_ki_unit = units::compound_unit<
units::volts,
units::inverse<units::compound_unit<units::turns, units::seconds>>>;
using turn_volt_ki_unit_t = units::unit_t<turn_volt_ki_unit>;

using turn_volt_kd_unit =
units::compound_unit<units::volts,
units::inverse<units::turns_per_second>>;
using turn_volt_kd_unit_t = units::unit_t<turn_volt_kd_unit>;

using turn_amp_ka_unit =
units::compound_unit<units::amperes, units::inverse<radial_accel>>;
using turn_amp_ka_unit_t = units::unit_t<turn_amp_ka_unit>;
Expand Down
Loading

0 comments on commit f5a5c55

Please sign in to comment.