blob: 59e715d4ceea08f138dd5f4659b73e90a2c553fe [file] [log] [blame]
Brian Silvermanc71537c2016-01-01 13:43:14 -08001#include <stdio.h>
2#include <string.h>
3#include <unistd.h>
4#include <inttypes.h>
5
Austin Schuh8aec1ed2016-05-01 13:29:20 -07006#include <chrono>
Brian Silvermanc71537c2016-01-01 13:43:14 -08007#include <thread>
8#include <mutex>
9#include <functional>
10
11#include "Encoder.h"
12#include "Talon.h"
13#include "DriverStation.h"
14#include "AnalogInput.h"
15#include "Compressor.h"
16#include "Relay.h"
Brian Silverman5090c432016-01-02 14:44:26 -080017#include "frc971/wpilib/wpilib_robot_base.h"
Brian Silvermanc71537c2016-01-01 13:43:14 -080018#ifndef WPILIB2015
19#include "DigitalGlitchFilter.h"
20#endif
21#include "PowerDistributionPanel.h"
22#undef ERROR
23
24#include "aos/common/logging/logging.h"
25#include "aos/common/logging/queue_logging.h"
26#include "aos/common/time.h"
27#include "aos/common/util/log_interval.h"
28#include "aos/common/util/phased_loop.h"
29#include "aos/common/util/wrapping_counter.h"
30#include "aos/common/stl_mutex.h"
31#include "aos/linux_code/init.h"
32#include "aos/common/messages/robot_state.q.h"
33
Austin Schuh2a671df2016-11-26 15:00:06 -080034#include "frc971/control_loops/drivetrain/drivetrain.q.h"
Brian Silvermanc71537c2016-01-01 13:43:14 -080035#include "frc971/wpilib/buffered_pcm.h"
Austin Schuh2a671df2016-11-26 15:00:06 -080036#include "frc971/wpilib/buffered_solenoid.h"
Brian Silvermanb5b46ca2016-03-13 01:14:17 -050037#include "frc971/wpilib/dma.h"
Austin Schuh2a671df2016-11-26 15:00:06 -080038#include "frc971/wpilib/dma_edge_counting.h"
39#include "frc971/wpilib/encoder_and_potentiometer.h"
40#include "frc971/wpilib/gyro_sender.h"
41#include "frc971/wpilib/interrupt_edge_counting.h"
42#include "frc971/wpilib/joystick_sender.h"
43#include "frc971/wpilib/logging.q.h"
44#include "frc971/wpilib/loop_output_handler.h"
45#include "frc971/wpilib/wpilib_interface.h"
46#include "y2012/control_loops/accessories/accessories.q.h"
Brian Silvermanc71537c2016-01-01 13:43:14 -080047
48#ifndef M_PI
49#define M_PI 3.14159265358979323846
50#endif
51
Austin Schuh2a671df2016-11-26 15:00:06 -080052using ::frc971::control_loops::drivetrain_queue;
Brian Silvermanc71537c2016-01-01 13:43:14 -080053using ::y2012::control_loops::accessories_queue;
54
55namespace y2012 {
56namespace wpilib {
57
58template <class T, class... U>
59std::unique_ptr<T> make_unique(U &&... u) {
60 return std::unique_ptr<T>(new T(std::forward<U>(u)...));
61}
62
63double drivetrain_translate(int32_t in) {
64 return -static_cast<double>(in) /
65 (256.0 /*cpr*/ * 4.0 /*4x*/) *
66 1 *
67 (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
68}
69
70double drivetrain_velocity_translate(double in) {
71 return (1.0 / in) / 256.0 /*cpr*/ *
72 1 *
73 (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
74}
75
Brian Silvermanc71537c2016-01-01 13:43:14 -080076class SensorReader {
77 public:
78 SensorReader() {}
79
80 void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
81 drivetrain_left_encoder_ = ::std::move(encoder);
82 drivetrain_left_encoder_->SetMaxPeriod(0.005);
83 }
84
85 void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
86 drivetrain_right_encoder_ = ::std::move(encoder);
87 drivetrain_right_encoder_->SetMaxPeriod(0.005);
88 }
89
90 void operator()() {
91 ::aos::SetCurrentThreadName("SensorReader");
92
93 my_pid_ = getpid();
Brian Silverman5090c432016-01-02 14:44:26 -080094
Austin Schuh8aec1ed2016-05-01 13:29:20 -070095 ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
96 ::std::chrono::milliseconds(4));
Brian Silvermanc71537c2016-01-01 13:43:14 -080097
Brian Silverman5090c432016-01-02 14:44:26 -080098 ::aos::SetCurrentThreadRealtimePriority(40);
Brian Silvermanc71537c2016-01-01 13:43:14 -080099 while (run_) {
100 {
101 const int iterations = phased_loop.SleepUntilNext();
102 if (iterations != 1) {
103 LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
104 }
105 }
106 RunIteration();
107 }
108 }
109
110 void RunIteration() {
Austin Schuh94f51e92017-10-30 19:25:32 -0700111 ::frc971::wpilib::SendRobotState(my_pid_);
Brian Silvermanc71537c2016-01-01 13:43:14 -0800112
113 {
114 auto drivetrain_message = drivetrain_queue.position.MakeMessage();
115 drivetrain_message->right_encoder =
116 drivetrain_translate(drivetrain_right_encoder_->GetRaw());
117 drivetrain_message->left_encoder =
118 -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
119 drivetrain_message->left_speed =
120 drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
121 drivetrain_message->right_speed =
122 drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
123
124 drivetrain_message.Send();
125 }
126
127 accessories_queue.position.MakeMessage().Send();
128 }
129
130 void Quit() { run_ = false; }
131
132 private:
Brian Silvermanc71537c2016-01-01 13:43:14 -0800133 int32_t my_pid_;
Brian Silvermanc71537c2016-01-01 13:43:14 -0800134
135 ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
136 ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
137 ::std::atomic<bool> run_{true};
138};
139
140class SolenoidWriter {
141 public:
142 SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
143 : pcm_(pcm),
144 drivetrain_(".y2012.control_loops.drivetrain_queue.output"),
145 accessories_(".y2012.control_loops.accessories_queue.output") {}
146
147 void set_drivetrain_high(
148 ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
149 drivetrain_high_ = ::std::move(s);
150 }
151
152 void set_drivetrain_low(
153 ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
154 drivetrain_low_ = ::std::move(s);
155 }
156
157 void set_s1(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
158 s1_ = ::std::move(s);
159 }
160
161 void set_s2(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
162 s2_ = ::std::move(s);
163 }
164
165 void set_s3(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
166 s3_ = ::std::move(s);
167 }
168
169 void operator()() {
170 ::aos::SetCurrentThreadName("Solenoids");
Brian Silverman5090c432016-01-02 14:44:26 -0800171 ::aos::SetCurrentThreadRealtimePriority(27);
172
Austin Schuh8aec1ed2016-05-01 13:29:20 -0700173 ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
174 ::std::chrono::milliseconds(1));
Brian Silvermanc71537c2016-01-01 13:43:14 -0800175
176 while (run_) {
Brian Silverman5090c432016-01-02 14:44:26 -0800177 {
178 const int iterations = phased_loop.SleepUntilNext();
179 if (iterations != 1) {
180 LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
181 }
182 }
Brian Silvermanc71537c2016-01-01 13:43:14 -0800183
184 {
185 accessories_.FetchLatest();
186 if (accessories_.get()) {
187 LOG_STRUCT(DEBUG, "solenoids", *accessories_);
188 s1_->Set(accessories_->solenoids[0]);
189 s2_->Set(accessories_->solenoids[1]);
190 s3_->Set(accessories_->solenoids[2]);
191 }
192 }
193
194 {
195 drivetrain_.FetchLatest();
196 if (drivetrain_.get()) {
197 LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
198 const bool high = drivetrain_->left_high || drivetrain_->right_high;
199 drivetrain_high_->Set(high);
200 drivetrain_low_->Set(!high);
201 }
202 }
203
204 {
205 ::frc971::wpilib::PneumaticsToLog to_log;
206 pcm_->Flush();
207 to_log.read_solenoids = pcm_->GetAll();
208 LOG_STRUCT(DEBUG, "pneumatics info", to_log);
209 }
210 }
211 }
212
213 void Quit() { run_ = false; }
214
215 private:
216 const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
217
218 ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_high_;
219 ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_low_;
220 ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s1_, s2_, s3_;
221
222 ::std::unique_ptr<Compressor> compressor_;
223
Austin Schuh2a671df2016-11-26 15:00:06 -0800224 ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
Brian Silvermanc71537c2016-01-01 13:43:14 -0800225 ::aos::Queue<::y2012::control_loops::AccessoriesQueue::Message> accessories_;
226
227 ::std::atomic<bool> run_{true};
228};
229
230class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
231 public:
232 void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
233 left_drivetrain_talon_ = ::std::move(t);
234 }
235
236 void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
237 right_drivetrain_talon_ = ::std::move(t);
238 }
239
240 private:
241 virtual void Read() override {
Austin Schuh2a671df2016-11-26 15:00:06 -0800242 drivetrain_queue.output.FetchAnother();
Brian Silvermanc71537c2016-01-01 13:43:14 -0800243 }
244
245 virtual void Write() override {
Austin Schuh2a671df2016-11-26 15:00:06 -0800246 auto &queue = drivetrain_queue.output;
Brian Silvermanc71537c2016-01-01 13:43:14 -0800247 LOG_STRUCT(DEBUG, "will output", *queue);
Brian Silverman6eaa2d82017-02-04 20:48:30 -0800248 left_drivetrain_talon_->SetSpeed(-queue->left_voltage / 12.0);
249 right_drivetrain_talon_->SetSpeed(queue->right_voltage / 12.0);
Brian Silvermanc71537c2016-01-01 13:43:14 -0800250 }
251
252 virtual void Stop() override {
253 LOG(WARNING, "drivetrain output too old\n");
Brian Silverman6eaa2d82017-02-04 20:48:30 -0800254 left_drivetrain_talon_->SetDisabled();
255 right_drivetrain_talon_->SetDisabled();
Brian Silvermanc71537c2016-01-01 13:43:14 -0800256 }
257
258 ::std::unique_ptr<Talon> left_drivetrain_talon_;
259 ::std::unique_ptr<Talon> right_drivetrain_talon_;
260};
261
262class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler {
263 public:
264 void set_talon1(::std::unique_ptr<Talon> t) {
265 talon1_ = ::std::move(t);
266 }
267
268 void set_talon2(::std::unique_ptr<Talon> t) {
269 talon2_ = ::std::move(t);
270 }
271
272 private:
273 virtual void Read() override {
274 ::y2012::control_loops::accessories_queue.output.FetchAnother();
275 }
276
277 virtual void Write() override {
278 auto &queue = ::y2012::control_loops::accessories_queue.output;
Brian Silverman6eaa2d82017-02-04 20:48:30 -0800279 talon1_->SetSpeed(queue->sticks[0]);
280 talon2_->SetSpeed(queue->sticks[1]);
Brian Silvermanc71537c2016-01-01 13:43:14 -0800281 LOG_STRUCT(DEBUG, "will output", *queue);
282 }
283
284 virtual void Stop() override {
285 LOG(WARNING, "shooter output too old\n");
Brian Silverman6eaa2d82017-02-04 20:48:30 -0800286 talon1_->SetDisabled();
287 talon2_->SetDisabled();
Brian Silvermanc71537c2016-01-01 13:43:14 -0800288 }
289
290 ::std::unique_ptr<Talon> talon1_, talon2_;
291};
292
Brian Silverman5090c432016-01-02 14:44:26 -0800293class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
Brian Silvermanc71537c2016-01-01 13:43:14 -0800294 public:
295 ::std::unique_ptr<Encoder> make_encoder(int index) {
296 return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
297 Encoder::k4X);
298 }
299
Brian Silverman5090c432016-01-02 14:44:26 -0800300 void Run() override {
Brian Silvermanc71537c2016-01-01 13:43:14 -0800301 ::aos::InitNRT();
302 ::aos::SetCurrentThreadName("StartCompetition");
303
304 ::frc971::wpilib::JoystickSender joystick_sender;
305 ::std::thread joystick_thread(::std::ref(joystick_sender));
306
307 SensorReader reader;
308
309 reader.set_drivetrain_left_encoder(make_encoder(0));
310 reader.set_drivetrain_right_encoder(make_encoder(1));
311
312 ::std::thread reader_thread(::std::ref(reader));
313
314 DrivetrainWriter drivetrain_writer;
315 drivetrain_writer.set_left_drivetrain_talon(
316 ::std::unique_ptr<Talon>(new Talon(3)));
317 drivetrain_writer.set_right_drivetrain_talon(
318 ::std::unique_ptr<Talon>(new Talon(4)));
319 ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
320
321 ::y2012::wpilib::AccessoriesWriter accessories_writer;
322 accessories_writer.set_talon1(::std::unique_ptr<Talon>(new Talon(5)));
323 accessories_writer.set_talon2(::std::unique_ptr<Talon>(new Talon(6)));
324 ::std::thread accessories_writer_thread(::std::ref(accessories_writer));
325
326 ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
327 new ::frc971::wpilib::BufferedPcm());
328 SolenoidWriter solenoid_writer(pcm);
329 solenoid_writer.set_drivetrain_high(pcm->MakeSolenoid(0));
330 solenoid_writer.set_drivetrain_low(pcm->MakeSolenoid(2));
331 solenoid_writer.set_s1(pcm->MakeSolenoid(1));
332 solenoid_writer.set_s2(pcm->MakeSolenoid(3));
333 solenoid_writer.set_s3(pcm->MakeSolenoid(4));
334
335 ::std::thread solenoid_thread(::std::ref(solenoid_writer));
336
337 // Wait forever. Not much else to do...
Brian Silverman5090c432016-01-02 14:44:26 -0800338 while (true) {
339 const int r = select(0, nullptr, nullptr, nullptr, nullptr);
340 if (r != 0) {
341 PLOG(WARNING, "infinite select failed");
342 } else {
343 PLOG(WARNING, "infinite select succeeded??\n");
344 }
345 }
Brian Silvermanc71537c2016-01-01 13:43:14 -0800346
347 LOG(ERROR, "Exiting WPILibRobot\n");
348
349 joystick_sender.Quit();
350 joystick_thread.join();
351 reader.Quit();
352 reader_thread.join();
353
354 drivetrain_writer.Quit();
355 drivetrain_writer_thread.join();
356 accessories_writer.Quit();
357 accessories_writer_thread.join();
358
359 ::aos::Cleanup();
360 }
361};
362
363} // namespace wpilib
364} // namespace y2012
365
366
Brian Silverman5090c432016-01-02 14:44:26 -0800367AOS_ROBOT_CLASS(::y2012::wpilib::WPILibRobot);