Add a can sensor reader
Signed-off-by: Maxwell Henderson <maxwell.henderson@mailbox.org>
Change-Id: I573052e68adeab0223d10db8be72b161a1c2edae
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index ca3dd0c..a9d4ebf 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -494,3 +494,39 @@
"//aos/network/www:proxy",
],
)
+
+cc_library(
+ name = "falcon",
+ srcs = [
+ "falcon.cc",
+ ],
+ hdrs = [
+ "falcon.h",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/logging",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//third_party:phoenix",
+ "//third_party:phoenixpro",
+ "//third_party:wpilib",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "can_sensor_reader",
+ srcs = ["can_sensor_reader.cc"],
+ hdrs = ["can_sensor_reader.h"],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":falcon",
+ "//aos:realtime",
+ "//aos/containers:sized_array",
+ "//aos/events:event_loop",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/frc971/wpilib/can_sensor_reader.cc b/frc971/wpilib/can_sensor_reader.cc
new file mode 100644
index 0000000..5a19e8f
--- /dev/null
+++ b/frc971/wpilib/can_sensor_reader.cc
@@ -0,0 +1,64 @@
+#include "frc971/wpilib/can_sensor_reader.h"
+
+using frc971::wpilib::CANSensorReader;
+using frc971::wpilib::kCANUpdateFreqHz;
+
+CANSensorReader::CANSensorReader(
+ aos::EventLoop *event_loop,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry,
+ std::vector<std::shared_ptr<Falcon>> falcons)
+ : event_loop_(event_loop),
+ signals_(signals_registry.begin(), signals_registry.end()),
+ can_position_sender_(
+ event_loop->MakeSender<control_loops::drivetrain::CANPosition>(
+ "/drivetrain")),
+ falcons_(falcons) {
+ event_loop->SetRuntimeRealtimePriority(40);
+
+ // TODO(max): Decide if we want to keep this on this core.
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
+ timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
+ timer_handler_->set_name("CANSensorReader Loop");
+
+ event_loop->OnRun([this]() {
+ timer_handler_->Schedule(event_loop_->monotonic_now(),
+ 1 / kCANUpdateFreqHz);
+ });
+}
+
+void CANSensorReader::Loop() {
+ ctre::phoenix::StatusCode status =
+ ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(20_ms, signals_);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ auto builder = can_position_sender_.MakeBuilder();
+
+ for (auto falcon : falcons_) {
+ falcon->RefreshNontimesyncedSignals();
+ falcon->SerializePosition(builder.fbb());
+ }
+
+ auto falcon_offsets =
+ builder.fbb()
+ ->CreateVector<flatbuffers::Offset<control_loops::CANFalcon>>(
+ falcons_.size(), [this](size_t index) {
+ auto offset = falcons_.at(index)->TakeOffset();
+ CHECK(offset.has_value());
+ return offset.value();
+ });
+
+ control_loops::drivetrain::CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<control_loops::drivetrain::CANPosition>();
+
+ can_position_builder.add_falcons(falcon_offsets);
+ if (!falcons_.empty()) {
+ can_position_builder.add_timestamp(falcons_.at(0)->GetTimestamp());
+ }
+ can_position_builder.add_status(static_cast<int>(status));
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+}
diff --git a/frc971/wpilib/can_sensor_reader.h b/frc971/wpilib/can_sensor_reader.h
new file mode 100644
index 0000000..2e1a406
--- /dev/null
+++ b/frc971/wpilib/can_sensor_reader.h
@@ -0,0 +1,38 @@
+#ifndef FRC971_WPILIB_CAN_SENSOR_READER_H_
+#define FRC971_WPILIB_CAN_SENSOR_READER_H_
+
+#include <vector>
+
+#include "aos/containers/sized_array.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/realtime.h"
+#include "frc971/wpilib/falcon.h"
+
+namespace frc971 {
+namespace wpilib {
+class CANSensorReader {
+ public:
+ CANSensorReader(
+ aos::EventLoop *event_loop,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry,
+ std::vector<std::shared_ptr<Falcon>> falcons);
+
+ private:
+ void Loop();
+
+ aos::EventLoop *event_loop_;
+
+ const std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_;
+ aos::Sender<control_loops::drivetrain::CANPosition> can_position_sender_;
+
+ // This is a vector of falcons becuase we don't need to care
+ // about falcons individually.
+ std::vector<std::shared_ptr<Falcon>> falcons_;
+
+ // Pointer to the timer handler used to modify the wakeup.
+ ::aos::TimerHandler *timer_handler_;
+};
+} // namespace wpilib
+} // namespace frc971
+#endif // FRC971_WPILIB_CAN_SENSOR_READER_H_
diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/falcon.cc
new file mode 100644
index 0000000..2dee390
--- /dev/null
+++ b/frc971/wpilib/falcon.cc
@@ -0,0 +1,100 @@
+#include "frc971/wpilib/falcon.h"
+
+using frc971::wpilib::Falcon;
+
+Falcon::Falcon(int device_id, std::string canbus,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> *signals,
+ double stator_current_limit, double supply_current_limit)
+ : talon_(device_id, canbus),
+ device_id_(device_id),
+ device_temp_(talon_.GetDeviceTemp()),
+ supply_voltage_(talon_.GetSupplyVoltage()),
+ supply_current_(talon_.GetSupplyCurrent()),
+ torque_current_(talon_.GetTorqueCurrent()),
+ position_(talon_.GetPosition()),
+ duty_cycle_(talon_.GetDutyCycle()),
+ stator_current_limit_(stator_current_limit),
+ supply_current_limit_(supply_current_limit) {
+ // device temp is not timesynced so don't add it to the list of signals
+ device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
+
+ CHECK_NOTNULL(signals);
+
+ supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&supply_voltage_);
+
+ supply_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&supply_current_);
+
+ torque_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&torque_current_);
+
+ position_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&position_);
+
+ duty_cycle_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&duty_cycle_);
+}
+
+void Falcon::PrintConfigs() {
+ ctre::phoenixpro::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 Falcon::WriteConfigs(ctre::phoenixpro::signals::InvertedValue invert) {
+ inverted_ = invert;
+
+ ctre::phoenixpro::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::phoenixpro::configs::MotorOutputConfigs output_configs;
+ output_configs.NeutralMode =
+ ctre::phoenixpro::signals::NeutralModeValue::Brake;
+ output_configs.DutyCycleNeutralDeadband = 0;
+
+ output_configs.Inverted = inverted_;
+
+ ctre::phoenixpro::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+ configuration.MotorOutput = output_configs;
+
+ 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();
+}
+
+void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb) {
+ control_loops::CANFalcon::Builder builder(*fbb);
+ builder.add_id(device_id_);
+ builder.add_device_temp(device_temp());
+ builder.add_supply_voltage(supply_voltage());
+ builder.add_supply_current(supply_current());
+ builder.add_torque_current(torque_current());
+ builder.add_duty_cycle(duty_cycle());
+ builder.add_position(position());
+
+ last_position_offset_ = builder.Finish();
+}
+
+std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
+Falcon::TakeOffset() {
+ auto option_offset = last_position_offset_;
+
+ last_position_offset_.reset();
+
+ return option_offset;
+}
diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/falcon.h
new file mode 100644
index 0000000..7137d82
--- /dev/null
+++ b/frc971/wpilib/falcon.h
@@ -0,0 +1,91 @@
+#ifndef FRC971_WPILIB_FALCON_H_
+#define FRC971_WPILIB_FALCON_H_
+
+#include <chrono>
+#include <cinttypes>
+#include <vector>
+
+#include "ctre/phoenixpro/TalonFX.hpp"
+#include "glog/logging.h"
+
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+
+namespace control_loops = ::frc971::control_loops;
+
+namespace frc971 {
+namespace wpilib {
+
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+
+// Gets info from and writes to falcon motors using the TalonFX controller.
+class Falcon {
+ public:
+ Falcon(int device_id, std::string canbus,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> *signals,
+ double stator_current_limit, double supply_current_limit);
+
+ void PrintConfigs();
+
+ void WriteConfigs(ctre::phoenixpro::signals::InvertedValue invert);
+
+ ctre::phoenixpro::hardware::TalonFX *talon() { return &talon_; }
+
+ void SerializePosition(flatbuffers::FlatBufferBuilder *fbb);
+
+ std::optional<flatbuffers::Offset<control_loops::CANFalcon>> TakeOffset();
+
+ int device_id() const { return device_id_; }
+ float device_temp() const { return device_temp_.GetValue().value(); }
+ float supply_voltage() const { return supply_voltage_.GetValue().value(); }
+ float supply_current() const { return supply_current_.GetValue().value(); }
+ float torque_current() const { return torque_current_.GetValue().value(); }
+ float duty_cycle() const { return duty_cycle_.GetValue().value(); }
+ float position() const {
+ return static_cast<units::angle::radian_t>(position_.GetValue()).value();
+ }
+
+ // returns the monotonic timestamp of the latest timesynced reading in the
+ // timebase of the the syncronized CAN bus clock.
+ int64_t GetTimestamp() {
+ std::chrono::nanoseconds latest_timestamp =
+ torque_current_.GetTimestamp().GetTime();
+
+ return latest_timestamp.count();
+ }
+
+ void RefreshNontimesyncedSignals() { device_temp_.Refresh(); };
+
+ void set_stator_current_limit(double stator_current_limit) {
+ stator_current_limit_ = stator_current_limit;
+ }
+
+ void set_supply_current_limit(double supply_current_limit) {
+ supply_current_limit_ = supply_current_limit;
+ }
+
+ private:
+ ctre::phoenixpro::hardware::TalonFX talon_;
+ int device_id_;
+
+ ctre::phoenixpro::signals::InvertedValue inverted_;
+
+ ctre::phoenixpro::StatusSignalValue<units::temperature::celsius_t>
+ device_temp_;
+ ctre::phoenixpro::StatusSignalValue<units::voltage::volt_t> supply_voltage_;
+ ctre::phoenixpro::StatusSignalValue<units::current::ampere_t> supply_current_,
+ torque_current_;
+ ctre::phoenixpro::StatusSignalValue<units::angle::turn_t> position_;
+ ctre::phoenixpro::StatusSignalValue<units::dimensionless::scalar_t>
+ duty_cycle_;
+
+ double stator_current_limit_;
+ double supply_current_limit_;
+
+ std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
+ last_position_offset_;
+};
+} // namespace wpilib
+} // namespace frc971
+#endif // FRC971_WPILIB_FALCON_H_