update the information about enabled vs not
With the roboRIO, we get different information about whether motors are
enabled (and some about sensors too), so the system for propagating it
around to everything needs to change.
Change-Id: I7e5f9591eeac1fdfe57b271333c3827431fbef01
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 4712bb9..6903bc2 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -1,11 +1,10 @@
#include <stddef.h>
+#include <inttypes.h>
#include "aos/common/logging/logging.h"
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/queue_logging.h"
#include "aos/common/util/phased_loop.h"
-#include "aos/common/controls/sensor_generation.q.h"
-#include "aos/common/controls/output_check.q.h"
namespace aos {
namespace controls {
@@ -14,6 +13,8 @@
template <class T, bool fail_no_goal>
constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kStaleLogInterval;
+template <class T, bool fail_no_goal>
+constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kPwmDisableTime;
template <class T, bool fail_no_goal>
void
@@ -26,10 +27,11 @@
template <class T, bool fail_no_goal>
void ControlLoop<T, fail_no_goal>::Iterate() {
- no_prior_goal_.Print();
- no_sensor_generation_.Print();
+ no_goal_.Print();
driver_station_old_.Print();
+ no_sensor_state_.Print();
no_driver_station_.Print();
+ motors_off_log_.Print();
control_loop_->position.FetchAnother();
const PositionType *const position = control_loop_->position.get();
@@ -43,71 +45,61 @@
// TODO(aschuh): Check the age here if we want the loop to stop on old
// goals.
const GoalType *goal = control_loop_->goal.get();
- if (goal == NULL) {
- LOG_INTERVAL(no_prior_goal_);
+ if (goal) {
+ LOG_STRUCT(DEBUG, "goal", *goal);
+ } else {
+ LOG_INTERVAL(no_goal_);
if (fail_no_goal) {
ZeroOutputs();
return;
}
}
- sensor_generation.FetchLatest();
- if (sensor_generation.get() == nullptr) {
- LOG_INTERVAL(no_sensor_generation_);
- ZeroOutputs();
+ ::aos::robot_state.FetchLatest();
+ if (!::aos::robot_state.get()) {
+ LOG_INTERVAL(no_sensor_state_);
return;
}
- if (!has_sensor_reset_counters_ ||
- sensor_generation->reader_pid != reader_pid_ ||
- sensor_generation->cape_resets != cape_resets_) {
- LOG_STRUCT(INFO, "new sensor_generation message",
- *sensor_generation.get());
-
- reader_pid_ = sensor_generation->reader_pid;
- cape_resets_ = sensor_generation->cape_resets;
- has_sensor_reset_counters_ = true;
+ if (sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
+ ::aos::robot_state->reader_pid, sensor_reader_pid_);
reset_ = true;
- }
-
- if (goal) {
- LOG_STRUCT(DEBUG, "goal", *goal);
+ sensor_reader_pid_ = ::aos::robot_state->reader_pid;
}
bool outputs_enabled = false;
// Check to see if we got a driver station packet recently.
- if (::aos::robot_state.FetchLatest()) {
+ if (::aos::joystick_state.FetchLatest()) {
outputs_enabled = true;
- } else if (::aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
+ if (::aos::robot_state->outputs_enabled) {
+ last_pwm_sent_ = ::aos::robot_state->sent_time;
+ }
+ } else if (::aos::joystick_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
outputs_enabled = true;
} else {
- if (::aos::robot_state.get()) {
+ if (::aos::joystick_state.get()) {
LOG_INTERVAL(driver_station_old_);
} else {
LOG_INTERVAL(no_driver_station_);
}
}
- ::aos::controls::output_check_received.FetchLatest();
- // True if we're enabled but the motors aren't working.
// The 100ms is the result of using an oscilliscope to look at the PWM signal
// and output of a talon, and timing the delay between the last pulse and the
// talon turning off.
const bool motors_off =
- !::aos::controls::output_check_received.get() ||
- !::aos::controls::output_check_received.IsNewerThanMS(100);
- motors_off_log_.Print();
+ (::aos::time::Time::Now() - last_pwm_sent_) >= kPwmDisableTime;
if (motors_off) {
- if (!::aos::robot_state.get() || ::aos::robot_state->enabled) {
+ if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
LOG_INTERVAL(motors_off_log_);
}
outputs_enabled = false;
}
- // Run the iteration.
aos::ScopedMessagePtr<StatusType> status =
control_loop_->status.MakeMessage();
- if (status.get() == NULL) {
+ if (status.get() == nullptr) {
return;
}
@@ -119,7 +111,7 @@
LOG_STRUCT(DEBUG, "output", *output);
output.Send();
} else {
- // The outputs are disabled, so pass NULL in for the output.
+ // The outputs are disabled, so pass nullptr in for the output.
RunIteration(goal, position, nullptr, status.get());
ZeroOutputs();
}
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index e9454d1..a176fb6 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -47,16 +47,6 @@
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
- // invalid position data.
- static const int kPositionTimeoutMs = 1000;
- // Maximum age of driver station packets before the loop will be disabled.
- static const int kDSPacketTimeoutMs = 500;
-
- ControlLoop(T *control_loop)
- : control_loop_(control_loop),
- reset_(true),
- has_sensor_reset_counters_(false) {}
// Create some convenient typedefs to reference the Goal, Position, Status,
// and Output structures.
typedef typename std::remove_reference<
@@ -72,6 +62,19 @@
decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
OutputType;
+ ControlLoop(T *control_loop) : control_loop_(control_loop) {}
+
+ // Returns true if all the counters etc in the sensor data have been reset.
+ // This will return true only a single time per reset.
+ bool WasReset() {
+ if (reset_) {
+ reset_ = false;
+ return true;
+ } else {
+ return false;
+ }
+ }
+
// Constructs and sends a message on the output queue which sets everything to
// a safe state (generally motors off). For some subclasses, this will be a
// bit different (ie pistons).
@@ -79,47 +82,37 @@
// and then sends it.
virtual void ZeroOutputs();
- // Returns true if the device reading the sensors reset and potentially lost
- // track of encoder counts. Calling this read method clears the flag. After
- // a reset, RunIteration will not be called until there is a valid position
- // message.
- bool reset() {
- bool ans = reset_;
- reset_ = false;
- return ans;
- }
-
// Sets the output to zero.
// Over-ride if a value of zero is not "off" for this subsystem.
virtual void Zero(OutputType *output) { output->Zero(); }
// Runs the loop forever.
- virtual void Run();
+ void Run() override;
// Runs one cycle of the loop.
- virtual void Iterate();
+ void Iterate() override;
// Returns the name of the queue group.
const char *name() { return control_loop_->name(); }
// Methods to serialize all the data that should be sent over the network.
- virtual size_t SeralizedSize() { return control_loop_->goal->Size(); }
- virtual void Serialize(char *buffer) const {
+ size_t SeralizedSize() override { return control_loop_->goal->Size(); }
+ void Serialize(char *buffer) const override {
control_loop_->goal->Serialize(buffer);
}
- virtual void SerializeZeroMessage(char *buffer) const {
+ void SerializeZeroMessage(char *buffer) const override {
GoalType zero_goal;
zero_goal.Zero();
zero_goal.Serialize(buffer);
}
- virtual void Deserialize(const char *buffer) {
+ void Deserialize(const char *buffer) override {
ScopedMessagePtr<GoalType> new_msg = control_loop_->goal.MakeMessage();
new_msg->Deserialize(buffer);
new_msg.Send();
}
- virtual uint32_t UniqueID() { return control_loop_->hash(); }
+ uint32_t UniqueID() override { return control_loop_->hash(); }
protected:
// Runs an iteration of the control loop.
@@ -139,31 +132,39 @@
const T *queue_group() const { return control_loop_; }
private:
+ static constexpr ::aos::time::Time kStaleLogInterval =
+ ::aos::time::Time::InSeconds(0.1);
+ // 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;
+
// Pointer to the queue group
T *control_loop_;
- bool reset_;
- bool has_sensor_reset_counters_;
- int32_t reader_pid_;
- uint32_t cape_resets_;
+ bool reset_ = false;
+ int32_t sensor_reader_pid_ = 0;
+
+ ::aos::time::Time last_pwm_sent_{0, 0};
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
- static constexpr ::aos::time::Time kStaleLogInterval =
- ::aos::time::Time::InSeconds(0.1);
- SimpleLogInterval no_prior_goal_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "no prior goal");
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_generation_ =
- SimpleLogInterval(kStaleLogInterval, ERROR,
- "no sensor_generation message");
+ SimpleLogInterval no_sensor_state_ =
+ SimpleLogInterval(kStaleLogInterval, ERROR, "no sensor state");
SimpleLogInterval motors_off_log_ =
SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
+ SimpleLogInterval no_goal_ =
+ SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
};
} // namespace controls
diff --git a/aos/common/controls/control_loop_test.cc b/aos/common/controls/control_loop_test.cc
index a8deecf..899b96b 100644
--- a/aos/common/controls/control_loop_test.cc
+++ b/aos/common/controls/control_loop_test.cc
@@ -1,8 +1,6 @@
#include "aos/common/controls/control_loop_test.h"
#include "aos/common/messages/robot_state.q.h"
-#include "aos/common/controls/sensor_generation.q.h"
-#include "aos/common/controls/output_check.q.h"
namespace aos {
namespace testing {
@@ -11,44 +9,49 @@
constexpr ::aos::time::Time ControlLoopTest::kDSPacketTime;
ControlLoopTest::ControlLoopTest() {
+ ::aos::joystick_state.Clear();
::aos::robot_state.Clear();
- ::aos::controls::sensor_generation.Clear();
- ::aos::controls::output_check_received.Clear();
- ::aos::controls::sensor_generation.MakeWithBuilder()
- .reader_pid(254)
- .cape_resets(5)
- .Send();
::aos::time::Time::EnableMockTime(current_time_);
- SimulateTimestep(false);
+ SendMessages(false);
}
ControlLoopTest::~ControlLoopTest() {
+ ::aos::joystick_state.Clear();
::aos::robot_state.Clear();
- ::aos::controls::sensor_generation.Clear();
- ::aos::controls::output_check_received.Clear();
::aos::time::Time::DisableMockTime();
}
void ControlLoopTest::SendMessages(bool enabled) {
if (current_time_ - last_ds_time_ >= kDSPacketTime) {
- ::aos::robot_state.MakeWithBuilder()
- .enabled(enabled)
- .autonomous(false)
- .fake(true)
- .team_id(971)
- .Send();
last_ds_time_ = current_time_;
+ auto new_state = ::aos::joystick_state.MakeMessage();
+ new_state->fake = true;
+
+ new_state->enabled = enabled;
+ new_state->autonomous = false;
+ new_state->team_id = 971;
+
+ new_state.Send();
}
- if (enabled) {
- // TODO(brians): Actually make this realistic once we figure out what that
- // means.
- ::aos::controls::output_check_received.MakeWithBuilder()
- .pwm_value(0)
- .pulse_length(0)
- .Send();
+
+ {
+ auto new_state = ::aos::robot_state.MakeMessage();
+
+ new_state->reader_pid = 971;
+ new_state->outputs_enabled = enabled;
+ new_state->browned_out = false;
+
+ new_state->is_3v3_active = true;
+ new_state->is_5v_active = true;
+ new_state->voltage_3v3 = 3.3;
+ new_state->voltage_5v = 5.0;
+
+ new_state->voltage_roborio_in = 12.4;
+
+ new_state.Send();
}
}
diff --git a/aos/common/controls/control_loop_test.h b/aos/common/controls/control_loop_test.h
index 76c68d3..13c4f0d 100644
--- a/aos/common/controls/control_loop_test.h
+++ b/aos/common/controls/control_loop_test.h
@@ -33,7 +33,7 @@
}
private:
- static constexpr ::aos::time::Time kTimeTick = ::aos::time::Time::InUS(10000);
+ static constexpr ::aos::time::Time kTimeTick = ::aos::time::Time::InUS(5000);
static constexpr ::aos::time::Time kDSPacketTime =
::aos::time::Time::InMS(20);
diff --git a/aos/common/controls/controls.gyp b/aos/common/controls/controls.gyp
index ad05376..e24a121 100644
--- a/aos/common/controls/controls.gyp
+++ b/aos/common/controls/controls.gyp
@@ -9,8 +9,6 @@
'dependencies': [
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/messages/messages.gyp:robot_state',
- 'sensor_generation',
- 'output_check',
'<(EXTERNALS):gtest',
'<(AOS)/common/common.gyp:queue_testutils',
],
@@ -36,28 +34,6 @@
],
},
{
- 'target_name': 'sensor_generation',
- 'type': 'static_library',
- 'sources': [
- 'sensor_generation.q',
- ],
- 'variables': {
- 'header_path': 'aos/common/controls',
- },
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'output_check',
- 'type': 'static_library',
- 'sources': [
- 'output_check.q',
- ],
- 'variables': {
- 'header_path': 'aos/common/controls',
- },
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
'target_name': 'control_loop_queues',
'type': 'static_library',
'sources': [ '<(AOS)/common/controls/control_loops.q' ],
@@ -80,8 +56,6 @@
'control_loop_queues',
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(AOS)/common/util/util.gyp:log_interval',
- 'sensor_generation',
- 'output_check',
'<(AOS)/common/common.gyp:queues',
],
'export_dependent_settings': [
@@ -92,8 +66,6 @@
'control_loop_queues',
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(AOS)/common/util/util.gyp:log_interval',
- 'sensor_generation',
- 'output_check',
'<(AOS)/common/common.gyp:queues',
],
},
diff --git a/aos/common/controls/output_check.q b/aos/common/controls/output_check.q
deleted file mode 100644
index e7c8190..0000000
--- a/aos/common/controls/output_check.q
+++ /dev/null
@@ -1,16 +0,0 @@
-package aos.controls;
-
-message OutputCheck {
- // The 1-255 sent to the FPGA.
- uint8_t pwm_value;
- // The length of the pulse in milliseconds.
- double pulse_length;
-};
-
-// Each message here represents a value that was sent out.
-// The sent timestamp of the message is when the value was sent.
-queue OutputCheck output_check_sent;
-
-// Each message here represents a value that was received by the sensor.
-// The sent timestamp of the message is when it was received.
-queue OutputCheck output_check_received;
diff --git a/aos/common/controls/sensor_generation.q b/aos/common/controls/sensor_generation.q
deleted file mode 100644
index 50e17e3..0000000
--- a/aos/common/controls/sensor_generation.q
+++ /dev/null
@@ -1,14 +0,0 @@
-package aos.controls;
-
-// Each different set of values here represents a different set of sensor
-// values (ie different encoder zeros etc).
-message SensorGeneration {
- // The PID of the process reading sensor values.
- int32_t reader_pid;
- // A count of how many times a sensor reading process sees the cape reset.
- uint32_t cape_resets;
-};
-
-// A new message is placed on here by the process that reads sensor values each
-// time it starts up or detects the cape resetting.
-queue SensorGeneration sensor_generation;
diff --git a/aos/common/input/driver_station_data.cc b/aos/common/input/driver_station_data.cc
index d97601a..f825bd4 100644
--- a/aos/common/input/driver_station_data.cc
+++ b/aos/common/input/driver_station_data.cc
@@ -6,7 +6,7 @@
Data::Data() : current_values_(), old_values_() {}
-void Data::Update(const RobotState &new_values) {
+void Data::Update(const JoystickState &new_values) {
old_values_ = current_values_;
current_values_ = new_values;
}
@@ -14,13 +14,13 @@
namespace {
bool GetButton(const ButtonLocation location,
- const RobotState &values) {
+ const JoystickState &values) {
return values.joysticks[location.joystick() - 1].buttons &
(1 << (location.number() - 1));
}
bool GetControlBitValue(const ControlBit bit,
- const RobotState &values) {
+ const JoystickState &values) {
switch (bit) {
case ControlBit::kTestMode:
return values.test_mode;
diff --git a/aos/common/input/driver_station_data.h b/aos/common/input/driver_station_data.h
index a8a9ddf..7a946eb 100644
--- a/aos/common/input/driver_station_data.h
+++ b/aos/common/input/driver_station_data.h
@@ -20,8 +20,8 @@
: joystick_(joystick), number_(number) {}
// How many joysticks there are.
- static const int kJoysticks = sizeof(RobotState::joysticks) /
- sizeof(RobotState::joysticks[0]);
+ static const int kJoysticks = sizeof(JoystickState::joysticks) /
+ sizeof(JoystickState::joysticks[0]);
// Which joystick number this is (1-based).
int joystick() const { return joystick_; }
@@ -70,7 +70,7 @@
Data();
// Updates the current information with a new set of values.
- void Update(const RobotState &new_values);
+ void Update(const JoystickState &new_values);
bool IsPressed(ButtonLocation location) const;
bool PosEdge(ButtonLocation location) const;
@@ -84,7 +84,7 @@
float GetAxis(JoystickAxis axis) const;
private:
- RobotState current_values_, old_values_;
+ JoystickState current_values_, old_values_;
};
} // namespace driver_station
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index bd66e1d..74fe5d0 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -8,7 +8,7 @@
double[4] axis;
};
-message RobotState {
+message JoystickState {
Joystick[4] joysticks;
bool test_mode;
@@ -16,17 +16,49 @@
bool enabled;
bool autonomous;
uint16_t team_id;
+
// If this is true, then this message isn't actually from the control
// system and so should not be trusted as evidence that the button inputs
// etc are actually real and should be acted on.
// However, most things should ignore this so that sending fake messages is
- // useful for testing.
+ // useful for testing. The only difference in behavior should be motors not
+ // actually turning on.
bool fake;
};
-// The robot_state Queue is checked by all control loops to make sure that the
+// This queue is checked by all control loops to make sure that the
// joystick code hasn't died.
-// It also provides information about whether or not the robot is in autonomous
-// mode and what the team_id is.
+queue JoystickState joystick_state;
+message RobotState {
+ // The PID of the process reading sensors.
+ // This is here so control loops can tell when it changes.
+ int32_t reader_pid;
+
+ // True when outputs are enabled.
+ // Motor controllers keep going for a bit after this goes to false.
+ bool outputs_enabled;
+ // Indicates whether something is browned out (I think motor controller
+ // outputs). IMPORTANT: This is NOT !outputs_enabled. outputs_enabled goes to
+ // false for other reasons too (disabled, e-stopped, maybe more).
+ bool browned_out;
+
+ // Whether the two sensor rails are currently working.
+ bool is_3v3_active;
+ bool is_5v_active;
+ // The current voltages measured on the two sensor rails.
+ double voltage_3v3;
+ double voltage_5v;
+
+ // The input voltage to the roboRIO.
+ double voltage_roborio_in;
+
+ // From the DriverStation object, aka what FMS sees and what shows up on the
+ // actual driver's station.
+ double voltage_battery;
+};
+
+// Messages are sent out on this queue along with reading sensors. It contains
+// global robot state and information about whether the process reading sensors
+// has been restarted, along with all counters etc it keeps track of.
queue RobotState robot_state;