control loop timeout code now uses robot_state->outputs_enabled
The outputs_enabled flag falls low as soon as the robot disables the
outputs. They will stay at the last value that was commanded before
it disables for 100 ms, but we can conservatively tell the robot that
they are disabled at that point (since they practically are).
Change-Id: I47f883591dbe2e3696d3958a0ce4281232a5f1e1
diff --git a/aos/common/controls/BUILD b/aos/common/controls/BUILD
index edf0707..44e6db4 100644
--- a/aos/common/controls/BUILD
+++ b/aos/common/controls/BUILD
@@ -25,8 +25,9 @@
'control_loop_test.h',
],
deps = [
- '//aos/common:time',
+ '//aos/common/logging:queue_logging',
'//aos/common/messages:robot_state',
+ '//aos/common:time',
'//aos/testing:googletest',
'//aos/testing:test_shm',
],
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 6b698ba..81b5d20 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -11,9 +11,9 @@
// TODO(aschuh): Tests.
template <class T>
-constexpr ::aos::time::Time ControlLoop<T>::kStaleLogInterval;
+constexpr ::std::chrono::milliseconds ControlLoop<T>::kStaleLogInterval;
template <class T>
-constexpr ::aos::time::Time ControlLoop<T>::kPwmDisableTime;
+constexpr ::std::chrono::milliseconds ControlLoop<T>::kPwmDisableTime;
template <class T>
void ControlLoop<T>::ZeroOutputs() {
@@ -26,9 +26,7 @@
template <class T>
void ControlLoop<T>::Iterate() {
no_goal_.Print();
- driver_station_old_.Print();
no_sensor_state_.Print();
- no_driver_station_.Print();
motors_off_log_.Print();
control_loop_->position.FetchAnother();
@@ -45,7 +43,7 @@
LOG_INTERVAL(no_goal_);
}
- ::aos::robot_state.FetchLatest();
+ const bool new_robot_state = ::aos::robot_state.FetchLatest();
if (!::aos::robot_state.get()) {
LOG_INTERVAL(no_sensor_state_);
return;
@@ -57,37 +55,22 @@
sensor_reader_pid_ = ::aos::robot_state->reader_pid;
}
- bool outputs_enabled = false;
+ bool outputs_enabled = ::aos::robot_state->outputs_enabled;
// Check to see if we got a driver station packet recently.
- if (::aos::joystick_state.FetchLatest()) {
- if (::aos::joystick_state->enabled) outputs_enabled = true;
+ if (new_robot_state) {
if (::aos::robot_state->outputs_enabled) {
// If the driver's station reports being disabled, we're probably not
// actually going to send motor values regardless of what the FPGA
// reports.
- if (::aos::joystick_state->enabled) {
- last_pwm_sent_ = ::aos::robot_state->sent_time;
- } else {
- LOG(WARNING, "outputs enabled while disabled\n");
- }
- } else if (::aos::joystick_state->enabled) {
- LOG(WARNING, "outputs disabled while enabled\n");
- }
- } else if (::aos::joystick_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
- if (::aos::joystick_state->enabled) {
- outputs_enabled = true;
- }
- } else {
- if (::aos::joystick_state.get()) {
- LOG_INTERVAL(driver_station_old_);
- } else {
- LOG_INTERVAL(no_driver_station_);
+ last_pwm_sent_ = monotonic_clock::time_point(
+ ::std::chrono::nanoseconds(::aos::robot_state->sent_time.ToNSec()));
}
}
- const bool motors_off =
- (::aos::time::Time::Now() - last_pwm_sent_) >= kPwmDisableTime;
+ const ::aos::monotonic_clock::time_point now = ::aos::monotonic_clock::now();
+ const bool motors_off = now >= kPwmDisableTime + last_pwm_sent_;
+ ::aos::joystick_state.FetchLatest();
if (motors_off) {
if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
LOG_INTERVAL(motors_off_log_);
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index f589ed7..02aa067 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -134,17 +134,14 @@
const T *queue_group() const { return control_loop_; }
private:
- static constexpr ::aos::time::Time kStaleLogInterval =
- ::aos::time::Time::InSeconds(0.1);
+ static constexpr ::std::chrono::milliseconds kStaleLogInterval =
+ ::std::chrono::milliseconds(100);
// The amount of time after the last PWM pulse we consider motors enabled for.
// 100ms is the result of using an oscilliscope to look at the input and
// output of a Talon. The Info Sheet also lists 100ms for Talon SR, Talon SRX,
// and Victor SP.
- static constexpr ::aos::time::Time kPwmDisableTime =
- ::aos::time::Time::InMS(100);
-
- // Maximum age of driver station packets before the loop will be disabled.
- static const int kDSPacketTimeoutMs = 500;
+ static constexpr ::std::chrono::milliseconds kPwmDisableTime =
+ ::std::chrono::milliseconds(100);
// Pointer to the queue group
T *control_loop_;
@@ -152,15 +149,10 @@
bool reset_ = false;
int32_t sensor_reader_pid_ = 0;
- ::aos::time::Time last_pwm_sent_{0, 0};
+ ::aos::monotonic_clock::time_point last_pwm_sent_ =
+ ::aos::monotonic_clock::min_time;
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
- SimpleLogInterval no_driver_station_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "no driver station packet");
- SimpleLogInterval driver_station_old_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "driver station packet is too old");
SimpleLogInterval no_sensor_state_ =
SimpleLogInterval(kStaleLogInterval, ERROR, "no sensor state");
SimpleLogInterval motors_off_log_ =
diff --git a/aos/common/controls/control_loop_test.cc b/aos/common/controls/control_loop_test.cc
index b71aa3a..4e29062 100644
--- a/aos/common/controls/control_loop_test.cc
+++ b/aos/common/controls/control_loop_test.cc
@@ -1,6 +1,7 @@
#include "aos/common/controls/control_loop_test.h"
#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/logging/queue_logging.h"
namespace aos {
namespace testing {
@@ -54,6 +55,7 @@
new_state->voltage_roborio_in = battery_voltage_;
new_state->voltage_battery = battery_voltage_;
+ LOG_STRUCT(INFO, "robot_state", *new_state);
new_state.Send();
}
}
diff --git a/aos/common/util/log_interval.h b/aos/common/util/log_interval.h
index 7c8a329..3a7a85f 100644
--- a/aos/common/util/log_interval.h
+++ b/aos/common/util/log_interval.h
@@ -23,18 +23,19 @@
// }
class LogInterval {
public:
- constexpr LogInterval(const ::aos::time::Time &interval)
- : count_(0), interval_(interval), last_done_(0, 0) {}
+ constexpr LogInterval(::std::chrono::nanoseconds interval)
+ : interval_(interval) {}
void WantToLog() {
if (count_ == 0) {
- last_done_ = ::aos::time::Time::Now();
+ last_done_ = ::aos::monotonic_clock::now();
}
++count_;
}
bool ShouldLog() {
- const ::aos::time::Time now = ::aos::time::Time::Now();
- const bool r = (now - last_done_) >= interval_ && count_ > 0;
+ const ::aos::monotonic_clock::time_point now =
+ ::aos::monotonic_clock::now();
+ const bool r = now >= interval_ + last_done_ && count_ > 0;
if (r) {
last_done_ = now;
}
@@ -46,12 +47,13 @@
return r;
}
- const ::aos::time::Time &interval() const { return interval_; }
+ ::std::chrono::nanoseconds interval() const { return interval_; }
private:
- int count_;
- const ::aos::time::Time interval_;
- ::aos::time::Time last_done_;
+ int count_ = 0;
+ const ::std::chrono::nanoseconds interval_;
+ ::aos::monotonic_clock::time_point last_done_ =
+ ::aos::monotonic_clock::min_time;
};
// This one is even easier to use. It always logs with a message "%s %d
@@ -59,7 +61,7 @@
// called often (ie not after a conditional return)
class SimpleLogInterval {
public:
- SimpleLogInterval(const ::aos::time::Time &interval, log_level level,
+ SimpleLogInterval(::std::chrono::nanoseconds interval, log_level level,
const ::std::string &message)
: interval_(interval), level_(level), message_(message) {}
@@ -75,7 +77,9 @@
CHECK_NOTNULL(context_);
log_do(level_, "%s: %.*s %d times over %f sec\n", context_,
static_cast<int>(message_.size()), message_.data(),
- interval_.Count(), interval_.interval().ToSeconds());
+ interval_.Count(),
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ interval_.interval()).count());
context_ = NULL;
}
}
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 972d7ef..144093d 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -2,6 +2,7 @@
#define FRC971_WPILIB_LOOP_OUTPUT_HANDLER_H_
#include <atomic>
+#include <chrono>
#include "aos/common/scoped_fd.h"
#include "aos/common/time.h"
@@ -76,10 +77,10 @@
::std::atomic<bool> run_{true};
::aos::util::SimpleLogInterval no_joystick_state_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), INFO,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(500), INFO,
"no joystick state -> not outputting");
::aos::util::SimpleLogInterval fake_joystick_state_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.5), DEBUG,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(500), DEBUG,
"fake joystick state -> not outputting");
};
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index d3af8a4..2bef2f7 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -513,7 +513,7 @@
::aos::common::actions::ActionQueue action_queue_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
};
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 4103b0c..a29856b 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -142,7 +142,7 @@
double right_goal_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
};
diff --git a/y2015/joystick_reader.cc b/y2015/joystick_reader.cc
index 4a8df16..92e6e19 100644
--- a/y2015/joystick_reader.cc
+++ b/y2015/joystick_reader.cc
@@ -541,7 +541,7 @@
::aos::common::actions::ActionQueue action_queue_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
};
diff --git a/y2015_bot3/joystick_reader.cc b/y2015_bot3/joystick_reader.cc
index f90a19a..7457d15 100644
--- a/y2015_bot3/joystick_reader.cc
+++ b/y2015_bot3/joystick_reader.cc
@@ -369,7 +369,7 @@
StackingStateMachine stacking_state_machine_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
int grab_delay_ = 0;
};
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index bb966d7..af01a8b 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -488,7 +488,7 @@
bool is_expanding_ = false;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
};
diff --git a/y2016_bot3/joystick_reader.cc b/y2016_bot3/joystick_reader.cc
index a987e3d..198b002 100644
--- a/y2016_bot3/joystick_reader.cc
+++ b/y2016_bot3/joystick_reader.cc
@@ -252,7 +252,7 @@
const ::frc971::control_loops::drivetrain::DrivetrainConfig dt_config_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
- ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(200), WARNING,
"no drivetrain status");
};