merging in the hot goal reading infrastructure
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index ad8350e..b9f52d3 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -7,6 +7,7 @@
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
#include "aos/common/network/team_number.h"
+#include "aos/common/logging/queue_logging.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
@@ -16,6 +17,7 @@
#include "frc971/actions/action_client.h"
#include "frc971/actions/shoot_action.h"
#include "frc971/actions/drivetrain_action.h"
+#include "frc971/queues/hot_goal.q.h"
using ::aos::time::Time;
@@ -227,6 +229,19 @@
static const double kTurnAngle = 0.3;
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Handling auto mode\n");
+
+ ::frc971::HotGoal start_counts;
+ hot_goal.FetchLatest();
+ bool start_counts_valid = true;
+ if (!hot_goal.get()) {
+ LOG(WARNING, "no hot goal message. will ignore\n");
+ start_counts_valid = false;
+ } else {
+ memcpy(&start_counts, hot_goal.get(), sizeof(start_counts));
+ LOG_STRUCT(INFO, "counts at start", start_counts);
+ }
+ (void)start_counts_valid;
+
ResetDrivetrain();
if (ShouldExitAuto()) return;
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 118c199..68a026c 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -29,6 +29,8 @@
'<(DEPTH)/frc971/actions/actions.gyp:action_client',
'<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
'<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
+ '<(DEPTH)/frc971/queues/queues.gyp:hot_goal',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(AOS)/common/controls/controls.gyp:control_loop',
diff --git a/frc971/input/hot_goal_reader.cc b/frc971/input/hot_goal_reader.cc
new file mode 100644
index 0000000..fbe59c2
--- /dev/null
+++ b/frc971/input/hot_goal_reader.cc
@@ -0,0 +1,99 @@
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <errno.h>
+#include <string.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include "aos/common/time.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/logging.h"
+#include "aos/linux_code/init.h"
+
+#include "frc971/queues/hot_goal.q.h"
+
+int main() {
+ ::aos::InitNRT();
+
+ int my_socket = -1;
+ while (true) {
+ if (my_socket == -1) {
+ my_socket = socket(AF_INET, SOCK_STREAM, 0);
+ if (my_socket == -1) {
+ LOG(WARNING, "socket(AF_INET, SOCK_STREAM, 0) failed with %d: %s\n",
+ errno, strerror(errno));
+ continue;
+ } else {
+ LOG(INFO, "opened socket (is %d)\n", my_socket);
+ sockaddr_in address, *sockaddr_pointer;
+ memset(&address, 0, sizeof(address));
+ address.sin_family = AF_INET;
+ address.sin_port = htons(1180);
+ sockaddr *address_pointer;
+ sockaddr_pointer = &address;
+ memcpy(&address_pointer, &sockaddr_pointer, sizeof(void *));
+ if (bind(my_socket, address_pointer, sizeof(address)) == -1) {
+ LOG(WARNING, "bind(%d, %p, %zu) failed with %d: %s\n",
+ my_socket, &address, sizeof(address), errno, strerror(errno));
+ close(my_socket);
+ my_socket = -1;
+ continue;
+ }
+
+ if (listen(my_socket, 1) == -1) {
+ LOG(WARNING, "listen(%d, 1) failed with %d: %s\n",
+ my_socket, errno, strerror(errno));
+ close(my_socket);
+ my_socket = -1;
+ continue;
+ }
+ }
+ }
+
+ int connection = accept4(my_socket, nullptr, nullptr, SOCK_NONBLOCK);
+ if (connection == -1) {
+ LOG(WARNING, "accept(%d, nullptr, nullptr) failed with %d: %s\n",
+ my_socket, errno, strerror(errno));
+ continue;
+ }
+ LOG(INFO, "accepted (is %d)\n", connection);
+
+ fd_set fds;
+ while (connection != -1) {
+ FD_ZERO(&fds);
+ FD_SET(connection, &fds);
+ struct timeval timeout_timeval =
+ ::aos::time::Time::InSeconds(1).ToTimeval();
+ switch (select(connection + 1, &fds, nullptr, nullptr, &timeout_timeval)) {
+ case 1: {
+ uint8_t data;
+ ssize_t read_bytes = read(connection, &data, sizeof(data));
+ if (read_bytes != sizeof(data)) {
+ LOG(WARNING, "read %zd bytes instead of %zd\n", read_bytes,
+ sizeof(data));
+ break;
+ }
+ static uint32_t left_count = 0, right_count = 0;
+ if (data & 0x01) ++right_count;
+ if (data & 0x02) ++left_count;
+ auto message = ::frc971::hot_goal.MakeMessage();
+ message->left_count = left_count;
+ message->right_count = right_count;
+ LOG_STRUCT(DEBUG, "sending", *message);
+ message.Send();
+ } break;
+ case 0:
+ LOG(WARNING, "read on %d timed out\n", connection);
+ close(connection);
+ connection = -1;
+ break;
+ default:
+ LOG(FATAL,
+ "select(%d, %p, nullptr, nullptr, %p) failed with %d: %s\n",
+ connection + 1, &fds, &timeout_timeval, errno, strerror(errno));
+ }
+ }
+ }
+
+ LOG(FATAL, "finished???\n");
+}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 43ebd67..69340d3 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -1,6 +1,20 @@
{
'targets': [
{
+ 'target_name': 'hot_goal_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'hot_goal_reader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(DEPTH)/frc971/queues/queues.gyp:hot_goal',
+ ],
+ },
+ {
'target_name': 'joystick_reader',
'type': 'executable',
'sources': [
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 3ad28ba..e5805ce 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -18,6 +18,7 @@
'../actions/actions.gyp:catch_action',
'../actions/actions.gyp:drivetrain_action',
'../input/input.gyp:joystick_reader',
+ '../input/input.gyp:hot_goal_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',
'<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:uart_reader_main',
diff --git a/frc971/prime/start_list.txt b/frc971/prime/start_list.txt
index 94bf6bb..2e5e329 100644
--- a/frc971/prime/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -9,3 +9,4 @@
shoot_action
drivetrain_action
catch_action
+hot_goal_reader
diff --git a/frc971/queues/hot_goal.q b/frc971/queues/hot_goal.q
new file mode 100644
index 0000000..15d8358
--- /dev/null
+++ b/frc971/queues/hot_goal.q
@@ -0,0 +1,7 @@
+package frc971;
+
+message HotGoal {
+ uint32_t left_count;
+ uint32_t right_count;
+};
+queue HotGoal hot_goal;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index 60fbabf..9c6c38c 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -7,6 +7,17 @@
},
'targets': [
{
+ 'target_name': 'hot_goal',
+ 'type': 'static_library',
+ 'sources': [
+ 'hot_goal.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/queues',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'queues',
'type': 'static_library',
'sources': ['<@(queue_files)'],