Add Intake and Drivetrain Wpilib
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: Ie985062d67da657652fcab87a433ff1529f11d34
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 672667a..2217513 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "aos/util/wrapping_counter.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/can_configuration_generated.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/drivetrain_can_position_static.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"
@@ -42,6 +43,7 @@
#include "frc971/wpilib/can_sensor_reader.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/generic_can_writer.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -50,8 +52,11 @@
#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024/constants.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_can_position_static.h"
#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_static.h"
DEFINE_bool(ctre_diag_server, false,
"If true, enable the diagnostics server for interacting with "
@@ -72,9 +77,9 @@
constexpr double kMaxBringupPower = 12.0;
-// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
-// DMA stuff and then removing the * 2.0 in *_translate.
-// The low bit is direction.
+double intake_pot_translate(double voltage) {
+ return voltage * Values::kIntakePivotPotRadiansPerVolt();
+}
double drivetrain_velocity_translate(double in) {
return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
@@ -95,14 +100,14 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::ShmEventLoop *event_loop,
- std::shared_ptr<const Values> values)
+ const Constants *robot_constants)
: ::frc971::wpilib::SensorReader(event_loop),
- values_(std::move(values)),
+ robot_constants_(CHECK_NOTNULL(robot_constants)),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
"/autonomous")),
superstructure_position_sender_(
- event_loop->MakeSender<superstructure::Position>(
+ event_loop->MakeSender<superstructure::PositionStatic>(
"/superstructure")),
drivetrain_position_sender_(
event_loop->MakeSender<
@@ -124,11 +129,18 @@
void RunIteration() override {
{
- auto builder = superstructure_position_sender_.MakeBuilder();
+ aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+ superstructure_position_sender_.MakeStaticBuilder();
- superstructure::Position::Builder position_builder =
- builder.MakeBuilder<superstructure::Position>();
- builder.CheckOk(builder.Send(position_builder.Finish()));
+ CopyPosition(intake_pivot_encoder_, builder->add_intake_pivot(),
+ Values::kIntakePivotEncoderCountsPerRevolution(),
+ Values::kIntakePivotEncoderRatio(), intake_pot_translate,
+ true,
+ robot_constants_->robot()
+ ->intake_constants()
+ ->potentiometer_offset());
+
+ builder.CheckOk(builder.Send());
}
SendDrivetrainPosition(drivetrain_position_sender_.MakeStaticBuilder(),
@@ -185,11 +197,26 @@
}
}
+ void set_intake_pivot_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ intake_pivot_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_pivot_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ intake_pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_intake_pivot_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ intake_pivot_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
private:
- std::shared_ptr<const Values> values_;
+ const Constants *robot_constants_;
aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
- aos::Sender<superstructure::Position> superstructure_position_sender_;
+ aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
aos::Sender<frc971::control_loops::drivetrain::PositionStatic>
drivetrain_position_sender_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
@@ -198,6 +225,8 @@
std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+ frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_pivot_encoder_;
+
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
};
@@ -209,12 +238,16 @@
}
void Run() override {
- std::shared_ptr<const Values> values =
- std::make_shared<const Values>(constants::MakeValues());
-
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+ const Constants *robot_constants = &constants_fetcher.constants();
+
// Thread 1.
::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
@@ -228,11 +261,17 @@
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
- SensorReader sensor_reader(&sensor_reader_event_loop, values);
+ SensorReader sensor_reader(&sensor_reader_event_loop, robot_constants);
sensor_reader.set_pwm_trigger(true);
sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+ // TODO: (niko) change values once robot is wired
+ sensor_reader.set_intake_pivot_encoder(make_encoder(4));
+ sensor_reader.set_intake_pivot_absolute_pwm(
+ make_unique<frc::DigitalInput>(4));
+ sensor_reader.set_intake_pivot_potentiometer(
+ make_unique<frc::AnalogInput>(4));
AddLoop(&sensor_reader_event_loop);
@@ -261,6 +300,22 @@
3, false, "Drivetrain Bus", &signals_registry,
constants::Values::kDrivetrainStatorCurrentLimit(),
constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<TalonFX> intake_pivot =
+ std::make_shared<TalonFX>(4, false, "Drivetrain Bus", &signals_registry,
+ robot_constants->common()
+ ->current_limits()
+ ->intake_pivot_stator_current_limit(),
+ robot_constants->common()
+ ->current_limits()
+ ->intake_pivot_supply_current_limit());
+ std::shared_ptr<TalonFX> intake_roller =
+ std::make_shared<TalonFX>(5, false, "Drivetrain Bus", &signals_registry,
+ robot_constants->common()
+ ->current_limits()
+ ->intake_roller_stator_current_limit(),
+ robot_constants->common()
+ ->current_limits()
+ ->intake_roller_supply_current_limit());
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
@@ -271,33 +326,69 @@
can_sensor_reader_event_loop.set_name("CANSensorReader");
// Creating list of talonfx for CANSensorReader
+ std::vector<std::shared_ptr<TalonFX>> drivetrain_talonfxs;
std::vector<std::shared_ptr<TalonFX>> talonfxs;
+
for (auto talonfx : {right_front, right_back, left_front, left_back}) {
+ drivetrain_talonfxs.push_back(talonfx);
talonfxs.push_back(talonfx);
}
- aos::Sender<CANPositionStatic> can_position_sender =
- can_sensor_reader_event_loop.MakeSender<CANPositionStatic>(
- "/drivetrain");
+ for (auto talonfx : {intake_pivot, intake_roller}) {
+ talonfxs.push_back(talonfx);
+ }
+
+ aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>
+ drivetrain_can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<
+ frc971::control_loops::drivetrain::CANPositionStatic>(
+ "/drivetrain");
+
+ aos::Sender<y2024::control_loops::superstructure::CANPositionStatic>
+ superstructure_can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<
+ y2024::control_loops::superstructure::CANPositionStatic>(
+ "/superstructure");
frc971::wpilib::CANSensorReader can_sensor_reader(
&can_sensor_reader_event_loop, std::move(signals_registry), talonfxs,
- [talonfxs, &can_position_sender](ctre::phoenix::StatusCode status) {
- aos::Sender<CANPositionStatic>::StaticBuilder builder =
- can_position_sender.MakeStaticBuilder();
+ [drivetrain_talonfxs, &intake_pivot, &intake_roller,
+ &drivetrain_can_position_sender, &superstructure_can_position_sender](
+ ctre::phoenix::StatusCode status) {
+ aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
+ StaticBuilder drivetrain_can_builder =
+ drivetrain_can_position_sender.MakeStaticBuilder();
- auto falcon_vector = builder->add_talonfxs();
+ auto drivetrain_falcon_vector =
+ drivetrain_can_builder->add_talonfxs();
- for (auto talonfx : talonfxs) {
+ for (auto talonfx : drivetrain_talonfxs) {
talonfx->SerializePosition(
- falcon_vector->emplace_back(),
+ drivetrain_falcon_vector->emplace_back(),
control_loops::drivetrain::kHighOutputRatio);
}
- builder->set_timestamp(talonfxs.front()->GetTimestamp());
- builder->set_status(static_cast<int>(status));
+ drivetrain_can_builder->set_timestamp(
+ drivetrain_talonfxs.front()->GetTimestamp());
+ drivetrain_can_builder->set_status(static_cast<int>(status));
- builder.CheckOk(builder.Send());
+ drivetrain_can_builder.CheckOk(drivetrain_can_builder.Send());
+
+ aos::Sender<y2024::control_loops::superstructure::CANPositionStatic>::
+ StaticBuilder superstructure_can_builder =
+ superstructure_can_position_sender.MakeStaticBuilder();
+
+ intake_roller->SerializePosition(
+ superstructure_can_builder->add_intake_roller(),
+ control_loops::drivetrain::kHighOutputRatio);
+ intake_pivot->SerializePosition(
+ superstructure_can_builder->add_intake_pivot(),
+ control_loops::drivetrain::kHighOutputRatio);
+
+ superstructure_can_builder->set_timestamp(
+ intake_roller->GetTimestamp());
+ superstructure_can_builder->set_status(static_cast<int>(status));
+ superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
});
AddLoop(&can_sensor_reader_event_loop);
@@ -309,13 +400,29 @@
frc971::wpilib::CANDrivetrainWriter can_drivetrain_writer(
&can_output_event_loop);
+ frc971::wpilib::GenericCANWriter<control_loops::superstructure::Output>
+ can_superstructure_writer(
+ &can_output_event_loop,
+ [](const control_loops::superstructure::Output &output,
+ const std::map<std::string_view, std::shared_ptr<TalonFX>>
+ &talonfx_map) {
+ talonfx_map.find("intake_pivot")
+ ->second->WriteVoltage(output.intake_pivot_voltage());
+ talonfx_map.find("intake_roller")
+ ->second->WriteVoltage(output.intake_roller_voltage());
+ });
+
can_drivetrain_writer.set_talonfxs({right_front, right_back},
{left_front, left_back});
+ can_superstructure_writer.add_talonfx("intake_pivot", intake_pivot);
+ can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
+
can_output_event_loop.MakeWatcher(
- "/roborio", [&can_drivetrain_writer](
+ "/roborio", [&can_drivetrain_writer, &can_superstructure_writer](
const frc971::CANConfiguration &configuration) {
can_drivetrain_writer.HandleCANConfiguration(configuration);
+ can_superstructure_writer.HandleCANConfiguration(configuration);
});
AddLoop(&can_output_event_loop);