finished up the fitpc pc side joystick reading code
This meant writing a nice OO api for reading joysticks.
It also involved redoing the wire format to fix byte-order problems and
get rid of the need for packing structs.
diff --git a/aos/atom_code/input/FRCComm.h b/aos/atom_code/input/FRCComm.h
deleted file mode 100644
index 98619de..0000000
--- a/aos/atom_code/input/FRCComm.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/*************************************************************
- * NOTICE
- *
- * These are the only externally exposed functions to the
- * NetworkCommunication library
- *
- * This is an implementation of FRC Spec for Comm Protocol
- * Revision 4.5, June 30, 2008
- *
- * Copyright (c) National Instruments 2008. All Rights Reserved.
- *
- *************************************************************/
-
-#ifndef __FRC_COMM_H__
-#define __FRC_COMM_H__
-
-#include <stdint.h>
-
-typedef uint64_t UINT64;
-typedef uint32_t UINT32;
-typedef uint16_t UINT16;
-typedef uint8_t UINT8;
-typedef int8_t INT8;
-
-struct FRCCommonControlData{
- UINT16 packetIndex;
- union {
- UINT8 control;
- // The order of the bits has to be flipped on little-endian machines (aka
- // everything other than the cRIO that we build for) in order for it to
- // work. Upstream WPILib does this based off of a different macro.
-#ifndef __VXWORKS__
- struct {
- UINT8 checkVersions :1;
- UINT8 test :1;
- UINT8 resync : 1;
- UINT8 fmsAttached:1;
- UINT8 autonomous : 1;
- UINT8 enabled : 1;
- UINT8 notEStop : 1;
- UINT8 reset : 1;
- };
-#else
- struct {
- UINT8 reset : 1;
- UINT8 notEStop : 1;
- UINT8 enabled : 1;
- UINT8 autonomous : 1;
- UINT8 fmsAttached:1;
- UINT8 resync : 1;
- UINT8 test :1;
- UINT8 checkVersions :1;
- };
-#endif
- };
- UINT8 dsDigitalIn;
- UINT16 teamID;
-
- char dsID_Alliance;
- char dsID_Position;
-
- union {
- INT8 stick0Axes[6];
- struct {
- INT8 stick0Axis1;
- INT8 stick0Axis2;
- INT8 stick0Axis3;
- INT8 stick0Axis4;
- INT8 stick0Axis5;
- INT8 stick0Axis6;
- };
- };
- UINT16 stick0Buttons; // Left-most 4 bits are unused
-
- union {
- INT8 stick1Axes[6];
- struct {
- INT8 stick1Axis1;
- INT8 stick1Axis2;
- INT8 stick1Axis3;
- INT8 stick1Axis4;
- INT8 stick1Axis5;
- INT8 stick1Axis6;
- };
- };
- UINT16 stick1Buttons; // Left-most 4 bits are unused
-
- union {
- INT8 stick2Axes[6];
- struct {
- INT8 stick2Axis1;
- INT8 stick2Axis2;
- INT8 stick2Axis3;
- INT8 stick2Axis4;
- INT8 stick2Axis5;
- INT8 stick2Axis6;
- };
- };
- UINT16 stick2Buttons; // Left-most 4 bits are unused
-
- union {
- INT8 stick3Axes[6];
- struct {
- INT8 stick3Axis1;
- INT8 stick3Axis2;
- INT8 stick3Axis3;
- INT8 stick3Axis4;
- INT8 stick3Axis5;
- INT8 stick3Axis6;
- };
- };
- UINT16 stick3Buttons; // Left-most 4 bits are unused
-
- //Analog inputs are 10 bit right-justified
- UINT16 analog1;
- UINT16 analog2;
- UINT16 analog3;
- UINT16 analog4;
-
- UINT64 cRIOChecksum;
- UINT32 FPGAChecksum0;
- UINT32 FPGAChecksum1;
- UINT32 FPGAChecksum2;
- UINT32 FPGAChecksum3;
-
- char versionData[8];
-} __attribute__((packed));
-
-#endif
diff --git a/aos/atom_code/input/input.gyp b/aos/atom_code/input/input.gyp
index 25ef4b9..19d0d2f 100644
--- a/aos/atom_code/input/input.gyp
+++ b/aos/atom_code/input/input.gyp
@@ -1,15 +1,22 @@
{
'targets': [
{
- 'target_name': 'joystick',
+ 'target_name': 'joystick_input',
'type': 'static_library',
'sources': [
- 'JoystickInput.cpp'
+ 'joystick_input.cc',
],
'dependencies': [
+ '<(AOS)/common/input/input.gyp:driver_station_data',
'<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/common/network/network.gyp:socket',
- ]
+ '<(AOS)/common/common.gyp:common',
+ '<(EXTERNALS):WPILib-NetworkRobotValues',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/input/input.gyp:driver_station_data',
+ ],
},
],
}
diff --git a/aos/atom_code/input/joystick_input.cc b/aos/atom_code/input/joystick_input.cc
new file mode 100644
index 0000000..01bcbc6
--- /dev/null
+++ b/aos/atom_code/input/joystick_input.cc
@@ -0,0 +1,56 @@
+#include "aos/atom_code/input/joystick_input.h"
+
+#include <string.h>
+
+#include "aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h"
+
+#include "aos/common/Configuration.h"
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace input {
+
+void JoystickInput::Run() {
+ ReceiveSocket sock(NetworkPort::kDS);
+
+ 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 (!joysticks.DeserializeFrom(buffer, received)) {
+ LOG(WARNING, "deserializing data from %zd bytes failed\n", received);
+ continue;
+ }
+
+ 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);
+ }
+
+ data.Update(joysticks);
+ // TODO(brians): posedge/negedge logging
+
+ RunIteration(data);
+ }
+}
+
+} // namespace input
+} // namespace aos
diff --git a/aos/atom_code/input/joystick_input.h b/aos/atom_code/input/joystick_input.h
new file mode 100644
index 0000000..d905eaa
--- /dev/null
+++ b/aos/atom_code/input/joystick_input.h
@@ -0,0 +1,20 @@
+#ifndef AOS_ATOM_CODE_INPUT_JOYSTICKS_INPUT_H_
+#define AOS_ATOM_CODE_INPUT_JOYSTICKS_INPUT_H_
+
+#include "aos/common/input/driver_station_data.h"
+
+namespace aos {
+namespace input {
+
+class JoystickInput {
+ public:
+ void Run();
+
+ private:
+ virtual void RunIteration(const driver_station::Data &data) = 0;
+};
+
+} // namespace input
+} // namespace aos
+
+#endif // AOS_ATOM_CODE_INPUT_JOYSTICKS_INPUT_H_
diff --git a/aos/atom_code/output/motor_output.cc b/aos/atom_code/output/motor_output.cc
index d7022f5..b24204b 100644
--- a/aos/atom_code/output/motor_output.cc
+++ b/aos/atom_code/output/motor_output.cc
@@ -45,14 +45,15 @@
RunIteration();
- values_.digital_output_enables = hton(values_.digital_output_enables);
- values_.digital_output_values = hton(values_.digital_output_values);
-
- values_.FillInHashValue();
- values_.hash_value = hton(values_.hash_value);
-
- if (socket_.Send(&values_, sizeof(values_)) != sizeof(values_)) {
+ char buffer[sizeof(values_) + ::buffers::kOverhead];
+ ssize_t size = values_.SerializeTo(buffer, sizeof(buffer));
+ if (size <= 0) {
+ LOG(WARNING, "serializing outputs failed\n");
+ continue;
+ }
+ if (socket_.Send(buffer, size) != size) {
LOG(WARNING, "sending outputs failed\n");
+ continue;
} else {
LOG(DEBUG, "sent outputs\n");
}
diff --git a/aos/atom_code/output/motor_output.h b/aos/atom_code/output/motor_output.h
index 2c1af1d..73648ce 100644
--- a/aos/atom_code/output/motor_output.h
+++ b/aos/atom_code/output/motor_output.h
@@ -54,7 +54,7 @@
// The struct that's going to get sent over.
// Gets reset (everything set so that it won't do anything) each time through.
- NetworkRobotValues values_;
+ NetworkRobotMotors values_;
private:
// Subclasses need to actually fill out values_ here.
diff --git a/aos/common/input/driver_station_data.cc b/aos/common/input/driver_station_data.cc
new file mode 100644
index 0000000..803d46a
--- /dev/null
+++ b/aos/common/input/driver_station_data.cc
@@ -0,0 +1,74 @@
+#include "aos/common/input/driver_station_data.h"
+
+namespace aos {
+namespace input {
+namespace driver_station {
+
+Data::Data() : current_values_(), old_values_() {}
+
+void Data::Update(const NetworkRobotJoysticks &new_values) {
+ old_values_ = current_values_;
+ current_values_ = new_values;
+}
+
+namespace {
+
+bool GetButton(const ButtonLocation location,
+ const NetworkRobotJoysticks &values) {
+ return values.joysticks[location.joystick() - 1].buttons &
+ (1 << (location.number() - 1));
+}
+
+bool GetControlBitValue(const ControlBit bit,
+ const NetworkRobotJoysticks &values) {
+ switch (bit) {
+ case ControlBit::kTestMode:
+ return values.control.test_mode();
+ case ControlBit::kFmsAttached:
+ return values.control.fms_attached();
+ case ControlBit::kAutonomous:
+ return values.control.autonomous();
+ case ControlBit::kEnabled:
+ return values.control.enabled();
+ }
+}
+
+} // namespace
+
+bool Data::IsPressed(const ButtonLocation location) const {
+ return GetButton(location, current_values_);
+}
+
+bool Data::PosEdge(const ButtonLocation location) const {
+ return !GetButton(location, old_values_) &&
+ GetButton(location, current_values_);
+}
+
+bool Data::NegEdge(const ButtonLocation location) const {
+ return GetButton(location, old_values_) &&
+ !GetButton(location, current_values_);
+}
+
+bool Data::GetControlBit(const ControlBit bit) const {
+ return GetControlBitValue(bit, current_values_);
+}
+
+bool Data::PosEdge(const ControlBit bit) const {
+ return !GetControlBitValue(bit, old_values_) &&
+ GetControlBitValue(bit, current_values_);
+}
+
+bool Data::NegEdge(const ControlBit bit) const {
+ return GetControlBitValue(bit, old_values_) &&
+ !GetControlBitValue(bit, current_values_);
+}
+
+float Data::GetAxis(JoystickAxis axis) const {
+ // TODO(brians): check this math against what our joysticks report as their
+ // logical minimums and maximums
+ return current_values_.joysticks[axis.joystick()].axes[axis.number()] / 127.0;
+}
+
+} // namespace driver_station
+} // namespace input
+} // namespace aos
diff --git a/aos/common/input/driver_station_data.h b/aos/common/input/driver_station_data.h
new file mode 100644
index 0000000..41609ed
--- /dev/null
+++ b/aos/common/input/driver_station_data.h
@@ -0,0 +1,83 @@
+#ifndef AOS_COMMON_INPUT_DRIVER_STATION_DATA_H_
+#define AOS_COMMON_INPUT_DRIVER_STATION_DATA_H_
+
+// This file defines several types to support nicely looking at the data
+// received from the driver's station.
+
+#include <memory>
+
+#include "aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h"
+
+namespace aos {
+namespace input {
+namespace driver_station {
+
+// Represents a feature of a joystick (a button or an axis).
+// All indices are 1-based.
+class JoystickFeature {
+ public:
+ JoystickFeature(int joystick, int number)
+ : joystick_(joystick), number_(number) {}
+
+ // Which joystick number this is (1-based).
+ int joystick() const { return joystick_; }
+ // Which feature on joystick() this is (1-based).
+ int number() const { return number_; }
+
+ private:
+ const int joystick_, number_;
+};
+
+// Represents the location of a button.
+// Use Data to actually get the value.
+// Safe for static initialization.
+class ButtonLocation : public JoystickFeature {
+ public:
+ ButtonLocation(int joystick, int number)
+ : JoystickFeature(joystick, number) {}
+};
+
+// Represents various bits of control information that the DS sends.
+// Use Data to actually get the value.
+enum class ControlBit {
+ kTestMode, kFmsAttached, kAutonomous, kEnabled
+};
+
+// Represents a single axis of a joystick.
+// Use Data to actually get the value.
+// Safe for static initialization.
+class JoystickAxis : public JoystickFeature {
+ public:
+ JoystickAxis(int joystick, int number)
+ : JoystickFeature(joystick, number) {}
+};
+
+class Data {
+ public:
+ // Initializes the data to all buttons and control bits off and all joysticks
+ // at 0.
+ Data();
+
+ // Updates the current information with a new set of values.
+ void Update(const NetworkRobotJoysticks &new_values);
+
+ bool IsPressed(ButtonLocation location) const;
+ bool PosEdge(ButtonLocation location) const;
+ bool NegEdge(ButtonLocation location) const;
+
+ bool GetControlBit(ControlBit bit) const;
+ bool PosEdge(ControlBit bit) const;
+ bool NegEdge(ControlBit bit) const;
+
+ // Returns the value in the range [-1.0, 1.0].
+ float GetAxis(JoystickAxis axis) const;
+
+ private:
+ NetworkRobotJoysticks current_values_, old_values_;
+};
+
+} // namespace driver_station
+} // namespace input
+} // namespace aos
+
+#endif // AOS_COMMON_INPUT_DRIVER_STATION_DATA_H_
diff --git a/aos/common/input/input.gyp b/aos/common/input/input.gyp
new file mode 100644
index 0000000..e5c964c
--- /dev/null
+++ b/aos/common/input/input.gyp
@@ -0,0 +1,17 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'driver_station_data',
+ 'type': 'static_library',
+ 'sources': [
+ 'driver_station_data.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib-NetworkRobotValues',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib-NetworkRobotValues',
+ ],
+ },
+ ],
+}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
index ff1b77a..4944596 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
@@ -208,9 +208,10 @@
sockaddr_in in;
} sender_address;
int sender_address_length = sizeof(sender_address);
+ char buffer[sizeof(values_) + buffers::kOverhead];
int received = recvfrom(receive_socket_,
- reinterpret_cast<char *>(&values_),
- sizeof(values_),
+ buffer,
+ sizeof(buffer),
0,
&sender_address.addr,
&sender_address_length);
@@ -235,19 +236,12 @@
return;
}
- if (received != sizeof(values_)) {
- char buf[64];
- snprintf(buf, sizeof(buf),
- "Got packet with %zd bytes but expected %zd\n",
- received, sizeof(values_));
- wpi_setErrorWithContext(1, buf);
- return;
- }
- if (values_.CheckHashValue()) {
+ if (values_.DeserializeFrom(buffer, sizeof(buffer))) {
ProcessPacket();
} else {
char buf[64];
- snprintf(buf, sizeof(buf), "Hash Value Is %x", values_.hash_value);
+ snprintf(buf, sizeof(buf), "Deserializing from %d byte buffer",
+ sizeof(buffer));
wpi_setErrorWithContext(1, buf);
return;
}
@@ -374,10 +368,37 @@
joystick_values_.control.set_autonomous(data->autonomous);
joystick_values_.control.set_enabled(data->enabled);
}
-
++joystick_values_.index;
- joystick_values_.FillInHashValue();
+ char buffer[sizeof(joystick_values_) + buffers::kOverhead];
+ ssize_t size = joystick_values_.SerializeTo(buffer, sizeof(buffer));
+ if (size <= 0) {
+ char buf[64];
+ snprintf(buf, sizeof(buf),
+ "Serializing joystick values into %d byte buffer",
+ sizeof(buffer));
+ wpi_setErrorWithContext(-1, buf);
+ return;
+ }
+ ssize_t sent = send(send_socket_, buffer, size, 0);
+ if (sent != size) {
+ if (sent == -1) {
+ if (errno == EINTR || errno == ENOBUFS) {
+ // These are all errors that just mean it didn't manage to send this
+ // time.
+ continue;
+ } else {
+ wpi_setErrnoErrorWithContext("send to joystick output socket");
+ return;
+ }
+ } else {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "Wanted to send %d bytes but only sent %d",
+ size, sent);
+ wpi_setErrorWithContext(1, buf);
+ continue;
+ }
+ }
}
}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
index 4511d2b..c2b1998 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
@@ -101,7 +101,7 @@
const char *const receiver_address_;
int receive_socket_;
- NetworkRobotValues values_;
+ NetworkRobotMotors values_;
int send_socket_;
NetworkRobotJoysticks joystick_values_;
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
index 1d2e805..57760ab 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
@@ -8,6 +8,7 @@
#include <string.h>
+namespace buffers {
namespace {
uint32_t CalculateHashValue(const char *data, size_t size) {
@@ -20,26 +21,120 @@
} // namespace
-void NetworkRobotValues::FillInHashValue() {
- hash_value = CalculateHashValue(reinterpret_cast<const char *>(this) +
- sizeof(hash_value),
- sizeof(*this) - sizeof(hash_value));
+bool Read::Check() {
+ if (data_size_ < kOverhead) return false;
+
+ current_index_ = 0;
+ uint32_t size = Read32();
+ uint32_t hash = Read32();
+
+ if (data_size_ < size) return false;
+
+ uint32_t expected = CalculateHashValue(data_ + kOverhead, size - kOverhead);
+ return hash == expected;
}
-bool NetworkRobotValues::CheckHashValue() const {
- return hash_value == CalculateHashValue(reinterpret_cast<const char *>(this) +
- sizeof(hash_value),
- sizeof(*this) - sizeof(hash_value));
+bool Read::ReadCorrectAmount() {
+ if (overrun()) return false;
+
+ size_t read = current_index_;
+
+ current_index_ = 0;
+ uint32_t size = Read32();
+ current_index_ = kOverhead;
+
+ return read == size;
}
-void NetworkRobotJoysticks::FillInHashValue() {
- hash_value = CalculateHashValue(reinterpret_cast<const char *>(this) +
- sizeof(hash_value),
- sizeof(*this) - sizeof(hash_value));
+ssize_t Write::Finalize() {
+ if (overrun()) return -1;
+
+ uint32_t size = current_index_;
+ uint32_t hash = CalculateHashValue(data_ + kOverhead, size - kOverhead);
+
+ current_index_ = 0;
+ Write32(size);
+ Write32(hash);
+
+ return size;
}
-bool NetworkRobotJoysticks::CheckHashValue() const {
- return hash_value == CalculateHashValue(reinterpret_cast<const char *>(this) +
- sizeof(hash_value),
- sizeof(*this) - sizeof(hash_value));
+} // namespace buffers
+
+ssize_t NetworkRobotMotors::SerializeTo(char *data, size_t data_size) const {
+ buffers::Write buffer(data, data_size);
+
+ buffer.Write8(digital_module);
+ for (size_t i = 0; i < sizeof(pwm_outputs) / sizeof(pwm_outputs[0]); ++i) {
+ buffer.Write8(pwm_outputs[i]);
+ }
+ buffer.Write16(digital_output_enables);
+ buffer.Write16(digital_output_values);
+ buffer.Write8(pressure_switch_channel);
+ buffer.Write8(compressor_channel);
+ buffer.Write8(solenoid_module);
+ buffer.Write8(solenoid_values);
+
+ return buffer.Finalize();
+}
+
+bool NetworkRobotMotors::DeserializeFrom(const char *data, size_t data_size) {
+ buffers::Read buffer(data, data_size);
+ if (!buffer.Check()) return false;
+
+ digital_module = buffer.Read8();
+ for (size_t i = 0; i < sizeof(pwm_outputs) / sizeof(pwm_outputs[0]); ++i) {
+ pwm_outputs[i] = buffer.Read8();
+ }
+ digital_output_enables = buffer.Read16();
+ digital_output_values = buffer.Read16();
+ pressure_switch_channel = buffer.Read8();
+ compressor_channel = buffer.Read8();
+ solenoid_module = buffer.Read8();
+ solenoid_values = buffer.Read8();
+
+ return buffer.ReadCorrectAmount();
+}
+
+ssize_t NetworkRobotJoysticks::SerializeTo(char *data, size_t data_size) const {
+ buffers::Write buffer(data, data_size);
+
+ for (size_t i = 0; i < sizeof(joysticks) / sizeof(joysticks[0]); ++i) {
+ buffer.Write16(joysticks[i].buttons);
+ for (size_t ii = 0;
+ ii < sizeof(joysticks[0].axes) / sizeof(joysticks[0].axes[0]);
+ ++ii) {
+ buffer.Write8(joysticks[i].axes[ii]);
+ }
+ }
+ buffer.Write16(control_packet_index);
+ buffer.Write16(index);
+ buffer.Write16(team_number);
+ buffer.Write8(alliance);
+ buffer.Write8(position);
+ buffer.Write8(control.bits);
+
+ return buffer.Finalize();
+}
+
+bool NetworkRobotJoysticks::DeserializeFrom(const char *data, size_t data_size) {
+ buffers::Read buffer(data, data_size);
+ if (!buffer.Check()) return false;
+
+ for (size_t i = 0; i < sizeof(joysticks) / sizeof(joysticks[0]); ++i) {
+ joysticks[i].buttons = buffer.Read16();
+ for (size_t ii = 0;
+ ii < sizeof(joysticks[0].axes) / sizeof(joysticks[0].axes[0]);
+ ++ii) {
+ joysticks[i].axes[ii] = buffer.Read8();
+ }
+ }
+ control_packet_index = buffer.Read16();
+ index = buffer.Read16();
+ team_number = buffer.Read16();
+ alliance = buffer.Read8();
+ position = buffer.Read8();
+ control.bits = buffer.Read8();
+
+ return buffer.ReadCorrectAmount();
}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
index b66ad7c..4b05c05 100644
--- a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
@@ -8,20 +8,157 @@
#define WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_VALUES_H_
#include <stdint.h>
+#include <arpa/inet.h>
// This file needs to not have any dependencies on any other parts of WPILib so
// that it can be #included by other code (like that which sends these values).
// All multi-byte values are sent over the network in big endian (network)
// byte order.
+//
+// The structures get run through a Serialize phase to avoid byte-order,
+// padding, etc compatibility problems.
-// The structure that actually gets sent over the network to the cRIO with motor
-// values.
+// Provides a convenient way to serialize/deserialize data.
+// The serialized format consists of a 4-byte size (of everything, including the
+// size and hash value), a 4-byte hash value, and then all of the actual data.
+namespace buffers {
+// The number of extra bytes needed on top of what the actual data takes.
+// Code should use this constant in case other information is added to the
+// start (or end).
+static const size_t kOverhead = 4 + 4;
+
+// Allows reading data out of a buffer.
+// Instances are not safe for concurrent use.
+//
+// To make writing code easier, the Read* methods simply return 0 on overrun.
+// Users should check at the end (see overrun()) to see if all of the data that
+// they read is valid.
+class Read {
+ public:
+ // Does not take ownership of data, but the object will use it throughout its
+ // lifetime.
+ Read(const char *data, size_t data_size)
+ : data_(data), data_size_(data_size), current_index_(kOverhead) {}
+
+ // Returns whether there is enough data according to the size recorded with
+ // the data and the hash value is correct.
+ // Will reset the current read position to the beginning of the data unless
+ // overrun() is true.
+ bool Check();
+
+ // Returns whether or not the correct amount of data (according to the size
+ // stored at the start) was read.
+ // Will reset the current read position to the beginning of the data unless
+ // overrun() is true.
+ bool ReadCorrectAmount();
+
+ uint8_t Read8() {
+ current_index_ += 1;
+ if (overrun()) return 0;
+ return data_[current_index_ - 1];
+ }
+ uint16_t Read16() {
+ current_index_ += 2;
+ if (overrun()) return 0;
+ union {
+ uint16_t value;
+ uint8_t bytes[2];
+ } value;
+ value.bytes[0] = data_[current_index_ - 2];
+ value.bytes[1] = data_[current_index_ - 1];
+ return ntohs(value.value);
+ }
+ uint32_t Read32() {
+ current_index_ += 4;
+ if (overrun()) return 0;
+ union {
+ uint32_t value;
+ uint8_t bytes[4];
+ } value;
+ value.bytes[0] = data_[current_index_ - 4];
+ value.bytes[1] = data_[current_index_ - 3];
+ value.bytes[2] = data_[current_index_ - 2];
+ value.bytes[3] = data_[current_index_ - 1];
+ return ntohl(value.value);
+ }
+
+ // Returns whether or not we ran over the end.
+ bool overrun() const { return current_index_ >= data_size_; }
+
+ private:
+ // Where we are reading or writing from. Not owned by this object.
+ const char *const data_;
+ const size_t data_size_;
+
+ // The index of the next read/write to data_.
+ size_t current_index_;
+};
+
+// Allows writing data into a buffer.
+// Instances are not safe for concurrent use.
+//
+// To make writing code easier, the Write* methods simply do nothing on overrun.
+// Users should check at the end (see overrun()) to see if all of the data that
+// they write actually got written.
+class Write {
+ public:
+ Write(char *data, size_t data_size)
+ : data_(data), data_size_(data_size), current_index_(kOverhead) {}
+
+ // Fills in the hash value and size at the beginning and returns the number of
+ // bytes used.
+ // Afterwards, further writes will go to the beginning of the data again.
+ // Returns the number of bytes used (including kOverhead at the beginning) or
+ // -1 if writing too much data was attempted.
+ ssize_t Finalize();
+
+ void Write8(uint8_t value) {
+ current_index_ += 1;
+ if (overrun()) return;
+ data_[current_index_ - 1] = value;
+ }
+ void Write16(uint16_t value) {
+ current_index_ += 2;
+ if (overrun()) return;
+ union {
+ uint16_t value;
+ uint8_t bytes[2];
+ } flipped;
+ flipped.value = htons(value);
+ data_[current_index_ - 2] = flipped.bytes[0];
+ data_[current_index_ - 1] = flipped.bytes[1];
+ }
+ void Write32(uint32_t value) {
+ current_index_ += 4;
+ if (overrun()) return;
+ union {
+ uint32_t value;
+ uint8_t bytes[4];
+ } flipped;
+ flipped.value = htonl(value);
+ data_[current_index_ - 4] = flipped.bytes[0];
+ data_[current_index_ - 3] = flipped.bytes[1];
+ data_[current_index_ - 2] = flipped.bytes[2];
+ data_[current_index_ - 1] = flipped.bytes[3];
+ }
+
+ // Returns whether or not we ran over the end.
+ bool overrun() const { return current_index_ >= data_size_; }
+
+ private:
+ // Where we are reading or writing from. Not owned by this object.
+ char *const data_;
+ const size_t data_size_;
+
+ // The index of the next read/write to data_.
+ size_t current_index_;
+};
+
+} // namespace buffers
+
+// Contains motor and other output values.
// All channel and module numbers are 1-based like usual.
-struct NetworkRobotValues {
- // A hash value to make sure that corrupted packets don't get used.
- // IMPORTANT: Must stay at the beginning.
- uint32_t hash_value;
-
+struct NetworkRobotMotors {
// Which digital module this packet has values for (1 or 2).
// 0 means the default one and -1 means to not update any one.
int8_t digital_module;
@@ -46,20 +183,24 @@
// 1 bit for each solenoid output.
uint8_t solenoid_values;
- // Sets hash_value to the correct value for the rest of the data.
- void FillInHashValue();
- // Returns whether or not hash_value matches the rest of the data. Any byte
- // order conversion must be performed ONLY on the hash_value field before
- // calling this.
- bool CheckHashValue() const;
-} __attribute__((packed));
+ // Serializes this object into data (which must be a buffer with at least
+ // data_size bytes in it).
+ // data_size should be at least sizeof(*this) + buffers::kOverhead to make sure
+ // that it is big enough.
+ // See DeserializeFrom to decode data back into an instance of this class.
+ // Returns the number of bytes of data that were used or -1 if data_size is
+ // too small.
+ ssize_t SerializeTo(char *data, size_t data_size) const;
+ // Deserializes data into this object (which must be a buffer of at least
+ // data_size bytes).
+ // data should be (all of) the data written by SerializeTo.
+ // Returns whether or not the hash value etc information in data matched (if
+ // false, the members of this object may or may not have been modified).
+ bool DeserializeFrom(const char *data, size_t data_size);
+};
// The structure that the cRIO sends out with joystick positions etc.
struct NetworkRobotJoysticks {
- // A hash value to make sure that corrupted packets don't get used.
- // IMPORTANT: Must stay at the beginning.
- uint32_t hash_value;
-
// A structure that stores the information about a joystick and instances for
// each of the 4 joysticks.
struct Joystick {
@@ -69,7 +210,7 @@
// Raw values for each of the 6 joystick axes.
// The range of values depends on the joystick.
int8_t axes[6];
- } __attribute__((packed)) joysticks[4];
+ } joysticks[4];
// The index number from the DS.
uint16_t control_packet_index;
@@ -89,13 +230,13 @@
// of it for actually sending the information.
// Not just a C bitfield because those are a mess for portability.
struct ControlInformation {
- bool test_mode() { return GetBit(kTestMode); }
+ bool test_mode() const { return GetBit(kTestMode); }
void set_test_mode(bool value) { SetBit(kTestMode, value); }
- bool fms_attached() { return GetBit(kFmsAttached); }
+ bool fms_attached() const { return GetBit(kFmsAttached); }
void set_fms_attached(bool value) { SetBit(kFmsAttached, value); }
- bool autonomous() { return GetBit(kAutonomous); }
+ bool autonomous() const { return GetBit(kAutonomous); }
void set_autonomous(bool value) { SetBit(kAutonomous, value); }
- bool enabled() { return GetBit(kEnabled); }
+ bool enabled() const { return GetBit(kEnabled); }
void set_enabled(bool value) { SetBit(kEnabled, value); }
private:
@@ -105,7 +246,7 @@
static const int kAutonomous = 2;
static const int kEnabled = 3;
- bool GetBit(int bit) {
+ bool GetBit(int bit) const {
return bits & (1 << bit);
}
void SetBit(int bit, bool value) {
@@ -114,15 +255,26 @@
bits |= (mask & (value ? 0xFF : 0x00));
}
- uint8_t bits;
- } __attribute__((packed)) control;
+ // So that it can access bits directly for Serialize/Deserialize.
+ friend class NetworkRobotJoysticks;
- // Sets hash_value to the correct value for the rest of the data.
- void FillInHashValue();
- // Returns whether or not hash_value matches the rest of the data. Any byte
- // order conversion must be performed ONLY on the hash_value field before
- // calling this.
- bool CheckHashValue() const;
-} __attribute__((packed));
+ uint8_t bits;
+ } control;
+
+ // Serializes this object into data (which must be a buffer with at least
+ // data_size bytes in it).
+ // data_size should be at least sizeof(*this) + buffers::kOverhead to make sure
+ // that it is big enough.
+ // See DeserializeFrom to decode data back into an instance of this class.
+ // Returns the number of bytes of data that were used or -1 if data_size is
+ // too small.
+ ssize_t SerializeTo(char *data, size_t data_size) const;
+ // Deserializes data into this object (which must be a buffer of at least
+ // data_size bytes).
+ // data should be (all of) the data written by SerializeTo.
+ // Returns whether or not the hash value etc information in data matched (if
+ // false, the members of this object may or may not have been modified).
+ bool DeserializeFrom(const char *data, size_t data_size);
+};
#endif // WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_VALUES_H_
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 7b11625..20f5d6e 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -3,9 +3,9 @@
#include <unistd.h>
#include <math.h>
-#include "aos/aos_core.h"
-#include "aos/atom_code/input/FRCComm.h"
-#include "aos/atom_code/input/JoystickInput.h"
+#include "aos/atom_code/init.h"
+#include "aos/atom_code/input/joystick_input.h"
+#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -27,41 +27,70 @@
using ::frc971::control_loops::hangers;
using ::frc971::vision::target_angle;
-namespace frc971 {
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
-class JoystickReader : public aos::JoystickInput {
+namespace frc971 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kDriveControlLoopEnable1(1, 7),
+ kDriveControlLoopEnable2(1, 11);
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+const ButtonLocation kLongShot(3, 5);
+const ButtonLocation kMediumShot(3, 3);
+const ButtonLocation kShortShot(3, 6);
+const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
+
+const ButtonLocation kWristDown(3, 8);
+
+const ButtonLocation kFire(3, 11);
+const ButtonLocation kIntake(3, 10);
+const ButtonLocation kForceFire(3, 12);
+const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+
+const ButtonLocation kDeployHangers(3, 1);
+
+class Reader : public ::aos::input::JoystickInput {
public:
static const bool kWristAlwaysDown = false;
- JoystickReader() : aos::JoystickInput() {
+ Reader() {
shifters.MakeWithBuilder().set(true).Send();
}
- virtual void RunIteration() {
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
static bool is_high_gear = false;
- if (Pressed(0, AUTONOMOUS)) {
- if (PosEdge(0, ENABLED)){
+ if (data.GetControlBit(ControlBit::kAutonomous)) {
+ if (data.PosEdge(ControlBit::kEnabled)){
LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
- }
- if (NegEdge(0, ENABLED)) {
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true).Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(false).Send();
}
} else { // teleop
bool is_control_loop_driving = false;
double left_goal = 0.0;
double right_goal = 0.0;
- const double wheel = control_data_.stick0Axis1 / 127.0;
- const double throttle = -control_data_.stick1Axis2 / 127.0;
+ const double wheel = data.GetAxis(kSteeringWheel);
+ const double throttle = data.GetAxis(kDriveThrottle);
LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
const double kThrottleGain = 1.0 / 2.5;
- if (Pressed(0, 7) || Pressed(0, 11)) {
+ if (data.IsPressed(kDriveControlLoopEnable1) ||
+ data.IsPressed(kDriveControlLoopEnable2)) {
static double distance = 0.0;
static double angle = 0.0;
static double filtered_goal_distance = 0.0;
- if (PosEdge(0, 7) || PosEdge(0, 11)) {
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
distance = (drivetrain.position->left_encoder +
drivetrain.position->right_encoder) / 2.0
@@ -79,7 +108,8 @@
const double kMaxVelocity = 0.6;
if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance < -kMaxVelocity * 0.02 + filtered_goal_distance) {
+ } else if (goal_distance < -kMaxVelocity * 0.02 +
+ filtered_goal_distance) {
filtered_goal_distance -= kMaxVelocity * 0.02;
} else {
filtered_goal_distance = goal_distance;
@@ -93,16 +123,16 @@
if (!(drivetrain.goal.MakeWithBuilder()
.steering(wheel)
.throttle(throttle)
- .highgear(is_high_gear).quickturn(Pressed(0, 5))
+ .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
.control_loop_driving(is_control_loop_driving)
.left_goal(left_goal).right_goal(right_goal).Send())) {
LOG(WARNING, "sending stick values failed\n");
}
- if (PosEdge(1, 1)) {
+ if (data.PosEdge(kShiftHigh)) {
is_high_gear = false;
}
- if (PosEdge(1, 3)) {
+ if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
@@ -120,7 +150,7 @@
shooter.goal.MakeMessage();
shooter_goal->velocity = 0;
static double angle_adjust_goal = 0.42;
- if (Pressed(2, 5)) {
+ if (data.IsPressed(kLongShot)) {
#if 0
target_angle.FetchLatest();
if (target_angle.IsNewerThanMS(500)) {
@@ -136,8 +166,7 @@
shooter_goal->velocity = 360;
wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.596;
- } else if (Pressed(2, 3)) {
- // medium shot
+ } else if (data.IsPressed(kMediumShot)) {
#if 0
shooter_goal->velocity = 375;
wrist_up_position = 0.70;
@@ -147,18 +176,16 @@
shooter_goal->velocity = 395;
wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.520;
- } else if (Pressed(2, 6)) {
- // short shot
+ } else if (data.IsPressed(kShortShot)) {
shooter_goal->velocity = 375;
angle_adjust_goal = 0.7267;
- } else if (Pressed(1, 7) && Pressed(1, 10)) {
- // pit shot
+ } else if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
shooter_goal->velocity = 131;
angle_adjust_goal = 0.70;
}
angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
- double wrist_pickup_position = Pressed(2, 10) /*intake*/ ?
+ double wrist_pickup_position = data.IsPressed(kIntake) ?
kWristPickup : kWristNearGround;
index_loop.status.FetchLatest();
if (index_loop.status.get()) {
@@ -169,28 +196,30 @@
}
}
wrist.goal.MakeWithBuilder()
- .goal(Pressed(2, 8) ? wrist_down_position : wrist_up_position).Send();
+ .goal(data.IsPressed(kWristDown) ?
+ wrist_down_position :
+ wrist_up_position)
+ .Send();
::aos::ScopedMessagePtr<control_loops::IndexLoop::Goal> index_goal =
index_loop.goal.MakeMessage();
- // TODO(brians): replace these with the enum values
- if (Pressed(2, 11)) {
+ if (data.IsPressed(kFire)) {
// FIRE
index_goal->goal_state = 4;
} else if (shooter_goal->velocity != 0) {
// get ready to shoot
index_goal->goal_state = 3;
- } else if (Pressed(2, 10)) {
+ } else if (data.IsPressed(kIntake)) {
// intake
index_goal->goal_state = 2;
} else {
// get ready to intake
index_goal->goal_state = 1;
}
- index_goal->force_fire = Pressed(2, 12);
+ index_goal->force_fire = data.IsPressed(kForceFire);
- const bool index_up = Pressed(2, 9);
- const bool index_down = Pressed(2, 7);
+ const bool index_up = data.IsPressed(kForceIndexUp);
+ const bool index_down = data.IsPressed(kForceIndexDown);
index_goal->override_index = index_up || index_down;
if (index_up && index_down) {
index_goal->index_voltage = 0.0;
@@ -205,7 +234,7 @@
}
static int hanger_cycles = 0;
- if (Pressed(2, 1)) {
+ if (data.IsPressed(kDeployHangers)) {
++hanger_cycles;
} else {
hanger_cycles = 0;
@@ -214,6 +243,13 @@
}
};
+} // namespace joysticks
+} // namespace input
} // namespace frc971
-AOS_RUN(frc971::JoystickReader)
+int main() {
+ ::aos::Init();
+ ::frc971::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 45f68a2..04d24a1 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -19,11 +19,12 @@
'JoystickReader.cc',
],
'dependencies': [
- '<(AOS)/atom_code/input/input.gyp:joystick',
- 'actions',
+ '<(AOS)/atom_code/input/input.gyp:joystick_input',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/atom_code/atom_code.gyp:init',
'<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
'<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',