blob: 5b3cd96f41ec189203bddbf7b83380330d72d92c [file] [log] [blame]
milind-u086d7262022-01-19 20:44:18 -08001#include <unistd.h>
2
3#include <array>
4#include <chrono>
5#include <cinttypes>
6#include <cmath>
7#include <cstdio>
8#include <cstring>
9#include <functional>
10#include <memory>
11#include <mutex>
12#include <thread>
13
14#include "ctre/phoenix/CANifier.h"
Philipp Schrader790cb542023-07-05 21:06:52 -070015
milind-u086d7262022-01-19 20:44:18 -080016#include "frc971/wpilib/ahal/AnalogInput.h"
17#include "frc971/wpilib/ahal/Counter.h"
18#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
19#include "frc971/wpilib/ahal/DriverStation.h"
20#include "frc971/wpilib/ahal/Encoder.h"
milind-u6e7d8d42022-04-06 18:30:43 -070021#include "frc971/wpilib/ahal/Servo.h"
milind-u086d7262022-01-19 20:44:18 -080022#include "frc971/wpilib/ahal/TalonFX.h"
23#include "frc971/wpilib/ahal/VictorSP.h"
24#undef ERROR
25
Maxwell Henderson1c0843c2023-12-22 16:20:59 -080026#include "ctre/phoenix6/TalonFX.hpp"
Philipp Schrader790cb542023-07-05 21:06:52 -070027
milind-u086d7262022-01-19 20:44:18 -080028#include "aos/commonmath.h"
29#include "aos/events/event_loop.h"
30#include "aos/events/shm_event_loop.h"
31#include "aos/init.h"
32#include "aos/logging/logging.h"
33#include "aos/realtime.h"
34#include "aos/time/time.h"
35#include "aos/util/log_interval.h"
36#include "aos/util/phased_loop.h"
37#include "aos/util/wrapping_counter.h"
milind-u086d7262022-01-19 20:44:18 -080038#include "frc971/autonomous/auto_mode_generated.h"
39#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
40#include "frc971/input/robot_state_generated.h"
Ravago Jones0e86e242022-02-12 18:38:14 -080041#include "frc971/queues/gyro_generated.h"
milind-u086d7262022-01-19 20:44:18 -080042#include "frc971/wpilib/ADIS16448.h"
43#include "frc971/wpilib/buffered_pcm.h"
44#include "frc971/wpilib/buffered_solenoid.h"
45#include "frc971/wpilib/dma.h"
46#include "frc971/wpilib/drivetrain_writer.h"
47#include "frc971/wpilib/encoder_and_potentiometer.h"
48#include "frc971/wpilib/joystick_sender.h"
49#include "frc971/wpilib/logging_generated.h"
50#include "frc971/wpilib/loop_output_handler.h"
51#include "frc971/wpilib/pdp_fetcher.h"
52#include "frc971/wpilib/sensor_reader.h"
53#include "frc971/wpilib/wpilib_robot_base.h"
54#include "y2022/constants.h"
Henry Speiser77747b72022-03-06 17:18:29 -080055#include "y2022/control_loops/superstructure/led_indicator.h"
Milind Upadhyay482b0ba2022-02-26 21:51:59 -080056#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
milind-u086d7262022-01-19 20:44:18 -080057#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
58#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
59
60using ::aos::monotonic_clock;
61using ::y2022::constants::Values;
62namespace superstructure = ::y2022::control_loops::superstructure;
63namespace chrono = ::std::chrono;
64using std::make_unique;
65
Austin Schuhda7e3e12022-03-26 15:14:31 -070066DEFINE_bool(can_catapult, false, "If true, use CAN to control the catapult.");
67
milind-u086d7262022-01-19 20:44:18 -080068namespace y2022 {
69namespace wpilib {
70namespace {
71
72constexpr double kMaxBringupPower = 12.0;
73
74// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
75// DMA stuff and then removing the * 2.0 in *_translate.
76// The low bit is direction.
77
milind-u086d7262022-01-19 20:44:18 -080078double drivetrain_velocity_translate(double in) {
79 return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
80 (2.0 * M_PI)) *
81 Values::kDrivetrainEncoderRatio() *
82 control_loops::drivetrain::kWheelRadius;
83}
84
Siddhant Kanwar0e37f592022-02-21 19:26:50 -080085double climber_pot_translate(double voltage) {
Nathan Leong342b85e2023-01-08 13:49:33 -080086 return voltage * Values::kClimberPotMetersPerVolt();
Siddhant Kanwar0e37f592022-02-21 19:26:50 -080087}
88
Griffin Buibcbef482022-02-23 15:32:10 -080089double flipper_arms_pot_translate(double voltage) {
Nathan Leong342b85e2023-01-08 13:49:33 -080090 return voltage * Values::kFlipperArmsPotRadiansPerVolt();
Griffin Buibcbef482022-02-23 15:32:10 -080091}
92
Henry Speiser55aa3ba2022-02-21 23:21:12 -080093double intake_pot_translate(double voltage) {
Nathan Leong342b85e2023-01-08 13:49:33 -080094 return voltage * Values::kIntakePotRadiansPerVolt();
Henry Speiser55aa3ba2022-02-21 23:21:12 -080095}
96
97double turret_pot_translate(double voltage) {
Nathan Leong342b85e2023-01-08 13:49:33 -080098 return voltage * Values::kTurretPotRadiansPerVolt();
Henry Speiser55aa3ba2022-02-21 23:21:12 -080099}
100
milind-u086d7262022-01-19 20:44:18 -0800101constexpr double kMaxFastEncoderPulsesPerSecond =
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800102 std::max({Values::kMaxDrivetrainEncoderPulsesPerSecond(),
103 Values::kMaxIntakeEncoderPulsesPerSecond()});
milind-u086d7262022-01-19 20:44:18 -0800104static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
105 "fast encoders are too fast");
106constexpr double kMaxMediumEncoderPulsesPerSecond =
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800107 Values::kMaxTurretEncoderPulsesPerSecond();
milind-u086d7262022-01-19 20:44:18 -0800108
109static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
110 "medium encoders are too fast");
111
Austin Schuh39f26f62022-02-24 21:34:46 -0800112double catapult_pot_translate(double voltage) {
113 return voltage * Values::kCatapultPotRatio() *
114 (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
115}
116
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800117void PrintConfigs(ctre::phoenix6::hardware::TalonFX *talon) {
118 ctre::phoenix6::configs::TalonFXConfiguration configuration;
119 ctre::phoenix::StatusCode status =
120 talon->GetConfigurator().Refresh(configuration);
121 if (!status.IsOK()) {
122 AOS_LOG(ERROR, "Failed to get falcon configuration: %s: %s",
123 status.GetName(), status.GetDescription());
124 }
125 AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
126}
127
128void WriteConfigs(ctre::phoenix6::hardware::TalonFX *talon,
129 double stator_current_limit, double supply_current_limit) {
130 ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
131 current_limits.StatorCurrentLimit = stator_current_limit;
132 current_limits.StatorCurrentLimitEnable = true;
133 current_limits.SupplyCurrentLimit = supply_current_limit;
134 current_limits.SupplyCurrentLimitEnable = true;
135
136 ctre::phoenix6::configs::TalonFXConfiguration configuration;
137 configuration.CurrentLimits = current_limits;
138
139 ctre::phoenix::StatusCode status =
140 talon->GetConfigurator().Apply(configuration);
141 if (!status.IsOK()) {
142 AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
143 status.GetName(), status.GetDescription());
144 }
145
146 PrintConfigs(talon);
147}
148
149void Disable(ctre::phoenix6::hardware::TalonFX *talon) {
150 ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
151 stop_command.UpdateFreqHz = 0_Hz;
152 stop_command.EnableFOC = true;
153
154 talon->SetControl(stop_command);
155}
156
milind-u086d7262022-01-19 20:44:18 -0800157} // namespace
158
159// Class to send position messages with sensor readings to our loops.
160class SensorReader : public ::frc971::wpilib::SensorReader {
161 public:
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800162 SensorReader(::aos::ShmEventLoop *event_loop,
163 std::shared_ptr<const Values> values)
milind-u086d7262022-01-19 20:44:18 -0800164 : ::frc971::wpilib::SensorReader(event_loop),
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800165 values_(std::move(values)),
milind-u086d7262022-01-19 20:44:18 -0800166 auto_mode_sender_(
167 event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
168 "/autonomous")),
169 superstructure_position_sender_(
170 event_loop->MakeSender<superstructure::Position>(
171 "/superstructure")),
172 drivetrain_position_sender_(
173 event_loop
174 ->MakeSender<::frc971::control_loops::drivetrain::Position>(
Ravago Jones0e86e242022-02-12 18:38:14 -0800175 "/drivetrain")),
176 gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
177 "/drivetrain")) {
milind-u086d7262022-01-19 20:44:18 -0800178 // Set to filter out anything shorter than 1/4 of the minimum pulse width
179 // we should ever see.
180 UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
181 UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
182 }
183
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800184 void Start() override {
185 // TODO(Ravago): Figure out why adding multiple DMA readers results in weird
186 // behavior
187 // AddToDMA(&imu_heading_reader_);
188 AddToDMA(&imu_yaw_rate_reader_);
189 }
190
milind-u086d7262022-01-19 20:44:18 -0800191 // Auto mode switches.
192 void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
193 autonomous_modes_.at(i) = ::std::move(sensor);
194 }
195
Austin Schuh39f26f62022-02-24 21:34:46 -0800196 void set_catapult_encoder(::std::unique_ptr<frc::Encoder> encoder) {
197 medium_encoder_filter_.Add(encoder.get());
198 catapult_encoder_.set_encoder(::std::move(encoder));
199 }
200
201 void set_catapult_absolute_pwm(
202 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
203 catapult_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
204 }
205
206 void set_catapult_potentiometer(
207 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
208 catapult_encoder_.set_potentiometer(::std::move(potentiometer));
209 }
210
Ravago Jones0e86e242022-02-12 18:38:14 -0800211 void set_heading_input(::std::unique_ptr<frc::DigitalInput> sensor) {
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800212 imu_heading_input_ = ::std::move(sensor);
213 imu_heading_reader_.set_input(imu_heading_input_.get());
Ravago Jones0e86e242022-02-12 18:38:14 -0800214 }
215
216 void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800217 imu_yaw_rate_input_ = ::std::move(sensor);
218 imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
Ravago Jones0e86e242022-02-12 18:38:14 -0800219 }
Austin Schuhda7e3e12022-03-26 15:14:31 -0700220 void set_catapult_falcon_1(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800221 ::std::shared_ptr<ctre::phoenix6::hardware::TalonFX> t1,
222 ::std::shared_ptr<ctre::phoenix6::hardware::TalonFX> t2) {
Austin Schuhda7e3e12022-03-26 15:14:31 -0700223 catapult_falcon_1_can_ = ::std::move(t1);
224 catapult_falcon_2_can_ = ::std::move(t2);
225 }
Ravago Jones0e86e242022-02-12 18:38:14 -0800226
milind-u086d7262022-01-19 20:44:18 -0800227 void RunIteration() override {
Austin Schuhda7e3e12022-03-26 15:14:31 -0700228 superstructure_reading_->Set(true);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800229 {
230 auto builder = superstructure_position_sender_.MakeBuilder();
231
Austin Schuh39f26f62022-02-24 21:34:46 -0800232 frc971::PotAndAbsolutePositionT catapult;
233 CopyPosition(catapult_encoder_, &catapult,
234 Values::kCatapultEncoderCountsPerRevolution(),
235 Values::kCatapultEncoderRatio(), catapult_pot_translate,
Austin Schuh275f9812022-03-05 14:02:37 -0800236 false, values_->catapult.potentiometer_offset);
Austin Schuh39f26f62022-02-24 21:34:46 -0800237 flatbuffers::Offset<frc971::PotAndAbsolutePosition> catapult_offset =
238 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &catapult);
239
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800240 frc971::RelativePositionT climber;
241 CopyPosition(*climber_potentiometer_, &climber, climber_pot_translate,
242 false, values_->climber.potentiometer_offset);
243 flatbuffers::Offset<frc971::RelativePosition> climber_offset =
244 frc971::RelativePosition::Pack(*builder.fbb(), &climber);
245
Griffin Buibcbef482022-02-23 15:32:10 -0800246 frc971::RelativePositionT flipper_arm_left;
247 CopyPosition(*flipper_arm_left_potentiometer_, &flipper_arm_left,
248 flipper_arms_pot_translate, false,
249 values_->flipper_arm_left.potentiometer_offset);
250
251 frc971::RelativePositionT flipper_arm_right;
252 CopyPosition(*flipper_arm_right_potentiometer_, &flipper_arm_right,
Austin Schuhab0eae32022-03-06 14:42:33 -0800253 flipper_arms_pot_translate, true,
Griffin Buibcbef482022-02-23 15:32:10 -0800254 values_->flipper_arm_right.potentiometer_offset);
255
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800256 // Intake
257 frc971::PotAndAbsolutePositionT intake_front;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800258 CopyPosition(intake_encoder_front_, &intake_front,
259 Values::kIntakeEncoderCountsPerRevolution(),
Austin Schuh275f9812022-03-05 14:02:37 -0800260 Values::kIntakeEncoderRatio(), intake_pot_translate, true,
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800261 values_->intake_front.potentiometer_offset);
Griffin Buibcbef482022-02-23 15:32:10 -0800262 frc971::PotAndAbsolutePositionT intake_back;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800263 CopyPosition(intake_encoder_back_, &intake_back,
264 Values::kIntakeEncoderCountsPerRevolution(),
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800265 Values::kIntakeEncoderRatio(), intake_pot_translate, true,
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800266 values_->intake_back.potentiometer_offset);
Griffin Buibcbef482022-02-23 15:32:10 -0800267 frc971::PotAndAbsolutePositionT turret;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800268 CopyPosition(turret_encoder_, &turret,
269 Values::kTurretEncoderCountsPerRevolution(),
270 Values::kTurretEncoderRatio(), turret_pot_translate, false,
271 values_->turret.potentiometer_offset);
272
273 flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset_front =
274 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_front);
275 flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset_back =
276 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_back);
277 flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
278 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
Griffin Buibcbef482022-02-23 15:32:10 -0800279 flatbuffers::Offset<frc971::RelativePosition> flipper_arm_left_offset =
280 frc971::RelativePosition::Pack(*builder.fbb(), &flipper_arm_left);
281 flatbuffers::Offset<frc971::RelativePosition> flipper_arm_right_offset =
282 frc971::RelativePosition::Pack(*builder.fbb(), &flipper_arm_right);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800283
284 superstructure::Position::Builder position_builder =
285 builder.MakeBuilder<superstructure::Position>();
286 position_builder.add_climber(climber_offset);
Griffin Buibcbef482022-02-23 15:32:10 -0800287 position_builder.add_flipper_arm_left(flipper_arm_left_offset);
288 position_builder.add_flipper_arm_right(flipper_arm_right_offset);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800289 position_builder.add_intake_front(intake_offset_front);
290 position_builder.add_intake_back(intake_offset_back);
291 position_builder.add_turret(turret_offset);
Milo Lin4950ac52022-02-25 19:56:11 -0800292 position_builder.add_intake_beambreak_front(
293 intake_beambreak_front_->Get());
294 position_builder.add_intake_beambreak_back(intake_beambreak_back_->Get());
295 position_builder.add_turret_beambreak(turret_beambreak_->Get());
Austin Schuh39f26f62022-02-24 21:34:46 -0800296 position_builder.add_catapult(catapult_offset);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800297 builder.CheckOk(builder.Send(position_builder.Finish()));
298 }
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800299
milind-u086d7262022-01-19 20:44:18 -0800300 {
301 auto builder = drivetrain_position_sender_.MakeBuilder();
302 frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
303 builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
304 drivetrain_builder.add_left_encoder(
James Kuszmaul53507e12022-02-12 18:36:40 -0800305 constants::Values::DrivetrainEncoderToMeters(
306 drivetrain_left_encoder_->GetRaw()));
milind-u086d7262022-01-19 20:44:18 -0800307 drivetrain_builder.add_left_speed(
308 drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
309
310 drivetrain_builder.add_right_encoder(
James Kuszmaul53507e12022-02-12 18:36:40 -0800311 -constants::Values::DrivetrainEncoderToMeters(
312 drivetrain_right_encoder_->GetRaw()));
milind-u086d7262022-01-19 20:44:18 -0800313 drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
314 drivetrain_right_encoder_->GetPeriod()));
315
316 builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
317 }
318
319 {
Ravago Jones0e86e242022-02-12 18:38:14 -0800320 auto builder = gyro_sender_.MakeBuilder();
321 ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
322 builder.MakeBuilder<::frc971::sensors::GyroReading>();
James Kuszmaulf34e7fd2022-03-13 20:30:34 -0700323 // +/- 2000 deg / sec
324 constexpr double kMaxVelocity = 4000; // degrees / second
Ravago Jones0e86e242022-02-12 18:38:14 -0800325 constexpr double kVelocityRadiansPerSecond =
326 kMaxVelocity / 360 * (2.0 * M_PI);
327
328 // Only part of the full range is used to prevent being 100% on or off.
329 constexpr double kScaledRangeLow = 0.1;
330 constexpr double kScaledRangeHigh = 0.9;
331
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800332 constexpr double kPWMFrequencyHz = 200;
333 double heading_duty_cycle =
334 imu_heading_reader_.last_width() * kPWMFrequencyHz;
335 double velocity_duty_cycle =
336 imu_yaw_rate_reader_.last_width() * kPWMFrequencyHz;
337
Ravago Jones0e86e242022-02-12 18:38:14 -0800338 constexpr double kDutyCycleScale =
339 1 / (kScaledRangeHigh - kScaledRangeLow);
Ravago Jones0e86e242022-02-12 18:38:14 -0800340 // scale from 0.1 - 0.9 to 0 - 1
341 double rescaled_heading_duty_cycle =
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800342 (heading_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
Ravago Jones0e86e242022-02-12 18:38:14 -0800343 double rescaled_velocity_duty_cycle =
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800344 (velocity_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
Ravago Jones0e86e242022-02-12 18:38:14 -0800345
346 if (!std::isnan(rescaled_heading_duty_cycle)) {
347 gyro_reading_builder.add_angle(rescaled_heading_duty_cycle *
348 (2.0 * M_PI));
349 }
350 if (!std::isnan(rescaled_velocity_duty_cycle)) {
351 gyro_reading_builder.add_velocity((rescaled_velocity_duty_cycle - 0.5) *
352 kVelocityRadiansPerSecond);
353 }
354 builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
355 }
356
357 {
milind-u086d7262022-01-19 20:44:18 -0800358 auto builder = auto_mode_sender_.MakeBuilder();
359
360 uint32_t mode = 0;
361 for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
362 if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
363 mode |= 1 << i;
364 }
365 }
366
367 auto auto_mode_builder =
368 builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
369
370 auto_mode_builder.add_mode(mode);
371
372 builder.CheckOk(builder.Send(auto_mode_builder.Finish()));
373 }
374 }
375
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800376 void set_climber_potentiometer(
377 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
378 climber_potentiometer_ = ::std::move(potentiometer);
379 }
380
Griffin Buibcbef482022-02-23 15:32:10 -0800381 void set_flipper_arm_left_potentiometer(
382 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
383 flipper_arm_left_potentiometer_ = ::std::move(potentiometer);
384 }
385
386 void set_flipper_arm_right_potentiometer(
387 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
388 flipper_arm_right_potentiometer_ = ::std::move(potentiometer);
389 }
390
Austin Schuhda7e3e12022-03-26 15:14:31 -0700391 std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
392
393 void set_superstructure_reading(
394 std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
395 superstructure_reading_ = superstructure_reading;
396 }
397
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800398 void set_intake_encoder_front(::std::unique_ptr<frc::Encoder> encoder) {
399 fast_encoder_filter_.Add(encoder.get());
400 intake_encoder_front_.set_encoder(::std::move(encoder));
401 }
402
403 void set_intake_encoder_back(::std::unique_ptr<frc::Encoder> encoder) {
404 fast_encoder_filter_.Add(encoder.get());
405 intake_encoder_back_.set_encoder(::std::move(encoder));
406 }
407
408 void set_intake_front_absolute_pwm(
409 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
410 intake_encoder_front_.set_absolute_pwm(::std::move(absolute_pwm));
411 }
412
413 void set_intake_front_potentiometer(
414 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
415 intake_encoder_front_.set_potentiometer(::std::move(potentiometer));
416 }
417
418 void set_intake_back_absolute_pwm(
419 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
420 intake_encoder_back_.set_absolute_pwm(::std::move(absolute_pwm));
421 }
422
423 void set_intake_back_potentiometer(
424 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
425 intake_encoder_back_.set_potentiometer(::std::move(potentiometer));
426 }
427
428 void set_turret_encoder(::std::unique_ptr<frc::Encoder> encoder) {
429 medium_encoder_filter_.Add(encoder.get());
430 turret_encoder_.set_encoder(::std::move(encoder));
431 }
432
433 void set_turret_absolute_pwm(
434 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
435 turret_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
436 }
437
438 void set_turret_potentiometer(
439 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
440 turret_encoder_.set_potentiometer(::std::move(potentiometer));
441 }
442
Milo Lin4950ac52022-02-25 19:56:11 -0800443 void set_intake_beambreak_front(::std::unique_ptr<frc::DigitalInput> sensor) {
444 intake_beambreak_front_ = ::std::move(sensor);
445 }
446 void set_intake_beambreak_back(::std::unique_ptr<frc::DigitalInput> sensor) {
447 intake_beambreak_back_ = ::std::move(sensor);
448 }
449 void set_turret_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
450 turret_beambreak_ = ::std::move(sensor);
451 }
452
milind-u086d7262022-01-19 20:44:18 -0800453 private:
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800454 std::shared_ptr<const Values> values_;
455
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800456 aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
457 aos::Sender<superstructure::Position> superstructure_position_sender_;
458 aos::Sender<frc971::control_loops::drivetrain::Position>
milind-u086d7262022-01-19 20:44:18 -0800459 drivetrain_position_sender_;
Ravago Jones0e86e242022-02-12 18:38:14 -0800460 ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
milind-u086d7262022-01-19 20:44:18 -0800461
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800462 std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800463
Milo Lin4950ac52022-02-25 19:56:11 -0800464 std::unique_ptr<frc::DigitalInput> intake_beambreak_front_,
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800465 intake_beambreak_back_, turret_beambreak_, imu_heading_input_,
466 imu_yaw_rate_input_;
Milo Lin4950ac52022-02-25 19:56:11 -0800467
Griffin Buibcbef482022-02-23 15:32:10 -0800468 std::unique_ptr<frc::AnalogInput> climber_potentiometer_,
469 flipper_arm_right_potentiometer_, flipper_arm_left_potentiometer_;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800470 frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
Ravago Jones0e86e242022-02-12 18:38:14 -0800471 intake_encoder_back_, turret_encoder_, catapult_encoder_;
Austin Schuh39f26f62022-02-24 21:34:46 -0800472
Ravago Jones7b28b1b2022-02-27 20:55:53 -0800473 frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
Austin Schuhda7e3e12022-03-26 15:14:31 -0700474
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800475 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult_falcon_1_can_,
476 catapult_falcon_2_can_;
milind-u086d7262022-01-19 20:44:18 -0800477};
478
479class SuperstructureWriter
480 : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
481 public:
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800482 SuperstructureWriter(aos::EventLoop *event_loop)
483 : frc971::wpilib::LoopOutputHandler<superstructure::Output>(
Austin Schuhda7e3e12022-03-26 15:14:31 -0700484 event_loop, "/superstructure"),
485 catapult_reversal_(make_unique<frc::DigitalOutput>(0)) {}
milind-u086d7262022-01-19 20:44:18 -0800486
milind-u6e7d8d42022-04-06 18:30:43 -0700487 void set_climber_servo_left(::std::unique_ptr<::frc::Servo> t) {
488 climber_servo_left_ = ::std::move(t);
489 }
490 void set_climber_servo_right(::std::unique_ptr<::frc::Servo> t) {
491 climber_servo_right_ = ::std::move(t);
492 }
493
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800494 void set_climber_falcon(std::unique_ptr<frc::TalonFX> t) {
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800495 climber_falcon_ = std::move(t);
Griffin Bui67abb912022-01-22 16:16:21 -0800496 }
497
Jacob Ismael322ebb92022-02-09 20:12:47 -0800498 void set_turret_falcon(::std::unique_ptr<::frc::TalonFX> t) {
499 turret_falcon_ = ::std::move(t);
500 }
501
502 void set_catapult_falcon_1(::std::unique_ptr<::frc::TalonFX> t) {
503 catapult_falcon_1_ = ::std::move(t);
504 }
505
Austin Schuhe040ddc2022-03-17 00:16:47 -0700506 void set_catapult_falcon_1(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800507 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t1,
508 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t2) {
Austin Schuhe040ddc2022-03-17 00:16:47 -0700509 catapult_falcon_1_can_ = ::std::move(t1);
510 catapult_falcon_2_can_ = ::std::move(t2);
511
512 for (auto &falcon : {catapult_falcon_1_can_, catapult_falcon_2_can_}) {
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800513 ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
514 current_limits.StatorCurrentLimit =
515 Values::kIntakeRollerStatorCurrentLimit();
516 current_limits.StatorCurrentLimitEnable = true;
517 current_limits.SupplyCurrentLimit =
518 Values::kIntakeRollerSupplyCurrentLimit();
519 current_limits.SupplyCurrentLimitEnable = true;
520
521 ctre::phoenix6::configs::TalonFXConfiguration configuration;
522 configuration.CurrentLimits = current_limits;
523
524 ctre::phoenix::StatusCode status =
525 falcon->GetConfigurator().Apply(configuration);
526 if (!status.IsOK()) {
527 AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
528 status.GetName(), status.GetDescription());
529 }
530
531 PrintConfigs(falcon.get());
532
533 // TODO(max): Figure out how to migrate these configs to phoenix6
534 /*falcon->SetStatusFramePeriod(
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700535 ctre::phoenix::motorcontrol::Status_1_General, 1);
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700536 falcon->SetStatusFramePeriod(
537 ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800538
Austin Schuhda7e3e12022-03-26 15:14:31 -0700539 falcon->ConfigOpenloopRamp(0.0);
540 falcon->ConfigClosedloopRamp(0.0);
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800541 falcon->ConfigVoltageMeasurementFilter(1);*/
Austin Schuhe040ddc2022-03-17 00:16:47 -0700542 }
Austin Schuhe040ddc2022-03-17 00:16:47 -0700543 }
544
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800545 void set_intake_falcon_front(::std::unique_ptr<frc::TalonFX> t) {
546 intake_falcon_front_ = ::std::move(t);
Griffin Bui67abb912022-01-22 16:16:21 -0800547 }
milind-u086d7262022-01-19 20:44:18 -0800548
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800549 void set_intake_falcon_back(::std::unique_ptr<frc::TalonFX> t) {
550 intake_falcon_back_ = ::std::move(t);
551 }
552
553 void set_roller_falcon_front(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800554 ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800555 roller_falcon_front_ = ::std::move(t);
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800556 WriteConfigs(roller_falcon_front_.get(),
557 Values::kIntakeRollerStatorCurrentLimit(),
558 Values::kIntakeRollerSupplyCurrentLimit());
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800559 }
560
561 void set_roller_falcon_back(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800562 ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800563 roller_falcon_back_ = ::std::move(t);
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800564 WriteConfigs(roller_falcon_back_.get(),
565 Values::kIntakeRollerStatorCurrentLimit(),
566 Values::kIntakeRollerSupplyCurrentLimit());
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800567 }
568
Griffin Buibcbef482022-02-23 15:32:10 -0800569 void set_flipper_arms_falcon(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800570 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800571 flipper_arms_falcon_ = t;
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800572 WriteConfigs(flipper_arms_falcon_.get(),
573 Values::kFlipperArmSupplyCurrentLimit(),
574 Values::kFlipperArmStatorCurrentLimit());
Griffin Buibcbef482022-02-23 15:32:10 -0800575 }
576
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800577 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> flipper_arms_falcon() {
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800578 return flipper_arms_falcon_;
579 }
580
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700581 void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
582 transfer_roller_victor_ = ::std::move(t);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800583 }
584
Austin Schuhda7e3e12022-03-26 15:14:31 -0700585 std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
586
587 void set_superstructure_reading(
588 std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
589 superstructure_reading_ = superstructure_reading;
590 }
591
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800592 private:
Griffin Bui67abb912022-01-22 16:16:21 -0800593 void Stop() override {
594 AOS_LOG(WARNING, "Superstructure output too old.\n");
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800595 climber_falcon_->SetDisabled();
milind-u6e7d8d42022-04-06 18:30:43 -0700596 climber_servo_left_->SetRaw(0);
597 climber_servo_right_->SetRaw(0);
598
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800599 Disable(roller_falcon_front_.get());
600 Disable(roller_falcon_back_.get());
601 Disable(flipper_arms_falcon_.get());
602
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800603 intake_falcon_front_->SetDisabled();
604 intake_falcon_back_->SetDisabled();
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700605 transfer_roller_victor_->SetDisabled();
Austin Schuhe040ddc2022-03-17 00:16:47 -0700606 if (catapult_falcon_1_) {
607 catapult_falcon_1_->SetDisabled();
608 }
609 if (catapult_falcon_1_can_) {
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800610 Disable(catapult_falcon_1_can_.get());
611 Disable(catapult_falcon_2_can_.get());
Austin Schuhe040ddc2022-03-17 00:16:47 -0700612 }
Jacob Ismael322ebb92022-02-09 20:12:47 -0800613 turret_falcon_->SetDisabled();
Griffin Bui67abb912022-01-22 16:16:21 -0800614 }
615
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800616 void Write(const superstructure::Output &output) override {
Austin Schuh8507c9f2022-03-13 18:08:28 -0700617 WritePwm(-output.climber_voltage(), climber_falcon_.get());
milind-u6e7d8d42022-04-06 18:30:43 -0700618 climber_servo_left_->SetPosition(output.climber_servo_left());
619 climber_servo_right_->SetPosition(output.climber_servo_right());
Griffin Buibcbef482022-02-23 15:32:10 -0800620
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800621 WritePwm(output.intake_voltage_front(), intake_falcon_front_.get());
622 WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
623 WriteCan(output.roller_voltage_front(), roller_falcon_front_.get());
624 WriteCan(output.roller_voltage_back(), roller_falcon_back_.get());
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700625 WritePwm(output.transfer_roller_voltage(), transfer_roller_victor_.get());
Griffin Buibcbef482022-02-23 15:32:10 -0800626
Austin Schuh465a2882022-03-05 15:39:04 -0800627 WriteCan(-output.flipper_arms_voltage(), flipper_arms_falcon_.get());
Griffin Buibcbef482022-02-23 15:32:10 -0800628
Austin Schuhe040ddc2022-03-17 00:16:47 -0700629 if (catapult_falcon_1_) {
630 WritePwm(output.catapult_voltage(), catapult_falcon_1_.get());
Austin Schuhda7e3e12022-03-26 15:14:31 -0700631 superstructure_reading_->Set(false);
632 if (output.catapult_voltage() > 0) {
633 catapult_reversal_->Set(true);
634 } else {
635 catapult_reversal_->Set(false);
636 }
Austin Schuhe040ddc2022-03-17 00:16:47 -0700637 }
638 if (catapult_falcon_1_can_) {
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800639 WriteCanCatapult(output.catapult_voltage(), catapult_falcon_1_can_.get());
640 WriteCanCatapult(output.catapult_voltage(), catapult_falcon_2_can_.get());
Austin Schuhe040ddc2022-03-17 00:16:47 -0700641 }
Griffin Buibcbef482022-02-23 15:32:10 -0800642
Austin Schuh465a2882022-03-05 15:39:04 -0800643 WritePwm(-output.turret_voltage(), turret_falcon_.get());
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800644 }
645
646 static void WriteCan(const double voltage,
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800647 ::ctre::phoenix6::hardware::TalonFX *falcon) {
648 ctre::phoenix6::controls::DutyCycleOut control(
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800649 std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800650 control.UpdateFreqHz = 0_Hz;
651 control.EnableFOC = true;
652
653 falcon->SetControl(control);
654 }
655 // We do this to set our UpdateFreqHz higher
656 static void WriteCanCatapult(const double voltage,
657 ::ctre::phoenix6::hardware::TalonFX *falcon) {
658 ctre::phoenix6::controls::DutyCycleOut control(
659 std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
660 control.UpdateFreqHz = 1000_Hz;
661 control.EnableFOC = true;
662
663 falcon->SetControl(control);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800664 }
665
666 template <typename T>
667 static void WritePwm(const double voltage, T *motor) {
668 motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
669 12.0);
670 }
671
672 ::std::unique_ptr<frc::TalonFX> intake_falcon_front_, intake_falcon_back_;
673
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800674 ::std::unique_ptr<::ctre::phoenix6::hardware::TalonFX> roller_falcon_front_,
675 roller_falcon_back_;
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800676
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800677 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> flipper_arms_falcon_;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800678
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800679 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult_falcon_1_can_,
680 catapult_falcon_2_can_;
Austin Schuhe040ddc2022-03-17 00:16:47 -0700681
Jacob Ismael322ebb92022-02-09 20:12:47 -0800682 ::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
Austin Schuh465a2882022-03-05 15:39:04 -0800683 climber_falcon_;
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700684 ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
Austin Schuhda7e3e12022-03-26 15:14:31 -0700685
686 std::unique_ptr<frc::DigitalOutput> catapult_reversal_;
milind-u6e7d8d42022-04-06 18:30:43 -0700687
688 ::std::unique_ptr<::frc::Servo> climber_servo_left_, climber_servo_right_;
milind-u086d7262022-01-19 20:44:18 -0800689};
690
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800691class CANSensorReader {
692 public:
693 CANSensorReader(aos::EventLoop *event_loop)
694 : event_loop_(event_loop),
695 can_position_sender_(
696 event_loop->MakeSender<superstructure::CANPosition>(
697 "/superstructure")) {
698 event_loop->SetRuntimeRealtimePriority(16);
699
700 phased_loop_handler_ =
701 event_loop_->AddPhasedLoop([this](int) { Loop(); }, kPeriod);
702 phased_loop_handler_->set_name("CAN SensorReader Loop");
703
704 event_loop->OnRun([this]() { Loop(); });
705 }
706
707 void set_flipper_arms_falcon(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800708 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> t) {
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800709 flipper_arms_falcon_ = std::move(t);
710 }
711
712 private:
713 void Loop() {
714 auto builder = can_position_sender_.MakeBuilder();
715 superstructure::CANPosition::Builder can_position_builder =
716 builder.MakeBuilder<superstructure::CANPosition>();
717 can_position_builder.add_flipper_arm_integrated_sensor_velocity(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800718 flipper_arms_falcon_->GetVelocity().GetValue().value() *
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800719 kVelocityConversion);
720 builder.CheckOk(builder.Send(can_position_builder.Finish()));
721 }
722
723 static constexpr std::chrono::milliseconds kPeriod =
724 std::chrono::milliseconds(20);
725 // 2048 encoder counts / 100 ms to rad/sec
726 static constexpr double kVelocityConversion = (2.0 * M_PI / 2048) * 0.100;
727 aos::EventLoop *event_loop_;
728 ::aos::PhasedLoopHandler *phased_loop_handler_;
729
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800730 ::std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> flipper_arms_falcon_;
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800731 aos::Sender<superstructure::CANPosition> can_position_sender_;
732};
733
milind-u086d7262022-01-19 20:44:18 -0800734class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
735 public:
736 ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
737 return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
738 frc::Encoder::k4X);
739 }
740
741 void Run() override {
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800742 std::shared_ptr<const Values> values =
743 std::make_shared<const Values>(constants::MakeValues());
744
milind-u086d7262022-01-19 20:44:18 -0800745 aos::FlatbufferDetachedBuffer<aos::Configuration> config =
Austin Schuhc5fa6d92022-02-25 14:36:28 -0800746 aos::configuration::ReadConfig("aos_config.json");
milind-u086d7262022-01-19 20:44:18 -0800747
748 // Thread 1.
749 ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
750 ::frc971::wpilib::JoystickSender joystick_sender(
751 &joystick_sender_event_loop);
752 AddLoop(&joystick_sender_event_loop);
753
754 // Thread 2.
755 ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
756 ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800757 AddLoop(&pdp_fetcher_event_loop);
milind-u086d7262022-01-19 20:44:18 -0800758
Austin Schuhda7e3e12022-03-26 15:14:31 -0700759 std::shared_ptr<frc::DigitalOutput> superstructure_reading =
760 make_unique<frc::DigitalOutput>(25);
761
milind-u086d7262022-01-19 20:44:18 -0800762 // Thread 3.
763 ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800764 SensorReader sensor_reader(&sensor_reader_event_loop, values);
Austin Schuh028d81d2022-03-26 15:11:42 -0700765 sensor_reader.set_pwm_trigger(true);
Austin Schuha8014282022-03-05 12:36:38 -0800766 sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
767 sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
Austin Schuhda7e3e12022-03-26 15:14:31 -0700768 sensor_reader.set_superstructure_reading(superstructure_reading);
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800769
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800770 sensor_reader.set_intake_encoder_front(make_encoder(3));
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800771 sensor_reader.set_intake_front_absolute_pwm(
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800772 make_unique<frc::DigitalInput>(3));
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800773 sensor_reader.set_intake_front_potentiometer(
Austin Schuha8014282022-03-05 12:36:38 -0800774 make_unique<frc::AnalogInput>(3));
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800775
776 sensor_reader.set_intake_encoder_back(make_encoder(4));
777 sensor_reader.set_intake_back_absolute_pwm(
778 make_unique<frc::DigitalInput>(4));
779 sensor_reader.set_intake_back_potentiometer(
Austin Schuha8014282022-03-05 12:36:38 -0800780 make_unique<frc::AnalogInput>(4));
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800781
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800782 sensor_reader.set_turret_encoder(make_encoder(5));
783 sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(5));
Austin Schuha8014282022-03-05 12:36:38 -0800784 sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(5));
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800785
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800786 // TODO(milind): correct intake beambreak ports once set
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800787 sensor_reader.set_intake_beambreak_front(make_unique<frc::DigitalInput>(1));
Austin Schuh445ae832022-03-05 22:52:23 -0800788 sensor_reader.set_intake_beambreak_back(make_unique<frc::DigitalInput>(6));
Milo Lin4950ac52022-02-25 19:56:11 -0800789 sensor_reader.set_turret_beambreak(make_unique<frc::DigitalInput>(7));
790
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800791 sensor_reader.set_climber_potentiometer(make_unique<frc::AnalogInput>(7));
milind-u086d7262022-01-19 20:44:18 -0800792
Griffin Buibcbef482022-02-23 15:32:10 -0800793 sensor_reader.set_flipper_arm_left_potentiometer(
Austin Schuha8014282022-03-05 12:36:38 -0800794 make_unique<frc::AnalogInput>(0));
Griffin Buibcbef482022-02-23 15:32:10 -0800795 sensor_reader.set_flipper_arm_right_potentiometer(
Austin Schuha8014282022-03-05 12:36:38 -0800796 make_unique<frc::AnalogInput>(1));
Griffin Buibcbef482022-02-23 15:32:10 -0800797
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800798 // TODO(milind): correct catapult encoder and absolute pwm ports
Austin Schuha8014282022-03-05 12:36:38 -0800799 sensor_reader.set_catapult_encoder(make_encoder(2));
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800800 sensor_reader.set_catapult_absolute_pwm(
Austin Schuha8014282022-03-05 12:36:38 -0800801 std::make_unique<frc::DigitalInput>(2));
Milind Upadhyay482b0ba2022-02-26 21:51:59 -0800802 sensor_reader.set_catapult_potentiometer(
Austin Schuha8014282022-03-05 12:36:38 -0800803 std::make_unique<frc::AnalogInput>(2));
Austin Schuh39f26f62022-02-24 21:34:46 -0800804
Austin Schuhf9a166e2022-03-05 17:53:12 -0800805 sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(9));
806 sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(8));
Ravago Jones0e86e242022-02-12 18:38:14 -0800807
milind-u086d7262022-01-19 20:44:18 -0800808 AddLoop(&sensor_reader_event_loop);
809
810 // Thread 4.
811 ::aos::ShmEventLoop output_event_loop(&config.message());
812 ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
813 drivetrain_writer.set_left_controller0(
Austin Schuh2d22fb12022-03-06 14:43:29 -0800814 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), false);
Austin Schuha8014282022-03-05 12:36:38 -0800815 drivetrain_writer.set_right_controller0(
Austin Schuh2d22fb12022-03-06 14:43:29 -0800816 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), true);
milind-u086d7262022-01-19 20:44:18 -0800817
818 SuperstructureWriter superstructure_writer(&output_event_loop);
819
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800820 superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(3));
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800821 superstructure_writer.set_roller_falcon_front(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800822 make_unique<::ctre::phoenix6::hardware::TalonFX>(0));
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800823 superstructure_writer.set_roller_falcon_back(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800824 make_unique<::ctre::phoenix6::hardware::TalonFX>(1));
Austin Schuh465a2882022-03-05 15:39:04 -0800825
Milind Upadhyay29dcc172022-04-02 19:21:30 -0700826 superstructure_writer.set_transfer_roller_victor(
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800827 make_unique<::frc::VictorSP>(5));
Austin Schuh465a2882022-03-05 15:39:04 -0800828
Milind Upadhyay3020daa2022-03-04 19:44:47 -0800829 superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(2));
830 superstructure_writer.set_intake_falcon_back(make_unique<frc::TalonFX>(4));
Austin Schuh465a2882022-03-05 15:39:04 -0800831 superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(8));
milind-u6e7d8d42022-04-06 18:30:43 -0700832 superstructure_writer.set_climber_servo_left(make_unique<frc::Servo>(7));
833 superstructure_writer.set_climber_servo_right(make_unique<frc::Servo>(6));
Griffin Buibcbef482022-02-23 15:32:10 -0800834 superstructure_writer.set_flipper_arms_falcon(
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800835 make_unique<::ctre::phoenix6::hardware::TalonFX>(2));
Austin Schuhda7e3e12022-03-26 15:14:31 -0700836 superstructure_writer.set_superstructure_reading(superstructure_reading);
Griffin Bui67abb912022-01-22 16:16:21 -0800837
Austin Schuhda7e3e12022-03-26 15:14:31 -0700838 if (!FLAGS_can_catapult) {
839 superstructure_writer.set_catapult_falcon_1(make_unique<frc::TalonFX>(9));
840 } else {
Maxwell Henderson1c0843c2023-12-22 16:20:59 -0800841 std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult1 =
842 make_unique<::ctre::phoenix6::hardware::TalonFX>(3, "Catapult");
843 std::shared_ptr<::ctre::phoenix6::hardware::TalonFX> catapult2 =
844 make_unique<::ctre::phoenix6::hardware::TalonFX>(4, "Catapult");
Austin Schuhda7e3e12022-03-26 15:14:31 -0700845 superstructure_writer.set_catapult_falcon_1(catapult1, catapult2);
846 sensor_reader.set_catapult_falcon_1(catapult1, catapult2);
847 }
Austin Schuh39f26f62022-02-24 21:34:46 -0800848
milind-u086d7262022-01-19 20:44:18 -0800849 AddLoop(&output_event_loop);
850
Henry Speiser77747b72022-03-06 17:18:29 -0800851 // Thread 5.
852 ::aos::ShmEventLoop led_indicator_event_loop(&config.message());
853 control_loops::superstructure::LedIndicator led_indicator(
854 &led_indicator_event_loop);
855 AddLoop(&led_indicator_event_loop);
856
milind-u086d7262022-01-19 20:44:18 -0800857 RunLoops();
858 }
859};
860
861} // namespace wpilib
862} // namespace y2022
863
864AOS_ROBOT_CLASS(::y2022::wpilib::WPILibRobot);