added an LED that turns on when the wrist is zeroed
diff --git a/aos/common/queue.h b/aos/common/queue.h
index e2d2af4..6c33822 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -214,8 +214,6 @@
// Fetches the next message from the queue.
// Returns true if there was a new message available and we successfully
// fetched it. This removes the message from the queue for all readers.
- // TODO(aschuh): Fix this to use a different way of fetching messages so other
- // readers can also FetchNext.
bool FetchNext();
bool FetchNextBlocking();
diff --git a/bbb_cape/src/bbb/bbb.gyp b/bbb_cape/src/bbb/bbb.gyp
index 8efe553..1da4732 100644
--- a/bbb_cape/src/bbb/bbb.gyp
+++ b/bbb_cape/src/bbb/bbb.gyp
@@ -11,6 +11,16 @@
},
'targets': [
{
+ 'target_name': 'led',
+ 'type': 'static_library',
+ 'sources': [
+ 'led.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'crc',
'type': 'static_library',
'sources': [
diff --git a/bbb_cape/src/bbb/gpios.h b/bbb_cape/src/bbb/gpios.h
index 1471b43..0ba3323 100644
--- a/bbb_cape/src/bbb/gpios.h
+++ b/bbb_cape/src/bbb/gpios.h
@@ -1,5 +1,5 @@
-#ifndef BBB_CAPE_SRC_BBB_CAPE_CONTROL_H_
-#define BBB_CAPE_SRC_BBB_CAPE_CONTROL_H_
+#ifndef BBB_CAPE_SRC_BBB_GPIOS_H_
+#define BBB_CAPE_SRC_BBB_GPIOS_H_
#include <stdio.h>
@@ -31,4 +31,4 @@
} // namespace bbb
-#endif
+#endif // BBB_CAPE_SRC_BBB_GPIOS_H_
diff --git a/bbb_cape/src/bbb/led.cc b/bbb_cape/src/bbb/led.cc
new file mode 100644
index 0000000..5dc5e71
--- /dev/null
+++ b/bbb_cape/src/bbb/led.cc
@@ -0,0 +1,60 @@
+#include "bbb_cape/src/bbb/led.h"
+
+#include "aos/common/logging/logging.h"
+
+#include <string.h>
+#include <errno.h>
+
+#define DIRECTORY "/sys/class/leds/beaglebone:green:usr%d/"
+
+namespace bbb {
+
+LED::LED(int number) : number_(number) {
+ char trigger_path[64];
+ snprintf(trigger_path, sizeof(trigger_path), DIRECTORY "trigger", number_);
+ FILE *trigger_handle = fopen(trigger_path, "w");
+ if (trigger_handle == nullptr) {
+ LOG(FATAL, "couldn't open trigger file for LED %d because of %d: %s\n",
+ number_, errno, strerror(errno));
+ }
+ if (fputs("none", trigger_handle) < 0) {
+ LOG(FATAL,
+ "writing 'none' to file %p (trigger for LED %d) failed with %d: %s\n",
+ trigger_handle, number_, errno, strerror(errno));
+ }
+ if (fclose(trigger_handle) == -1) {
+ LOG(WARNING, "fclose(%p) failed with %d: %s\n",
+ trigger_handle, errno, strerror(errno));
+ }
+
+ char brightness_path[64];
+ snprintf(brightness_path, sizeof(brightness_path),
+ DIRECTORY "brightness", number_);
+
+ brightness_handle_ = fopen(brightness_path, "w");
+ if (brightness_handle_ == nullptr) {
+ LOG(FATAL, "fopen('%s', 'w') failed with %d: %s\n",
+ brightness_path, errno, strerror(errno));
+ }
+}
+
+LED::~LED() {
+ if (fclose(brightness_handle_) == -1) {
+ LOG(WARNING, "fclose(%p) failed with %d: %s\n",
+ brightness_handle_, errno, strerror(errno));
+ }
+}
+
+void LED::Set(bool on) {
+ rewind(brightness_handle_);
+ if (fputs(on ? "255" : "0", brightness_handle_) == EOF) {
+ LOG(FATAL, "fputs(255|0, %p) for LED %d failed with %d: %s\n",
+ brightness_handle_, number_, errno, strerror(errno));
+ }
+ if (fflush(brightness_handle_) == EOF) {
+ LOG(FATAL, "fflush(%p) for LED %d failed with %d: %s\n",
+ brightness_handle_, number_, errno, strerror(errno));
+ }
+}
+
+} // namespace bbb
diff --git a/bbb_cape/src/bbb/led.h b/bbb_cape/src/bbb/led.h
new file mode 100644
index 0000000..19976e6
--- /dev/null
+++ b/bbb_cape/src/bbb/led.h
@@ -0,0 +1,27 @@
+#ifndef BBB_CAPE_SRC_BBB_LED_H_
+#define BBB_CAPE_SRC_BBB_LED_H_
+
+#include <stdio.h>
+
+#include "aos/common/macros.h"
+
+namespace bbb {
+
+// Allows easily controlling one of the LEDs.
+class LED {
+ public:
+ LED(int number);
+ ~LED();
+
+ void Set(bool on);
+
+ private:
+ FILE *brightness_handle_ = nullptr;
+ const int number_;
+
+ DISALLOW_COPY_AND_ASSIGN(LED);
+};
+
+} // namespace bbb
+
+#endif // BBB_CAPE_SRC_BBB_LED_H_
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