made it so the claw can (disabled) without a driver's station
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index e96ae66..4735051 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -12,20 +12,21 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position, bool fail_no_position>
-constexpr ::aos::time::Time
- ControlLoop<T, has_position, fail_no_position>::kStaleLogInterval;
+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 has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::ZeroOutputs() {
+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>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Iterate() {
+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() {
no_prior_goal_.Print();
no_sensor_generation_.Print();
very_stale_position_.Print();
@@ -43,8 +44,10 @@
const GoalType *goal = control_loop_->goal.get();
if (goal == NULL) {
LOG_INTERVAL(no_prior_goal_);
- ZeroOutputs();
- return;
+ if (fail_no_goal) {
+ ZeroOutputs();
+ return;
+ }
}
::bbb::sensor_generation.FetchLatest();
@@ -56,18 +59,18 @@
if (!has_sensor_reset_counters_ ||
::bbb::sensor_generation->reader_pid != reader_pid_ ||
::bbb::sensor_generation->cape_resets != cape_resets_) {
- char buffer[128];
- size_t characters = ::bbb::sensor_generation->Print(buffer, sizeof(buffer));
- LOG(INFO, "new sensor_generation message %.*s\n",
- static_cast<int>(characters), buffer);
+ LOG_STRUCT(INFO, "new sensor_generation message",
+ *::bbb::sensor_generation.get());
reader_pid_ = ::bbb::sensor_generation->reader_pid;
cape_resets_ = ::bbb::sensor_generation->cape_resets;
has_sensor_reset_counters_ = true;
reset_ = true;
}
-
- LOG_STRUCT(DEBUG, "goal", *goal);
+
+ if (goal) {
+ LOG_STRUCT(DEBUG, "goal", *goal);
+ }
// Only pass in a position if we got one this cycle.
const PositionType *position = NULL;
@@ -141,8 +144,8 @@
status.Send();
}
-template <class T, bool has_position, bool fail_no_position>
-void ControlLoop<T, has_position, fail_no_position>::Run() {
+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() {
while (true) {
time::SleepUntil(NextLoopTime());
Iterate();
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index 0bce1e3..0e5c144 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -51,7 +51,8 @@
// 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>
+template <class T, bool has_position = true, bool fail_no_position = true,
+ bool fail_no_goal = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 8de4dd8..f85613f 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -177,7 +177,8 @@
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
- : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup, true, true,
+ false>(my_claw),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -386,7 +387,8 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (::aos::robot_state->autonomous &&
+ (((::aos::robot_state.get() == NULL) ? true
+ : ::aos::robot_state->autonomous) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -413,10 +415,12 @@
output->tusk_voltage = 0;
}
- if (::std::isnan(goal->bottom_angle) ||
- ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
- ::std::isnan(goal->centering)) {
- return;
+ if (goal) {
+ if (::std::isnan(goal->bottom_angle) ||
+ ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
+ ::std::isnan(goal->centering)) {
+ return;
+ }
}
if (reset()) {
@@ -424,10 +428,6 @@
bottom_claw_.Reset(position->bottom);
}
- if (::aos::robot_state.get() == nullptr) {
- return;
- }
-
const frc971::constants::Values &values = constants::GetValues();
if (position) {
@@ -456,21 +456,28 @@
bottom_claw_.absolute_position()));
}
- const bool autonomous = ::aos::robot_state->autonomous;
- const bool enabled = ::aos::robot_state->enabled;
+ bool autonomous, enabled;
+ if (::aos::robot_state.get() == nullptr) {
+ autonomous = true;
+ enabled = false;
+ } else {
+ autonomous = ::aos::robot_state->autonomous;
+ enabled = ::aos::robot_state->enabled;
+ }
double bottom_claw_velocity_ = 0.0;
double top_claw_velocity_ = 0.0;
- if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
- bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (autonomous &&
- ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- top_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
- (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
- bottom_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
+ if (goal != NULL &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
+ bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
+ (autonomous &&
+ ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ top_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
+ (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
+ bottom_claw_.zeroing_state() ==
+ ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
bottom_claw_goal_ = goal->bottom_angle;
@@ -755,18 +762,22 @@
status->bottom_velocity = claw_.X_hat(2, 0);
status->separation_velocity = claw_.X_hat(3, 0);
- bool bottom_done =
- ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
- bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
- bool separation_done =
- ::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.020;
- bool separation_done_with_ball =
- ::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.06;
- status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
- status->done_with_ball =
- is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ if (goal) {
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.020;
+ bool separation_done_with_ball =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) < 0.06;
+ status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status->done_with_ball =
+ is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ } else {
+ status->done = status->done_with_ball = false;
+ }
status->zeroed = is_ready();
status->zeroed_for_auto =
@@ -777,7 +788,7 @@
bottom_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
- was_enabled_ = ::aos::robot_state->enabled;
+ was_enabled_ = enabled;
}
} // namespace control_loops
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 37a1787..ced9b80 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -168,8 +168,8 @@
JointZeroingState zeroing_state);
};
-class ClawMotor
- : public aos::control_loops::ControlLoop<control_loops::ClawGroup> {
+class ClawMotor : public aos::control_loops::ControlLoop<
+ control_loops::ClawGroup, true, true, false> {
public:
explicit ClawMotor(control_loops::ClawGroup *my_claw =
&control_loops::claw_queue_group);