run control loops and write their output on new sensor values
This also includes sending solenoid values from their own thread at 50Hz
(formerly I613f95a6efb5efe428029e4825ba6caeb34ea326).
Change-Id: I3d3021cdbbf2ddf895e5ceebd4db299b4743e124
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 142da9f..4712bb9 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -12,29 +12,30 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, has_position, fail_no_position,
- fail_no_goal>::kStaleLogInterval;
+template <class T, bool fail_no_goal>
+constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kStaleLogInterval;
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
+template <class T, bool fail_no_goal>
void
-ControlLoop<T, has_position, fail_no_position, fail_no_goal>::ZeroOutputs() {
+ControlLoop<T, fail_no_goal>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
-void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Iterate() {
+template <class T, bool fail_no_goal>
+void ControlLoop<T, fail_no_goal>::Iterate() {
no_prior_goal_.Print();
no_sensor_generation_.Print();
- very_stale_position_.Print();
- no_prior_position_.Print();
driver_station_old_.Print();
no_driver_station_.Print();
- // Fetch the latest control loop goal and position. If there is no new
+ control_loop_->position.FetchAnother();
+ const PositionType *const position = control_loop_->position.get();
+ LOG_STRUCT(DEBUG, "position", *position);
+
+ // Fetch the latest control loop goal. If there is no new
// goal, we will just reuse the old one.
// If there is no goal, we haven't started up fully. It isn't worth
// the added complexity for each loop implementation to handle that case.
@@ -72,39 +73,6 @@
LOG_STRUCT(DEBUG, "goal", *goal);
}
- // Only pass in a position if we got one this cycle.
- const PositionType *position = NULL;
-
- // Only fetch the latest position if we have one.
- if (has_position) {
- // If the position is stale, this is really bad. Try fetching a position
- // and check how fresh it is, and then take the appropriate action.
- if (control_loop_->position.FetchLatest()) {
- position = control_loop_->position.get();
- } else {
- if (control_loop_->position.get() && !reset_) {
- int msec_age = control_loop_->position.Age().ToMSec();
- if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
- LOG_INTERVAL(very_stale_position_);
- ZeroOutputs();
- return;
- } else {
- LOG(ERROR, "Stale position. %d ms (< %d ms)\n", msec_age,
- kPositionTimeoutMs);
- }
- } else {
- LOG_INTERVAL(no_prior_position_);
- if (fail_no_position) {
- ZeroOutputs();
- return;
- }
- }
- }
- if (position) {
- LOG_STRUCT(DEBUG, "position", *position);
- }
- }
-
bool outputs_enabled = false;
// Check to see if we got a driver station packet recently.
@@ -160,14 +128,9 @@
status.Send();
}
-template <class T, bool has_position, bool fail_no_position, bool fail_no_goal>
-void ControlLoop<T, has_position, fail_no_position, fail_no_goal>::Run() {
- ::aos::time::Time::EnableMockTime();
+template <class T, bool fail_no_goal>
+void ControlLoop<T, fail_no_goal>::Run() {
while (true) {
- ::aos::time::Time::UpdateMockTime();
- const ::aos::time::Time next_loop = NextLoopTime();
- time::SleepUntil(next_loop);
- ::aos::time::Time::SetMockTime(next_loop);
Iterate();
}
}
diff --git a/aos/common/controls/control_loop.cc b/aos/common/controls/control_loop.cc
index 72201bf..c314254 100644
--- a/aos/common/controls/control_loop.cc
+++ b/aos/common/controls/control_loop.cc
@@ -3,11 +3,5 @@
namespace aos {
namespace controls {
-time::Time NextLoopTime(time::Time start) {
- return (start / static_cast<int32_t>(kLoopFrequency.ToNSec())) *
- static_cast<int32_t>(kLoopFrequency.ToNSec()) +
- kLoopFrequency;
-}
-
} // namespace controls
} // namespace aos
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index 76c06e7..e9454d1 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -39,19 +39,12 @@
// Control loops run this often, "starting" at time 0.
constexpr time::Time kLoopFrequency = time::Time::InSeconds(0.01);
-// Calculates the next time to run control loops after start.
-time::Time NextLoopTime(time::Time start = time::Time::Now());
-
// Provides helper methods to assist in writing control loops.
// This template expects to be constructed with a queue group as an argument
// that has a goal, position, status, and output queue.
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
-// If has_position is false, the control loop will always use NULL as the
-// position and not check the queue. This is used for "loops" that control
-// motors open loop.
-template <class T, bool has_position = true, bool fail_no_position = true,
- bool fail_no_goal = true>
+template <class T, bool fail_no_goal = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
@@ -157,15 +150,9 @@
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
static constexpr ::aos::time::Time kStaleLogInterval =
::aos::time::Time::InSeconds(0.1);
- SimpleLogInterval very_stale_position_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "outputs disabled because position is very stale");
SimpleLogInterval no_prior_goal_ =
SimpleLogInterval(kStaleLogInterval, ERROR,
"no prior goal");
- SimpleLogInterval no_prior_position_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "no prior position");
SimpleLogInterval no_driver_station_ =
SimpleLogInterval(kStaleLogInterval, ERROR,
"no driver station packet");
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 32fe567..10ea25f 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -333,8 +333,7 @@
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
- : aos::controls::ControlLoop<control_loops::ClawGroup, true, true,
- false>(my_claw),
+ : aos::controls::ControlLoop<control_loops::ClawGroup, false>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 12f7264..175657c 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -185,7 +185,7 @@
};
class ClawMotor : public aos::controls::ControlLoop<
- control_loops::ClawGroup, true, true, false> {
+ control_loops::ClawGroup, false> {
public:
explicit ClawMotor(control_loops::ClawGroup *my_claw =
&control_loops::claw_queue_group);
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index cba0e9a..f1b0453 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -13,14 +13,13 @@
namespace control_loops {
class DrivetrainLoop
- : public aos::controls::ControlLoop<control_loops::Drivetrain, true, false> {
+ : public aos::controls::ControlLoop<control_loops::Drivetrain> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::controls::ControlLoop<control_loops::Drivetrain, true, false>(
- my_drivetrain) {
+ : aos::controls::ControlLoop<control_loops::Drivetrain>(my_drivetrain) {
::aos::controls::HPolytope<0>::Init();
}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 61accc0..f5b2047 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -198,6 +198,17 @@
}
}
+ // Only send them out (approximately) every 10ms because the loops are now
+ // clocked off of this.
+ static int i = 0;
+ ++i;
+ if (i < 5) {
+ LOG(DEBUG, "skipping\n");
+ return;
+ } else {
+ i = 0;
+ }
+
other_sensors.MakeWithBuilder()
.sonar_distance(sonar_translate(data->main.ultrasonic_pulse_length))
.Send();
diff --git a/frc971/wpilib/buffered_pcm.cc b/frc971/wpilib/buffered_pcm.cc
new file mode 100644
index 0000000..084d4b5
--- /dev/null
+++ b/frc971/wpilib/buffered_pcm.cc
@@ -0,0 +1,29 @@
+#include "frc971/wpilib/buffered_pcm.h"
+
+#include <inttypes.h>
+
+#include "aos/common/logging/logging.h"
+
+namespace frc971 {
+namespace wpilib {
+
+::std::unique_ptr<BufferedSolenoid> BufferedPcm::MakeSolenoid(int number) {
+ return ::std::unique_ptr<BufferedSolenoid>(
+ new BufferedSolenoid(number, this));
+}
+
+void BufferedPcm::Set(int number, bool value) {
+ if (value) {
+ values_ |= 1 << number;
+ } else {
+ values_ &= ~(1 << number);
+ }
+}
+
+void BufferedPcm::Flush() {
+ LOG(DEBUG, "sending solenoids 0x%" PRIx8 "\n", values_);
+ SolenoidBase::Set(values_, 0xFF);
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/buffered_pcm.h b/frc971/wpilib/buffered_pcm.h
new file mode 100644
index 0000000..50f86fc
--- /dev/null
+++ b/frc971/wpilib/buffered_pcm.h
@@ -0,0 +1,44 @@
+#ifndef FRC971_WPILIB_BUFFERED_PCM_H_
+#define FRC971_WPILIB_BUFFERED_PCM_H_
+
+#include <memory>
+
+#include "SolenoidBase.h"
+
+#include "frc971/wpilib/buffered_solenoid.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Manages setting values for all solenoids for a single PCM in a single CAN
+// message.
+//
+// The way to use this is to call MakeSolenoid for whichever solenoid numbers
+// you want, call Set on those, and then periodically call Flush on this object
+// to write all of the buffered values out.
+class BufferedPcm : public SolenoidBase {
+ public:
+ explicit BufferedPcm(uint8_t module = GetDefaultSolenoidModule())
+ : SolenoidBase(module) {}
+
+ // Creates a new BufferedSolenoid for a specified port number.
+ ::std::unique_ptr<BufferedSolenoid> MakeSolenoid(int number);
+
+ // Actually sends all of the values out.
+ void Flush();
+
+ private:
+ // WPILib declares this pure virtual and then never calls it...
+ virtual void InitSolenoid() override {}
+
+ void Set(int number, bool value);
+
+ uint8_t values_ = 0;
+
+ friend class BufferedSolenoid;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_BUFFERED_PCM_H_
diff --git a/frc971/wpilib/buffered_solenoid.cc b/frc971/wpilib/buffered_solenoid.cc
new file mode 100644
index 0000000..cd68da8
--- /dev/null
+++ b/frc971/wpilib/buffered_solenoid.cc
@@ -0,0 +1,13 @@
+#include "frc971/wpilib/buffered_solenoid.h"
+
+#include "frc971/wpilib/buffered_pcm.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void BufferedSolenoid::Set(bool value) {
+ pcm_->Set(number_, value);
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/buffered_solenoid.h b/frc971/wpilib/buffered_solenoid.h
new file mode 100644
index 0000000..f0a4c11
--- /dev/null
+++ b/frc971/wpilib/buffered_solenoid.h
@@ -0,0 +1,28 @@
+#ifndef FRC971_WPILIB_BUFFERED_SOLENOID_H_
+#define FRC971_WPILIB_BUFFERED_SOLENOID_H_
+
+namespace frc971 {
+namespace wpilib {
+
+class BufferedPcm;
+
+// Handles sending values for a single solenoid to a BufferedPcm. Instances are
+// created with BufferedPcm::MakeSolenoid.
+class BufferedSolenoid {
+ public:
+ // Sets or unsets the solenoid.
+ void Set(bool value);
+
+ private:
+ BufferedSolenoid(int number, BufferedPcm *pcm) : number_(number), pcm_(pcm) {}
+
+ const int number_;
+ BufferedPcm *const pcm_;
+
+ friend class BufferedPcm;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_BUFFERED_SOLENOID_H_
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
new file mode 100644
index 0000000..b7b112e
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -0,0 +1,72 @@
+#include "frc971/wpilib/loop_output_handler.h"
+
+#include <sys/timerfd.h>
+
+#include <thread>
+#include <functional>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/messages/robot_state.q.h"
+
+namespace frc971 {
+namespace wpilib {
+
+constexpr ::aos::time::Time LoopOutputHandler::kStopTimeout;
+
+LoopOutputHandler::LoopOutputHandler() : watchdog_(this) {}
+
+void LoopOutputHandler::operator()() {
+ ::aos::SetCurrentThreadName("OutputHandler");
+ ::std::thread watchdog_thread(::std::ref(watchdog_));
+
+ ::aos::SetCurrentThreadRealtimePriority(30);
+ while (run_) {
+ no_robot_state_.Print();
+ fake_robot_state_.Print();
+ Read();
+ ::aos::robot_state.FetchLatest();
+ if (!::aos::robot_state.get()) {
+ LOG_INTERVAL(no_robot_state_);
+ continue;
+ }
+ if (::aos::robot_state->fake) {
+ LOG_INTERVAL(fake_robot_state_);
+ continue;
+ }
+
+ watchdog_.Reset();
+ Write();
+ }
+
+ Stop();
+
+ watchdog_.Quit();
+ watchdog_thread.join();
+}
+
+LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler)
+ : handler_(handler),
+ timerfd_(timerfd_create(::aos::time::Time::kDefaultClock, 0)) {
+ if (timerfd_.get() == -1) {
+ PLOG(FATAL, "timerfd_create(Time::kDefaultClock, 0)");
+ }
+ ::aos::SetCurrentThreadRealtimePriority(35);
+ ::aos::SetCurrentThreadName("OutputWatchdog");
+}
+
+void LoopOutputHandler::Watchdog::operator()() {
+ uint8_t buf[8];
+ while (run_) {
+ PCHECK(read(timerfd_.get(), buf, sizeof(buf)));
+ handler_->Stop();
+ }
+}
+
+void LoopOutputHandler::Watchdog::Reset() {
+ itimerspec value = itimerspec();
+ value.it_value = kStopTimeout.ToTimespec();
+ PCHECK(timerfd_settime(timerfd_.get(), 0, &value, nullptr));
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
new file mode 100644
index 0000000..1c8d4da
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler.h
@@ -0,0 +1,89 @@
+#ifndef FRC971_WPILIB_LOOP_OUTPUT_HANDLER_H_
+#define FRC971_WPILIB_LOOP_OUTPUT_HANDLER_H_
+
+#include "aos/common/scoped_fd.h"
+#include "aos/common/time.h"
+#include "aos/common/util/log_interval.h"
+
+#include <atomic>
+
+namespace frc971 {
+namespace wpilib {
+
+// Handles sending the output from a single control loop to the hardware.
+//
+// This class implements stopping motors when no new values are received for too
+// long efficiently.
+//
+// The intended use is to have a subclass for each loop which implements the
+// pure virtual methods and is then run in a separate thread. The operator()
+// loops writing values until Quit() is called.
+class LoopOutputHandler {
+ public:
+ LoopOutputHandler();
+
+ void Quit() { run_ = false; }
+
+ void operator()();
+
+ protected:
+ // Read a message from the appropriate queue.
+ // This must block.
+ virtual void Read() = 0;
+
+ // Send the output from the appropriate queue to the hardware.
+ // Read() will always be called at least once before per invocation of this.
+ virtual void Write() = 0;
+
+ // Stop all outputs. This will be called in a separate thread (if at all).
+ // The subclass implementation should handle any appropriate logging.
+ // This will be called exactly once if Read() blocks for too long and once
+ // after Quit is called.
+ virtual void Stop() = 0;
+
+ private:
+ // The thread that actually contains a timerfd to implement timeouts. The
+ // idea is to have a timerfd that is repeatedly set to the timeout expiration
+ // in the future so it will never actually expire unless it is not reset for
+ // too long.
+ //
+ // This class nicely encapsulates the raw fd and manipulating it. Its
+ // operator() loops until Quit() is called, calling Stop() on its associated
+ // LoopOutputHandler whenever the timerfd expires.
+ class Watchdog {
+ public:
+ Watchdog(LoopOutputHandler *handler);
+
+ void operator()();
+
+ void Reset();
+
+ void Quit() { run_ = false; }
+
+ private:
+ LoopOutputHandler *const handler_;
+
+ ::aos::ScopedFD timerfd_;
+
+ ::std::atomic<bool> run_{true};
+ };
+
+ static constexpr ::aos::time::Time kStopTimeout =
+ ::aos::time::Time::InSeconds(0.02);
+
+ Watchdog watchdog_;
+
+ ::std::atomic<bool> run_{true};
+
+ ::aos::util::SimpleLogInterval no_robot_state_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), INFO,
+ "no robot state -> not outputting");
+ ::aos::util::SimpleLogInterval fake_robot_state_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), DEBUG,
+ "fake robot state -> not outputting");
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_LOOP_OUTPUT_HANDLER_H_
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index 32a213a..d69a0ed 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -24,6 +24,42 @@
'<(AOS)/common/messages/messages.gyp:robot_state',
'hall_effect',
'joystick_sender',
+ 'loop_output_handler',
+ 'buffered_pcm',
+ ],
+ },
+ {
+ 'target_name': 'buffered_pcm',
+ 'type': 'static_library',
+ 'sources': [
+ 'buffered_solenoid.cc',
+ 'buffered_pcm.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ ],
+ },
+ {
+ 'target_name': 'loop_output_handler',
+ 'type': 'static_library',
+ 'sources': [
+ 'loop_output_handler.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:scoped_fd',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:scoped_fd',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:log_interval',
],
},
{
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index dfcc072..7b2c3a3 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -1,16 +1,18 @@
#include <stdio.h>
#include <string.h>
-#include <thread>
-#include <mutex>
#include <unistd.h>
#include <inttypes.h>
+#include <thread>
+#include <mutex>
+#include <functional>
+
#include "aos/prime/output/motor_output.h"
#include "aos/common/controls/output_check.q.h"
-#include "aos/common/messages/robot_state.q.h"
#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
#include "aos/common/util/phased_loop.h"
@@ -27,12 +29,14 @@
#include "frc971/wpilib/hall_effect.h"
#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/buffered_pcm.h"
#include "Encoder.h"
#include "Talon.h"
#include "DriverStation.h"
#include "AnalogInput.h"
-#include "Solenoid.h"
#include "Compressor.h"
#include "RobotBase.h"
@@ -736,7 +740,7 @@
.battery_voltage(ds->GetBatteryVoltage())
.Send();
- // Signal that we are allive.
+ // Signal that we are alive.
::aos::controls::sensor_generation.MakeWithBuilder()
.reader_pid(getpid())
.cape_resets(0)
@@ -775,186 +779,254 @@
DigitalGlitchFilter filter_;
};
-class MotorWriter {
+class SolenoidWriter {
public:
- MotorWriter()
- : right_drivetrain_talon_(new Talon(2)),
- left_drivetrain_talon_(new Talon(5)),
- shooter_talon_(new Talon(6)),
- top_claw_talon_(new Talon(1)),
- bottom_claw_talon_(new Talon(0)),
- left_tusk_talon_(new Talon(4)),
- right_tusk_talon_(new Talon(3)),
- intake1_talon_(new Talon(7)),
- intake2_talon_(new Talon(8)),
- left_shifter_(new Solenoid(6)),
- right_shifter_(new Solenoid(7)),
- shooter_latch_(new Solenoid(5)),
- shooter_brake_(new Solenoid(4)),
- compressor_(new Compressor()) {
- compressor_->SetClosedLoopControl(true);
- // right_drivetrain_talon_->EnableDeadbandElimination(true);
- // left_drivetrain_talon_->EnableDeadbandElimination(true);
- // shooter_talon_->EnableDeadbandElimination(true);
- // top_claw_talon_->EnableDeadbandElimination(true);
- // bottom_claw_talon_->EnableDeadbandElimination(true);
- // left_tusk_talon_->EnableDeadbandElimination(true);
- // right_tusk_talon_->EnableDeadbandElimination(true);
- // intake1_talon_->EnableDeadbandElimination(true);
- // intake2_talon_->EnableDeadbandElimination(true);
+ SolenoidWriter(const ::std::unique_ptr<BufferedPcm> &pcm)
+ : pcm_(pcm),
+ drivetrain_(".frc971.control_loops.drivetrain.output"),
+ shooter_(".frc971.control_loops.shooter_queue_group.output") {}
+
+ void set_drivetrain_left(::std::unique_ptr<BufferedSolenoid> s) {
+ drivetrain_left_ = ::std::move(s);
}
- // Maximum age of an output packet before the motors get zeroed instead.
- static const int kOutputMaxAgeMS = 20;
- static constexpr ::aos::time::Time kOldLogInterval =
- ::aos::time::Time::InSeconds(0.5);
+ void set_drivetrain_right(::std::unique_ptr<BufferedSolenoid> s) {
+ drivetrain_right_ = ::std::move(s);
+ }
- void Run() {
- //::aos::time::Time::EnableMockTime();
- while (true) {
- //::aos::time::Time::UpdateMockTime();
- // 200 hz loop
- ::aos::time::PhasedLoopXMS(5, 1000);
- //::aos::time::Time::UpdateMockTime();
+ void set_shooter_latch(::std::unique_ptr<BufferedSolenoid> s) {
+ shooter_latch_ = ::std::move(s);
+ }
- no_robot_state_.Print();
- fake_robot_state_.Print();
- sending_failed_.Print();
+ void set_shooter_brake(::std::unique_ptr<BufferedSolenoid> s) {
+ shooter_brake_ = ::std::move(s);
+ }
- RunIteration();
+ void operator()() {
+ ::aos::SetCurrentThreadName("Solenoids");
+ ::aos::SetCurrentThreadRealtimePriority(30);
+
+ while (run_) {
+ ::aos::time::PhasedLoopXMS(20, 1000);
+
+ {
+ drivetrain_.FetchLatest();
+ if (drivetrain_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
+ drivetrain_left_->Set(drivetrain_->left_high);
+ drivetrain_right_->Set(drivetrain_->right_high);
+ }
+ }
+
+ {
+ shooter_.FetchLatest();
+ if (shooter_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+ shooter_latch_->Set(!shooter_->latch_piston);
+ shooter_brake_->Set(!shooter_->brake_piston);
+ }
+ }
+
+ pcm_->Flush();
}
}
- virtual void RunIteration() {
- ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get()) {
- LOG_INTERVAL(no_robot_state_);
- return;
- }
- if (::aos::robot_state->fake) {
- LOG_INTERVAL(fake_robot_state_);
- return;
- }
+ void Quit() { run_ = false; }
- // TODO(austin): Write the motor values out when they change! One thread
- // per queue.
- // TODO(austin): Figure out how to synchronize everything to the PWM update
- // rate, or get the pulse to go out clocked off of this code. That would be
- // awesome.
- {
- static auto &drivetrain = ::frc971::control_loops::drivetrain.output;
- drivetrain.FetchLatest();
- if (drivetrain.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *drivetrain);
- left_drivetrain_talon_->Set(-drivetrain->left_voltage / 12.0);
- right_drivetrain_talon_->Set(drivetrain->right_voltage / 12.0);
- left_shifter_->Set(drivetrain->left_high);
- right_shifter_->Set(drivetrain->right_high);
- } else {
- left_drivetrain_talon_->Disable();
- right_drivetrain_talon_->Disable();
- LOG_INTERVAL(drivetrain_old_);
- }
- drivetrain_old_.Print();
- }
+ private:
+ const ::std::unique_ptr<BufferedPcm> &pcm_;
+ ::std::unique_ptr<BufferedSolenoid> drivetrain_left_;
+ ::std::unique_ptr<BufferedSolenoid> drivetrain_right_;
+ ::std::unique_ptr<BufferedSolenoid> shooter_latch_;
+ ::std::unique_ptr<BufferedSolenoid> shooter_brake_;
- {
- static auto &shooter =
- ::frc971::control_loops::shooter_queue_group.output;
- shooter.FetchLatest();
- if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *shooter);
- shooter_talon_->Set(shooter->voltage / 12.0);
- shooter_latch_->Set(!shooter->latch_piston);
- shooter_brake_->Set(!shooter->brake_piston);
- } else {
- shooter_talon_->Disable();
- shooter_brake_->Set(false); // engage the brake
- LOG_INTERVAL(shooter_old_);
- }
- shooter_old_.Print();
- }
+ ::aos::Queue<::frc971::control_loops::Drivetrain::Output> drivetrain_;
+ ::aos::Queue<::frc971::control_loops::ShooterGroup::Output> shooter_;
- {
- static auto &claw = ::frc971::control_loops::claw_queue_group.output;
- claw.FetchLatest();
- if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *claw);
- intake1_talon_->Set(claw->intake_voltage / 12.0);
- intake2_talon_->Set(claw->intake_voltage / 12.0);
- bottom_claw_talon_->Set(-claw->bottom_claw_voltage / 12.0);
- top_claw_talon_->Set(claw->top_claw_voltage / 12.0);
- left_tusk_talon_->Set(claw->tusk_voltage / 12.0);
- right_tusk_talon_->Set(-claw->tusk_voltage / 12.0);
- } else {
- intake1_talon_->Disable();
- intake2_talon_->Disable();
- bottom_claw_talon_->Disable();
- top_claw_talon_->Disable();
- left_tusk_talon_->Disable();
- right_tusk_talon_->Disable();
- LOG_INTERVAL(claw_old_);
- }
- claw_old_.Print();
- }
+ ::std::atomic<bool> run_{true};
+};
+
+class DrivetrainWriter : public LoopOutputHandler {
+ public:
+ void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
+ left_drivetrain_talon_ = ::std::move(t);
}
- SimpleLogInterval drivetrain_old_ =
- SimpleLogInterval(kOldLogInterval, WARNING, "drivetrain too old");
- SimpleLogInterval shooter_old_ =
- SimpleLogInterval(kOldLogInterval, WARNING, "shooter too old");
- SimpleLogInterval claw_old_ =
- SimpleLogInterval(kOldLogInterval, WARNING, "claw too old");
+ void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
+ right_drivetrain_talon_ = ::std::move(t);
+ }
- ::std::unique_ptr<Talon> right_drivetrain_talon_;
+ private:
+ virtual void Read() override {
+ ::frc971::control_loops::drivetrain.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::frc971::control_loops::drivetrain.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ left_drivetrain_talon_->Set(-queue->left_voltage / 12.0);
+ right_drivetrain_talon_->Set(queue->right_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "drivetrain output too old\n");
+ left_drivetrain_talon_->Disable();
+ right_drivetrain_talon_->Disable();
+ }
+
::std::unique_ptr<Talon> left_drivetrain_talon_;
+ ::std::unique_ptr<Talon> right_drivetrain_talon_;
+};
+
+class ShooterWriter : public LoopOutputHandler {
+ public:
+ void set_shooter_talon(::std::unique_ptr<Talon> t) {
+ shooter_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::frc971::control_loops::shooter_queue_group.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::frc971::control_loops::shooter_queue_group.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ shooter_talon_->Set(queue->voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "shooter output too old\n");
+ shooter_talon_->Disable();
+ }
+
::std::unique_ptr<Talon> shooter_talon_;
+};
+
+class ClawWriter : public LoopOutputHandler {
+ public:
+ void set_top_claw_talon(::std::unique_ptr<Talon> t) {
+ top_claw_talon_ = ::std::move(t);
+ }
+
+ void set_bottom_claw_talon(::std::unique_ptr<Talon> t) {
+ bottom_claw_talon_ = ::std::move(t);
+ }
+
+ void set_left_tusk_talon(::std::unique_ptr<Talon> t) {
+ left_tusk_talon_ = ::std::move(t);
+ }
+
+ void set_right_tusk_talon(::std::unique_ptr<Talon> t) {
+ right_tusk_talon_ = ::std::move(t);
+ }
+
+ void set_intake1_talon(::std::unique_ptr<Talon> t) {
+ intake1_talon_ = ::std::move(t);
+ }
+
+ void set_intake2_talon(::std::unique_ptr<Talon> t) {
+ intake2_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::frc971::control_loops::claw_queue_group.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::frc971::control_loops::claw_queue_group.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ intake1_talon_->Set(queue->intake_voltage / 12.0);
+ intake2_talon_->Set(queue->intake_voltage / 12.0);
+ bottom_claw_talon_->Set(-queue->bottom_claw_voltage / 12.0);
+ top_claw_talon_->Set(queue->top_claw_voltage / 12.0);
+ left_tusk_talon_->Set(queue->tusk_voltage / 12.0);
+ right_tusk_talon_->Set(-queue->tusk_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "claw output too old\n");
+ intake1_talon_->Disable();
+ intake2_talon_->Disable();
+ bottom_claw_talon_->Disable();
+ top_claw_talon_->Disable();
+ left_tusk_talon_->Disable();
+ right_tusk_talon_->Disable();
+ }
+
::std::unique_ptr<Talon> top_claw_talon_;
::std::unique_ptr<Talon> bottom_claw_talon_;
::std::unique_ptr<Talon> left_tusk_talon_;
::std::unique_ptr<Talon> right_tusk_talon_;
::std::unique_ptr<Talon> intake1_talon_;
::std::unique_ptr<Talon> intake2_talon_;
-
- ::std::unique_ptr<Solenoid> left_shifter_;
- ::std::unique_ptr<Solenoid> right_shifter_;
- ::std::unique_ptr<Solenoid> shooter_latch_;
- ::std::unique_ptr<Solenoid> shooter_brake_;
-
- ::std::unique_ptr<Compressor> compressor_;
-
- ::aos::util::SimpleLogInterval no_robot_state_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), INFO,
- "no robot state -> not outputting");
- ::aos::util::SimpleLogInterval fake_robot_state_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), DEBUG,
- "fake robot state -> not outputting");
- ::aos::util::SimpleLogInterval sending_failed_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.1), WARNING,
- "sending outputs failed");
};
-constexpr ::aos::time::Time MotorWriter::kOldLogInterval;
-
} // namespace wpilib
} // namespace frc971
class WPILibRobot : public RobotBase {
public:
virtual void StartCompetition() {
- ::aos::Init();
+ ::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::MotorWriter writer;
- ::frc971::wpilib::SensorReader reader;
- ::std::thread reader_thread(::std::ref(reader));
+
::frc971::wpilib::JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
- writer.Run();
+ ::frc971::wpilib::SensorReader reader;
+ ::std::thread reader_thread(::std::ref(reader));
+ ::std::unique_ptr<Compressor> compressor(new Compressor());
+ compressor->SetClosedLoopControl(true);
+
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(5)));
+ drivetrain_writer.set_right_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(2)));
+ ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+
+
+ ::frc971::wpilib::ClawWriter claw_writer;
+ claw_writer.set_top_claw_talon(::std::unique_ptr<Talon>(new Talon(1)));
+ claw_writer.set_bottom_claw_talon(::std::unique_ptr<Talon>(new Talon(0)));
+ claw_writer.set_left_tusk_talon(::std::unique_ptr<Talon>(new Talon(4)));
+ claw_writer.set_right_tusk_talon(::std::unique_ptr<Talon>(new Talon(3)));
+ claw_writer.set_intake1_talon(::std::unique_ptr<Talon>(new Talon(7)));
+ claw_writer.set_intake2_talon(::std::unique_ptr<Talon>(new Talon(8)));
+ ::std::thread claw_writer_thread(::std::ref(claw_writer));
+
+ ::frc971::wpilib::ShooterWriter shooter_writer;
+ shooter_writer.set_shooter_talon(::std::unique_ptr<Talon>(new Talon(6)));
+ ::std::thread shooter_writer_thread(::std::ref(shooter_writer));
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
+ new ::frc971::wpilib::BufferedPcm());
+ ::frc971::wpilib::SolenoidWriter solenoid_writer(pcm);
+ solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
+ solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(7));
+ solenoid_writer.set_shooter_latch(pcm->MakeSolenoid(5));
+ solenoid_writer.set_shooter_brake(pcm->MakeSolenoid(4));
+ ::std::thread solenoid_thread(::std::ref(solenoid_writer));
+
+ // Wait forever. Not much else to do...
+ PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));
+
LOG(ERROR, "Exiting WPILibRobot\n");
- reader.Quit();
- reader_thread.join();
joystick_sender.Quit();
joystick_thread.join();
+ reader.Quit();
+ reader_thread.join();
+
+ drivetrain_writer.Quit();
+ drivetrain_writer_thread.join();
+ claw_writer.Quit();
+ claw_writer_thread.join();
+ shooter_writer.Quit();
+ shooter_writer_thread.join();
+ solenoid_writer.Quit();
+ solenoid_thread.join();
+
::aos::Cleanup();
}
};