Remove motorcontrol from wpilib_interface
In the 2024 version of phoenix motorcontrol deprecates so we neeed to go
through every wpilib_interface where we use it and make it use falcon
objects and DutyCycleOut instead of PercentOutput
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I0e1e09330b74fcc0445b469f71791a185e629a1c
diff --git a/y2020/BUILD b/y2020/BUILD
index 7c04d99..04ca0d8 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -16,6 +16,8 @@
],
data = [
":aos_config",
+ "@ctre_phoenix6_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix6_tools_athena//:shared_libraries",
"@ctre_phoenix_api_cpp_athena//:shared_libraries",
"@ctre_phoenix_cci_athena//:shared_libraries",
],
@@ -128,6 +130,7 @@
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
+ "//third_party:phoenix6",
"//third_party:wpilib",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index e3cb5b3..2f7b184 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -20,8 +20,7 @@
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/VictorSPX.h"
+#include "ctre/phoenix6/TalonFX.hpp"
#include "gflags/gflags.h"
#include "aos/commonmath.h"
@@ -111,6 +110,46 @@
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000.0,
"medium encoders are too fast");
+void PrintConfigs(ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ ctre::phoenix::StatusCode status =
+ talon->GetConfigurator().Refresh(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to get falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
+}
+
+void WriteConfigs(ctre::phoenix6::hardware::TalonFX *talon,
+ double stator_current_limit, double supply_current_limit) {
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit = stator_current_limit;
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit = supply_current_limit;
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+
+ ctre::phoenix::StatusCode status =
+ talon->GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs(talon);
+}
+
+void Disable(ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
+ stop_command.UpdateFreqHz = 0_Hz;
+ stop_command.EnableFOC = true;
+
+ talon->SetControl(stop_command);
+}
+
} // namespace
// Class to send position messages with sensor readings to our loops.
@@ -393,14 +432,12 @@
}
void set_intake_roller_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
intake_roller_falcon_ = ::std::move(t);
- intake_roller_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kIntakeRollerSupplyCurrentLimit(),
- Values::kIntakeRollerSupplyCurrentLimit(), 0});
- intake_roller_falcon_->ConfigStatorCurrentLimit(
- {true, Values::kIntakeRollerStatorCurrentLimit(),
- Values::kIntakeRollerStatorCurrentLimit(), 0});
+
+ WriteConfigs(intake_roller_falcon_.get(),
+ Values::kIntakeRollerStatorCurrentLimit(),
+ Values::kIntakeRollerSupplyCurrentLimit());
}
void set_turret_victor(::std::unique_ptr<::frc::VictorSP> t) {
@@ -408,26 +445,11 @@
}
void set_feeder_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
feeder_falcon_ = ::std::move(t);
- {
- auto result = feeder_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kFeederSupplyCurrentLimit(),
- Values::kFeederSupplyCurrentLimit(), 0});
- if (result != ctre::phoenix::OKAY) {
- LOG(WARNING) << "Failed to configure feeder supply current limit: "
- << result;
- }
- }
- {
- auto result = feeder_falcon_->ConfigStatorCurrentLimit(
- {true, Values::kFeederStatorCurrentLimit(),
- Values::kFeederStatorCurrentLimit(), 0});
- if (result != ctre::phoenix::OKAY) {
- LOG(WARNING) << "Failed to configure feeder stator current limit: "
- << result;
- }
- }
+
+ WriteConfigs(feeder_falcon_.get(), Values::kFeederStatorCurrentLimit(),
+ Values::kFeederSupplyCurrentLimit());
}
void set_washing_machine_control_panel_victor(
@@ -452,11 +474,23 @@
}
void set_climber_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
climber_falcon_ = ::std::move(t);
- climber_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kClimberSupplyCurrentLimit(),
- Values::kClimberSupplyCurrentLimit(), 0});
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.SupplyCurrentLimit = Values::kClimberSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+
+ ctre::phoenix::StatusCode status =
+ climber_falcon_->GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs(climber_falcon_.get());
}
private:
@@ -470,20 +504,12 @@
kMaxBringupPower) /
12.0);
- intake_roller_falcon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(-output.intake_roller_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ WriteCan(-output.intake_roller_voltage(), intake_roller_falcon_.get());
turret_victor_->SetSpeed(std::clamp(output.turret_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
-
- feeder_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(output.feeder_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ WriteCan(output.feeder_voltage(), feeder_falcon_.get());
if (washing_machine_control_panel_victor_) {
washing_machine_control_panel_victor_->SetSpeed(
std::clamp(-output.washing_machine_spinner_voltage(),
@@ -511,14 +537,20 @@
12.0);
if (climber_falcon_) {
- climber_falcon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(output.climber_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ WriteCan(output.climber_voltage(), climber_falcon_.get());
}
}
+ static void WriteCan(const double voltage,
+ ::ctre::phoenix6::hardware::TalonFX *falcon) {
+ ctre::phoenix6::controls::DutyCycleOut control(
+ std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ control.UpdateFreqHz = 0_Hz;
+ control.EnableFOC = true;
+
+ falcon->SetControl(control);
+ }
+
void Stop() override {
AOS_LOG(WARNING, "Superstructure output too old.\n");
hood_victor_->SetDisabled();
@@ -531,10 +563,9 @@
accelerator_right_falcon_->SetDisabled();
finisher_falcon0_->SetDisabled();
finisher_falcon1_->SetDisabled();
- feeder_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- intake_roller_falcon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ Disable(feeder_falcon_.get());
+ Disable(intake_roller_falcon_.get());
+ Disable(climber_falcon_.get());
}
::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
@@ -543,8 +574,8 @@
::std::unique_ptr<::frc::TalonFX> accelerator_left_falcon_,
accelerator_right_falcon_, finisher_falcon0_, finisher_falcon1_;
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- intake_roller_falcon_, climber_falcon_, feeder_falcon_;
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> intake_roller_falcon_,
+ climber_falcon_, feeder_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -654,10 +685,10 @@
superstructure_writer.set_intake_joint_victor(
make_unique<frc::VictorSP>(2));
superstructure_writer.set_intake_roller_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(0));
superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(7));
superstructure_writer.set_feeder_falcon(
- make_unique<ctre::phoenix::motorcontrol::can::TalonFX>(1));
+ make_unique<ctre::phoenix6::hardware::TalonFX>(1));
superstructure_writer.set_washing_machine_control_panel_victor(
make_unique<frc::VictorSP>(6));
superstructure_writer.set_accelerator_left_falcon(
@@ -668,7 +699,7 @@
superstructure_writer.set_finisher_falcon1(make_unique<::frc::TalonFX>(3));
// TODO: check port
superstructure_writer.set_climber_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(2));
AddLoop(&output_event_loop);
diff --git a/y2021_bot3/BUILD b/y2021_bot3/BUILD
index ea9c3c5..0fc468b 100644
--- a/y2021_bot3/BUILD
+++ b/y2021_bot3/BUILD
@@ -70,6 +70,7 @@
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
+ "//third_party:phoenix6",
"//third_party:wpilib",
"//y2021_bot3/control_loops/superstructure:superstructure_output_fbs",
"//y2021_bot3/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2021_bot3/wpilib_interface.cc b/y2021_bot3/wpilib_interface.cc
index 02c2ecf..6f5908c 100644
--- a/y2021_bot3/wpilib_interface.cc
+++ b/y2021_bot3/wpilib_interface.cc
@@ -22,8 +22,7 @@
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+#include "ctre/phoenix6/TalonFX.hpp"
#include "aos/commonmath.h"
#include "aos/events/event_loop.h"
@@ -107,6 +106,46 @@
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");
+void PrintConfigs(ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ ctre::phoenix::StatusCode status =
+ talon->GetConfigurator().Refresh(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to get falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
+}
+
+void WriteConfigs(ctre::phoenix6::hardware::TalonFX *talon,
+ double stator_current_limit, double supply_current_limit) {
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit = stator_current_limit;
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit = supply_current_limit;
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+
+ ctre::phoenix::StatusCode status =
+ talon->GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs(talon);
+}
+
+void Disable(ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
+ stop_command.UpdateFreqHz = 0_Hz;
+ stop_command.EnableFOC = true;
+
+ talon->SetControl(stop_command);
+}
+
} // namespace
// Class to send position messages with sensor readings to our loops.
@@ -196,39 +235,51 @@
event_loop, "/superstructure") {}
void set_intake_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
intake_falcon_ = ::std::move(t);
ConfigureRollerFalcon(intake_falcon_.get());
}
void set_outtake_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
outtake_falcon_ = ::std::move(t);
ConfigureRollerFalcon(outtake_falcon_.get());
}
void set_climber_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
climber_falcon_ = ::std::move(t);
- climber_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kClimberSupplyCurrentLimit(),
- Values::kClimberSupplyCurrentLimit(), 0});
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.SupplyCurrentLimit = Values::kClimberSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+
+ ctre::phoenix::StatusCode status =
+ climber_falcon_->GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs(climber_falcon_.get());
}
private:
- void ConfigureRollerFalcon(
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->ConfigSupplyCurrentLimit({true, Values::kRollerSupplyCurrentLimit(),
- Values::kRollerSupplyCurrentLimit(), 0});
- falcon->ConfigStatorCurrentLimit({true, Values::kRollerStatorCurrentLimit(),
- Values::kRollerStatorCurrentLimit(), 0});
+ void ConfigureRollerFalcon(::ctre::phoenix6::hardware::TalonFX *falcon) {
+ WriteConfigs(falcon, Values::kRollerSupplyCurrentLimit(),
+ Values::kRollerStatorCurrentLimit());
}
void WriteToFalcon(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ ::ctre::phoenix6::hardware::TalonFX *falcon) {
+ ctre::phoenix6::controls::DutyCycleOut control(
std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ control.UpdateFreqHz = 0_Hz;
+ control.EnableFOC = true;
+
+ falcon->SetControl(control);
}
void Write(const superstructure::Output &output) override {
@@ -240,16 +291,15 @@
void Stop() override {
AOS_LOG(WARNING, "Superstructure output too old.\n");
- climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- intake_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- outtake_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ Disable(climber_falcon_.get());
+ Disable(intake_falcon_.get());
+ Disable(outtake_falcon_.get());
}
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> intake_falcon_,
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> intake_falcon_,
outtake_falcon_;
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- climber_falcon_;
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> climber_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -292,11 +342,11 @@
SuperstructureWriter superstructure_writer(&output_event_loop);
superstructure_writer.set_intake_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(0));
superstructure_writer.set_outtake_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(1));
superstructure_writer.set_climber_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(2));
AddLoop(&output_event_loop);
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 39e8dd2..5b3cd96 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -23,8 +23,7 @@
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+#include "ctre/phoenix6/TalonFX.hpp"
#include "aos/commonmath.h"
#include "aos/events/event_loop.h"
@@ -115,6 +114,46 @@
(3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
+void PrintConfigs(ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ ctre::phoenix::StatusCode status =
+ talon->GetConfigurator().Refresh(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to get falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
+}
+
+void WriteConfigs(ctre::phoenix6::hardware::TalonFX *talon,
+ double stator_current_limit, double supply_current_limit) {
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit = stator_current_limit;
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit = supply_current_limit;
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+
+ ctre::phoenix::StatusCode status =
+ talon->GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs(talon);
+}
+
+void Disable(ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
+ stop_command.UpdateFreqHz = 0_Hz;
+ stop_command.EnableFOC = true;
+
+ talon->SetControl(stop_command);
+}
+
} // namespace
// Class to send position messages with sensor readings to our loops.
@@ -179,8 +218,8 @@
imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
}
void set_catapult_falcon_1(
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t1,
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t2) {
+ ::std::shared_ptr<ctre::phoenix6::hardware::TalonFX> t1,
+ ::std::shared_ptr<ctre::phoenix6::hardware::TalonFX> t2) {
catapult_falcon_1_can_ = ::std::move(t1);
catapult_falcon_2_can_ = ::std::move(t2);
}
@@ -433,8 +472,8 @@
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- catapult_falcon_1_can_, catapult_falcon_2_can_;
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult_falcon_1_can_,
+ catapult_falcon_2_can_;
};
class SuperstructureWriter
@@ -465,27 +504,41 @@
}
void set_catapult_falcon_1(
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t1,
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t2) {
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t1,
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t2) {
catapult_falcon_1_can_ = ::std::move(t1);
catapult_falcon_2_can_ = ::std::move(t2);
for (auto &falcon : {catapult_falcon_1_can_, catapult_falcon_2_can_}) {
- falcon->ConfigSupplyCurrentLimit(
- {false, Values::kIntakeRollerSupplyCurrentLimit(),
- Values::kIntakeRollerSupplyCurrentLimit(), 0});
- falcon->ConfigStatorCurrentLimit(
- {false, Values::kIntakeRollerStatorCurrentLimit(),
- Values::kIntakeRollerStatorCurrentLimit(), 0});
- falcon->SetStatusFramePeriod(
+ ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit =
+ Values::kIntakeRollerStatorCurrentLimit();
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit =
+ Values::kIntakeRollerSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenix6::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+
+ ctre::phoenix::StatusCode status =
+ falcon->GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ PrintConfigs(falcon.get());
+
+ // TODO(max): Figure out how to migrate these configs to phoenix6
+ /*falcon->SetStatusFramePeriod(
ctre::phoenix::motorcontrol::Status_1_General, 1);
- falcon->SetControlFramePeriod(
- ctre::phoenix::motorcontrol::Control_3_General, 1);
falcon->SetStatusFramePeriod(
ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
+
falcon->ConfigOpenloopRamp(0.0);
falcon->ConfigClosedloopRamp(0.0);
- falcon->ConfigVoltageMeasurementFilter(1);
+ falcon->ConfigVoltageMeasurementFilter(1);*/
}
}
@@ -498,40 +551,30 @@
}
void set_roller_falcon_front(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
roller_falcon_front_ = ::std::move(t);
- roller_falcon_front_->ConfigSupplyCurrentLimit(
- {true, Values::kIntakeRollerSupplyCurrentLimit(),
- Values::kIntakeRollerSupplyCurrentLimit(), 0});
- roller_falcon_front_->ConfigStatorCurrentLimit(
- {true, Values::kIntakeRollerStatorCurrentLimit(),
- Values::kIntakeRollerStatorCurrentLimit(), 0});
+ WriteConfigs(roller_falcon_front_.get(),
+ Values::kIntakeRollerStatorCurrentLimit(),
+ Values::kIntakeRollerSupplyCurrentLimit());
}
void set_roller_falcon_back(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
roller_falcon_back_ = ::std::move(t);
- roller_falcon_back_->ConfigSupplyCurrentLimit(
- {true, Values::kIntakeRollerSupplyCurrentLimit(),
- Values::kIntakeRollerSupplyCurrentLimit(), 0});
- roller_falcon_back_->ConfigStatorCurrentLimit(
- {true, Values::kIntakeRollerStatorCurrentLimit(),
- Values::kIntakeRollerStatorCurrentLimit(), 0});
+ WriteConfigs(roller_falcon_back_.get(),
+ Values::kIntakeRollerStatorCurrentLimit(),
+ Values::kIntakeRollerSupplyCurrentLimit());
}
void set_flipper_arms_falcon(
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
flipper_arms_falcon_ = t;
- flipper_arms_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kFlipperArmSupplyCurrentLimit(),
- Values::kFlipperArmSupplyCurrentLimit(), 0});
- flipper_arms_falcon_->ConfigStatorCurrentLimit(
- {true, Values::kFlipperArmStatorCurrentLimit(),
- Values::kFlipperArmStatorCurrentLimit(), 0});
+ WriteConfigs(flipper_arms_falcon_.get(),
+ Values::kFlipperArmSupplyCurrentLimit(),
+ Values::kFlipperArmStatorCurrentLimit());
}
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- flipper_arms_falcon() {
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> flipper_arms_falcon() {
return flipper_arms_falcon_;
}
@@ -553,12 +596,10 @@
climber_servo_left_->SetRaw(0);
climber_servo_right_->SetRaw(0);
- roller_falcon_front_->Set(
- ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- roller_falcon_back_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled,
- 0);
- flipper_arms_falcon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ Disable(roller_falcon_front_.get());
+ Disable(roller_falcon_back_.get());
+ Disable(flipper_arms_falcon_.get());
+
intake_falcon_front_->SetDisabled();
intake_falcon_back_->SetDisabled();
transfer_roller_victor_->SetDisabled();
@@ -566,10 +607,8 @@
catapult_falcon_1_->SetDisabled();
}
if (catapult_falcon_1_can_) {
- catapult_falcon_1_can_->Set(
- ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- catapult_falcon_2_can_->Set(
- ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ Disable(catapult_falcon_1_can_.get());
+ Disable(catapult_falcon_2_can_.get());
}
turret_falcon_->SetDisabled();
}
@@ -597,18 +636,31 @@
}
}
if (catapult_falcon_1_can_) {
- WriteCan(output.catapult_voltage(), catapult_falcon_1_can_.get());
- WriteCan(output.catapult_voltage(), catapult_falcon_2_can_.get());
+ WriteCanCatapult(output.catapult_voltage(), catapult_falcon_1_can_.get());
+ WriteCanCatapult(output.catapult_voltage(), catapult_falcon_2_can_.get());
}
WritePwm(-output.turret_voltage(), turret_falcon_.get());
}
static void WriteCan(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ ::ctre::phoenix6::hardware::TalonFX *falcon) {
+ ctre::phoenix6::controls::DutyCycleOut control(
std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ control.UpdateFreqHz = 0_Hz;
+ control.EnableFOC = true;
+
+ falcon->SetControl(control);
+ }
+ // We do this to set our UpdateFreqHz higher
+ static void WriteCanCatapult(const double voltage,
+ ::ctre::phoenix6::hardware::TalonFX *falcon) {
+ ctre::phoenix6::controls::DutyCycleOut control(
+ std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ control.UpdateFreqHz = 1000_Hz;
+ control.EnableFOC = true;
+
+ falcon->SetControl(control);
}
template <typename T>
@@ -619,14 +671,13 @@
::std::unique_ptr<frc::TalonFX> intake_falcon_front_, intake_falcon_back_;
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- roller_falcon_front_, roller_falcon_back_;
+ ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> roller_falcon_front_,
+ roller_falcon_back_;
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- flipper_arms_falcon_;
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> flipper_arms_falcon_;
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- catapult_falcon_1_can_, catapult_falcon_2_can_;
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult_falcon_1_can_,
+ catapult_falcon_2_can_;
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
climber_falcon_;
@@ -654,7 +705,7 @@
}
void set_flipper_arms_falcon(
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
flipper_arms_falcon_ = std::move(t);
}
@@ -664,7 +715,7 @@
superstructure::CANPosition::Builder can_position_builder =
builder.MakeBuilder<superstructure::CANPosition>();
can_position_builder.add_flipper_arm_integrated_sensor_velocity(
- flipper_arms_falcon_->GetSelectedSensorVelocity() *
+ flipper_arms_falcon_->GetVelocity().GetValue().value() *
kVelocityConversion);
builder.CheckOk(builder.Send(can_position_builder.Finish()));
}
@@ -676,8 +727,7 @@
aos::EventLoop *event_loop_;
::aos::PhasedLoopHandler *phased_loop_handler_;
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- flipper_arms_falcon_;
+ ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> flipper_arms_falcon_;
aos::Sender<superstructure::CANPosition> can_position_sender_;
};
@@ -769,9 +819,9 @@
superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(3));
superstructure_writer.set_roller_falcon_front(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(0));
superstructure_writer.set_roller_falcon_back(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(1));
superstructure_writer.set_transfer_roller_victor(
make_unique<::frc::VictorSP>(5));
@@ -782,18 +832,16 @@
superstructure_writer.set_climber_servo_left(make_unique<frc::Servo>(7));
superstructure_writer.set_climber_servo_right(make_unique<frc::Servo>(6));
superstructure_writer.set_flipper_arms_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(2));
superstructure_writer.set_superstructure_reading(superstructure_reading);
if (!FLAGS_can_catapult) {
superstructure_writer.set_catapult_falcon_1(make_unique<frc::TalonFX>(9));
} else {
- std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> catapult1 =
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(3,
- "Catapult");
- std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> catapult2 =
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(4,
- "Catapult");
+ std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult1 =
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(3, "Catapult");
+ std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult2 =
+ make_unique<::ctre::phoenix6::hardware::TalonFX>(4, "Catapult");
superstructure_writer.set_catapult_falcon_1(catapult1, catapult2);
sensor_reader.set_catapult_falcon_1(catapult1, catapult2);
}
diff --git a/y2022_bot3/wpilib_interface.cc b/y2022_bot3/wpilib_interface.cc
index b917ddb..4641e84 100644
--- a/y2022_bot3/wpilib_interface.cc
+++ b/y2022_bot3/wpilib_interface.cc
@@ -23,9 +23,6 @@
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
-
#include "aos/commonmath.h"
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index abf11da..b4d3acb 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -24,8 +24,6 @@
#undef ERROR
#include "ctre/phoenix/cci/Diagnostics_CCI.h"
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "ctre/phoenix6/TalonFX.hpp"
#include "aos/commonmath.h"
@@ -716,16 +714,27 @@
}
static void WriteCan(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ ::ctre::phoenix6::hardware::TalonFX *talon) {
+ ctre::phoenix6::controls::DutyCycleOut motor_control(SafeSpeed(voltage));
+
+ motor_control.UpdateFreqHz = 0_Hz;
+ motor_control.EnableFOC = true;
+
+ ctre::phoenix::StatusCode status = talon->SetControl(motor_control);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
}
template <typename T>
static void WritePwm(const double voltage, T *motor) {
- motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ motor->SetSpeed(SafeSpeed(voltage));
+ }
+
+ static double SafeSpeed(double voltage) {
+ return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
}
::std::unique_ptr<::frc::TalonFX> proximal_falcon_, distal_falcon_;
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index af560e5..e91f6e7 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -24,8 +24,6 @@
#undef ERROR
#include "ctre/phoenix/cci/Diagnostics_CCI.h"
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "ctre/phoenix6/TalonFX.hpp"
#include "aos/commonmath.h"
@@ -599,42 +597,6 @@
CANSensorReader *can_sensor_reader_;
};
-class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
- public:
- SuperstructureWriter(aos::EventLoop *event_loop)
- : frc971::wpilib::LoopOutputHandler<superstructure::Output>(
- event_loop, "/superstructure") {
- event_loop->SetRuntimeRealtimePriority(
- constants::Values::kDrivetrainWriterPriority);
- }
-
- std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
-
- void set_superstructure_reading(
- std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
- superstructure_reading_ = superstructure_reading;
- }
-
- private:
- void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
-
- void Write(const superstructure::Output &output) override { (void)output; }
-
- static void WriteCan(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
- }
-
- template <typename T>
- static void WritePwm(const double voltage, T *motor) {
- motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- }
-};
-
class SuperstructureCANWriter
: public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
@@ -926,16 +888,6 @@
AddLoop(&can_output_event_loop);
- // Thread 6
- // Set up superstructure output.
- ::aos::ShmEventLoop output_event_loop(&config.message());
- output_event_loop.set_name("PWMOutputWriter");
- SuperstructureWriter superstructure_writer(&output_event_loop);
-
- superstructure_writer.set_superstructure_reading(superstructure_reading);
-
- AddLoop(&output_event_loop);
-
RunLoops();
}
};
@@ -943,4 +895,4 @@
} // namespace wpilib
} // namespace y2023_bot3
-AOS_ROBOT_CLASS(::y2023_bot3::wpilib::WPILibRobot);
\ No newline at end of file
+AOS_ROBOT_CLASS(::y2023_bot3::wpilib::WPILibRobot);