Create CANPosition sender
Read the velocity of the integrated sensor on the flipper arms over can
and send it in it's own position message
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I3fa46526c2577f7cd1331924125bdad59111eb63
diff --git a/y2022/BUILD b/y2022/BUILD
index 02d1bb1..fd82eb5 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -155,6 +155,7 @@
"//y2022/control_loops/superstructure:superstructure_goal_fbs",
"//y2022/control_loops/superstructure:superstructure_output_fbs",
"//y2022/control_loops/superstructure:superstructure_position_fbs",
+ "//y2022/control_loops/superstructure:superstructure_can_position_fbs",
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -238,6 +239,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2022/control_loops/superstructure:superstructure_can_position_fbs",
"//y2022/control_loops/superstructure:superstructure_output_fbs",
"//y2022/control_loops/superstructure:superstructure_position_fbs",
],
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index f08bb80..cd3f7c3 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -24,6 +24,14 @@
)
flatbuffer_cc_library(
+ name = "superstructure_can_position_fbs",
+ srcs = [
+ "superstructure_can_position.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
name = "superstructure_status_fbs",
srcs = [
"superstructure_status.fbs",
diff --git a/y2022/control_loops/superstructure/superstructure_can_position.fbs b/y2022/control_loops/superstructure/superstructure_can_position.fbs
new file mode 100644
index 0000000..e521d8c
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_can_position.fbs
@@ -0,0 +1,10 @@
+namespace y2022.control_loops.superstructure;
+
+// CAN readings from the CAN sensor reader loop
+table CANPosition {
+ // Velocity of the flipper arms (rad/s), obtained from the integrated sensor
+ // in the falcon.
+ flipper_arm_integrated_sensor_velocity:double (id: 0);
+}
+
+root_type CANPosition;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 2868539..b95a4ec 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -49,6 +49,7 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -412,8 +413,8 @@
}
void set_flipper_arms_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
- flipper_arms_falcon_ = ::std::move(t);
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ flipper_arms_falcon_ = t;
flipper_arms_falcon_->ConfigSupplyCurrentLimit(
{true, Values::kFlipperArmSupplyCurrentLimit(),
Values::kFlipperArmSupplyCurrentLimit(), 0});
@@ -422,6 +423,11 @@
Values::kFlipperArmStatorCurrentLimit(), 0});
}
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ flipper_arms_falcon() {
+ return flipper_arms_falcon_;
+ }
+
void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
transfer_roller_victor_ = ::std::move(t);
}
@@ -477,13 +483,60 @@
::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_, flipper_arms_falcon_;
+ roller_falcon_front_, roller_falcon_back_;
+
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ flipper_arms_falcon_;
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
catapult_falcon_2_, climber_falcon_;
::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
};
+class CANSensorReader {
+ public:
+ CANSensorReader(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ can_position_sender_(
+ event_loop->MakeSender<superstructure::CANPosition>(
+ "/superstructure")) {
+ event_loop->SetRuntimeRealtimePriority(16);
+
+ phased_loop_handler_ =
+ event_loop_->AddPhasedLoop([this](int) { Loop(); }, kPeriod);
+ phased_loop_handler_->set_name("CAN SensorReader Loop");
+
+ event_loop->OnRun([this]() { Loop(); });
+ }
+
+ void set_flipper_arms_falcon(
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ flipper_arms_falcon_ = std::move(t);
+ }
+
+ private:
+ void Loop() {
+ auto builder = can_position_sender_.MakeBuilder();
+ superstructure::CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<superstructure::CANPosition>();
+ can_position_builder.add_flipper_arm_integrated_sensor_velocity(
+ flipper_arms_falcon_->GetSelectedSensorVelocity() *
+ kVelocityConversion);
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ }
+
+ static constexpr std::chrono::milliseconds kPeriod =
+ std::chrono::milliseconds(20);
+ // 2048 encoder counts / 100 ms to rad/sec
+ static constexpr double kVelocityConversion = (2.0 * M_PI / 2048) * 0.100;
+ aos::EventLoop *event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handler_;
+
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ flipper_arms_falcon_;
+ aos::Sender<superstructure::CANPosition> can_position_sender_;
+};
+
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<frc::Encoder> make_encoder(int index) {
@@ -544,8 +597,10 @@
sensor_reader.set_catapult_encoder(
make_unique<frc::Encoder>(0, 1, false, frc::Encoder::k4X));
- sensor_reader.set_catapult_absolute_pwm(std::make_unique<frc::DigitalInput>(2));
- sensor_reader.set_catapult_potentiometer(std::make_unique<frc::AnalogInput>(2));
+ sensor_reader.set_catapult_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_catapult_potentiometer(
+ std::make_unique<frc::AnalogInput>(2));
AddLoop(&sensor_reader_event_loop);
@@ -577,6 +632,13 @@
AddLoop(&output_event_loop);
+ // Thread 5
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
+ can_sensor_reader.set_flipper_arms_falcon(
+ superstructure_writer.flipper_arms_falcon());
+ AddLoop(&can_sensor_reader_event_loop);
+
RunLoops();
}
};
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index edbb258..f3bf5d0 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -238,6 +238,14 @@
"max_size": 448
},
{
+ "name": "/superstructure",
+ "type": "y2022.control_loops.superstructure.CANPosition",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 72
+ },
+ {
"name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",