Merge "Add Kraken Motor Class to control_loop.py"
diff --git a/WORKSPACE b/WORKSPACE
index 15e71d8..ec5e8e0 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1643,3 +1643,15 @@
load("@hedron_compile_commands//:workspace_setup.bzl", "hedron_compile_commands_setup")
hedron_compile_commands_setup()
+
+http_archive(
+ name = "calibrate_multi_cameras_data",
+ build_file_content = """
+filegroup(
+ name = "calibrate_multi_cameras_data",
+ srcs = glob(["**"]),
+ visibility = ["//visibility:public"],
+)""",
+ sha256 = "b106b3b975d3cf3ad3fcd5e4be7409f6095e1d531346a90c4ad6bdb7da1d08a5",
+ url = "https://software.frc971.org/Build-Dependencies/2023_calibrate_multi_cameras_data.tar.gz",
+)
diff --git a/frc971/can_configuration.fbs b/frc971/can_configuration.fbs
index 4ce70ab..c85b0e8 100644
--- a/frc971/can_configuration.fbs
+++ b/frc971/can_configuration.fbs
@@ -3,7 +3,7 @@
// Message which triggers wpilib_interface to print out the current
// configuration, and optionally re-apply it.
table CANConfiguration {
- // If true, re-apply the configs to see if that fixes the falcon.
+ // If true, re-apply the configs to see if that fixes the TalonFX Motor.
reapply:bool = false (id: 0);
}
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 1a98691..53d6b27 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -200,16 +200,16 @@
)
flatbuffer_ts_library(
- name = "can_falcon_ts_fbs",
+ name = "can_talonfx_ts_fbs",
srcs = [
- "can_falcon.fbs",
+ "can_talonfx.fbs",
],
)
static_flatbuffer(
- name = "can_falcon_fbs",
+ name = "can_talonfx_fbs",
srcs = [
- "can_falcon.fbs",
+ "can_talonfx.fbs",
],
)
diff --git a/frc971/control_loops/can_falcon.fbs b/frc971/control_loops/can_talonfx.fbs
similarity index 90%
rename from frc971/control_loops/can_falcon.fbs
rename to frc971/control_loops/can_talonfx.fbs
index 2adbae0..81efbb1 100644
--- a/frc971/control_loops/can_falcon.fbs
+++ b/frc971/control_loops/can_talonfx.fbs
@@ -1,7 +1,7 @@
namespace frc971.control_loops;
-table CANFalcon {
- // The CAN id of the falcon
+table CANTalonFX {
+ // The CAN id of the talonfx motor
id:int (id: 0);
// In Amps
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 424c257..7a25bcd 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -10,7 +10,7 @@
static_flatbuffer(
name = "drivetrain_can_position_fbs",
srcs = ["drivetrain_can_position.fbs"],
- deps = ["//frc971/control_loops:can_falcon_fbs"],
+ deps = ["//frc971/control_loops:can_talonfx_fbs"],
)
static_flatbuffer(
@@ -74,7 +74,7 @@
name = "drivetrain_can_position_ts_fbs",
srcs = ["drivetrain_can_position.fbs"],
target_compatible_with = ["@platforms//os:linux"],
- deps = ["//frc971/control_loops:can_falcon_ts_fbs"],
+ deps = ["//frc971/control_loops:can_talonfx_ts_fbs"],
)
genrule(
diff --git a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
index 192f23d..b451ea9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -1,9 +1,9 @@
-include "frc971/control_loops/can_falcon.fbs";
+include "frc971/control_loops/can_talonfx.fbs";
namespace frc971.control_loops.drivetrain;
// CAN readings from the CAN sensor reader loop
table CANPosition {
- falcons: [CANFalcon] (id: 0);
+ talonfxs: [CANTalonFX] (id: 0);
// The timestamp of the measurement on the canivore clock in nanoseconds
// This will have less jitter than the
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index caa8e63..2cf2109 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -492,12 +492,12 @@
)
cc_library(
- name = "falcon",
+ name = "talonfx",
srcs = [
- "falcon.cc",
+ "talonfx.cc",
],
hdrs = [
- "falcon.h",
+ "talonfx.h",
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
@@ -523,8 +523,8 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
- ":falcon",
":loop_output_handler",
+ ":talonfx",
"//frc971:can_configuration_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
],
@@ -540,8 +540,8 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
- ":falcon",
":loop_output_handler",
+ ":talonfx",
"//frc971:can_configuration_fbs",
],
)
@@ -553,7 +553,7 @@
target_compatible_with = ["//tools/platforms/hardware:roborio"],
visibility = ["//visibility:public"],
deps = [
- ":falcon",
+ ":talonfx",
"//aos:realtime",
"//aos/containers:sized_array",
"//aos/events:event_loop",
diff --git a/frc971/wpilib/can_drivetrain_writer.cc b/frc971/wpilib/can_drivetrain_writer.cc
index bdff308..ae82812 100644
--- a/frc971/wpilib/can_drivetrain_writer.cc
+++ b/frc971/wpilib/can_drivetrain_writer.cc
@@ -11,21 +11,21 @@
event_loop->OnRun([this]() { WriteConfigs(); });
}
-void CANDrivetrainWriter::set_falcons(
- std::vector<std::shared_ptr<Falcon>> right_falcons,
- std::vector<std::shared_ptr<Falcon>> left_falcons) {
- right_falcons_ = std::move(right_falcons);
- left_falcons_ = std::move(left_falcons);
+void CANDrivetrainWriter::set_talonfxs(
+ std::vector<std::shared_ptr<TalonFX>> right_talonfxs,
+ std::vector<std::shared_ptr<TalonFX>> left_talonfxs) {
+ right_talonfxs_ = std::move(right_talonfxs);
+ left_talonfxs_ = std::move(left_talonfxs);
}
void CANDrivetrainWriter::HandleCANConfiguration(
const CANConfiguration &configuration) {
- for (auto falcon : right_falcons_) {
- falcon->PrintConfigs();
+ for (auto talonfx : right_talonfxs_) {
+ talonfx->PrintConfigs();
}
- for (auto falcon : left_falcons_) {
- falcon->PrintConfigs();
+ for (auto talonfx : left_talonfxs_) {
+ talonfx->PrintConfigs();
}
if (configuration.reapply()) {
@@ -34,33 +34,33 @@
}
void CANDrivetrainWriter::WriteConfigs() {
- for (auto falcon : right_falcons_) {
- falcon->WriteConfigs();
+ for (auto talonfx : right_talonfxs_) {
+ talonfx->WriteConfigs();
}
- for (auto falcon : left_falcons_) {
- falcon->WriteConfigs();
+ for (auto talonfx : left_talonfxs_) {
+ talonfx->WriteConfigs();
}
}
void CANDrivetrainWriter::Write(
const ::frc971::control_loops::drivetrain::Output &output) {
- for (auto falcon : right_falcons_) {
- falcon->WriteVoltage(output.right_voltage());
+ for (auto talonfx : right_talonfxs_) {
+ talonfx->WriteVoltage(output.right_voltage());
}
- for (auto falcon : left_falcons_) {
- falcon->WriteVoltage(output.left_voltage());
+ for (auto talonfx : left_talonfxs_) {
+ talonfx->WriteVoltage(output.left_voltage());
}
}
void CANDrivetrainWriter::Stop() {
AOS_LOG(WARNING, "Drivetrain CAN output too old.\n");
- for (auto falcon : right_falcons_) {
- falcon->WriteVoltage(0);
+ for (auto talonfx : right_talonfxs_) {
+ talonfx->WriteVoltage(0);
}
- for (auto falcon : left_falcons_) {
- falcon->WriteVoltage(0);
+ for (auto talonfx : left_talonfxs_) {
+ talonfx->WriteVoltage(0);
}
}
diff --git a/frc971/wpilib/can_drivetrain_writer.h b/frc971/wpilib/can_drivetrain_writer.h
index bd396bf..1c0ba95 100644
--- a/frc971/wpilib/can_drivetrain_writer.h
+++ b/frc971/wpilib/can_drivetrain_writer.h
@@ -1,7 +1,7 @@
#include "frc971/can_configuration_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
-#include "frc971/wpilib/falcon.h"
#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/talonfx.h"
namespace frc971 {
namespace wpilib {
@@ -11,8 +11,8 @@
public:
CANDrivetrainWriter(::aos::EventLoop *event_loop);
- void set_falcons(std::vector<std::shared_ptr<Falcon>> right_falcons,
- std::vector<std::shared_ptr<Falcon>> left_falcons);
+ void set_talonfxs(std::vector<std::shared_ptr<TalonFX>> right_talonfxs,
+ std::vector<std::shared_ptr<TalonFX>> left_talonfxs);
void HandleCANConfiguration(const CANConfiguration &configuration);
@@ -26,8 +26,8 @@
void Stop() override;
- std::vector<std::shared_ptr<Falcon>> right_falcons_;
- std::vector<std::shared_ptr<Falcon>> left_falcons_;
+ std::vector<std::shared_ptr<TalonFX>> right_talonfxs_;
+ std::vector<std::shared_ptr<TalonFX>> left_talonfxs_;
};
} // namespace wpilib
diff --git a/frc971/wpilib/can_sensor_reader.cc b/frc971/wpilib/can_sensor_reader.cc
index ad244fb..54be05d 100644
--- a/frc971/wpilib/can_sensor_reader.cc
+++ b/frc971/wpilib/can_sensor_reader.cc
@@ -6,11 +6,11 @@
CANSensorReader::CANSensorReader(
aos::EventLoop *event_loop,
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry,
- std::vector<std::shared_ptr<Falcon>> falcons,
+ std::vector<std::shared_ptr<TalonFX>> talonfxs,
std::function<void(ctre::phoenix::StatusCode status)> flatbuffer_callback)
: event_loop_(event_loop),
signals_(signals_registry.begin(), signals_registry.end()),
- falcons_(falcons),
+ talonfxs_(talonfxs),
flatbuffer_callback_(flatbuffer_callback) {
event_loop->SetRuntimeRealtimePriority(40);
@@ -32,7 +32,7 @@
ctre::phoenix6::BaseStatusSignal::WaitForAll(20_ms, signals_);
if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
+ AOS_LOG(ERROR, "Failed to read signals from talonfx motors: %s: %s",
status.GetName(), status.GetDescription());
}
diff --git a/frc971/wpilib/can_sensor_reader.h b/frc971/wpilib/can_sensor_reader.h
index 764a041..8ef5fd1 100644
--- a/frc971/wpilib/can_sensor_reader.h
+++ b/frc971/wpilib/can_sensor_reader.h
@@ -7,7 +7,7 @@
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
#include "aos/realtime.h"
-#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/talonfx.h"
namespace frc971 {
namespace wpilib {
@@ -16,7 +16,7 @@
CANSensorReader(
aos::EventLoop *event_loop,
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry,
- std::vector<std::shared_ptr<Falcon>> falcons,
+ std::vector<std::shared_ptr<TalonFX>> talonfxs,
std::function<void(ctre::phoenix::StatusCode status)>
flatbuffer_callback);
@@ -27,9 +27,9 @@
const std::vector<ctre::phoenix6::BaseStatusSignal *> signals_;
- // This is a vector of falcons becuase we don't need to care
- // about falcons individually.
- std::vector<std::shared_ptr<Falcon>> falcons_;
+ // This is a vector of talonfxs becuase we don't need to care
+ // about talonfxs individually.
+ std::vector<std::shared_ptr<TalonFX>> talonfxs_;
// Pointer to the timer handler used to modify the wakeup.
::aos::TimerHandler *timer_handler_;
diff --git a/frc971/wpilib/generic_can_writer.h b/frc971/wpilib/generic_can_writer.h
index 8bacfbd..7555c0a 100644
--- a/frc971/wpilib/generic_can_writer.h
+++ b/frc971/wpilib/generic_can_writer.h
@@ -2,14 +2,14 @@
#include <string_view>
#include "frc971/can_configuration_generated.h"
-#include "frc971/wpilib/falcon.h"
#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/talonfx.h"
namespace frc971 {
namespace wpilib {
/// This class uses a callback whenever it writes so that the caller can use any
-/// flatbuffer to write to the falcon.
+/// flatbuffer to write to the talonfx motor.
template <typename T>
class GenericCANWriter : public LoopOutputHandler<T> {
public:
@@ -17,7 +17,7 @@
::aos::EventLoop *event_loop,
std::function<
void(const T &output,
- std::map<std::string, std::shared_ptr<Falcon>> falcon_map)>
+ std::map<std::string, std::shared_ptr<TalonFX>> talonfx_map)>
write_callback)
: LoopOutputHandler<T>(event_loop, "/superstructure"),
write_callback_(write_callback) {
@@ -27,8 +27,8 @@
}
void HandleCANConfiguration(const CANConfiguration &configuration) {
- for (auto &[_, falcon] : falcon_map_) {
- falcon->PrintConfigs();
+ for (auto &[_, talonfx] : talonfx_map_) {
+ talonfx->PrintConfigs();
}
if (configuration.reapply()) {
@@ -36,33 +36,37 @@
}
}
- void add_falcon(std::string_view name, std::shared_ptr<Falcon> falcon) {
- falcon_map_.insert({name, std::move(falcon)});
+ void add_talonfx(std::string_view name, std::shared_ptr<TalonFX> talonfx) {
+ talonfx_map_.insert({name, std::move(talonfx)});
}
static constexpr int kGenericCANWriterPriority = 35;
private:
void WriteConfigs() {
- for (auto &[_, falcon] : falcon_map_) {
- falcon->WriteConfigs();
+ for (auto &[_, talonfx] : talonfx_map_) {
+ talonfx->WriteConfigs();
}
}
- void Write(const T &output) override { write_callback_(output, falcon_map_); }
+ void Write(const T &output) override {
+ write_callback_(output, talonfx_map_);
+ }
void Stop() override {
AOS_LOG(WARNING, "Generic CAN output too old.\n");
- for (auto &[_, falcon] : falcon_map_) {
- falcon->WriteVoltage(0);
+ for (auto &[_, talonfx] : talonfx_map_) {
+ talonfx->WriteVoltage(0);
}
}
- // Maps each name to a falcon to let the caller retreive them when writing
- std::map<std::string_view, std::shared_ptr<Falcon>> falcon_map_;
+ // Maps each name to a talonfx to let the caller retreive them when
+ // writing
+ std::map<std::string_view, std::shared_ptr<TalonFX>> talonfx_map_;
- std::function<void(const T &output,
- std::map<std::string, std::shared_ptr<Falcon>> falcon_map)>
+ std::function<void(
+ const T &output,
+ std::map<std::string, std::shared_ptr<TalonFX>> talonfx_map)>
write_callback_;
};
diff --git a/frc971/wpilib/swerve/BUILD b/frc971/wpilib/swerve/BUILD
index 9f559f6..4493781 100644
--- a/frc971/wpilib/swerve/BUILD
+++ b/frc971/wpilib/swerve/BUILD
@@ -13,8 +13,8 @@
"//aos/logging",
"//frc971:can_configuration_fbs",
"//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
- "//frc971/wpilib:falcon",
"//frc971/wpilib:loop_output_handler",
+ "//frc971/wpilib:talonfx",
"//third_party:phoenix6",
],
)
@@ -25,6 +25,6 @@
"swerve_module.h",
],
deps = [
- "//frc971/wpilib:falcon",
+ "//frc971/wpilib:talonfx",
],
)
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
index bfc5d73..e4b4ee9 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
@@ -14,10 +14,10 @@
event_loop->OnRun([this]() { WriteConfigs(); });
}
-void DrivetrainWriter::set_falcons(std::shared_ptr<SwerveModule> front_left,
- std::shared_ptr<SwerveModule> front_right,
- std::shared_ptr<SwerveModule> back_left,
- std::shared_ptr<SwerveModule> back_right) {
+void DrivetrainWriter::set_talonfxs(std::shared_ptr<SwerveModule> front_left,
+ std::shared_ptr<SwerveModule> front_right,
+ std::shared_ptr<SwerveModule> back_left,
+ std::shared_ptr<SwerveModule> back_right) {
front_left_ = std::move(front_left);
front_right_ = std::move(front_right);
back_left_ = std::move(back_left);
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.h b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
index 4bd6639..d02d4dc 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.h
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
@@ -5,9 +5,9 @@
#include "frc971/can_configuration_generated.h"
#include "frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output_generated.h"
-#include "frc971/wpilib/falcon.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/swerve/swerve_module.h"
+#include "frc971/wpilib/talonfx.h"
namespace frc971 {
namespace wpilib {
@@ -22,10 +22,10 @@
DrivetrainWriter(::aos::EventLoop *event_loop, int drivetrain_writer_priority,
double max_voltage);
- void set_falcons(std::shared_ptr<SwerveModule> front_left,
- std::shared_ptr<SwerveModule> front_right,
- std::shared_ptr<SwerveModule> back_left,
- std::shared_ptr<SwerveModule> back_right);
+ void set_talonfxs(std::shared_ptr<SwerveModule> front_left,
+ std::shared_ptr<SwerveModule> front_right,
+ std::shared_ptr<SwerveModule> back_left,
+ std::shared_ptr<SwerveModule> back_right);
void HandleCANConfiguration(const CANConfiguration &configuration);
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index f449afa..d65547a 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -1,23 +1,23 @@
#ifndef FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
#define FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
-#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/talonfx.h"
namespace frc971 {
namespace wpilib {
namespace swerve {
struct SwerveModule {
- SwerveModule(FalconParams rotation_params, FalconParams translation_params,
+ SwerveModule(TalonFXParams rotation_params, TalonFXParams translation_params,
std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit)
- : rotation(std::make_shared<Falcon>(rotation_params, canbus, signals,
- stator_current_limit,
- supply_current_limit)),
- translation(std::make_shared<Falcon>(translation_params, canbus,
- signals, stator_current_limit,
- supply_current_limit)) {}
+ : rotation(std::make_shared<TalonFX>(rotation_params, canbus, signals,
+ stator_current_limit,
+ supply_current_limit)),
+ translation(std::make_shared<TalonFX>(translation_params, canbus,
+ signals, stator_current_limit,
+ supply_current_limit)) {}
void WriteModule(
const frc971::control_loops::drivetrain::swerve::SwerveModuleOutput
@@ -35,8 +35,8 @@
translation->WriteCurrent(translation_current, max_voltage);
}
- std::shared_ptr<Falcon> rotation;
- std::shared_ptr<Falcon> translation;
+ std::shared_ptr<TalonFX> rotation;
+ std::shared_ptr<TalonFX> translation;
};
} // namespace swerve
diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/talonfx.cc
similarity index 68%
rename from frc971/wpilib/falcon.cc
rename to frc971/wpilib/talonfx.cc
index deeb9f8..dd30f9d 100644
--- a/frc971/wpilib/falcon.cc
+++ b/frc971/wpilib/talonfx.cc
@@ -1,11 +1,11 @@
-#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/talonfx.h"
-using frc971::wpilib::Falcon;
using frc971::wpilib::kMaxBringupPower;
+using frc971::wpilib::TalonFX;
-Falcon::Falcon(int device_id, bool inverted, std::string canbus,
- std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
- double stator_current_limit, double supply_current_limit)
+TalonFX::TalonFX(int device_id, bool inverted, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit)
: talon_(device_id, canbus),
device_id_(device_id),
inverted_(inverted),
@@ -38,24 +38,24 @@
signals->push_back(&duty_cycle_);
}
-Falcon::Falcon(FalconParams params, std::string canbus,
- std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
- double stator_current_limit, double supply_current_limit)
- : Falcon(params.device_id, params.inverted, canbus, signals,
- stator_current_limit, supply_current_limit) {}
+TalonFX::TalonFX(TalonFXParams params, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit)
+ : TalonFX(params.device_id, params.inverted, canbus, signals,
+ stator_current_limit, supply_current_limit) {}
-void Falcon::PrintConfigs() {
+void TalonFX::PrintConfigs() {
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",
+ AOS_LOG(ERROR, "Failed to get talonfx motor configuration: %s: %s",
status.GetName(), status.GetDescription());
}
AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
}
-void Falcon::WriteConfigs() {
+void TalonFX::WriteConfigs() {
ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
current_limits.StatorCurrentLimit = stator_current_limit_;
current_limits.StatorCurrentLimitEnable = true;
@@ -75,15 +75,15 @@
ctre::phoenix::StatusCode status =
talon_.GetConfigurator().Apply(configuration);
if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ AOS_LOG(ERROR, "Failed to set talonfx motor configuration: %s: %s",
status.GetName(), status.GetDescription());
}
PrintConfigs();
}
-ctre::phoenix::StatusCode Falcon::WriteCurrent(double current,
- double max_voltage) {
+ctre::phoenix::StatusCode TalonFX::WriteCurrent(double current,
+ double max_voltage) {
ctre::phoenix6::controls::TorqueCurrentFOC control(
static_cast<units::current::ampere_t>(current));
// Using 0_Hz here makes it a one-shot update.
@@ -91,14 +91,14 @@
control.MaxAbsDutyCycle = SafeSpeed(max_voltage);
ctre::phoenix::StatusCode status = talon()->SetControl(control);
if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon %d: %s: %s", device_id(),
- status.GetName(), status.GetDescription());
+ AOS_LOG(ERROR, "Failed to write control to talonfx motor %d: %s: %s",
+ device_id(), status.GetName(), status.GetDescription());
}
return status;
}
-ctre::phoenix::StatusCode Falcon::WriteVoltage(double voltage) {
+ctre::phoenix::StatusCode TalonFX::WriteVoltage(double voltage) {
ctre::phoenix6::controls::DutyCycleOut control(SafeSpeed(voltage));
// Using 0_Hz here makes it a one-shot update.
@@ -107,16 +107,16 @@
ctre::phoenix::StatusCode status = talon()->SetControl(control);
if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon %d: %s: %s", device_id(),
- status.GetName(), status.GetDescription());
+ AOS_LOG(ERROR, "Failed to write control to talonfx motor %d: %s: %s",
+ device_id(), status.GetName(), status.GetDescription());
}
return status;
}
-void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
- double gear_ratio) {
- control_loops::CANFalcon::Builder builder(*fbb);
+void TalonFX::SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ double gear_ratio) {
+ control_loops::CANTalonFX::Builder builder(*fbb);
builder.add_id(device_id_);
builder.add_device_temp(device_temp());
builder.add_supply_voltage(supply_voltage());
@@ -128,8 +128,8 @@
last_position_offset_ = builder.Finish();
}
-std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
-Falcon::TakeOffset() {
+std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
+TalonFX::TakeOffset() {
auto option_offset = last_position_offset_;
last_position_offset_.reset();
diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/talonfx.h
similarity index 76%
rename from frc971/wpilib/falcon.h
rename to frc971/wpilib/talonfx.h
index 804c7a0..603f4a2 100644
--- a/frc971/wpilib/falcon.h
+++ b/frc971/wpilib/talonfx.h
@@ -1,5 +1,5 @@
-#ifndef FRC971_WPILIB_FALCON_H_
-#define FRC971_WPILIB_FALCON_H_
+#ifndef FRC971_WPILIB_TALONFX_MOTOR_H_
+#define FRC971_WPILIB_TALONFX_MOTOR_H_
#include <chrono>
#include <cinttypes>
@@ -18,7 +18,7 @@
namespace frc971 {
namespace wpilib {
-struct FalconParams {
+struct TalonFXParams {
int device_id;
bool inverted;
};
@@ -26,16 +26,17 @@
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
static constexpr double kMaxBringupPower = 12.0;
-// Gets info from and writes to falcon motors using the TalonFX controller.
-class Falcon {
+// Class which represents a motor controlled by a TalonFX motor controller over
+// CAN.
+class TalonFX {
public:
- Falcon(int device_id, bool inverted, std::string canbus,
- std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
- double stator_current_limit, double supply_current_limit);
+ TalonFX(int device_id, bool inverted, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit);
- Falcon(FalconParams params, std::string canbus,
- std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
- double stator_current_limit, double supply_current_limit);
+ TalonFX(TalonFXParams params, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit);
void PrintConfigs();
@@ -46,11 +47,11 @@
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
- // The position of the Falcon output shaft is multiplied by gear_ratio
+ // The position of the TalonFX output shaft is multiplied by gear_ratio
void SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
double gear_ratio);
- std::optional<flatbuffers::Offset<control_loops::CANFalcon>> TakeOffset();
+ std::optional<flatbuffers::Offset<control_loops::CANTalonFX>> TakeOffset();
int device_id() const { return device_id_; }
float device_temp() const { return device_temp_.GetValue().value(); }
@@ -101,9 +102,9 @@
double stator_current_limit_;
double supply_current_limit_;
- std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
+ std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
last_position_offset_;
};
} // namespace wpilib
} // namespace frc971
-#endif // FRC971_WPILIB_FALCON_H_
+#endif // FRC971_WPILIB_TALONFX_MOTOR_H_
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 82b804b..07911a8 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -263,6 +263,7 @@
"//frc971/control_loops:pose",
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
+ "//frc971/vision:extrinsics_calibration",
"//frc971/vision:target_mapper",
"//third_party:opencv",
"//y2023/constants:constants_fbs",
@@ -271,6 +272,23 @@
],
)
+py_test(
+ name = "calibrate_multi_cameras_test",
+ srcs = ["calibrate_multi_cameras_test.py"],
+ args = [
+ "--calibrate_binary",
+ "$(location :calibrate_multi_cameras)",
+ "--logfile",
+ "external/calibrate_multi_cameras_data/fbs_log-268",
+ ],
+ data = [
+ ":calibrate_multi_cameras",
+ "//y2023/vision/calib_files",
+ "@calibrate_multi_cameras_data",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+)
+
cc_binary(
name = "game_pieces_detector",
srcs = [
diff --git a/y2023/vision/calibrate_multi_cameras.cc b/y2023/vision/calibrate_multi_cameras.cc
index 4cd70c2..d3cfa6f 100644
--- a/y2023/vision/calibrate_multi_cameras.cc
+++ b/y2023/vision/calibrate_multi_cameras.cc
@@ -9,6 +9,7 @@
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/extrinsics_calibration.h"
#include "frc971/vision/target_mapper.h"
#include "frc971/vision/visualize_robot.h"
// clang-format off
@@ -31,14 +32,26 @@
"If set, override the log's config file with this one.");
DEFINE_string(constants_path, "y2023/constants/constants.json",
"Path to the constant file");
+DEFINE_double(max_pose_error, 5e-5,
+ "Throw out target poses with a higher pose error than this");
+DEFINE_double(
+ max_pose_error_ratio, 0.4,
+ "Throw out target poses with a higher pose error ratio than this");
+DEFINE_string(output_folder, "/tmp",
+ "Directory in which to store the updated calibration files");
DEFINE_string(target_type, "charuco_diamond",
"Type of target being used [aruco, charuco, charuco_diamond]");
DEFINE_int32(team_number, 0,
"Required: Use the calibration for a node with this team number");
+DEFINE_bool(use_full_logs, false,
+ "If true, extract data from logs with images");
DEFINE_uint64(
wait_key, 1,
"Time in ms to wait between images (0 to wait indefinitely until click)");
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+
// Calibrate extrinsic relationship between cameras using two targets
// seen jointly between cameras. Uses two types of information: 1)
// when a single camera sees two targets, we estimate the pose between
@@ -50,7 +63,6 @@
// and then map each subsequent camera based on the data collected and
// the extrinsic poses computed here.
-// TODO<Jim>: Should export a new extrinsic file for each camera
// TODO<Jim>: Not currently using estimate from pi1->pi4-- should do full
// estimation, and probably also include camera->imu extrinsics from all
// cameras, not just pi1
@@ -159,20 +171,10 @@
translation_variance, rotation_variance);
}
-void ProcessImage(aos::EventLoop *event_loop, cv::Mat rgb_image,
- const aos::monotonic_clock::time_point eof,
- aos::distributed_clock::time_point distributed_eof,
- frc971::vision::CharucoExtractor &charuco_extractor,
- std::string pi_name) {
- std::vector<cv::Vec4i> charuco_ids;
- std::vector<std::vector<cv::Point2f>> charuco_corners;
- bool valid = false;
- std::vector<Eigen::Vector3d> rvecs_eigen;
- std::vector<Eigen::Vector3d> tvecs_eigen;
- charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(),
- charuco_ids, charuco_corners, valid,
- rvecs_eigen, tvecs_eigen);
-
+void HandlePoses(cv::Mat rgb_image,
+ std::vector<TargetMapper::TargetPose> target_poses,
+ aos::distributed_clock::time_point distributed_eof,
+ std::string node_name) {
// This is used to transform points for visualization
// Assumes targets are aligned with x->right, y->up, z->out of board
Eigen::Affine3d H_world_board;
@@ -184,178 +186,228 @@
}
bool draw_vis = false;
- if (valid) {
- CHECK_LE(tvecs_eigen.size(), 2u)
- << "Can't handle more than two tags in field of view";
- if (tvecs_eigen.size() == 2) {
- draw_vis = true;
- VLOG(2) << "Saw two boards in same view from " << pi_name;
- // Handle when we see two boards at once
- // We'll store them referenced to the lower id board
- int from_index = 0;
- int to_index = 1;
- if (charuco_ids[from_index][0] > charuco_ids[to_index][0]) {
- std::swap<int>(from_index, to_index);
+ CHECK_LE(target_poses.size(), 2u)
+ << "Can't handle more than two tags in field of view";
+ if (target_poses.size() == 2) {
+ draw_vis = true;
+ VLOG(1) << "Saw two boards in same view from " << node_name;
+ int from_index = 0;
+ int to_index = 1;
+ // Handle when we see two boards at once
+ // We'll store them referenced to the lower id board
+ if (target_poses[from_index].id > target_poses[to_index].id) {
+ std::swap<int>(from_index, to_index);
+ }
+
+ // Create "from" (A) and "to" (B) transforms
+ Eigen::Affine3d H_camera_boardA =
+ PoseUtils::Pose3dToAffine3d(target_poses[from_index].pose);
+ Eigen::Affine3d H_camera_boardB =
+ PoseUtils::Pose3dToAffine3d(target_poses[to_index].pose);
+
+ Eigen::Affine3d H_boardA_boardB =
+ H_camera_boardA.inverse() * H_camera_boardB;
+
+ TimestampedPiDetection boardA_boardB{
+ .time = distributed_eof,
+ .H_camera_target = H_boardA_boardB,
+ .pi_name = node_name,
+ .board_id = target_poses[from_index].id};
+
+ VLOG(1) << "Map from board " << from_index << " to " << to_index << " is\n"
+ << H_boardA_boardB.matrix();
+ // Store this observation of the transform between two boards
+ two_board_extrinsics_list.push_back(boardA_boardB);
+
+ if (FLAGS_visualize) {
+ vis_robot_.DrawFrameAxes(
+ H_world_board,
+ std::string("board ") + std::to_string(target_poses[from_index].id),
+ cv::Scalar(0, 255, 0));
+ vis_robot_.DrawFrameAxes(
+ H_world_board * boardA_boardB.H_camera_target,
+ std::string("board ") + std::to_string(target_poses[to_index].id),
+ cv::Scalar(255, 0, 0));
+ VLOG(2) << "Detection map from camera " << node_name << " to board "
+ << target_poses[from_index].id << " is\n"
+ << H_camera_boardA.matrix() << "\n and inverse is\n"
+ << H_camera_boardA.inverse().matrix()
+ << "\n and with world to board rotation is\n"
+ << H_world_board * H_camera_boardA.inverse().matrix();
+ vis_robot_.DrawRobotOutline(H_world_board * H_camera_boardA.inverse(),
+ node_name, cv::Scalar(0, 0, 255));
+ }
+ } else if (target_poses.size() == 1) {
+ VLOG(1) << "Saw single board in camera " << node_name;
+ Eigen::Affine3d H_camera2_board2 =
+ PoseUtils::Pose3dToAffine3d(target_poses[0].pose);
+ TimestampedPiDetection new_observation{.time = distributed_eof,
+ .H_camera_target = H_camera2_board2,
+ .pi_name = node_name,
+ .board_id = target_poses[0].id};
+
+ // Only take two observations if they're within half an image cycle of each
+ // other (i.e., as close in time as possible)
+ if (std::abs((distributed_eof - last_observation.time).count()) <
+ kImagePeriodMs / 2.0 * 1000000.0) {
+ // Sort by pi numbering, since this is how we will handle them
+ std::pair<TimestampedPiDetection, TimestampedPiDetection> new_pair;
+ if (last_observation.pi_name < new_observation.pi_name) {
+ new_pair = std::pair(last_observation, new_observation);
+ } else if (last_observation.pi_name > new_observation.pi_name) {
+ new_pair = std::pair(new_observation, last_observation);
+ } else {
+ LOG(WARNING) << "Got 2 observations in a row from same pi. Probably "
+ "not too much of an issue???";
}
+ detection_list.push_back(new_pair);
- // Create "from" (A) and "to" (B) transforms
- Eigen::Quaternion<double> rotationA(
- frc971::controls::ToQuaternionFromRotationVector(
- rvecs_eigen[from_index]));
- Eigen::Translation3d translationA(tvecs_eigen[from_index]);
- Eigen::Affine3d H_camera_boardA = translationA * rotationA;
+ // This bit is just for visualization and checking purposes-- use the
+ // last two-board observation to figure out the current estimate
+ // between the two cameras
+ if (FLAGS_visualize && two_board_extrinsics_list.size() > 0) {
+ draw_vis = true;
+ TimestampedPiDetection &last_two_board_ext =
+ two_board_extrinsics_list[two_board_extrinsics_list.size() - 1];
+ Eigen::Affine3d &H_boardA_boardB = last_two_board_ext.H_camera_target;
+ int boardA_boardB_id = last_two_board_ext.board_id;
- Eigen::Quaternion<double> rotationB(
- frc971::controls::ToQuaternionFromRotationVector(
- rvecs_eigen[to_index]));
- Eigen::Translation3d translationB(tvecs_eigen[to_index]);
- Eigen::Affine3d H_camera_boardB = translationB * rotationB;
+ TimestampedPiDetection camera1_boardA = new_pair.first;
+ TimestampedPiDetection camera2_boardB = new_pair.second;
+ // If camera1_boardA doesn't point to the first target, then swap
+ // these two
+ if (camera1_boardA.board_id != boardA_boardB_id) {
+ camera1_boardA = new_pair.second;
+ camera2_boardB = new_pair.first;
+ }
+ VLOG(1) << "Camera " << camera1_boardA.pi_name << " seeing board "
+ << camera1_boardA.board_id << " and camera "
+ << camera2_boardB.pi_name << " seeing board "
+ << camera2_boardB.board_id;
- Eigen::Affine3d H_boardA_boardB =
- H_camera_boardA.inverse() * H_camera_boardB;
-
- TimestampedPiDetection boardA_boardB{
- .time = distributed_eof,
- .H_camera_target = H_boardA_boardB,
- .pi_name = pi_name,
- .board_id = charuco_ids[from_index][0]};
-
- VLOG(1) << "Map from board " << from_index << " to " << to_index
- << " is\n"
- << H_boardA_boardB.matrix();
- // Store this observation of the transform between two boards
- two_board_extrinsics_list.push_back(boardA_boardB);
-
- if (FLAGS_visualize) {
+ vis_robot_.DrawRobotOutline(
+ H_world_board * camera1_boardA.H_camera_target.inverse(),
+ camera1_boardA.pi_name, cv::Scalar(0, 0, 255));
+ vis_robot_.DrawRobotOutline(
+ H_world_board * H_boardA_boardB *
+ camera2_boardB.H_camera_target.inverse(),
+ camera2_boardB.pi_name, cv::Scalar(128, 128, 0));
vis_robot_.DrawFrameAxes(
H_world_board,
- std::string("board ") + std::to_string(charuco_ids[from_index][0]),
+ std::string("Board ") + std::to_string(last_two_board_ext.board_id),
cv::Scalar(0, 255, 0));
- vis_robot_.DrawFrameAxes(
- H_world_board * boardA_boardB.H_camera_target,
- std::string("board ") + std::to_string(charuco_ids[to_index][0]),
- cv::Scalar(255, 0, 0));
- VLOG(2) << "Detection map from camera " << pi_name << " to board "
- << charuco_ids[from_index][0] << " is\n"
- << H_camera_boardA.matrix() << "\n and inverse is\n"
- << H_camera_boardA.inverse().matrix()
- << "\n and with world to board rotation is\n"
- << H_world_board * H_camera_boardA.inverse().matrix();
- vis_robot_.DrawRobotOutline(H_world_board * H_camera_boardA.inverse(),
- pi_name, cv::Scalar(0, 0, 255));
+ vis_robot_.DrawFrameAxes(H_world_board * H_boardA_boardB, "Board B",
+ cv::Scalar(255, 0, 0));
}
-
+ VLOG(1) << "Storing observation between " << new_pair.first.pi_name
+ << ", target " << new_pair.first.board_id << " and "
+ << new_pair.second.pi_name << ", target "
+ << new_pair.second.board_id;
} else {
- VLOG(1) << "Saw single board in camera " << pi_name;
- Eigen::Quaternion<double> rotation(
- frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[0]));
- Eigen::Translation3d translation(tvecs_eigen[0]);
- Eigen::Affine3d H_camera2_board2 = translation * rotation;
- TimestampedPiDetection new_observation{
- .time = distributed_eof,
- .H_camera_target = H_camera2_board2,
- .pi_name = pi_name,
- .board_id = charuco_ids[0][0]};
-
- VLOG(2) << "Checking versus last result from " << last_observation.pi_name
- << " at time " << last_observation.time << " with delta time = "
- << std::abs((distributed_eof - last_observation.time).count());
- // Now, check if this new observation can be paired with a
- // previous single board observation.
-
- // Only take two observations if they're near enough to each
- // other in time. This should be within +/- kImagePeriodMs of
- // each other (e.g., +/-16.666ms for 30 Hz capture). This
- // should make sure we're always getting the closest images, and
- // not miss too many possible pairs, between cameras
-
- // TODO<Jim>: Should also check that (rotational) velocity of the robot is
- // small
- if (std::abs((distributed_eof - last_observation.time).count()) <
- static_cast<int>(kImagePeriodMs / 2.0 * 1000000)) {
- // Sort by pi numbering, since this is how we will handle them
- std::pair<TimestampedPiDetection, TimestampedPiDetection> new_pair;
- if (last_observation.pi_name < new_observation.pi_name) {
- new_pair = std::pair(last_observation, new_observation);
- } else if (last_observation.pi_name > new_observation.pi_name) {
- new_pair = std::pair(new_observation, last_observation);
- } else {
- LOG(WARNING) << "Got 2 observations in a row from same pi. Probably "
- "not too much of an issue???";
- }
- detection_list.push_back(new_pair);
-
- // This bit is just for visualization and checking purposes-- use the
- // last two-board observation to figure out the current estimate between
- // the two cameras
- if (FLAGS_visualize && two_board_extrinsics_list.size() > 0) {
- draw_vis = true;
- TimestampedPiDetection &last_two_board_ext =
- two_board_extrinsics_list[two_board_extrinsics_list.size() - 1];
- Eigen::Affine3d &H_boardA_boardB = last_two_board_ext.H_camera_target;
- int boardA_boardB_id = last_two_board_ext.board_id;
-
- TimestampedPiDetection camera1_boardA = new_pair.first;
- TimestampedPiDetection camera2_boardB = new_pair.second;
- // If camera1_boardA doesn't point to the first target, then swap
- // these two
- if (camera1_boardA.board_id != boardA_boardB_id) {
- camera1_boardA = new_pair.second;
- camera2_boardB = new_pair.first;
- }
- VLOG(1) << "Camera " << camera1_boardA.pi_name << " seeing board "
- << camera1_boardA.board_id << " and camera "
- << camera2_boardB.pi_name << " seeing board "
- << camera2_boardB.board_id;
-
- vis_robot_.DrawRobotOutline(
- H_world_board * camera1_boardA.H_camera_target.inverse(),
- camera1_boardA.pi_name, cv::Scalar(0, 0, 255));
- vis_robot_.DrawRobotOutline(
- H_world_board * H_boardA_boardB *
- camera2_boardB.H_camera_target.inverse(),
- camera2_boardB.pi_name, cv::Scalar(128, 128, 0));
- vis_robot_.DrawFrameAxes(
- H_world_board,
- std::string("Board ") +
- std::to_string(last_two_board_ext.board_id),
- cv::Scalar(0, 255, 0));
- vis_robot_.DrawFrameAxes(H_world_board * H_boardA_boardB, "Board B",
- cv::Scalar(255, 0, 0));
- Eigen::Affine3d H_camera1_camera2 =
- camera1_boardA.H_camera_target * H_boardA_boardB *
- camera2_boardB.H_camera_target.inverse();
-
- VLOG(1) << "Storing observation between " << new_pair.first.pi_name
- << ", target " << new_pair.first.board_id << " and "
- << new_pair.second.pi_name << ", target "
- << new_pair.second.board_id
- << " and camera-to-camera offset:\n"
- << H_camera1_camera2.matrix();
- }
- } else {
- VLOG(2) << "Storing observation for " << pi_name << " at time "
- << distributed_eof;
- last_observation = new_observation;
- }
+ VLOG(2) << "Storing observation for " << node_name << " at time "
+ << distributed_eof;
+ last_observation = new_observation;
}
}
if (FLAGS_visualize) {
- std::string image_name = pi_name + " Image";
- cv::Mat rgb_small;
- cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5);
- cv::imshow(image_name, rgb_small);
- cv::waitKey(FLAGS_wait_key);
+ if (!rgb_image.empty()) {
+ std::string image_name = node_name + " Image";
+ cv::Mat rgb_small;
+ cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5);
+ cv::imshow(image_name, rgb_small);
+ cv::waitKey(FLAGS_wait_key);
+ }
if (draw_vis) {
cv::imshow("View", vis_robot_.image_);
- cv::waitKey(1);
+ cv::waitKey(FLAGS_wait_key);
vis_robot_.ClearImage();
}
}
}
+void HandleTargetMap(const TargetMap &map,
+ aos::distributed_clock::time_point distributed_eof,
+ std::string node_name) {
+ VLOG(1) << "Got april tag map call from node " << node_name;
+ // Create empty RGB image in this case
+ cv::Mat rgb_image;
+ std::vector<TargetMapper::TargetPose> target_poses;
+
+ for (const auto *target_pose_fbs : *map.target_poses()) {
+ // Skip detections with invalid ids
+ if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) <
+ FLAGS_min_target_id ||
+ static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) >
+ FLAGS_max_target_id) {
+ LOG(INFO) << "Skipping tag with invalid id of " << target_pose_fbs->id();
+ continue;
+ }
+
+ // Skip detections with high pose errors
+ if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+ LOG(INFO) << "Skipping tag " << target_pose_fbs->id()
+ << " due to pose error of " << target_pose_fbs->pose_error();
+ continue;
+ }
+ // Skip detections with high pose error ratios
+ if (target_pose_fbs->pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+ LOG(INFO) << "Skipping tag " << target_pose_fbs->id()
+ << " due to pose error ratio of "
+ << target_pose_fbs->pose_error_ratio();
+ continue;
+ }
+
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+
+ target_poses.emplace_back(target_pose);
+
+ Eigen::Affine3d H_camera_target =
+ PoseUtils::Pose3dToAffine3d(target_pose.pose);
+ VLOG(2) << node_name << " saw target " << target_pose.id
+ << " from TargetMap at timestamp " << distributed_eof
+ << " with pose = " << H_camera_target.matrix();
+ }
+ HandlePoses(rgb_image, target_poses, distributed_eof, node_name);
+}
+
+void HandleImage(aos::EventLoop *event_loop, cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof,
+ aos::distributed_clock::time_point distributed_eof,
+ frc971::vision::CharucoExtractor &charuco_extractor,
+ std::string node_name) {
+ std::vector<cv::Vec4i> charuco_ids;
+ std::vector<std::vector<cv::Point2f>> charuco_corners;
+ bool valid = false;
+ std::vector<Eigen::Vector3d> rvecs_eigen;
+ std::vector<Eigen::Vector3d> tvecs_eigen;
+ // Why eof vs. distributed_eof?
+ charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(),
+ charuco_ids, charuco_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+ if (rvecs_eigen.size() > 0 && !valid) {
+ LOG(WARNING) << "Charuco extractor returned not valid";
+ return;
+ }
+
+ std::vector<TargetMapper::TargetPose> target_poses;
+ for (size_t i = 0; i < tvecs_eigen.size(); i++) {
+ Eigen::Quaterniond rotation(
+ frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[i]));
+ ceres::examples::Pose3d pose(Eigen::Vector3d(tvecs_eigen[i]), rotation);
+ TargetMapper::TargetPose target_pose{charuco_ids[i][0], pose};
+ target_poses.emplace_back(target_pose);
+
+ Eigen::Affine3d H_camera_target = PoseUtils::Pose3dToAffine3d(pose);
+ VLOG(2) << node_name << " saw target " << target_pose.id
+ << " from image at timestamp " << distributed_eof
+ << " with pose = " << H_camera_target.matrix();
+ }
+ HandlePoses(rgb_image, target_poses, distributed_eof, node_name);
+}
+
void ExtrinsicsMain(int argc, char *argv[]) {
vis_robot_ = VisualizeRobot(cv::Size(1000, 1000));
vis_robot_.ClearImage();
@@ -375,14 +427,13 @@
constexpr size_t kNumPis = 4;
for (size_t i = 1; i <= kNumPis; i++) {
- reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
- "frc971.vision.TargetMap");
- reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
- "foxglove.ImageAnnotations");
reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
"y2023.Constants");
}
+
reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
+ reader.RemapLoggedChannel("/logger/constants", "y2023.Constants");
+ reader.RemapLoggedChannel("/roborio/constants", "y2023.Constants");
reader.Register();
SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
@@ -394,6 +445,7 @@
node_list.push_back("pi2");
node_list.push_back("pi3");
node_list.push_back("pi4");
+ std::vector<const calibration::CameraCalibration *> calibration_list;
std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
std::vector<frc971::vision::CharucoExtractor *> charuco_extractors;
@@ -414,6 +466,7 @@
const calibration::CameraCalibration *calibration =
FindCameraCalibration(constants_fetcher.constants(), node);
+ calibration_list.push_back(calibration);
frc971::vision::TargetType target_type =
frc971::vision::TargetTypeFromString(FLAGS_target_type);
@@ -430,22 +483,42 @@
VLOG(1) << "Got extrinsics for " << node << " as\n"
<< default_extrinsics.back().matrix();
- frc971::vision::ImageCallback *image_callback =
- new frc971::vision::ImageCallback(
- detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
- [&reader, &charuco_extractors, &detection_event_loops, &node_list,
- i](cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader.event_loop_factory()
- ->GetNodeEventLoopFactory(
- detection_event_loops[i].get()->node())
- ->ToDistributedClock(eof);
- ProcessImage(detection_event_loops[i].get(), rgb_image, eof,
- pi_distributed_time, *charuco_extractors[i],
- node_list[i]);
- });
+ if (FLAGS_use_full_logs) {
+ LOG(INFO) << "Set up image callback for node " << node_list[i];
+ frc971::vision::ImageCallback *image_callback =
+ new frc971::vision::ImageCallback(
+ detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
+ [&reader, &charuco_extractors, &detection_event_loops, &node_list,
+ i](cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(
+ detection_event_loops[i].get()->node())
+ ->ToDistributedClock(eof);
+ HandleImage(detection_event_loops[i].get(), rgb_image, eof,
+ pi_distributed_time, *charuco_extractors[i],
+ node_list[i]);
+ });
- image_callbacks.emplace_back(image_callback);
+ image_callbacks.emplace_back(image_callback);
+ } else {
+ detection_event_loops[i]->MakeWatcher(
+ "/camera", [&reader, &detection_event_loops, &node_list,
+ i](const TargetMap &map) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(detection_event_loops[i]->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(
+ map.monotonic_timestamp_ns())));
+
+ HandleTargetMap(map, pi_distributed_time, node_list[i]);
+ });
+ LOG(INFO) << "Created watcher for using the detection event loop for "
+ << node_list[i] << " with i = " << i << " and size "
+ << detection_event_loops.size();
+ }
}
reader.event_loop_factory()->Run();
@@ -502,7 +575,8 @@
// Now, get the camera2->boardA map (notice it's boardA, same as
// camera1, so we can compute the difference based both on boardA)
Eigen::Affine3d H_camera2_boardA = pose2.H_camera_target;
- // If pose2 isn't referenced to boardA (base_target_id), correct that
+ // If pose2 isn't referenced to boardA (base_target_id), correct
+ // that
if (pose2.board_id != base_target_id) {
// pose2.H_camera_target references boardB, so map back to boardA
H_camera2_boardA =
@@ -568,6 +642,54 @@
<< default_extrinsics[i + 1].matrix();
LOG(INFO) << "--> Updated Extrinsic for " << node_list[i + 1] << " is \n"
<< next_extrinsic.matrix();
+
+ // Wirte out this extrinsic to a file
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+ fbb.CreateVector(
+ frc971::vision::MatrixToVector(next_extrinsic.matrix()));
+ calibration::TransformationMatrix::Builder matrix_builder(fbb);
+ matrix_builder.add_data(data_offset);
+ flatbuffers::Offset<calibration::TransformationMatrix>
+ extrinsic_calibration_offset = matrix_builder.Finish();
+
+ calibration::CameraCalibration::Builder calibration_builder(fbb);
+ calibration_builder.add_fixed_extrinsics(extrinsic_calibration_offset);
+ const aos::realtime_clock::time_point realtime_now =
+ aos::realtime_clock::now();
+ calibration_builder.add_calibration_timestamp(
+ realtime_now.time_since_epoch().count());
+ fbb.Finish(calibration_builder.Finish());
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ solved_extrinsics = fbb.Release();
+
+ aos::FlatbufferDetachedBuffer<
+ frc971::vision::calibration::CameraCalibration>
+ cal_copy = aos::RecursiveCopyFlatBuffer(calibration_list[i + 1]);
+ cal_copy.mutable_message()->clear_fixed_extrinsics();
+ cal_copy.mutable_message()->clear_calibration_timestamp();
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ merged_calibration = aos::MergeFlatBuffers(
+ &cal_copy.message(), &solved_extrinsics.message());
+
+ std::stringstream time_ss;
+ time_ss << realtime_now;
+
+ // Assumes node_list name is of form "pi#" to create camera id
+ const std::string calibration_filename =
+ FLAGS_output_folder +
+ absl::StrFormat("/calibration_pi-%d-%s_cam-%s_%s.json",
+ FLAGS_team_number, node_list[i + 1].substr(2, 3),
+ calibration_list[i + 1]->camera_id()->data(),
+ time_ss.str());
+
+ LOG(INFO) << calibration_filename << " -> "
+ << aos::FlatbufferToJson(merged_calibration,
+ {.multi_line = true});
+
+ aos::util::WriteStringToFileOrDie(
+ calibration_filename,
+ aos::FlatbufferToJson(merged_calibration, {.multi_line = true}));
}
}
diff --git a/y2023/vision/calibrate_multi_cameras_test.py b/y2023/vision/calibrate_multi_cameras_test.py
new file mode 100755
index 0000000..2b54355
--- /dev/null
+++ b/y2023/vision/calibrate_multi_cameras_test.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+# This script runs the multi camera calibration code through its paces using log data
+# It uses the AprilTag output logs, rather than directly from images
+import argparse
+import os
+import subprocess
+import sys
+import time
+from typing import Sequence, Text
+
+
+# Compare two json files that may have a different timestamp
+def compare_files(gt_file: str, calc_file: str):
+ with open(gt_file, "r") as f_gt:
+ with open(calc_file, "r") as f_calc:
+ while True:
+ line_gt = f_gt.readline()
+ line_calc = f_calc.readline()
+ if not line_gt:
+ if not line_calc:
+ return True
+ else:
+ return False
+
+ # timestamp field will be different, so ignore this line
+ if "timestamp" in line_gt:
+ if "timestamp" in line_calc:
+ continue
+ else:
+ return False
+
+ # Compare line and return False if different
+ if line_gt != line_calc:
+ print("Lines don't match!")
+ print("\tGround truth file:", line_gt, end='')
+ print("\tCalculated file:", line_calc, end='')
+ return False
+ return True
+
+ return False
+
+
+# Run through the calibration routine and file checking with max_pose_error arg
+def check_calib_match(args, max_pose_error: float):
+ calibrate_result = subprocess.run(
+ [
+ args.calibrate_binary,
+ args.logfile,
+ "--target_type",
+ "apriltag",
+ "--team_number",
+ "971",
+ "--max_pose_error",
+ str(max_pose_error),
+ ],
+ stdout=subprocess.PIPE,
+ stderr=subprocess.PIPE,
+ encoding="utf-8",
+ )
+
+ calibrate_result.check_returncode()
+
+ # Check for the 3 pi's that get calibrated
+ for pi in [2, 3, 4]:
+ pi_name = "pi-971-" + str(pi)
+ # Look for calculated calibration file in /tmp dir with pi_name
+ calc_calib_dir = "/tmp/"
+ files = os.listdir(calc_calib_dir)
+ calc_file = ""
+ # Read the calculated files in reverse order, so that we pick
+ # up the most newly created file each time
+ for file in files[::-1]:
+ # check if file contains substring pi_name
+ if pi_name in file:
+ calc_file = calc_calib_dir + file
+
+ # Next find the "ground truth" file with this pi_name
+ external_dir = 'external/calibrate_multi_cameras_data/'
+ files = os.listdir(external_dir)
+ gt_file = ""
+ for file in files[::-1]:
+ if pi_name in file:
+ gt_file = external_dir + file
+
+ if calc_file != "" and gt_file != "":
+ if not compare_files(gt_file, calc_file):
+ return False
+
+ return True
+
+
+def main(argv: Sequence[Text]):
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--logfile",
+ required=True,
+ default="calib1",
+ help="Path to logfile.")
+ parser.add_argument(
+ "--calibrate_binary",
+ required=False,
+ default=
+ "/home/jimostrowski/code/FRC/971-Robot-Code/bazel-bin/y2023/vision/calibrate_multi_cameras",
+ help="Path to calibrate_multi_cameras binary",
+ )
+ args = parser.parse_args(argv)
+
+ # Run once with correct max_pose_error
+ # These were the flags used to create the test file
+ # max_pose_error = 5e-5
+ # max_pose_error_ratio = 0.4
+ if not check_calib_match(args, 5e-5):
+ return -1
+
+ # And once with the incorrect value for max_pose_error to see that it fails
+ if check_calib_match(args, 1e-5):
+ return -1
+
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main(sys.argv[1:]))
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 18383af..83705b9 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -160,6 +160,8 @@
}
reader_->RemapLoggedChannel("/imu/constants", "y2023.Constants");
+ reader_->RemapLoggedChannel("/logger/constants", "y2023.Constants");
+ reader_->RemapLoggedChannel("/roborio/constants", "y2023.Constants");
reader_->Register();
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index b4d3acb..c05062e 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -223,9 +223,9 @@
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
- flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX> WritePosition(
flatbuffers::FlatBufferBuilder *fbb) {
- frc971::control_loops::CANFalcon::Builder builder(*fbb);
+ frc971::control_loops::CANTalonFX::Builder builder(*fbb);
builder.add_id(device_id_);
builder.add_device_temp(device_temp());
builder.add_supply_voltage(supply_voltage());
@@ -338,7 +338,7 @@
falcon->RefreshNontimesyncedSignals();
}
- aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANFalcon>,
+ aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANTalonFX>,
kCANFalconCount>
falcons;
@@ -350,14 +350,15 @@
auto falcons_list =
builder.fbb()
->CreateVector<
- flatbuffers::Offset<frc971::control_loops::CANFalcon>>(falcons);
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>>(
+ falcons);
frc971::control_loops::drivetrain::CANPosition::Builder
can_position_builder =
builder
.MakeBuilder<frc971::control_loops::drivetrain::CANPosition>();
- can_position_builder.add_falcons(falcons_list);
+ can_position_builder.add_talonfxs(falcons_list);
can_position_builder.add_timestamp(right_front_->GetTimestamp());
can_position_builder.add_status(static_cast<int>(status));
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index d6ff479..29f71a2 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -553,22 +553,22 @@
if (this.drivetrainCANPosition) {
this.falconRightFrontPosition.innerHTML = //TODO: (niko) Improve this so that falcons are not hard-coded
- this.drivetrainCANPosition.falcons(0).position().toString();
+ this.drivetrainCANPosition.talonfxs(0).position().toString();
this.falconRightBackPosition.innerHTML =
- this.drivetrainCANPosition.falcons(1).position().toString();
+ this.drivetrainCANPosition.talonfxs(1).position().toString();
this.falconRightUnderPosition.innerHTML =
- this.drivetrainCANPosition.falcons(2).position().toString();
+ this.drivetrainCANPosition.talonfxs(2).position().toString();
this.falconLeftFrontPosition.innerHTML =
- this.drivetrainCANPosition.falcons(3).position().toString();
+ this.drivetrainCANPosition.talonfxs(3).position().toString();
this.falconLeftBackPosition.innerHTML =
- this.drivetrainCANPosition.falcons(4).position().toString();
+ this.drivetrainCANPosition.talonfxs(4).position().toString();
this.falconLeftUnderPosition.innerHTML =
- this.drivetrainCANPosition.falcons(5).position().toString();
+ this.drivetrainCANPosition.talonfxs(5).position().toString();
}
if (this.localizerOutput) {
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index f096ee4..d74d033 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -50,7 +50,7 @@
],
deps = [
"//frc971:can_configuration_fbs",
- "//frc971/control_loops:can_falcon_fbs",
+ "//frc971/control_loops:can_talonfx_fbs",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
],
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
index 28e4849..dc6282f 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -1,6 +1,5 @@
include "frc971/control_loops/control_loops.fbs";
include "frc971/can_configuration.fbs";
-include "frc971/control_loops/can_falcon.fbs";
namespace y2023_bot3.control_loops.superstructure;
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index 9b8df5d..231f2c6 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -96,7 +96,7 @@
} // namespace
-static constexpr int kCANFalconCount = 6;
+static constexpr int kCANTalonFXCount = 6;
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
class Falcon {
@@ -177,9 +177,9 @@
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
- flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX> WritePosition(
flatbuffers::FlatBufferBuilder *fbb) {
- frc971::control_loops::CANFalcon::Builder builder(*fbb);
+ frc971::control_loops::CANTalonFX::Builder builder(*fbb);
builder.add_id(device_id_);
builder.add_device_temp(device_temp());
builder.add_supply_voltage(supply_voltage());
@@ -275,8 +275,8 @@
falcon->RefreshNontimesyncedSignals();
}
- aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANFalcon>,
- kCANFalconCount>
+ aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANTalonFX>,
+ kCANTalonFXCount>
falcons;
for (auto falcon : {right_front_, right_back_}) {
@@ -286,14 +286,15 @@
auto falcons_list =
builder.fbb()
->CreateVector<
- flatbuffers::Offset<frc971::control_loops::CANFalcon>>(falcons);
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>>(
+ falcons);
frc971::control_loops::drivetrain::CANPosition::Builder
can_position_builder =
builder
.MakeBuilder<frc971::control_loops::drivetrain::CANPosition>();
- can_position_builder.add_falcons(falcons_list);
+ can_position_builder.add_talonfxs(falcons_list);
can_position_builder.add_timestamp(right_front_->GetTimestamp());
can_position_builder.add_status(static_cast<int>(status));
diff --git a/y2023_bot4/BUILD b/y2023_bot4/BUILD
index 24a7c73..095731a 100644
--- a/y2023_bot4/BUILD
+++ b/y2023_bot4/BUILD
@@ -89,7 +89,7 @@
static_flatbuffer(
name = "drivetrain_can_position_fbs",
srcs = ["drivetrain_can_position.fbs"],
- deps = ["//frc971/control_loops:can_falcon_fbs"],
+ deps = ["//frc971/control_loops:can_talonfx_fbs"],
)
cc_binary(
@@ -142,8 +142,8 @@
"//aos/events:shm_event_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/wpilib:can_sensor_reader",
- "//frc971/wpilib:falcon",
"//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:talonfx",
"//frc971/wpilib:wpilib_robot_base",
"//frc971/wpilib/swerve:swerve_drivetrain_writer",
],
diff --git a/y2023_bot4/drivetrain_can_position.fbs b/y2023_bot4/drivetrain_can_position.fbs
index e8c1235..ef8edf7 100644
--- a/y2023_bot4/drivetrain_can_position.fbs
+++ b/y2023_bot4/drivetrain_can_position.fbs
@@ -1,9 +1,9 @@
-include "frc971/control_loops/can_falcon.fbs";
+include "frc971/control_loops/can_talonfx.fbs";
namespace y2023_bot4;
table SwerveModuleCANPosition {
- rotation: frc971.control_loops.CANFalcon (id: 0);
- translation: frc971.control_loops.CANFalcon (id: 1);
+ rotation: frc971.control_loops.CANTalonFX (id: 0);
+ translation: frc971.control_loops.CANTalonFX (id: 1);
}
// CAN Readings from the CAN sensor reader loop for each swerve module
diff --git a/y2023_bot4/wpilib_interface.cc b/y2023_bot4/wpilib_interface.cc
index a744ae0..8bac186 100644
--- a/y2023_bot4/wpilib_interface.cc
+++ b/y2023_bot4/wpilib_interface.cc
@@ -5,9 +5,9 @@
#include "aos/init.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/wpilib/can_sensor_reader.h"
-#include "frc971/wpilib/falcon.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/swerve/swerve_drivetrain_writer.h"
+#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2023_bot4/constants.h"
#include "y2023_bot4/drivetrain_can_position_generated.h"
@@ -18,7 +18,7 @@
"devices on the CAN bus using Phoenix Tuner");
using frc971::wpilib::CANSensorReader;
-using frc971::wpilib::Falcon;
+using frc971::wpilib::TalonFX;
using frc971::wpilib::swerve::DrivetrainWriter;
using frc971::wpilib::swerve::SwerveModule;
@@ -46,9 +46,9 @@
flatbuffers::Offset<SwerveModuleCANPosition> can_module_offset(
SwerveModuleCANPosition::Builder builder,
std::shared_ptr<SwerveModule> module) {
- std::optional<flatbuffers::Offset<control_loops::CANFalcon>> rotation_offset =
- module->rotation->TakeOffset();
- std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
+ std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
+ rotation_offset = module->rotation->TakeOffset();
+ std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
translation_offset = module->translation->TakeOffset();
CHECK(rotation_offset.has_value());
@@ -185,27 +185,27 @@
aos::configuration::ReadConfig("aos_config.json");
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
- std::vector<std::shared_ptr<Falcon>> falcons;
+ std::vector<std::shared_ptr<TalonFX>> falcons;
// TODO(max): Change the CanBus names with TalonFX software.
std::shared_ptr<SwerveModule> front_left = std::make_shared<SwerveModule>(
- frc971::wpilib::FalconParams{6, false},
- frc971::wpilib::FalconParams{5, false}, "Drivetrain Bus",
+ frc971::wpilib::TalonFXParams{6, false},
+ frc971::wpilib::TalonFXParams{5, false}, "Drivetrain Bus",
&signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
std::shared_ptr<SwerveModule> front_right = std::make_shared<SwerveModule>(
- frc971::wpilib::FalconParams{3, false},
- frc971::wpilib::FalconParams{4, false}, "Drivetrain Bus",
+ frc971::wpilib::TalonFXParams{3, false},
+ frc971::wpilib::TalonFXParams{4, false}, "Drivetrain Bus",
&signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
std::shared_ptr<SwerveModule> back_left = std::make_shared<SwerveModule>(
- frc971::wpilib::FalconParams{8, false},
- frc971::wpilib::FalconParams{7, false}, "Drivetrain Bus",
+ frc971::wpilib::TalonFXParams{8, false},
+ frc971::wpilib::TalonFXParams{7, false}, "Drivetrain Bus",
&signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
std::shared_ptr<SwerveModule> back_right = std::make_shared<SwerveModule>(
- frc971::wpilib::FalconParams{2, false},
- frc971::wpilib::FalconParams{1, false}, "Drivetrain Bus",
+ frc971::wpilib::TalonFXParams{2, false},
+ frc971::wpilib::TalonFXParams{1, false}, "Drivetrain Bus",
&signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
@@ -284,8 +284,8 @@
&drivetrain_writer_event_loop,
constants::Values::kDrivetrainWriterPriority, 12);
- drivetrain_writer.set_falcons(front_left, front_right, back_left,
- back_right);
+ drivetrain_writer.set_talonfxs(front_left, front_right, back_left,
+ back_right);
AddLoop(&drivetrain_writer_event_loop);
diff --git a/y2024_defense/BUILD b/y2024_defense/BUILD
index 12eeec3..63b9502 100644
--- a/y2024_defense/BUILD
+++ b/y2024_defense/BUILD
@@ -182,13 +182,13 @@
"//frc971/wpilib:can_sensor_reader",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
- "//frc971/wpilib:falcon",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
"//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:talonfx",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
diff --git a/y2024_defense/wpilib_interface.cc b/y2024_defense/wpilib_interface.cc
index 7cecb21..f3a0b8f 100644
--- a/y2024_defense/wpilib_interface.cc
+++ b/y2024_defense/wpilib_interface.cc
@@ -51,12 +51,12 @@
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "frc971/wpilib/falcon.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024_defense/constants.h"
@@ -69,7 +69,7 @@
using ::y2024_defense::constants::Values;
using frc971::control_loops::drivetrain::CANPosition;
-using frc971::wpilib::Falcon;
+using frc971::wpilib::TalonFX;
using std::make_unique;
@@ -266,27 +266,27 @@
// Thread 4.
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
- std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
+ std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
1, true, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
+ std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
2, true, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
+ std::shared_ptr<TalonFX> right_under = std::make_shared<TalonFX>(
3, true, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
+ std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
4, false, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
+ std::shared_ptr<TalonFX> left_back = std::make_shared<TalonFX>(
5, false, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
+ std::shared_ptr<TalonFX> left_under = std::make_shared<TalonFX>(
6, false, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
@@ -298,7 +298,7 @@
}
// Creating list of falcons for CANSensorReader
- std::vector<std::shared_ptr<Falcon>> falcons;
+ std::vector<std::shared_ptr<TalonFX>> falcons;
for (auto falcon : {right_front, right_back, right_under, left_front,
left_back, left_under}) {
falcons.push_back(falcon);
@@ -319,14 +319,15 @@
&can_sensor_reader_event_loop, std::move(signals_registry), falcons,
[falcons, &can_position_sender](ctre::phoenix::StatusCode status) {
auto builder = can_position_sender.MakeBuilder();
- aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANFalcon>,
- 6>
+ aos::SizedArray<
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>, 6>
flatbuffer_falcons;
for (auto falcon : falcons) {
falcon->SerializePosition(
builder.fbb(), control_loops::drivetrain::kHighOutputRatio);
- std::optional<flatbuffers::Offset<frc971::control_loops::CANFalcon>>
+ std::optional<
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>>
falcon_offset = falcon->TakeOffset();
CHECK(falcon_offset.has_value());
@@ -337,14 +338,14 @@
auto falcons_list =
builder.fbb()
->CreateVector<
- flatbuffers::Offset<frc971::control_loops::CANFalcon>>(
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>>(
flatbuffer_falcons);
frc971::control_loops::drivetrain::CANPosition::Builder
can_position_builder = builder.MakeBuilder<
frc971::control_loops::drivetrain::CANPosition>();
- can_position_builder.add_falcons(falcons_list);
+ can_position_builder.add_talonfxs(falcons_list);
can_position_builder.add_timestamp(falcons.front()->GetTimestamp());
can_position_builder.add_status(static_cast<int>(status));
@@ -360,8 +361,8 @@
frc971::wpilib::CANDrivetrainWriter can_drivetrain_writer(
&can_drivetrain_writer_event_loop);
- can_drivetrain_writer.set_falcons({right_front, right_back, right_under},
- {left_front, left_back, left_under});
+ can_drivetrain_writer.set_talonfxs({right_front, right_back, right_under},
+ {left_front, left_back, left_under});
can_drivetrain_writer_event_loop.MakeWatcher(
"/roborio", [&can_drivetrain_writer](