blob: cf2dcad5e3d8ac5b8131391f98b367d611990f07 [file] [log] [blame]
Campbell Crowleyae6e8422017-02-05 12:38:50 -08001#include <stdio.h>
2#include <string.h>
3#include <unistd.h>
4#include <inttypes.h>
5
6#include <chrono>
7#include <thread>
8#include <mutex>
9#include <functional>
10#include <array>
Brian Silverman50826c02017-02-18 14:40:25 -080011#include <cmath>
Campbell Crowleyae6e8422017-02-05 12:38:50 -080012
Brian Silverman50826c02017-02-18 14:40:25 -080013#include "Counter.h"
Campbell Crowleyae6e8422017-02-05 12:38:50 -080014#include "Encoder.h"
15#include "VictorSP.h"
16#include "Relay.h"
17#include "DriverStation.h"
18#include "AnalogInput.h"
19#include "Compressor.h"
20#include "DigitalGlitchFilter.h"
21#undef ERROR
22
23#include "aos/common/logging/logging.h"
24#include "aos/common/logging/queue_logging.h"
25#include "aos/common/time.h"
26#include "aos/common/util/log_interval.h"
27#include "aos/common/util/phased_loop.h"
28#include "aos/common/util/wrapping_counter.h"
29#include "aos/common/stl_mutex.h"
30#include "aos/linux_code/init.h"
31#include "aos/common/messages/robot_state.q.h"
32#include "aos/common/commonmath.h"
33
34#include "frc971/control_loops/control_loops.q.h"
35#include "frc971/control_loops/drivetrain/drivetrain.q.h"
36#include "y2017/constants.h"
Campbell Crowleyae6e8422017-02-05 12:38:50 -080037#include "y2017/control_loops/superstructure/superstructure.q.h"
38#include "y2017/actors/autonomous_action.q.h"
39
40#include "frc971/wpilib/wpilib_robot_base.h"
41#include "frc971/wpilib/joystick_sender.h"
42#include "frc971/wpilib/loop_output_handler.h"
43#include "frc971/wpilib/buffered_solenoid.h"
44#include "frc971/wpilib/buffered_pcm.h"
45#include "frc971/wpilib/gyro_sender.h"
46#include "frc971/wpilib/dma_edge_counting.h"
47#include "frc971/wpilib/interrupt_edge_counting.h"
48#include "frc971/wpilib/encoder_and_potentiometer.h"
49#include "frc971/wpilib/logging.q.h"
50#include "frc971/wpilib/wpilib_interface.h"
51#include "frc971/wpilib/pdp_fetcher.h"
52#include "frc971/wpilib/ADIS16448.h"
53#include "frc971/wpilib/dma.h"
54
55#ifndef M_PI
56#define M_PI 3.14159265358979323846
57#endif
58
59using ::frc971::control_loops::drivetrain_queue;
60using ::y2017::control_loops::superstructure_queue;
Brian Silverman052e69d2017-02-12 16:19:55 -080061using ::y2017::constants::Values;
Campbell Crowleyae6e8422017-02-05 12:38:50 -080062
63namespace y2017 {
64namespace wpilib {
65namespace {
Brian Silverman052e69d2017-02-12 16:19:55 -080066
Campbell Crowleyae6e8422017-02-05 12:38:50 -080067constexpr double kMaxBringupPower = 12.0;
Campbell Crowleyae6e8422017-02-05 12:38:50 -080068
69// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
70// DMA stuff and then removing the * 2.0 in *_translate.
71// The low bit is direction.
72
73// TODO(brian): Replace this with ::std::make_unique once all our toolchains
74// have support.
75template <class T, class... U>
76std::unique_ptr<T> make_unique(U &&... u) {
77 return std::unique_ptr<T>(new T(std::forward<U>(u)...));
78}
79
Brian Silverman052e69d2017-02-12 16:19:55 -080080// TODO(brian): Use ::std::max instead once we have C++14 so that can be
81// constexpr.
82template <typename T>
83constexpr T max(T a, T b) {
84 return (a > b) ? a : b;
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}
Campbell Crowleyae6e8422017-02-05 12:38:50 -080090
Campbell Crowleyae6e8422017-02-05 12:38:50 -080091double drivetrain_translate(int32_t in) {
Brian Silverman052e69d2017-02-12 16:19:55 -080092 return static_cast<double>(in) /
93 Values::kDrivetrainEncoderCountsPerRevolution *
94 Values::kDrivetrainEncoderRatio * 2.0 * M_PI;
Campbell Crowleyae6e8422017-02-05 12:38:50 -080095}
96
97double drivetrain_velocity_translate(double in) {
Brian Silverman052e69d2017-02-12 16:19:55 -080098 return (1.0 / in) / Values::kDrivetrainCyclesPerRevolution *
99 Values::kDrivetrainEncoderRatio * 2.0 * M_PI;
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800100}
101
Brian Silverman50826c02017-02-18 14:40:25 -0800102// TODO(Travis): Make sure the number of turns is right.
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800103double intake_pot_translate(double voltage) {
Brian Silverman50826c02017-02-18 14:40:25 -0800104 return voltage * Values::kIntakePotRatio * (3.0 /*turns*/ / 5.0 /*volts*/) *
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800105 (2 * M_PI /*radians*/);
106}
107
Brian Silverman052e69d2017-02-12 16:19:55 -0800108double turret_pot_translate(double voltage) {
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800109 return -voltage * Values::kTurretPotRatio * (10.0 /*turns*/ / 5.0 /*volts*/) *
Brian Silverman052e69d2017-02-12 16:19:55 -0800110 (2 * M_PI /*radians*/);
111}
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800112
Brian Silverman052e69d2017-02-12 16:19:55 -0800113constexpr double kMaxFastEncoderPulsesPerSecond =
114 max(Values::kMaxDrivetrainEncoderPulsesPerSecond,
115 Values::kMaxShooterEncoderPulsesPerSecond);
116static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
117 "fast encoders are too fast");
118constexpr double kMaxMediumEncoderPulsesPerSecond =
119 max(Values::kMaxIntakeEncoderPulsesPerSecond,
120 Values::kMaxTurretEncoderPulsesPerSecond,
121 Values::kMaxIndexerEncoderPulsesPerSecond);
122static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
123 "medium encoders are too fast");
124constexpr double kMaxSlowEncoderPulsesPerSecond =
125 Values::kMaxHoodEncoderPulsesPerSecond;
126static_assert(kMaxSlowEncoderPulsesPerSecond <= 100000,
127 "slow encoders are too fast");
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800128
Brian Silverman50826c02017-02-18 14:40:25 -0800129// Handles reading the duty cycle on a DigitalInput.
130class DutyCycleReader {
131 public:
132 void set_input(::std::unique_ptr<DigitalInput> input) {
133 high_counter_.reset(new Counter(input.get()));
134 high_counter_->SetMaxPeriod(kMaxPeriod);
135 high_counter_->SetSemiPeriodMode(true);
136
137 period_length_counter_.reset(new Counter(input.get()));
138 period_length_counter_->SetMaxPeriod(kMaxPeriod);
139 period_length_counter_->SetUpSourceEdge(true, false);
140
141 input_ = ::std::move(input);
142 }
143
144 double Read() const {
145 const double high_time = high_counter_->GetPeriod();
146 const double period_length = period_length_counter_->GetPeriod();
147 if (!::std::isfinite(high_time) || !::std::isfinite(period_length)) {
148 return ::std::numeric_limits<double>::quiet_NaN();
149 }
150 return high_time / period_length;
151 }
152
153 private:
154 static constexpr ::std::chrono::nanoseconds kNominalPeriod =
155 ::std::chrono::microseconds(4096);
156 static constexpr double kMaxPeriod =
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800157 (::std::chrono::duration_cast<::std::chrono::duration<double>>(
158 kNominalPeriod) *
159 2).count();
Brian Silverman50826c02017-02-18 14:40:25 -0800160
161 ::std::unique_ptr<Counter> high_counter_, period_length_counter_;
162 ::std::unique_ptr<DigitalInput> input_;
163};
164
165class AbsoluteEncoderAndPotentiometer {
166 public:
167 void set_absolute_pwm(::std::unique_ptr<DigitalInput> input) {
168 duty_cycle_.set_input(::std::move(input));
169 }
170
171 void set_encoder(::std::unique_ptr<Encoder> encoder) {
172 encoder_ = ::std::move(encoder);
173 }
174
175 void set_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
176 potentiometer_ = ::std::move(potentiometer);
177 }
178
179 double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
180
181 int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
182
183 double ReadPotentiometerVoltage() const {
184 return potentiometer_->GetVoltage();
185 }
186
187 private:
188 DutyCycleReader duty_cycle_;
189 ::std::unique_ptr<Encoder> encoder_;
190 ::std::unique_ptr<AnalogInput> potentiometer_;
191};
192
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800193// Class to send position messages with sensor readings to our loops.
194class SensorReader {
195 public:
196 SensorReader() {
Brian Silverman052e69d2017-02-12 16:19:55 -0800197 // Set to filter out anything shorter than 1/4 of the minimum pulse width
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800198 // we should ever see.
Brian Silverman052e69d2017-02-12 16:19:55 -0800199 fast_encoder_filter_.SetPeriodNanoSeconds(
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800200 static_cast<int>(1 / 4.0 /* built-in tolerance */ /
Brian Silverman052e69d2017-02-12 16:19:55 -0800201 kMaxFastEncoderPulsesPerSecond * 1e9 +
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800202 0.5));
Brian Silverman052e69d2017-02-12 16:19:55 -0800203 medium_encoder_filter_.SetPeriodNanoSeconds(
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800204 static_cast<int>(1 / 4.0 /* built-in tolerance */ /
Brian Silverman052e69d2017-02-12 16:19:55 -0800205 kMaxMediumEncoderPulsesPerSecond * 1e9 +
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800206 0.5));
Brian Silverman052e69d2017-02-12 16:19:55 -0800207 slow_encoder_filter_.SetPeriodNanoSeconds(
208 static_cast<int>(1 / 4.0 /* built-in tolerance */ /
209 kMaxSlowEncoderPulsesPerSecond * 1e9 +
210 0.5));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800211 }
212
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800213 void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800214 fast_encoder_filter_.Add(encoder.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800215 drivetrain_left_encoder_ = ::std::move(encoder);
216 }
217
218 void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800219 fast_encoder_filter_.Add(encoder.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800220 drivetrain_right_encoder_ = ::std::move(encoder);
221 }
222
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800223 void set_shooter_encoder(::std::unique_ptr<Encoder> encoder) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800224 fast_encoder_filter_.Add(encoder.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800225 shooter_encoder_ = ::std::move(encoder);
226 }
227
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800228 void set_intake_encoder(::std::unique_ptr<Encoder> encoder) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800229 medium_encoder_filter_.Add(encoder.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800230 intake_encoder_.set_encoder(::std::move(encoder));
231 }
232
233 void set_intake_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
234 intake_encoder_.set_potentiometer(::std::move(potentiometer));
235 }
236
Brian Silverman50826c02017-02-18 14:40:25 -0800237 void set_intake_absolute(::std::unique_ptr<DigitalInput> input) {
238 intake_encoder_.set_absolute_pwm(::std::move(input));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800239 }
240
Brian Silverman052e69d2017-02-12 16:19:55 -0800241 void set_indexer_encoder(::std::unique_ptr<Encoder> encoder) {
242 medium_encoder_filter_.Add(encoder.get());
243 indexer_encoder_ = ::std::move(encoder);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800244 }
245
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800246 void set_turret_encoder(::std::unique_ptr<Encoder> encoder) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800247 medium_encoder_filter_.Add(encoder.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800248 turret_encoder_.set_encoder(::std::move(encoder));
249 }
250
251 void set_turret_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
252 turret_encoder_.set_potentiometer(::std::move(potentiometer));
253 }
254
Brian Silverman50826c02017-02-18 14:40:25 -0800255 void set_turret_absolute(::std::unique_ptr<DigitalInput> input) {
256 turret_encoder_.set_absolute_pwm(::std::move(input));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800257 }
258
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800259 void set_hood_encoder(::std::unique_ptr<Encoder> encoder) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800260 slow_encoder_filter_.Add(encoder.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800261 hood_encoder_.set_encoder(::std::move(encoder));
262 }
263
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800264 void set_hood_index(::std::unique_ptr<DigitalInput> index) {
Brian Silverman052e69d2017-02-12 16:19:55 -0800265 slow_encoder_filter_.Add(index.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800266 hood_encoder_.set_index(::std::move(index));
267 }
268
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800269 void set_autonomous_mode(int i, ::std::unique_ptr<DigitalInput> sensor) {
270 autonomous_modes_.at(i) = ::std::move(sensor);
271 }
272
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800273 // All of the DMA-related set_* calls must be made before this, and it doesn't
274 // hurt to do all of them.
275 void set_dma(::std::unique_ptr<DMA> dma) {
276 dma_synchronizer_.reset(
277 new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800278 dma_synchronizer_->Add(&hood_encoder_);
279 }
280
281 void operator()() {
282 ::aos::SetCurrentThreadName("SensorReader");
283
284 my_pid_ = getpid();
285 ds_ =
286 &DriverStation::GetInstance();
287
288 dma_synchronizer_->Start();
289
290 ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
291 ::std::chrono::milliseconds(4));
292
293 ::aos::SetCurrentThreadRealtimePriority(40);
294 while (run_) {
295 {
296 const int iterations = phased_loop.SleepUntilNext();
297 if (iterations != 1) {
298 LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
299 }
300 }
301 RunIteration();
302 }
303 }
304
305 void RunIteration() {
306 ::frc971::wpilib::SendRobotState(my_pid_, ds_);
307
308 const auto values = constants::GetValues();
309
310 {
311 auto drivetrain_message = drivetrain_queue.position.MakeMessage();
312 drivetrain_message->right_encoder =
313 drivetrain_translate(drivetrain_right_encoder_->GetRaw());
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800314 drivetrain_message->right_speed =
315 drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
316
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800317 drivetrain_message->left_encoder =
318 -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
319 drivetrain_message->left_speed =
320 drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800321
322 drivetrain_message.Send();
323 }
324
325 dma_synchronizer_->RunIteration();
326
327 {
328 auto superstructure_message = superstructure_queue.position.MakeMessage();
Brian Silverman052e69d2017-02-12 16:19:55 -0800329 CopyPosition(intake_encoder_, &superstructure_message->intake,
Brian Silverman50826c02017-02-18 14:40:25 -0800330 Values::kIntakeEncoderCountsPerRevolution,
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800331 Values::kIntakeEncoderRatio, intake_pot_translate, true,
Brian Silverman052e69d2017-02-12 16:19:55 -0800332 values.intake.pot_offset);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800333
Brian Silverman052e69d2017-02-12 16:19:55 -0800334 superstructure_message->theta_indexer =
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800335 -encoder_translate(indexer_encoder_->GetRaw(),
336 Values::kMaxIndexerEncoderCountsPerRevolution,
337 Values::kIndexerEncoderRatio);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800338
Brian Silverman50826c02017-02-18 14:40:25 -0800339 superstructure_message->theta_shooter =
340 encoder_translate(shooter_encoder_->GetRaw(),
341 Values::kShooterEncoderCountsPerRevolution,
342 Values::kShooterEncoderRatio);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800343
Brian Silverman50826c02017-02-18 14:40:25 -0800344 CopyPosition(hood_encoder_, &superstructure_message->hood,
345 Values::kHoodEncoderCountsPerRevolution,
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800346 Values::kHoodEncoderRatio, true);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800347
Brian Silverman052e69d2017-02-12 16:19:55 -0800348 CopyPosition(turret_encoder_, &superstructure_message->turret,
Brian Silverman50826c02017-02-18 14:40:25 -0800349 Values::kTurretEncoderCountsPerRevolution,
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800350 Values::kTurretEncoderRatio, turret_pot_translate, true,
Brian Silverman052e69d2017-02-12 16:19:55 -0800351 values.turret.pot_offset);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800352
353 superstructure_message.Send();
354 }
355
356 {
357 auto auto_mode_message = ::y2017::actors::auto_mode.MakeMessage();
358 auto_mode_message->mode = 0;
359 for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
360 if (autonomous_modes_[i]->Get()) {
361 auto_mode_message->mode |= 1 << i;
362 }
363 }
364 LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
365 auto_mode_message.Send();
366 }
367 }
368
369 void Quit() { run_ = false; }
370
371 private:
Brian Silverman50826c02017-02-18 14:40:25 -0800372 double encoder_translate(int32_t value, double counts_per_revolution,
373 double ratio) {
374 return static_cast<double>(value) / counts_per_revolution * ratio *
375 (2.0 * M_PI);
376 }
377
Brian Silverman7cce2d32017-02-19 21:48:48 -0800378 void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
379 ::frc971::IndexPosition *position,
Brian Silverman50826c02017-02-18 14:40:25 -0800380 double encoder_counts_per_revolution, double encoder_ratio,
Brian Silverman7cce2d32017-02-19 21:48:48 -0800381 bool reverse) {
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800382 const double multiplier = reverse ? -1.0 : 1.0;
383 position->encoder =
Brian Silverman50826c02017-02-18 14:40:25 -0800384 multiplier * encoder_translate(encoder.polled_encoder_value(),
385 encoder_counts_per_revolution,
386 encoder_ratio);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800387 position->latched_encoder =
Brian Silverman50826c02017-02-18 14:40:25 -0800388 multiplier * encoder_translate(encoder.last_encoder_value(),
389 encoder_counts_per_revolution,
390 encoder_ratio);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800391 position->index_pulses = encoder.index_posedge_count();
392 }
393
Brian Silverman50826c02017-02-18 14:40:25 -0800394 void CopyPosition(const AbsoluteEncoderAndPotentiometer &encoder,
395 ::frc971::PotAndAbsolutePosition *position,
396 double encoder_counts_per_revolution, double encoder_ratio,
397 ::std::function<double(double)> potentiometer_translate,
398 bool reverse, double pot_offset) {
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800399 const double multiplier = reverse ? -1.0 : 1.0;
400 position->pot = multiplier * potentiometer_translate(
Brian Silverman50826c02017-02-18 14:40:25 -0800401 encoder.ReadPotentiometerVoltage()) +
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800402 pot_offset;
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800403 position->encoder =
Brian Silverman50826c02017-02-18 14:40:25 -0800404 multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
405 encoder_counts_per_revolution,
406 encoder_ratio);
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800407
408 position->absolute_encoder =
409 (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
410 : encoder.ReadAbsoluteEncoder()) *
411 encoder_ratio * (2.0 * M_PI);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800412 }
413
414 int32_t my_pid_;
415 DriverStation *ds_;
416
417 ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
418
Brian Silverman052e69d2017-02-12 16:19:55 -0800419 DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
420 slow_encoder_filter_;
421
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800422 ::std::unique_ptr<Encoder> drivetrain_left_encoder_,
423 drivetrain_right_encoder_;
424
Brian Silverman50826c02017-02-18 14:40:25 -0800425 AbsoluteEncoderAndPotentiometer intake_encoder_;
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800426
Brian Silverman052e69d2017-02-12 16:19:55 -0800427 ::std::unique_ptr<Encoder> indexer_encoder_;
428 ::std::unique_ptr<AnalogInput> indexer_hall_;
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800429
Brian Silverman50826c02017-02-18 14:40:25 -0800430 AbsoluteEncoderAndPotentiometer turret_encoder_;
Brian Silverman7cce2d32017-02-19 21:48:48 -0800431 ::frc971::wpilib::DMAEncoder hood_encoder_;
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800432 ::std::unique_ptr<Encoder> shooter_encoder_;
433
434 ::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
435
436 ::std::atomic<bool> run_{true};
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800437};
438
439class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
440 public:
441 void set_drivetrain_left_victor(::std::unique_ptr<VictorSP> t) {
442 drivetrain_left_victor_ = ::std::move(t);
443 }
444
445 void set_drivetrain_right_victor(::std::unique_ptr<VictorSP> t) {
446 drivetrain_right_victor_ = ::std::move(t);
447 }
448
449 private:
450 virtual void Read() override {
451 ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
452 }
453
454 virtual void Write() override {
455 auto &queue = ::frc971::control_loops::drivetrain_queue.output;
456 LOG_STRUCT(DEBUG, "will output", *queue);
457 drivetrain_left_victor_->SetSpeed(queue->left_voltage / 12.0);
458 drivetrain_right_victor_->SetSpeed(-queue->right_voltage / 12.0);
459 }
460
461 virtual void Stop() override {
462 LOG(WARNING, "drivetrain output too old\n");
463 drivetrain_left_victor_->SetDisabled();
464 drivetrain_right_victor_->SetDisabled();
465 }
466
467 ::std::unique_ptr<VictorSP> drivetrain_left_victor_, drivetrain_right_victor_;
468};
469
470class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
471 public:
472 void set_intake_victor(::std::unique_ptr<VictorSP> t) {
473 intake_victor_ = ::std::move(t);
474 }
475 void set_intake_rollers_victor(::std::unique_ptr<VictorSP> t) {
476 intake_rollers_victor_ = ::std::move(t);
477 }
478
Brian Silverman052e69d2017-02-12 16:19:55 -0800479 void set_indexer_victor(::std::unique_ptr<VictorSP> t) {
480 indexer_victor_ = ::std::move(t);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800481 }
Brian Silverman052e69d2017-02-12 16:19:55 -0800482 void set_indexer_roller_victor(::std::unique_ptr<VictorSP> t) {
483 indexer_roller_victor_ = ::std::move(t);
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800484 }
485
486 void set_shooter_victor(::std::unique_ptr<VictorSP> t) {
487 shooter_victor_ = ::std::move(t);
488 }
489 void set_turret_victor(::std::unique_ptr<VictorSP> t) {
490 turret_victor_ = ::std::move(t);
491 }
492 void set_hood_victor(::std::unique_ptr<VictorSP> t) {
493 hood_victor_ = ::std::move(t);
494 }
495
496 private:
497 virtual void Read() override {
498 ::y2017::control_loops::superstructure_queue.output.FetchAnother();
499 }
500
501 virtual void Write() override {
502 auto &queue = ::y2017::control_loops::superstructure_queue.output;
503 LOG_STRUCT(DEBUG, "will output", *queue);
504 intake_victor_->SetSpeed(::aos::Clip(queue->voltage_intake,
505 -kMaxBringupPower, kMaxBringupPower) /
506 12.0);
507 intake_rollers_victor_->SetSpeed(queue->voltage_intake_rollers / 12.0);
Brian Silverman052e69d2017-02-12 16:19:55 -0800508 indexer_victor_->SetSpeed(queue->voltage_indexer / 12.0);
509 indexer_roller_victor_->SetSpeed(queue->voltage_indexer_rollers /
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800510 12.0);
511 turret_victor_->SetSpeed(::aos::Clip(queue->voltage_turret,
512 -kMaxBringupPower, kMaxBringupPower) /
513 12.0);
514 hood_victor_->SetSpeed(
515 ::aos::Clip(queue->voltage_hood, -kMaxBringupPower, kMaxBringupPower) /
516 12.0);
517 shooter_victor_->SetSpeed(queue->voltage_shooter / 12.0);
518 }
519
520 virtual void Stop() override {
521 LOG(WARNING, "Superstructure output too old.\n");
522 intake_victor_->SetDisabled();
523 intake_rollers_victor_->SetDisabled();
Brian Silverman052e69d2017-02-12 16:19:55 -0800524 indexer_victor_->SetDisabled();
525 indexer_roller_victor_->SetDisabled();
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800526 turret_victor_->SetDisabled();
527 hood_victor_->SetDisabled();
528 shooter_victor_->SetDisabled();
529 }
530
531 ::std::unique_ptr<VictorSP> intake_victor_, intake_rollers_victor_,
Brian Silverman052e69d2017-02-12 16:19:55 -0800532 indexer_victor_, indexer_roller_victor_, shooter_victor_,
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800533 turret_victor_, hood_victor_;
534};
535
536class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
537 public:
538 ::std::unique_ptr<Encoder> make_encoder(int index) {
539 return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
540 Encoder::k4X);
541 }
542
543 void Run() override {
544 ::aos::InitNRT();
545 ::aos::SetCurrentThreadName("StartCompetition");
546
547 ::frc971::wpilib::JoystickSender joystick_sender;
548 ::std::thread joystick_thread(::std::ref(joystick_sender));
549
550 ::frc971::wpilib::PDPFetcher pdp_fetcher;
551 ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
552 SensorReader reader;
553
554 // TODO(campbell): Update port numbers
555 reader.set_drivetrain_left_encoder(make_encoder(0));
556 reader.set_drivetrain_right_encoder(make_encoder(1));
557
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800558 reader.set_intake_encoder(make_encoder(3));
Brian Silverman50826c02017-02-18 14:40:25 -0800559 reader.set_intake_absolute(make_unique<DigitalInput>(0));
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800560 reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800561
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800562 reader.set_indexer_encoder(make_encoder(5));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800563
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800564 reader.set_turret_encoder(make_encoder(6));
565 reader.set_turret_absolute(make_unique<DigitalInput>(2));
566 reader.set_turret_potentiometer(make_unique<AnalogInput>(5));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800567
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800568 reader.set_hood_encoder(make_encoder(4));
569 reader.set_hood_index(make_unique<DigitalInput>(1));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800570
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800571 reader.set_shooter_encoder(make_encoder(2));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800572
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800573 reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
574 reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
575 reader.set_autonomous_mode(2, make_unique<DigitalInput>(7));
576 reader.set_autonomous_mode(3, make_unique<DigitalInput>(6));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800577
578 reader.set_dma(make_unique<DMA>());
579 ::std::thread reader_thread(::std::ref(reader));
580
581 ::frc971::wpilib::GyroSender gyro_sender;
582 ::std::thread gyro_thread(::std::ref(gyro_sender));
583
584 auto imu_trigger = make_unique<DigitalInput>(5);
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800585 ::frc971::wpilib::ADIS16448 imu(SPI::Port::kOnboardCS1, imu_trigger.get());
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800586 ::std::thread imu_thread(::std::ref(imu));
587
588 DrivetrainWriter drivetrain_writer;
589 drivetrain_writer.set_drivetrain_left_victor(
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800590 ::std::unique_ptr<VictorSP>(new VictorSP(7)));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800591 drivetrain_writer.set_drivetrain_right_victor(
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800592 ::std::unique_ptr<VictorSP>(new VictorSP(3)));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800593 ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
594
595 SuperstructureWriter superstructure_writer;
596 superstructure_writer.set_intake_victor(
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800597 ::std::unique_ptr<VictorSP>(new VictorSP(1)));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800598 superstructure_writer.set_intake_rollers_victor(
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800599 ::std::unique_ptr<VictorSP>(new VictorSP(4)));
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800600 superstructure_writer.set_indexer_victor(
601 ::std::unique_ptr<VictorSP>(new VictorSP(6)));
Brian Silverman052e69d2017-02-12 16:19:55 -0800602 superstructure_writer.set_indexer_roller_victor(
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800603 ::std::unique_ptr<VictorSP>(new VictorSP(5)));
604 superstructure_writer.set_turret_victor(
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800605 ::std::unique_ptr<VictorSP>(new VictorSP(9)));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800606 superstructure_writer.set_hood_victor(
Austin Schuh0fc1e6d2017-02-21 02:04:10 -0800607 ::std::unique_ptr<VictorSP>(new VictorSP(2)));
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800608 superstructure_writer.set_shooter_victor(
609 ::std::unique_ptr<VictorSP>(new VictorSP(8)));
610 ::std::thread superstructure_writer_thread(
611 ::std::ref(superstructure_writer));
612
613 // Wait forever. Not much else to do...
614 while (true) {
615 const int r = select(0, nullptr, nullptr, nullptr, nullptr);
616 if (r != 0) {
617 PLOG(WARNING, "infinite select failed");
618 } else {
619 PLOG(WARNING, "infinite select succeeded??\n");
620 }
621 }
622
623 LOG(ERROR, "Exiting WPILibRobot\n");
624
625 joystick_sender.Quit();
626 joystick_thread.join();
627 pdp_fetcher.Quit();
628 pdp_fetcher_thread.join();
629 reader.Quit();
630 reader_thread.join();
631 gyro_sender.Quit();
632 gyro_thread.join();
633 imu.Quit();
634 imu_thread.join();
635
636 drivetrain_writer.Quit();
637 drivetrain_writer_thread.join();
638 superstructure_writer.Quit();
639 superstructure_writer_thread.join();
640
641 ::aos::Cleanup();
642 }
643};
644
Brian Silverman052e69d2017-02-12 16:19:55 -0800645} // namespace
Campbell Crowleyae6e8422017-02-05 12:38:50 -0800646} // namespace wpilib
647} // namespace y2017
648
649AOS_ROBOT_CLASS(::y2017::wpilib::WPILibRobot);