added an LED that turns on when the wrist is zeroed
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index b07d75b..8de4dd8 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -769,6 +769,13 @@
is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
status->zeroed = is_ready();
+ status->zeroed_for_auto =
+ (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);
was_enabled_ = ::aos::robot_state->enabled;
}
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 3225d2f..116a182 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -52,8 +52,10 @@
};
message Status {
- // True if zeroed.
+ // True if zeroed enough for the current period (autonomous or teleop).
bool zeroed;
+ // True if zeroed as much as we will force during autonomous.
+ bool zeroed_for_auto;
// True if zeroed and within tolerance for separation and bottom angle.
bool done;
// True if zeroed and within tolerance for separation and bottom angle.
diff --git a/frc971/output/led_setter.cc b/frc971/output/led_setter.cc
new file mode 100644
index 0000000..37326a2
--- /dev/null
+++ b/frc971/output/led_setter.cc
@@ -0,0 +1,16 @@
+#include "aos/common/logging/logging.h"
+
+#include "bbb/led.h"
+
+#include "frc971/control_loops/claw/claw.q.h"
+
+using ::frc971::control_loops::claw_queue_group;
+
+int main() {
+ ::bbb::LED claw_zeroed(3);
+
+ while (true) {
+ CHECK(claw_queue_group.status.FetchNextBlocking());
+ claw_zeroed.Set(claw_queue_group.status->zeroed_for_auto);
+ }
+}
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 6faca12..09f40f6 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -1,6 +1,18 @@
{
'targets': [
{
+ 'target_name': 'led_setter',
+ 'type': 'executable',
+ 'sources': [
+ 'led_setter.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:led',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'CameraServer',
'type': 'executable',
'sources': [
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index ecaeca2..c4db795 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -25,6 +25,7 @@
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:packet_finder_test',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:cows_test',
'<(DEPTH)/bbb_cape/src/flasher/flasher.gyp:stm32_flasher',
+ '../output/output.gyp:led_setter',
],
'copies': [
{
diff --git a/frc971/prime/start_list.txt b/frc971/prime/start_list.txt
index 94bf6bb..d851f3c 100644
--- a/frc971/prime/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -9,3 +9,4 @@
shoot_action
drivetrain_action
catch_action
+led_setter