made sending fake joystick messages for testing easier
diff --git a/aos/prime/input/input.gyp b/aos/prime/input/input.gyp
index 8aba065..f7ab848 100644
--- a/aos/prime/input/input.gyp
+++ b/aos/prime/input/input.gyp
@@ -12,6 +12,7 @@
'<(AOS)/common/network/network.gyp:socket',
'<(EXTERNALS):WPILib-NetworkRobotValues',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(AOS)/common/input/input.gyp:driver_station_data',
diff --git a/aos/prime/input/joystick_input.cc b/aos/prime/input/joystick_input.cc
index e53a180..1dc1fa9 100644
--- a/aos/prime/input/joystick_input.cc
+++ b/aos/prime/input/joystick_input.cc
@@ -8,41 +8,50 @@
#include "aos/common/network/ReceiveSocket.h"
#include "aos/common/messages/RobotState.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
namespace aos {
namespace input {
void JoystickInput::Run() {
ReceiveSocket sock(NetworkPort::kDS);
+ // If true, this code won't try to read anything from the network and instead
+ // feed all 0s to the joystick code.
+ // The RobotState messages will be marked as fake so anything that outputs
+ // values won't while this is enabled.
+ static const bool kFakeJoysticks = false;
NetworkRobotJoysticks joysticks;
char buffer[sizeof(joysticks) + ::buffers::kOverhead];
driver_station::Data data;
while (true) {
- int received = sock.Receive(buffer, sizeof(buffer));
- if (received == -1) {
- LOG(WARNING, "socket receive failed with %d: %s\n",
- errno, strerror(errno));
- continue;
+ if (kFakeJoysticks) {
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.02));
+ memset(&joysticks, 0, sizeof(joysticks));
+ } else {
+ int received = sock.Receive(buffer, sizeof(buffer));
+ if (received == -1) {
+ LOG(WARNING, "socket receive failed with %d: %s\n",
+ errno, strerror(errno));
+ continue;
+ }
+
+ if (!joysticks.DeserializeFrom(buffer, received)) {
+ LOG(WARNING, "deserializing data from %d bytes failed\n", received);
+ continue;
+ }
}
- if (!joysticks.DeserializeFrom(buffer, received)) {
- LOG(WARNING, "deserializing data from %d bytes failed\n", received);
- continue;
- }
+ auto new_state = robot_state.MakeMessage();
+ new_state->enabled = joysticks.control.enabled();
+ new_state->autonomous = joysticks.control.autonomous();
+ new_state->team_id = joysticks.team_number;
+ new_state->fake = kFakeJoysticks;
+ LOG_STRUCT(DEBUG, "sending", *new_state);
- if (!robot_state.MakeWithBuilder()
- .enabled(joysticks.control.enabled())
- .autonomous(joysticks.control.autonomous())
- .team_id(joysticks.team_number)
- .Send()) {
- LOG(WARNING, "sending robot_state failed\n");
- } else {
- LOG(DEBUG, "sent robot_state{%s, %s, %hu}\n",
- joysticks.control.enabled() ? "enabled" : "disabled",
- joysticks.control.autonomous() ? "auto" : "not auto",
- joysticks.team_number);
+ if (!new_state.Send()) {
+ LOG(WARNING, "sending robot_state failed\n");
}
data.Update(joysticks);
diff --git a/aos/prime/output/motor_output.cc b/aos/prime/output/motor_output.cc
index 0bea8b2..4d4e45f 100644
--- a/aos/prime/output/motor_output.cc
+++ b/aos/prime/output/motor_output.cc
@@ -5,6 +5,7 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/logging/logging.h"
#include "aos/common/network_port.h"
+#include "aos/common/messages/RobotState.q.h"
namespace aos {
@@ -52,6 +53,12 @@
values_.compressor_channel = 0;
values_.solenoid_module = -1;
+ ::aos::robot_state.FetchLatest();
+ if (!::aos::robot_state.get() || ::aos::robot_state->fake) {
+ LOG(DEBUG, "fake robot state -> not outputting\n");
+ continue;
+ }
+
RunIteration();
char buffer[sizeof(values_) + ::buffers::kOverhead];
diff --git a/aos/prime/output/output.gyp b/aos/prime/output/output.gyp
index b1e4b79..47569ec 100644
--- a/aos/prime/output/output.gyp
+++ b/aos/prime/output/output.gyp
@@ -11,6 +11,7 @@
'<(AOS)/common/common.gyp:timing',
'<(EXTERNALS):WPILib-NetworkRobotValues',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
],
'export_dependent_settings': [
'<(AOS)/common/network/network.gyp:socket',