blob: e918aaf4080a92f03b806b981b2becaa9a12a406 [file] [log] [blame]
Sabina Davisabeae332019-02-01 21:12:57 -08001#include <unistd.h>
2
3#include <array>
4#include <chrono>
Tyler Chatowbf0609c2021-07-31 16:13:27 -07005#include <cinttypes>
Sabina Davisabeae332019-02-01 21:12:57 -08006#include <cmath>
Tyler Chatowbf0609c2021-07-31 16:13:27 -07007#include <cstdio>
8#include <cstring>
Sabina Davisabeae332019-02-01 21:12:57 -08009#include <functional>
Tyler Chatowbf0609c2021-07-31 16:13:27 -070010#include <memory>
Sabina Davisabeae332019-02-01 21:12:57 -080011#include <mutex>
12#include <thread>
13
James Kuszmaul7077d342021-06-09 20:23:58 -070014#include "ctre/phoenix/CANifier.h"
Philipp Schrader790cb542023-07-05 21:06:52 -070015
Sabina Davisabeae332019-02-01 21:12:57 -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"
21#include "frc971/wpilib/ahal/VictorSP.h"
22#undef ERROR
23
Philipp Schrader790cb542023-07-05 21:06:52 -070024#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
25
Sabina Davisabeae332019-02-01 21:12:57 -080026#include "aos/commonmath.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070027#include "aos/events/event_loop.h"
28#include "aos/events/shm_event_loop.h"
Sabina Davisabeae332019-02-01 21:12:57 -080029#include "aos/init.h"
30#include "aos/logging/logging.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070031#include "aos/realtime.h"
Sabina Davisabeae332019-02-01 21:12:57 -080032#include "aos/time/time.h"
Sabina Davisabeae332019-02-01 21:12:57 -080033#include "aos/util/log_interval.h"
34#include "aos/util/phased_loop.h"
35#include "aos/util/wrapping_counter.h"
Austin Schuhed5b26d2019-12-05 20:51:59 -080036#include "frc971/autonomous/auto_mode_generated.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070037#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
James Kuszmaul7077d342021-06-09 20:23:58 -070038#include "frc971/input/robot_state_generated.h"
Sabina Davisabeae332019-02-01 21:12:57 -080039#include "frc971/wpilib/ADIS16448.h"
Austin Schuhc1d6f832019-02-15 23:22:17 -080040#include "frc971/wpilib/buffered_pcm.h"
41#include "frc971/wpilib/buffered_solenoid.h"
Sabina Davisabeae332019-02-01 21:12:57 -080042#include "frc971/wpilib/dma.h"
Sabina Davisd004fd62019-02-02 23:51:46 -080043#include "frc971/wpilib/drivetrain_writer.h"
Sabina Davisabeae332019-02-01 21:12:57 -080044#include "frc971/wpilib/encoder_and_potentiometer.h"
Sabina Davisabeae332019-02-01 21:12:57 -080045#include "frc971/wpilib/joystick_sender.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070046#include "frc971/wpilib/logging_generated.h"
Sabina Davisabeae332019-02-01 21:12:57 -080047#include "frc971/wpilib/loop_output_handler.h"
48#include "frc971/wpilib/pdp_fetcher.h"
Sabina Davisadc58542019-02-01 22:23:00 -080049#include "frc971/wpilib/sensor_reader.h"
Sabina Davisabeae332019-02-01 21:12:57 -080050#include "frc971/wpilib/wpilib_robot_base.h"
Austin Schuhed5b26d2019-12-05 20:51:59 -080051#include "y2019/camera_log_generated.h"
Sabina Davis7be49f32019-02-02 00:30:19 -080052#include "y2019/constants.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070053#include "y2019/control_loops/drivetrain/camera_generated.h"
54#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
55#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
Brian Silvermanf8b75252019-02-24 16:13:58 -080056#include "y2019/jevois/spi.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070057#include "y2019/status_light_generated.h"
Sabina Davisabeae332019-02-01 21:12:57 -080058
59#ifndef M_PI
60#define M_PI 3.14159265358979323846
61#endif
62
Sabina Davisabeae332019-02-01 21:12:57 -080063using ::aos::monotonic_clock;
James Kuszmaul7077d342021-06-09 20:23:58 -070064using ::y2019::constants::Values;
Alex Perrycb7da4b2019-08-28 19:35:56 -070065namespace superstructure = ::y2019::control_loops::superstructure;
Sabina Davisabeae332019-02-01 21:12:57 -080066namespace chrono = ::std::chrono;
Vinay Sivae52a6b32021-07-10 15:19:26 -070067using std::make_unique;
Sabina Davisabeae332019-02-01 21:12:57 -080068
69namespace y2019 {
70namespace wpilib {
71namespace {
72
73constexpr double kMaxBringupPower = 12.0;
74
75// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
76// DMA stuff and then removing the * 2.0 in *_translate.
77// The low bit is direction.
78
79// TODO(brian): Use ::std::max instead once we have C++14 so that can be
80// constexpr.
81template <typename T>
82constexpr T max(T a, T b) {
83 return (a > b) ? a : b;
84}
85
86template <typename T, typename... Rest>
87constexpr T max(T a, T b, T c, Rest... rest) {
88 return max(max(a, b), c, rest...);
89}
90
91double drivetrain_translate(int32_t in) {
Sabina Davis7be49f32019-02-02 00:30:19 -080092 return ((static_cast<double>(in) /
93 Values::kDrivetrainEncoderCountsPerRevolution()) *
Sabina Davisabeae332019-02-01 21:12:57 -080094 (2.0 * M_PI)) *
95 Values::kDrivetrainEncoderRatio() *
Sabina Davis7be49f32019-02-02 00:30:19 -080096 control_loops::drivetrain::kWheelRadius;
Sabina Davisabeae332019-02-01 21:12:57 -080097}
98
99double drivetrain_velocity_translate(double in) {
Sabina Davis7be49f32019-02-02 00:30:19 -0800100 return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
Sabina Davisabeae332019-02-01 21:12:57 -0800101 (2.0 * M_PI)) *
102 Values::kDrivetrainEncoderRatio() *
Sabina Davis7be49f32019-02-02 00:30:19 -0800103 control_loops::drivetrain::kWheelRadius;
Sabina Davisabeae332019-02-01 21:12:57 -0800104}
105
Alex Perry5fb5ff22019-02-09 21:53:17 -0800106double elevator_pot_translate(double voltage) {
107 return voltage * Values::kElevatorPotRatio() *
Austin Schuhed7f8632019-02-15 23:12:20 -0800108 (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800109}
110
111double wrist_pot_translate(double voltage) {
Austin Schuhed7f8632019-02-15 23:12:20 -0800112 return voltage * Values::kWristPotRatio() * (5.0 /*turns*/ / 5.0 /*volts*/) *
Alex Perry5fb5ff22019-02-09 21:53:17 -0800113 (2 * M_PI /*radians*/);
114}
115
116double stilts_pot_translate(double voltage) {
117 return voltage * Values::kStiltsPotRatio() *
118 (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
119}
120
Sabina Davisabeae332019-02-01 21:12:57 -0800121constexpr double kMaxFastEncoderPulsesPerSecond =
Alex Perry5fb5ff22019-02-09 21:53:17 -0800122 max(Values::kMaxDrivetrainEncoderPulsesPerSecond(),
123 Values::kMaxIntakeEncoderPulsesPerSecond());
Sabina Davisabeae332019-02-01 21:12:57 -0800124static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
125 "fast encoders are too fast");
Sabina Davisabeae332019-02-01 21:12:57 -0800126constexpr double kMaxMediumEncoderPulsesPerSecond =
Alex Perry5fb5ff22019-02-09 21:53:17 -0800127 max(Values::kMaxElevatorEncoderPulsesPerSecond(),
128 Values::kMaxWristEncoderPulsesPerSecond());
Theo Bafrali00e42272019-02-12 01:07:46 -0800129
Sabina Davisabeae332019-02-01 21:12:57 -0800130static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
131 "medium encoders are too fast");
132
133// Class to send position messages with sensor readings to our loops.
Sabina Davisadc58542019-02-01 22:23:00 -0800134class SensorReader : public ::frc971::wpilib::SensorReader {
Sabina Davisabeae332019-02-01 21:12:57 -0800135 public:
Austin Schuh217a9782019-12-21 23:02:50 -0800136 SensorReader(::aos::ShmEventLoop *event_loop)
Austin Schuha250b2d2019-05-27 16:14:02 -0700137 : ::frc971::wpilib::SensorReader(event_loop),
138 auto_mode_sender_(
139 event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
Austin Schuhed5b26d2019-12-05 20:51:59 -0800140 "/autonomous")),
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700141 superstructure_position_sender_(
Alex Perrycb7da4b2019-08-28 19:35:56 -0700142 event_loop->MakeSender<superstructure::Position>(
143 "/superstructure")),
Austin Schuhbd0a40f2019-06-30 14:56:31 -0700144 drivetrain_position_sender_(
Alex Perrycb7da4b2019-08-28 19:35:56 -0700145 event_loop
146 ->MakeSender<::frc971::control_loops::drivetrain::Position>(
147 "/drivetrain")) {
Sabina Davisabeae332019-02-01 21:12:57 -0800148 // Set to filter out anything shorter than 1/4 of the minimum pulse width
149 // we should ever see.
Austin Schuh45a549f2019-02-02 15:43:56 -0800150 UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
151 UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
Sabina Davisabeae332019-02-01 21:12:57 -0800152 }
153
Alex Perry5fb5ff22019-02-09 21:53:17 -0800154 // Elevator
155
156 void set_elevator_encoder(::std::unique_ptr<frc::Encoder> encoder) {
157 medium_encoder_filter_.Add(encoder.get());
158 elevator_encoder_.set_encoder(::std::move(encoder));
159 }
160
161 void set_elevator_absolute_pwm(
162 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
163 elevator_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
164 }
165
166 void set_elevator_potentiometer(
167 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
168 elevator_encoder_.set_potentiometer(::std::move(potentiometer));
169 }
170
171 // Intake
172
173 void set_intake_encoder(::std::unique_ptr<frc::Encoder> encoder) {
174 medium_encoder_filter_.Add(encoder.get());
175 intake_encoder_.set_encoder(::std::move(encoder));
176 }
177
178 void set_intake_absolute_pwm(
179 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
180 intake_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
181 }
182
183 // Wrist
184
185 void set_wrist_encoder(::std::unique_ptr<frc::Encoder> encoder) {
186 medium_encoder_filter_.Add(encoder.get());
187 wrist_encoder_.set_encoder(::std::move(encoder));
188 }
189
190 void set_wrist_absolute_pwm(
191 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
192 wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
193 }
194
195 void set_wrist_potentiometer(
196 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
197 wrist_encoder_.set_potentiometer(::std::move(potentiometer));
198 }
199
200 // Stilts
201
202 void set_stilts_encoder(::std::unique_ptr<frc::Encoder> encoder) {
203 medium_encoder_filter_.Add(encoder.get());
204 stilts_encoder_.set_encoder(::std::move(encoder));
205 }
206
207 void set_stilts_absolute_pwm(
208 ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
209 stilts_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
210 }
211
212 void set_stilts_potentiometer(
213 ::std::unique_ptr<frc::AnalogInput> potentiometer) {
214 stilts_encoder_.set_potentiometer(::std::move(potentiometer));
215 }
216
Austin Schuhe2f22482019-04-13 23:05:43 -0700217 void set_platform_left_detect(
218 ::std::unique_ptr<frc::DigitalInput> platform_left_detect) {
219 platform_left_detect_ = ::std::move(platform_left_detect);
220 }
221
222 void set_platform_right_detect(
223 ::std::unique_ptr<frc::DigitalInput> platform_right_detect) {
224 platform_right_detect_ = ::std::move(platform_right_detect);
225 }
226
Austin Schuh461e1182019-02-17 14:56:44 -0800227 // Vacuum pressure sensor
228 void set_vacuum_sensor(int port) {
229 vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
230 }
231
Austin Schuha9644062019-03-28 14:31:52 -0700232 // Auto mode switches.
233 void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
234 autonomous_modes_.at(i) = ::std::move(sensor);
235 }
236
Sabina Davis399dbd82019-02-01 23:06:08 -0800237 void RunIteration() override {
Sabina Davisabeae332019-02-01 21:12:57 -0800238 {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700239 auto builder = drivetrain_position_sender_.MakeBuilder();
240 frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
241 builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
242 drivetrain_builder.add_left_encoder(
243 drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
244 drivetrain_builder.add_left_speed(
245 drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
Sabina Davisabeae332019-02-01 21:12:57 -0800246
Alex Perrycb7da4b2019-08-28 19:35:56 -0700247 drivetrain_builder.add_right_encoder(
248 -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
249 drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
250 drivetrain_right_encoder_->GetPeriod()));
Sabina Davisabeae332019-02-01 21:12:57 -0800251
milind1f1dca32021-07-03 13:50:07 -0700252 builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
Sabina Davisabeae332019-02-01 21:12:57 -0800253 }
Alex Perry5fb5ff22019-02-09 21:53:17 -0800254 const auto values = constants::GetValues();
255
256 {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700257 auto builder = superstructure_position_sender_.MakeBuilder();
Alex Perry5fb5ff22019-02-09 21:53:17 -0800258
259 // Elevator
Alex Perrycb7da4b2019-08-28 19:35:56 -0700260 frc971::PotAndAbsolutePositionT elevator;
261 CopyPosition(elevator_encoder_, &elevator,
Alex Perry5fb5ff22019-02-09 21:53:17 -0800262 Values::kElevatorEncoderCountsPerRevolution(),
263 Values::kElevatorEncoderRatio(), elevator_pot_translate,
264 false, values.elevator.potentiometer_offset);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700265 flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
266 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &elevator);
267
Alex Perry5fb5ff22019-02-09 21:53:17 -0800268 // Intake
Alex Perrycb7da4b2019-08-28 19:35:56 -0700269 frc971::AbsolutePositionT intake_joint;
270 CopyPosition(intake_encoder_, &intake_joint,
Alex Perry5fb5ff22019-02-09 21:53:17 -0800271 Values::kIntakeEncoderCountsPerRevolution(),
272 Values::kIntakeEncoderRatio(), false);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700273 flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
274 frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800275
276 // Wrist
Alex Perrycb7da4b2019-08-28 19:35:56 -0700277 frc971::PotAndAbsolutePositionT wrist;
278 CopyPosition(wrist_encoder_, &wrist,
Alex Perry5fb5ff22019-02-09 21:53:17 -0800279 Values::kWristEncoderCountsPerRevolution(),
280 Values::kWristEncoderRatio(), wrist_pot_translate, false,
281 values.wrist.potentiometer_offset);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700282 flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
283 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &wrist);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800284
285 // Stilts
Alex Perrycb7da4b2019-08-28 19:35:56 -0700286 frc971::PotAndAbsolutePositionT stilts;
287 CopyPosition(stilts_encoder_, &stilts,
Alex Perry5fb5ff22019-02-09 21:53:17 -0800288 Values::kStiltsEncoderCountsPerRevolution(),
289 Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
290 values.stilts.potentiometer_offset);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700291 flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
292 frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &stilts);
293
294 superstructure::Position::Builder position_builder =
295 builder.MakeBuilder<superstructure::Position>();
296
297 position_builder.add_elevator(elevator_offset);
298 position_builder.add_intake_joint(intake_joint_offset);
299 position_builder.add_wrist(wrist_offset);
300 position_builder.add_stilts(stilts_offset);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800301
Austin Schuh461e1182019-02-17 14:56:44 -0800302 // Suction
303 constexpr float kMinVoltage = 0.5;
304 constexpr float kMaxVoltage = 2.1;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700305 position_builder.add_suction_pressure(
Austin Schuh461e1182019-02-17 14:56:44 -0800306 (vacuum_sensor_->GetVoltage() - kMinVoltage) /
Alex Perrycb7da4b2019-08-28 19:35:56 -0700307 (kMaxVoltage - kMinVoltage));
Austin Schuh461e1182019-02-17 14:56:44 -0800308
Alex Perrycb7da4b2019-08-28 19:35:56 -0700309 position_builder.add_platform_left_detect(!platform_left_detect_->Get());
310 position_builder.add_platform_right_detect(
311 !platform_right_detect_->Get());
Austin Schuhe2f22482019-04-13 23:05:43 -0700312
milind1f1dca32021-07-03 13:50:07 -0700313 builder.CheckOk(builder.Send(position_builder.Finish()));
Alex Perry5fb5ff22019-02-09 21:53:17 -0800314 }
Austin Schuha9644062019-03-28 14:31:52 -0700315
316 {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700317 auto builder = auto_mode_sender_.MakeBuilder();
318
319 uint32_t mode = 0;
Austin Schuha9644062019-03-28 14:31:52 -0700320 for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
321 if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700322 mode |= 1 << i;
Austin Schuha9644062019-03-28 14:31:52 -0700323 }
324 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700325
326 auto auto_mode_builder =
327 builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
328
329 auto_mode_builder.add_mode(mode);
330
milind1f1dca32021-07-03 13:50:07 -0700331 builder.CheckOk(builder.Send(auto_mode_builder.Finish()));
Austin Schuha9644062019-03-28 14:31:52 -0700332 }
Alex Perry5fb5ff22019-02-09 21:53:17 -0800333 }
334
335 private:
Austin Schuha250b2d2019-05-27 16:14:02 -0700336 ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700337 ::aos::Sender<superstructure::Position> superstructure_position_sender_;
338 ::aos::Sender<::frc971::control_loops::drivetrain::Position>
Austin Schuhbd0a40f2019-06-30 14:56:31 -0700339 drivetrain_position_sender_;
Austin Schuha250b2d2019-05-27 16:14:02 -0700340
Alex Perry5fb5ff22019-02-09 21:53:17 -0800341 ::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
342 wrist_encoder_, stilts_encoder_;
343
Austin Schuhe2f22482019-04-13 23:05:43 -0700344 ::std::unique_ptr<frc::DigitalInput> platform_left_detect_;
345 ::std::unique_ptr<frc::DigitalInput> platform_right_detect_;
346
Austin Schuh461e1182019-02-17 14:56:44 -0800347 ::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
348
Austin Schuha9644062019-03-28 14:31:52 -0700349 ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
350
Alex Perry5fb5ff22019-02-09 21:53:17 -0800351 ::frc971::wpilib::AbsoluteEncoder intake_encoder_;
Alex Perry5fb5ff22019-02-09 21:53:17 -0800352};
353
Brian Silvermanf8b75252019-02-24 16:13:58 -0800354class CameraReader {
355 public:
Austin Schuh8a633d52019-05-12 15:04:01 -0700356 CameraReader(::aos::EventLoop *event_loop)
357 : camera_frame_sender_(
358 event_loop
359 ->MakeSender<::y2019::control_loops::drivetrain::CameraFrame>(
Austin Schuhed5b26d2019-12-05 20:51:59 -0800360 "/camera")),
Austin Schuh5671a8c2019-05-19 17:01:04 -0700361 camera_log_fetcher_(
Austin Schuhed5b26d2019-12-05 20:51:59 -0800362 event_loop->MakeFetcher<::y2019::CameraLog>("/camera")) {}
Austin Schuh8a633d52019-05-12 15:04:01 -0700363
Brian Silvermanf8b75252019-02-24 16:13:58 -0800364 CameraReader(const CameraReader &) = delete;
365 CameraReader &operator=(const CameraReader &) = delete;
366
367 void set_spi(frc::SPI *spi) {
368 spi_ = spi;
369 spi_->SetClockRate(1e6);
370 spi_->SetChipSelectActiveHigh();
James Kuszmaul9776b392023-01-14 14:08:08 -0800371 spi_->SetMode(frc::SPI::Mode::kMode3);
Brian Silvermanf8b75252019-02-24 16:13:58 -0800372 }
373
Brian Silverman7ecf0672019-03-02 15:30:03 -0800374 void set_activate_usb(std::unique_ptr<frc::DigitalInput> activate_usb) {
375 activate_usb_ = std::move(activate_usb);
376 }
377
378 void set_activate_passthrough(
379 std::unique_ptr<frc::DigitalInput> activate_passthrough) {
380 activate_passthrough_ = std::move(activate_passthrough);
381 }
382
Brian Silvermanf8b75252019-02-24 16:13:58 -0800383 void DoSpiTransaction() {
384 using namespace frc971::jevois;
385 RoborioToTeensy to_teensy{};
386 to_teensy.realtime_now = aos::realtime_clock::now();
Austin Schuh5671a8c2019-05-19 17:01:04 -0700387 camera_log_fetcher_.Fetch();
Brian Silverman7ecf0672019-03-02 15:30:03 -0800388 if (activate_usb_ && !activate_usb_->Get()) {
389 to_teensy.camera_command = CameraCommand::kUsb;
390 } else if (activate_passthrough_ && !activate_passthrough_->Get()) {
391 to_teensy.camera_command = CameraCommand::kCameraPassthrough;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700392 } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log()) {
Austin Schuh4e2629d2019-03-28 14:44:37 -0700393 to_teensy.camera_command = CameraCommand::kLog;
Brian Silverman7ecf0672019-03-02 15:30:03 -0800394 } else {
395 to_teensy.camera_command = CameraCommand::kNormal;
396 }
Brian Silvermanf8b75252019-02-24 16:13:58 -0800397
398 std::array<char, spi_transfer_size() + 1> to_send{};
399 {
400 const auto to_send_data =
Austin Schuhb72be802022-01-02 12:26:28 -0800401 absl::MakeSpan(to_send).last(spi_transfer_size());
Brian Silvermanf8b75252019-02-24 16:13:58 -0800402 const auto encoded = SpiPackToTeensy(to_teensy);
403 std::copy(encoded.begin(), encoded.end(), to_send_data.begin());
404 }
405 rx_clearer_.ClearRxFifo();
406 // First, send recieve a dummy byte because the Teensy can't control what it
407 // sends for the first byte.
408 std::array<char, spi_transfer_size() + 1> to_receive;
Austin Schuhb72be802022-01-02 12:26:28 -0800409 DoTransaction(absl::Span<char>(to_send), absl::Span<char>(to_receive));
Brian Silvermanf8b75252019-02-24 16:13:58 -0800410 const auto unpacked = SpiUnpackToRoborio(
Austin Schuhb72be802022-01-02 12:26:28 -0800411 absl::MakeSpan(to_receive).last(spi_transfer_size()));
Brian Silvermanf8b75252019-02-24 16:13:58 -0800412 if (!unpacked) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700413 AOS_LOG(INFO, "Decoding SPI data failed\n");
Brian Silvermanf8b75252019-02-24 16:13:58 -0800414 return;
415 }
416
Alex Perrycb7da4b2019-08-28 19:35:56 -0700417 const aos::monotonic_clock::time_point now = aos::monotonic_clock::now();
Brian Silvermanc41fb862019-03-02 21:14:46 -0800418 for (const auto &received : unpacked->frames) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700419 auto builder = camera_frame_sender_.MakeBuilder();
420
421 std::array<
422 flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>,
423 3>
424 targets;
425
426 for (size_t i = 0; i < received.targets.size(); ++i) {
427 y2019::control_loops::drivetrain::CameraTarget::Builder
428 camera_target_builder = builder.MakeBuilder<
429 y2019::control_loops::drivetrain::CameraTarget>();
430
431 camera_target_builder.add_distance(received.targets[i].distance);
432 camera_target_builder.add_height(received.targets[i].height);
433 camera_target_builder.add_heading(received.targets[i].heading);
434 camera_target_builder.add_skew(received.targets[i].skew);
435
436 targets[i] = camera_target_builder.Finish();
437 }
438
439 flatbuffers::Offset<flatbuffers::Vector<
440 flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>>>
441 targets_offset = builder.fbb()->CreateVector(targets.begin(),
442 received.targets.size());
443
444 y2019::control_loops::drivetrain::CameraFrame::Builder
445 camera_frame_builder =
446 builder
447 .MakeBuilder<y2019::control_loops::drivetrain::CameraFrame>();
448
449 camera_frame_builder.add_targets(targets_offset);
450
James Kuszmaule08f04e2019-05-01 21:46:50 -0500451 // Add an extra 10ms delay to account for unmodeled delays that Austin
452 // thinks exists.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700453 camera_frame_builder.add_timestamp(
James Kuszmaule08f04e2019-05-01 21:46:50 -0500454 std::chrono::nanoseconds(
455 (now - received.age - ::std::chrono::milliseconds(10))
Alex Perrycb7da4b2019-08-28 19:35:56 -0700456 .time_since_epoch())
457 .count());
458 camera_frame_builder.add_camera(received.camera_index);
milind1f1dca32021-07-03 13:50:07 -0700459 builder.CheckOk(builder.Send(camera_frame_builder.Finish()));
Brian Silvermanc41fb862019-03-02 21:14:46 -0800460 }
Brian Silvermanf8b75252019-02-24 16:13:58 -0800461
462 if (dummy_spi_) {
463 uint8_t dummy_send, dummy_receive;
464 dummy_spi_->Transaction(&dummy_send, &dummy_receive, 1);
465 }
466 }
467
Austin Schuhb72be802022-01-02 12:26:28 -0800468 void DoTransaction(absl::Span<char> to_send, absl::Span<char> to_receive) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700469 AOS_CHECK_EQ(to_send.size(), to_receive.size());
Austin Schuhb72be802022-01-02 12:26:28 -0800470 const int result = spi_->Transaction(
Brian Silvermanf8b75252019-02-24 16:13:58 -0800471 reinterpret_cast<uint8_t *>(to_send.data()),
472 reinterpret_cast<uint8_t *>(to_receive.data()), to_send.size());
Austin Schuhb72be802022-01-02 12:26:28 -0800473 if (result == static_cast<int>(to_send.size())) {
Brian Silvermanf8b75252019-02-24 16:13:58 -0800474 return;
475 }
476 if (result == -1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700477 AOS_LOG(INFO, "SPI::Transaction of %zd bytes failed\n", to_send.size());
Brian Silvermanf8b75252019-02-24 16:13:58 -0800478 return;
479 }
Austin Schuhf257f3c2019-10-27 21:00:43 -0700480 AOS_LOG(FATAL, "SPI::Transaction returned something weird\n");
Brian Silvermanf8b75252019-02-24 16:13:58 -0800481 }
482
483 void SetDummySPI(frc::SPI::Port port) {
484 dummy_spi_.reset(new frc::SPI(port));
485 // Pick the same settings here in case the roboRIO decides to try something
486 // stupid when switching.
487 if (dummy_spi_) {
488 dummy_spi_->SetClockRate(1e5);
489 dummy_spi_->SetChipSelectActiveLow();
James Kuszmaul9776b392023-01-14 14:08:08 -0800490 dummy_spi_->SetMode(frc::SPI::Mode::kMode3);
Brian Silvermanf8b75252019-02-24 16:13:58 -0800491 }
492 }
493
494 private:
Austin Schuh8a633d52019-05-12 15:04:01 -0700495 ::aos::Sender<::y2019::control_loops::drivetrain::CameraFrame>
496 camera_frame_sender_;
Austin Schuh5671a8c2019-05-19 17:01:04 -0700497 ::aos::Fetcher<::y2019::CameraLog> camera_log_fetcher_;
Austin Schuh8a633d52019-05-12 15:04:01 -0700498
Brian Silvermanf8b75252019-02-24 16:13:58 -0800499 frc::SPI *spi_ = nullptr;
500 ::std::unique_ptr<frc::SPI> dummy_spi_;
501
Brian Silverman7ecf0672019-03-02 15:30:03 -0800502 std::unique_ptr<frc::DigitalInput> activate_usb_;
503 std::unique_ptr<frc::DigitalInput> activate_passthrough_;
504
Brian Silvermanf8b75252019-02-24 16:13:58 -0800505 frc971::wpilib::SpiRxClearer rx_clearer_;
506};
507
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700508class SuperstructureWriter
Alex Perrycb7da4b2019-08-28 19:35:56 -0700509 : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
Alex Perry5fb5ff22019-02-09 21:53:17 -0800510 public:
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800511 SuperstructureWriter(::aos::EventLoop *event_loop)
Alex Perrycb7da4b2019-08-28 19:35:56 -0700512 : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
513 event_loop, "/superstructure"),
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800514 robot_state_fetcher_(
Alex Perrycb7da4b2019-08-28 19:35:56 -0700515 event_loop->MakeFetcher<::aos::RobotState>("/aos")) {}
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800516
Alex Perry5fb5ff22019-02-09 21:53:17 -0800517 void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
518 elevator_victor_ = ::std::move(t);
519 }
520
Austin Schuh461e1182019-02-17 14:56:44 -0800521 void set_suction_victor(::std::unique_ptr<::frc::VictorSP> t) {
522 suction_victor_ = ::std::move(t);
523 }
524
Alex Perry5fb5ff22019-02-09 21:53:17 -0800525 void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
526 intake_victor_ = ::std::move(t);
527 }
Alex Perry5fb5ff22019-02-09 21:53:17 -0800528
529 void set_wrist_victor(::std::unique_ptr<::frc::VictorSP> t) {
530 wrist_victor_ = ::std::move(t);
531 }
532
533 void set_stilts_victor(::std::unique_ptr<::frc::VictorSP> t) {
534 stilts_victor_ = ::std::move(t);
535 }
536
537 private:
Alex Perrycb7da4b2019-08-28 19:35:56 -0700538 void Write(const superstructure::Output &output) override {
539 elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage(),
Alex Perry5fb5ff22019-02-09 21:53:17 -0800540 -kMaxBringupPower,
541 kMaxBringupPower) /
542 12.0);
543
Alex Perrycb7da4b2019-08-28 19:35:56 -0700544 intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage(),
Alex Perry5fb5ff22019-02-09 21:53:17 -0800545 -kMaxBringupPower, kMaxBringupPower) /
546 12.0);
547
Alex Perrycb7da4b2019-08-28 19:35:56 -0700548 wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage(),
Alex Perry5fb5ff22019-02-09 21:53:17 -0800549 -kMaxBringupPower, kMaxBringupPower) /
550 12.0);
551
Alex Perrycb7da4b2019-08-28 19:35:56 -0700552 stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage(),
Alex Perry5fb5ff22019-02-09 21:53:17 -0800553 -kMaxBringupPower, kMaxBringupPower) /
554 12.0);
Austin Schuh461e1182019-02-17 14:56:44 -0800555
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800556 robot_state_fetcher_.Fetch();
557 const double battery_voltage = robot_state_fetcher_.get()
Alex Perrycb7da4b2019-08-28 19:35:56 -0700558 ? robot_state_fetcher_->voltage_battery()
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800559 : 12.0;
Austin Schuhc2ee66b2019-02-19 13:37:46 -0800560
561 // Throw a fast low pass filter on the battery voltage so we don't respond
562 // too fast to noise.
563 filtered_battery_voltage_ =
564 0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
565
566 suction_victor_->SetSpeed(::aos::Clip(
Alex Perrycb7da4b2019-08-28 19:35:56 -0700567 output.pump_voltage() / filtered_battery_voltage_, -1.0, 1.0));
Alex Perry5fb5ff22019-02-09 21:53:17 -0800568 }
569
Austin Schuh461e1182019-02-17 14:56:44 -0800570 void Stop() override {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700571 AOS_LOG(WARNING, "Superstructure output too old.\n");
Alex Perry5fb5ff22019-02-09 21:53:17 -0800572
573 elevator_victor_->SetDisabled();
574 intake_victor_->SetDisabled();
Alex Perry5fb5ff22019-02-09 21:53:17 -0800575 wrist_victor_->SetDisabled();
576 stilts_victor_->SetDisabled();
Austin Schuh461e1182019-02-17 14:56:44 -0800577 suction_victor_->SetDisabled();
Alex Perry5fb5ff22019-02-09 21:53:17 -0800578 }
579
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800580 ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
581
Alex Perry5fb5ff22019-02-09 21:53:17 -0800582 ::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
Austin Schuh461e1182019-02-17 14:56:44 -0800583 wrist_victor_, stilts_victor_, suction_victor_;
Austin Schuhc2ee66b2019-02-19 13:37:46 -0800584
585 double filtered_battery_voltage_ = 12.0;
Sabina Davisabeae332019-02-01 21:12:57 -0800586};
587
Austin Schuhc1d6f832019-02-15 23:22:17 -0800588class SolenoidWriter {
589 public:
Austin Schuhff973552019-05-19 16:49:28 -0700590 SolenoidWriter(::aos::EventLoop *event_loop)
591 : event_loop_(event_loop),
Alex Perrycb7da4b2019-08-28 19:35:56 -0700592 superstructure_fetcher_(
593 event_loop->MakeFetcher<superstructure::Output>("/superstructure")),
594 status_light_fetcher_(
595 event_loop->MakeFetcher<::y2019::StatusLight>("/superstructure")),
596 pneumatics_to_log_sender_(
597 event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
Austin Schuh315275b2021-06-20 14:30:31 -0700598 event_loop_->SetRuntimeRealtimePriority(27);
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700599
600 event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
601 ::std::chrono::milliseconds(20),
602 ::std::chrono::milliseconds(1));
603 }
Austin Schuhc1d6f832019-02-15 23:22:17 -0800604
Austin Schuh461e1182019-02-17 14:56:44 -0800605 void set_big_suction_cup(int index0, int index1) {
606 big_suction_cup0_ = pcm_.MakeSolenoid(index0);
607 big_suction_cup1_ = pcm_.MakeSolenoid(index1);
Austin Schuhc1d6f832019-02-15 23:22:17 -0800608 }
Austin Schuh461e1182019-02-17 14:56:44 -0800609 void set_small_suction_cup(int index0, int index1) {
610 small_suction_cup0_ = pcm_.MakeSolenoid(index0);
611 small_suction_cup1_ = pcm_.MakeSolenoid(index1);
Austin Schuhc1d6f832019-02-15 23:22:17 -0800612 }
613
614 void set_intake_roller_talon(
615 ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX> t) {
616 intake_rollers_talon_ = ::std::move(t);
Austin Schuh23a51632019-02-19 16:50:36 -0800617 intake_rollers_talon_->ConfigContinuousCurrentLimit(10.0, 0);
Austin Schuhc1d6f832019-02-15 23:22:17 -0800618 intake_rollers_talon_->EnableCurrentLimit(true);
619 }
620
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700621 void Loop(const int iterations) {
622 if (iterations != 1) {
Austin Schuhf257f3c2019-10-27 21:00:43 -0700623 AOS_LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700624 }
Austin Schuhc1d6f832019-02-15 23:22:17 -0800625
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700626 {
627 superstructure_fetcher_.Fetch();
628 if (superstructure_fetcher_.get()) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700629 big_suction_cup0_->Set(
630 !superstructure_fetcher_->intake_suction_bottom());
631 big_suction_cup1_->Set(
632 !superstructure_fetcher_->intake_suction_bottom());
633 small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top());
634 small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700635
636 intake_rollers_talon_->Set(
637 ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
Alex Perrycb7da4b2019-08-28 19:35:56 -0700638 ::aos::Clip(superstructure_fetcher_->intake_roller_voltage(),
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700639 -kMaxBringupPower, kMaxBringupPower) /
640 12.0);
641 }
642 }
643
644 {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700645 auto builder = pneumatics_to_log_sender_.MakeBuilder();
646
647 ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
648 builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700649
650 pcm_.Flush();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700651 to_log_builder.add_read_solenoids(pcm_.GetAll());
milind1f1dca32021-07-03 13:50:07 -0700652 (void)builder.Send(to_log_builder.Finish());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700653 }
654
655 status_light_fetcher_.Fetch();
656 // If we don't have a light request (or it's an old one), we are borked.
657 // Flash the red light slowly.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700658 StatusLightT color;
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700659 if (!status_light_fetcher_.get() ||
Austin Schuhad154822019-12-27 15:45:13 -0800660 status_light_fetcher_.context().monotonic_event_time +
Alex Perrycb7da4b2019-08-28 19:35:56 -0700661 chrono::milliseconds(100) <
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700662 event_loop_->monotonic_now()) {
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700663 color.red = 0.0;
664 color.green = 0.0;
665 color.blue = 0.0;
666
667 ++light_flash_;
668 if (light_flash_ > 10) {
669 color.red = 0.5;
Austin Schuhc1d6f832019-02-15 23:22:17 -0800670 }
671
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700672 if (light_flash_ > 20) {
673 light_flash_ = 0;
Austin Schuhc1d6f832019-02-15 23:22:17 -0800674 }
675
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700676 } else {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700677 status_light_fetcher_->UnPackTo(&color);
Sabina Davisc6329342019-03-01 20:44:42 -0800678 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700679 SetColor(color);
Sabina Davisc6329342019-03-01 20:44:42 -0800680 }
681
Alex Perrycb7da4b2019-08-28 19:35:56 -0700682 void SetColor(const StatusLightT status_light) {
Sabina Davisc6329342019-03-01 20:44:42 -0800683 // Save CAN bandwidth and CPU at the cost of RT. Only change the light when
684 // it actually changes. This is pretty low priority anyways.
685 static int time_since_last_send = 0;
686 ++time_since_last_send;
687 if (time_since_last_send > 10) {
688 time_since_last_send = 0;
689 }
690 if (status_light.green != last_green_ || time_since_last_send == 0) {
Sabina Davis77a11cf2019-03-09 18:20:26 -0800691 canifier_.SetLEDOutput(status_light.green,
692 ::ctre::phoenix::CANifier::LEDChannelA);
Sabina Davisc6329342019-03-01 20:44:42 -0800693 last_green_ = status_light.green;
694 }
695
696 if (status_light.blue != last_blue_ || time_since_last_send == 0) {
Sabina Davis77a11cf2019-03-09 18:20:26 -0800697 canifier_.SetLEDOutput(status_light.blue,
698 ::ctre::phoenix::CANifier::LEDChannelC);
Sabina Davisc6329342019-03-01 20:44:42 -0800699 last_blue_ = status_light.blue;
700 }
701
702 if (status_light.red != last_red_ || time_since_last_send == 0) {
Sabina Davis77a11cf2019-03-09 18:20:26 -0800703 canifier_.SetLEDOutput(status_light.red,
704 ::ctre::phoenix::CANifier::LEDChannelB);
Sabina Davisc6329342019-03-01 20:44:42 -0800705 last_red_ = status_light.red;
Austin Schuhc1d6f832019-02-15 23:22:17 -0800706 }
707 }
708
Austin Schuhc1d6f832019-02-15 23:22:17 -0800709 private:
Austin Schuhff973552019-05-19 16:49:28 -0700710 ::aos::EventLoop *event_loop_;
711
Austin Schuhc1d6f832019-02-15 23:22:17 -0800712 ::frc971::wpilib::BufferedPcm pcm_;
713
Austin Schuh461e1182019-02-17 14:56:44 -0800714 ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup0_,
715 big_suction_cup1_, small_suction_cup0_, small_suction_cup1_;
Austin Schuhc1d6f832019-02-15 23:22:17 -0800716
717 ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
718 intake_rollers_talon_;
719
Alex Perrycb7da4b2019-08-28 19:35:56 -0700720 ::aos::Fetcher<::y2019::control_loops::superstructure::Output>
Austin Schuhff973552019-05-19 16:49:28 -0700721 superstructure_fetcher_;
722 ::aos::Fetcher<::y2019::StatusLight> status_light_fetcher_;
Austin Schuhc1d6f832019-02-15 23:22:17 -0800723
Alex Perrycb7da4b2019-08-28 19:35:56 -0700724 aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
725
Sabina Davisc6329342019-03-01 20:44:42 -0800726 ::ctre::phoenix::CANifier canifier_{0};
727
Sabina Davisc6329342019-03-01 20:44:42 -0800728 double last_red_ = -1.0;
729 double last_green_ = -1.0;
730 double last_blue_ = -1.0;
731
732 int light_flash_ = 0;
Austin Schuhc1d6f832019-02-15 23:22:17 -0800733};
734
Sabina Davisabeae332019-02-01 21:12:57 -0800735class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
736 public:
737 ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
738 return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
739 frc::Encoder::k4X);
740 }
741
742 void Run() override {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700743 aos::FlatbufferDetachedBuffer<aos::Configuration> config =
Austin Schuhc5fa6d92022-02-25 14:36:28 -0800744 aos::configuration::ReadConfig("aos_config.json");
Alex Perrycb7da4b2019-08-28 19:35:56 -0700745
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700746 // Thread 1.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700747 ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700748 ::frc971::wpilib::JoystickSender joystick_sender(
749 &joystick_sender_event_loop);
750 AddLoop(&joystick_sender_event_loop);
Sabina Davisabeae332019-02-01 21:12:57 -0800751
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700752 // Thread 2.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700753 ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700754 ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
755 AddLoop(&pdp_fetcher_event_loop);
Austin Schuhdf6cbb12019-02-02 13:46:52 -0800756
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700757 // Thread 3.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700758 ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700759 SensorReader sensor_reader(&sensor_reader_event_loop);
760 sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
761 sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
Sabina Davisabeae332019-02-01 21:12:57 -0800762
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700763 sensor_reader.set_elevator_encoder(make_encoder(4));
764 sensor_reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(4));
765 sensor_reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(4));
Sabina Davisabeae332019-02-01 21:12:57 -0800766
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700767 sensor_reader.set_wrist_encoder(make_encoder(5));
768 sensor_reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(5));
769 sensor_reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(5));
Sabina Davisabeae332019-02-01 21:12:57 -0800770
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700771 sensor_reader.set_intake_encoder(make_encoder(2));
772 sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(2));
Alex Perry5fb5ff22019-02-09 21:53:17 -0800773
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700774 sensor_reader.set_stilts_encoder(make_encoder(3));
775 sensor_reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(3));
776 sensor_reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
Alex Perry5fb5ff22019-02-09 21:53:17 -0800777
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700778 sensor_reader.set_pwm_trigger(true);
779 sensor_reader.set_vacuum_sensor(7);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800780
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700781 sensor_reader.set_platform_right_detect(make_unique<frc::DigitalInput>(6));
782 sensor_reader.set_platform_left_detect(make_unique<frc::DigitalInput>(7));
Alex Perry5fb5ff22019-02-09 21:53:17 -0800783
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700784 sensor_reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
785 sensor_reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
786 AddLoop(&sensor_reader_event_loop);
Sabina Davisabeae332019-02-01 21:12:57 -0800787
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700788 // Thread 4.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700789 ::aos::ShmEventLoop imu_event_loop(&config.message());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700790 CameraReader camera_reader(&imu_event_loop);
Brian Silvermanf8b75252019-02-24 16:13:58 -0800791 frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
792 camera_reader.set_spi(&camera_spi);
793 camera_reader.SetDummySPI(frc::SPI::Port::kOnboardCS2);
Brian Silverman7ecf0672019-03-02 15:30:03 -0800794 // Austin says 8, 9, 24, and 25 are good options to choose from for these.
795 camera_reader.set_activate_usb(make_unique<frc::DigitalInput>(24));
796 camera_reader.set_activate_passthrough(make_unique<frc::DigitalInput>(25));
Brian Silvermanf8b75252019-02-24 16:13:58 -0800797
Austin Schuh3e3d4ba2019-02-15 23:14:52 -0800798 auto imu_trigger = make_unique<frc::DigitalInput>(0);
James Kuszmaul7077d342021-06-09 20:23:58 -0700799 ::frc971::wpilib::ADIS16448 imu(
800 &imu_event_loop, frc::SPI::Port::kOnboardCS1, imu_trigger.get());
Brian Silvermanf8b75252019-02-24 16:13:58 -0800801 imu.set_spi_idle_callback(
802 [&camera_reader]() { camera_reader.DoSpiTransaction(); });
Austin Schuh3e3d4ba2019-02-15 23:14:52 -0800803 auto imu_reset = make_unique<frc::DigitalOutput>(1);
Sabina Davisabeae332019-02-01 21:12:57 -0800804 imu.set_reset(imu_reset.get());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700805 AddLoop(&imu_event_loop);
Sabina Davisabeae332019-02-01 21:12:57 -0800806
807 // While as of 2/9/18 the drivetrain Victors are SPX, it appears as though
808 // they are identical, as far as DrivetrainWriter is concerned, to the SP
809 // variety so all the Victors are written as SPs.
810
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700811 // Thread 5.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700812 ::aos::ShmEventLoop output_event_loop(&config.message());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700813 ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
Sabina Davisd004fd62019-02-02 23:51:46 -0800814 drivetrain_writer.set_left_controller0(
Sabina Davis1b84afa2019-02-09 01:20:21 -0800815 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
Sabina Davisd004fd62019-02-02 23:51:46 -0800816 drivetrain_writer.set_right_controller0(
Sabina Davis1b84afa2019-02-09 01:20:21 -0800817 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
Sabina Davisabeae332019-02-01 21:12:57 -0800818
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700819 SuperstructureWriter superstructure_writer(&output_event_loop);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800820 superstructure_writer.set_elevator_victor(
Alex Perry5fb5ff22019-02-09 21:53:17 -0800821 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
Austin Schuh3e3d4ba2019-02-15 23:14:52 -0800822 // TODO(austin): Do the vacuum
Austin Schuh461e1182019-02-17 14:56:44 -0800823 superstructure_writer.set_suction_victor(
824 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
Austin Schuh3e3d4ba2019-02-15 23:14:52 -0800825 superstructure_writer.set_intake_victor(
826 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
Alex Perry5fb5ff22019-02-09 21:53:17 -0800827 superstructure_writer.set_wrist_victor(
828 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
829 superstructure_writer.set_stilts_victor(
Austin Schuh3e3d4ba2019-02-15 23:14:52 -0800830 ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700831 AddLoop(&output_event_loop);
Alex Perry5fb5ff22019-02-09 21:53:17 -0800832
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700833 // Thread 6.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700834 ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700835 SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
Austin Schuhc1d6f832019-02-15 23:22:17 -0800836 solenoid_writer.set_intake_roller_talon(
837 make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
Austin Schuh461e1182019-02-17 14:56:44 -0800838 solenoid_writer.set_big_suction_cup(0, 1);
839 solenoid_writer.set_small_suction_cup(2, 3);
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700840 AddLoop(&solenoid_writer_event_loop);
Austin Schuhc1d6f832019-02-15 23:22:17 -0800841
Austin Schuhbd1fe9c2019-06-29 16:35:48 -0700842 RunLoops();
Sabina Davisabeae332019-02-01 21:12:57 -0800843 }
844};
845
846} // namespace
847} // namespace wpilib
848} // namespace y2019
849
850AOS_ROBOT_CLASS(::y2019::wpilib::WPILibRobot);