Merge branch 'sensor-data-timing' into hardware-interface-integration
Conflicts:
aos/common/time.h
diff --git a/aos/atom_code/input/JoystickInput.cpp b/aos/atom_code/input/JoystickInput.cpp
index b82a940..2c08f0f 100644
--- a/aos/atom_code/input/JoystickInput.cpp
+++ b/aos/atom_code/input/JoystickInput.cpp
@@ -41,7 +41,8 @@
void JoystickInput::Run() {
ReceiveSocket sock(NetworkPort::kDS);
while (true) {
- if (sock.Recv(&control_data_, sizeof(control_data_)) == -1) {
+ if (sock.Receive(&control_data_, sizeof(control_data_)) !=
+ sizeof(control_data_)) {
LOG(WARNING, "socket receive failed\n");
continue;
}
diff --git a/aos/atom_code/input/input.gyp b/aos/atom_code/input/input.gyp
index e77fc47..25ef4b9 100644
--- a/aos/atom_code/input/input.gyp
+++ b/aos/atom_code/input/input.gyp
@@ -8,6 +8,7 @@
],
'dependencies': [
'<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/network/network.gyp:socket',
]
},
],
diff --git a/aos/atom_code/output/output.gyp b/aos/atom_code/output/output.gyp
index d41fe7d..9e264d5 100644
--- a/aos/atom_code/output/output.gyp
+++ b/aos/atom_code/output/output.gyp
@@ -27,10 +27,10 @@
'MotorOutput.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/network/network.gyp:socket',
],
'export_dependent_settings': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/network/network.gyp:socket',
],
},
],
diff --git a/aos/build/aos.gyp b/aos/build/aos.gyp
index 0c1506c..7d570fd 100644
--- a/aos/build/aos.gyp
+++ b/aos/build/aos.gyp
@@ -8,7 +8,6 @@
'conditions': [
['OS=="crio"', {
'libaos_source_files': [
- '<!@(find <(AOS)/crio/controls <(AOS)/crio/messages <(AOS)/crio/motor_server -name *.c -or -name *.cpp -or -name *.cc)',
'<(AOS)/crio/Talon.cpp',
],
}, {
@@ -33,6 +32,8 @@
],
'dependencies': [
'<(EXTERNALS):WPILib',
+ # TODO(brians): socket should only depend on the interface
+ #'<(AOS)/common/network/network.gyp:socket',
]
}, {
'sources': [
@@ -70,6 +71,32 @@
'includes': ['../build/swig.gypi'],
},
{
+ 'target_name': 'aos_core',
+ 'type': 'static_library',
+ 'sources': [
+ #'<(AOS)/aos_core.h'
+ ],
+ 'conditions': [
+ ['OS=="atom"', {
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ }, {
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ ],
+ }]
+ ],
+ },
+ {
'target_name': 'libaos',
'type': 'static_library',
'sources': ['<@(libaos_source_files)'],
diff --git a/aos/build/aos_all.gyp b/aos/build/aos_all.gyp
index d1f446a..3ba7cc8 100644
--- a/aos/build/aos_all.gyp
+++ b/aos/build/aos_all.gyp
@@ -21,6 +21,7 @@
#'../common/messages/messages.gyp:*', # TODO(brians) did this test ever exist?
'../common/common.gyp:die_test',
'../common/util/util.gyp:trapezoid_profile_test',
+ '../common/sensors/sensors.gyp:sensor_receiver_test',
'Common',
],
},
diff --git a/aos/build/create_aos_ctdt.sh b/aos/build/create_aos_ctdt.sh
index 63c927c..6bf0a62 100755
--- a/aos/build/create_aos_ctdt.sh
+++ b/aos/build/create_aos_ctdt.sh
@@ -9,4 +9,3 @@
echo 'void aos_call_init_functions() {'
echo -e $calls
echo '}'
-
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index e4b64b5..95b7fd6 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -34,10 +34,10 @@
'time.cc'
],
'dependencies': [
- # TODO(aschuh): Fix this dependency loop by
- # providing a logging interface.
- # '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/build/aos.gyp:aos/ResourceList.h',
+ # TODO(aschuh): Fix this dependency loop by
+ # providing a logging interface.
+ # '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/build/aos.gyp:aos/ResourceList.h',
],
},
{
@@ -83,9 +83,11 @@
],
'dependencies': [
'<(AOS)/common/common.gyp:common',
+ 'time',
],
'export_dependent_settings': [
'<(AOS)/common/common.gyp:common',
+ 'time',
],
},
{
@@ -142,11 +144,13 @@
'<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/build/aos.gyp:logging',
'timing',
+ 'time',
],
'export_dependent_settings': [
'<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/build/aos.gyp:logging',
'timing',
+ 'time',
],
},
{
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index a15d909..018a2f5 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -115,7 +115,7 @@
template <class T, bool has_position>
void ControlLoop<T, has_position>::Run() {
while (true) {
- ::aos::time::PhasedLoop10MS(0);
+ ::aos::time::PhasedLoopXMS(kLoopFrequency.ToMSec(), 0);
Iterate();
}
}
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index ac88c98..bb76ffa 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -7,6 +7,7 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/type_traits.h"
#include "aos/common/queue.h"
+#include "aos/common/time.h"
namespace aos {
namespace control_loops {
@@ -36,6 +37,9 @@
virtual uint32_t UniqueID() = 0;
};
+// Control loops run this often, "starting" at time 0.
+const time::Time kLoopFrequency = time::Time::InSeconds(0.01);
+
// Provides helper methods to assist in writing control loops.
// This template expects to be constructed with a queue group as an argument
// that has a goal, position, status, and output queue.
diff --git a/aos/common/input/SensorInput-tmpl.h b/aos/common/input/SensorInput-tmpl.h
deleted file mode 100644
index e92a923..0000000
--- a/aos/common/input/SensorInput-tmpl.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#include "aos/common/input/SensorInput.h"
-#ifndef __VXWORKS__
-#include "aos/common/network/ReceiveSocket.h"
-#include "aos/common/Configuration.h"
-#endif
-
-namespace aos {
-
-#ifdef __VXWORKS__
-template<class Values> SEM_ID SensorInput<Values>::lock_ = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
-template<class Values> std::vector<SensorInput<Values> *> SensorInput<Values>::running_;
-#endif
-template<class Values> void SensorInput<Values>::Run() {
-#ifndef __VXWORKS__
- ReceiveSocket sock(NetworkPort::kSensors);
- Values values;
- while (true) {
- if (sock.Recv(&values, sizeof(values)) == -1) {
- LOG(WARNING, "socket receive failed\n");
- continue;
- }
- RunIteration(values);
- }
-#else
- semTake(lock_, WAIT_FOREVER);
- running_.push_back(this);
- semGive(lock_);
-#endif
-}
-
-#ifdef __VXWORKS__
-template<class Values> void SensorInput<Values>::RunIterationAll(Values &vals) {
- semTake(lock_, WAIT_FOREVER);
- for (auto it = running_.begin(); it != running_.end(); ++it) {
- (*it)->RunIteration(vals);
- }
- semGive(lock_);
-}
-#endif
-
-} // namespace aos
diff --git a/aos/common/input/SensorInput.h b/aos/common/input/SensorInput.h
deleted file mode 100644
index 7f8eaa7..0000000
--- a/aos/common/input/SensorInput.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef AOS_INPUT_SENSOR_INPUT_H_
-#define AOS_INPUT_SENSOR_INPUT_H_
-
-#include "aos/aos_core.h"
-#ifdef __VXWORKS__
-#include <vector>
-#include <semLib.h>
-#endif
-
-namespace aos {
-
-// Class for implementing code that takes information from a sensor struct and
-// places it into queues. Subclasses should be compiled for both the atom and
-// the crio to support crio control loops.
-template<class Values> class SensorInput {
- protected:
- virtual void RunIteration(Values &values) = 0;
- public:
- // Enters an infinite loop that reads values and calls RunIteration.
- void Run();
-
-#ifdef __VXWORKS__
- // Calls RunIteration on all instances with values.
- static void RunIterationAll(Values &values);
- private:
- static SEM_ID lock_;
- static std::vector<SensorInput *> running_;
-#endif
-};
-
-} // namespace aos
-
-#include "SensorInput-tmpl.h"
-
-#endif
-
diff --git a/aos/common/input/input.gyp b/aos/common/input/input.gyp
new file mode 100644
index 0000000..0a77075
--- /dev/null
+++ b/aos/common/input/input.gyp
@@ -0,0 +1,19 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'sensor_input',
+ 'type': 'static_library',
+ 'sources': [
+ #'SensorInput-tmpl.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ ],
+}
diff --git a/aos/common/logging/logging.gyp b/aos/common/logging/logging.gyp
index 52d2527..b6b339a 100644
--- a/aos/common/logging/logging.gyp
+++ b/aos/common/logging/logging.gyp
@@ -8,7 +8,7 @@
],
'dependencies': [
'<(EXTERNALS):gtest',
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
],
},
],
diff --git a/aos/common/messages/QueueHolder.h b/aos/common/messages/QueueHolder.h
index 2b7c0d7..23ddc95 100644
--- a/aos/common/messages/QueueHolder.h
+++ b/aos/common/messages/QueueHolder.h
@@ -6,11 +6,14 @@
#include <algorithm>
-#include "aos/aos_core.h"
#include "aos/common/control_loop/Timing.h"
#include "aos/common/byteorder.h"
#include "aos/common/time.h"
#include "aos/common/type_traits.h"
+#include "aos/common/logging/logging.h"
+#ifndef __VXWORKS__
+#include "aos/atom_code/ipc_lib/queue.h"
+#endif
namespace aos {
diff --git a/aos/common/messages/messages.gyp b/aos/common/messages/messages.gyp
index 94508c4..e7669c7 100644
--- a/aos/common/messages/messages.gyp
+++ b/aos/common/messages/messages.gyp
@@ -1,6 +1,33 @@
{
'targets': [
{
+ 'target_name': 'QueueHolder',
+ 'type': 'static_library',
+ 'sources': [
+ # 'QueueHolder.h'
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'conditions': [
+ ['OS!="crio"', {
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ ],
+ }],
+ ],
+ },
+ {
'target_name': 'aos_queues',
'type': 'static_library',
'sources': [
diff --git a/aos/common/network/ReceiveSocket.cpp b/aos/common/network/ReceiveSocket.cpp
index 671f966..c754f32 100644
--- a/aos/common/network/ReceiveSocket.cpp
+++ b/aos/common/network/ReceiveSocket.cpp
@@ -19,12 +19,12 @@
return ret;
}
- if (bind(socket_, &addr_.addr,
+ if (bind(socket_, &addr_.addr,
sizeof(addr_)) == -1) {
LOG(ERROR, "failed to bind to address '%s' because of %d: %s\n", localhost,
errno, strerror(errno));
return last_ret_ = -1;
- }
+ }
return last_ret_ = 0;
}
diff --git a/aos/common/network/ReceiveSocket.h b/aos/common/network/ReceiveSocket.h
index bd834c9..468cb22 100644
--- a/aos/common/network/ReceiveSocket.h
+++ b/aos/common/network/ReceiveSocket.h
@@ -7,11 +7,8 @@
class ReceiveSocket : public Socket {
public:
- inline ReceiveSocket(NetworkPort port) { Connect(port); }
+ inline ReceiveSocket(NetworkPort port) { Connect(port); }
int Connect(NetworkPort port);
-
- inline int Recv(void *buf, int length) { return Socket::Recv(buf, length); }
- inline int Recv(void *buf, int length, long usec) { return Socket::Recv(buf, length, usec); }
};
} // namespace aos
diff --git a/aos/common/network/SendSocket.cpp b/aos/common/network/SendSocket.cpp
index 89665dc..78aca79 100644
--- a/aos/common/network/SendSocket.cpp
+++ b/aos/common/network/SendSocket.cpp
@@ -18,24 +18,24 @@
return ret;
}
- if (connect(socket_, &addr_.addr,
+ if (connect(socket_, &addr_.addr,
sizeof(addr_)) < 0) {
LOG(ERROR, "couldn't connect to ip '%s' because of %d: %s\n", robot_ip,
errno, strerror(errno));
last_ret_ = 1;
}
- hold_msg_len_ = 0;
+ hold_msg_len_ = 0;
return last_ret_ = 0;
}
int SendSocket::SendHoldMsg() {
- if (hold_msg_len_ > 0) {
- int val = Send(hold_msg_, hold_msg_len_);
- hold_msg_len_ = 0;
- return val;
- }
- return -1;
+ if (hold_msg_len_ > 0) {
+ int val = Send(hold_msg_, hold_msg_len_);
+ hold_msg_len_ = 0;
+ return val;
+ }
+ return -1;
}
} // namespace aos
diff --git a/aos/common/network/SendSocket.h b/aos/common/network/SendSocket.h
index 6d3feb1..a0c1fe2 100644
--- a/aos/common/network/SendSocket.h
+++ b/aos/common/network/SendSocket.h
@@ -7,19 +7,19 @@
class SendSocket : public Socket {
public:
- //inline int Send(const void *buf, int length) { return Socket::Send(buf, length); }
+ //inline int Send(const void *buf, int length) { return Socket::Send(buf, length); }
// Connect must be called before use.
SendSocket() {}
// Calls Connect automatically.
- SendSocket(NetworkPort port, const char *robot_ip) {
+ SendSocket(NetworkPort port, const char *robot_ip) {
Connect(port, robot_ip);
}
- int Connect(NetworkPort port, const char *robot_ip, int type = SOCK_DGRAM);
+ int Connect(NetworkPort port, const char *robot_ip, int type = SOCK_DGRAM);
static const size_t MAX_MSG = 4096;
- char hold_msg_[MAX_MSG];
- size_t hold_msg_len_;
- int SendHoldMsg();
+ char hold_msg_[MAX_MSG];
+ size_t hold_msg_len_;
+ int SendHoldMsg();
};
} // namespace aos
diff --git a/aos/common/network/Socket.cpp b/aos/common/network/Socket.cpp
index b01bcb8..0366e7c 100644
--- a/aos/common/network/Socket.cpp
+++ b/aos/common/network/Socket.cpp
@@ -3,37 +3,40 @@
#include <string.h>
#include <errno.h>
-#include "aos/aos_core.h"
+#include "aos/common/logging/logging.h"
#include "aos/common/network/SocketLibraries.h"
namespace aos {
int Socket::Connect(NetworkPort port, const char *address, int type) {
last_ret_ = 0;
- if ((socket_ = socket(AF_INET, type, 0)) < 0) {
- LOG(ERROR, "failed to create socket because of %d: %s\n", errno, strerror(errno));
+ if ((socket_ = socket(AF_INET, type, 0)) < 0) {
+ LOG(ERROR, "failed to create socket because of %d: %s\n",
+ errno, strerror(errno));
return last_ret_ = 1;
- }
+ }
- memset(&addr_, 0, sizeof(addr_));
- addr_.in.sin_family = AF_INET;
+ memset(&addr_, 0, sizeof(addr_));
+ addr_.in.sin_family = AF_INET;
addr_.in.sin_port = htons(static_cast<uint16_t>(port));
#ifndef __VXWORKS__
const int failure_return = 0;
#else
const int failure_return = -1;
#endif
- if (inet_aton(lame_unconst(address), &addr_.in.sin_addr) == failure_return) {
- LOG(ERROR, "Invalid IP address '%s' because of %d: %s\n", address,
+ if (inet_aton(lame_unconst(address), &addr_.in.sin_addr) == failure_return) {
+ LOG(ERROR, "Invalid IP address '%s' because of %d: %s\n", address,
errno, strerror(errno));
return last_ret_ = -1;
- }
+ }
return last_ret_ = 0;
}
+
Socket::Socket() : socket_(-1), last_ret_(2) {}
+
Socket::~Socket() {
- close(socket_);
+ close(socket_);
}
void Socket::Reset() {
@@ -44,34 +47,36 @@
last_ret_ = 0;
}
-int Socket::Recv(void *buf, int length) {
- const int ret = recv(socket_, static_cast<char *>(buf), length, 0);
- last_ret_ = (ret == -1) ? -1 : 0;
- return ret;
-}
-int Socket::Recv(void *buf, int length, long usec) {
- timeval tv;
- tv.tv_sec = 0;
- tv.tv_usec = usec;
- fd_set fds;
- FD_ZERO(&fds);
- FD_SET(socket_, &fds);
- switch (select(FD_SETSIZE, &fds, NULL, NULL, &tv)) {
- case 1:
- return Recv(buf, length);
- case 0:
- return last_ret_ = 0;
- default:
- perror("select on socket to receive from failed");
- return last_ret_ = -1;
- }
-}
-int Socket::Send(const void *buf, int length) {
- const int ret = write(socket_,
- lame_unconst(static_cast<const char *>(buf)), length);
- //const int ret = length;
+int Socket::Receive(void *buf, int length) {
+ const int ret = recv(socket_, static_cast<char *>(buf), length, 0);
last_ret_ = (ret == -1) ? -1 : 0;
return ret;
}
-} // namespace aos
+int Socket::Receive(void *buf, int length, time::Time timeout) {
+ timeval timeout_timeval = timeout.ToTimeval();
+ fd_set fds;
+ FD_ZERO(&fds);
+ FD_SET(socket_, &fds);
+ switch (select(FD_SETSIZE, &fds, NULL, NULL, &timeout_timeval)) {
+ case 1:
+ return Receive(buf, length);
+ case 0:
+ return last_ret_ = 0;
+ default:
+ if (errno == EINTR) {
+ return last_ret_ = 0;
+ }
+ LOG(FATAL, "select(FD_SETSIZE, %p, NULL, NULL, %p) failed with %d: %s\n",
+ &fds, &timeout_timeval, errno, strerror(errno));
+ }
+}
+
+int Socket::Send(const void *buf, int length) {
+ const int ret = write(socket_,
+ lame_unconst(static_cast<const char *>(buf)), length);
+ last_ret_ = (ret == -1) ? -1 : 0;
+ return ret;
+}
+
+} // namespace aos
diff --git a/aos/common/network/Socket.h b/aos/common/network/Socket.h
index ceee754..68fd32c 100644
--- a/aos/common/network/Socket.h
+++ b/aos/common/network/Socket.h
@@ -1,5 +1,5 @@
-#ifndef AOS_NETWORK_SOCKET_H_
-#define AOS_NETWORK_SOCKET_H_
+#ifndef AOS_COMMON_NETWORK_SOCKET_H_
+#define AOS_COMMON_NETWORK_SOCKET_H_
#include <sys/socket.h>
#include <sys/types.h>
@@ -8,7 +8,9 @@
#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
+
#include "aos/common/Configuration.h"
+#include "aos/common/time.h"
namespace aos {
@@ -16,9 +18,21 @@
public:
int LastStatus() const { return last_ret_; }
- int Send(const void *buf, int length);
- int Recv(void *buf, int length);
- int Recv(void *buf, int length, long usec); // returns 0 if timed out
+ int Send(const void *buf, int length);
+
+ // buf is where to put the data and length is the maximum amount of data to
+ // put in for all overloads.
+ // All overloads return how many bytes were received or -1 for error. 0 is a
+ // valid return value for all overloads.
+ // No timeout.
+ int Receive(void *buf, int length);
+ // DEPRECATED(brians): use the time::Time overload instead
+ int Receive(void *buf, int length, long usec_timeout) {
+ return Receive(buf, length, time::Time::InUS(usec_timeout));
+ }
+ // timeout is relative
+ int Receive(void *buf, int length, time::Time timeout);
+
protected:
int Connect(NetworkPort port, const char *address, int type = SOCK_DGRAM);
Socket();
@@ -27,16 +41,15 @@
// Resets socket_ and last_ret_.
void Reset();
- union {
+ union {
sockaddr_in in;
sockaddr addr;
} addr_; // filled in by Connect
- int socket_;
+ int socket_;
int last_ret_;
};
-} // namespace aos
+} // namespace aos
-#endif
-
+#endif // AOS_COMMON_NETWORK_SOCKET_H_
diff --git a/aos/common/network/network.gyp b/aos/common/network/network.gyp
index ca3e653..a41509d 100644
--- a/aos/common/network/network.gyp
+++ b/aos/common/network/network.gyp
@@ -47,9 +47,11 @@
],
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
],
'export_dependent_settings': [
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
],
},
],
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 2ae2f0a..612429a 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -9,7 +9,7 @@
#undef USE_UNSAFE
#endif
-#include "aos/aos_core.h"
+#include "aos/common/time.h"
#include "aos/common/macros.h"
#ifndef USE_UNSAFE
#include "aos/atom_code/ipc_lib/queue.h"
diff --git a/aos/common/queue_testutils.cc b/aos/common/queue_testutils.cc
index 7db5a6e..1d47e62 100644
--- a/aos/common/queue_testutils.cc
+++ b/aos/common/queue_testutils.cc
@@ -70,9 +70,32 @@
test_info.name());
fputs("\tThis will include already printed WARNING and up messages.\n",
stdout);
+ fputs("\tIt will also include duplicates of all gtest failures.\n",
+ stdout);
TestLogImplementation::GetInstance()->PrintAllMessages();
}
}
+
+ virtual void OnTestPartResult( const ::testing::TestPartResult &result) {
+ if (result.failed()) {
+ const char *failure_type = "unknown";
+ switch (result.type()) {
+ case ::testing::TestPartResult::Type::kNonFatalFailure:
+ failure_type = "EXPECT";
+ break;
+ case ::testing::TestPartResult::Type::kFatalFailure:
+ failure_type = "ASSERT";
+ break;
+ case ::testing::TestPartResult::Type::kSuccess:
+ break;
+ }
+ log_do(ERROR, "%s: %d: gtest %s failure\n%s\n",
+ result.file_name(),
+ result.line_number(),
+ failure_type,
+ result.message());
+ }
+ }
};
void *DoEnableTestLogging() {
diff --git a/aos/common/sensors/sensor_broadcaster-tmpl.h b/aos/common/sensors/sensor_broadcaster-tmpl.h
new file mode 100644
index 0000000..fa3f2fd
--- /dev/null
+++ b/aos/common/sensors/sensor_broadcaster-tmpl.h
@@ -0,0 +1,52 @@
+#include "aos/common/Configuration.h"
+
+namespace aos {
+namespace sensors {
+
+template<class Values>
+SensorBroadcaster<Values>::SensorBroadcaster(
+ SensorPackerInterface<Values> *packer)
+ : packer_(packer),
+ notifier_(StaticNotify, this),
+ socket_(NetworkPort::kSensors,
+ configuration::GetIPAddress(
+ configuration::NetworkDevice::kAtom)),
+ crio_control_loop_runner_(NULL) {
+ static_assert(shm_ok<SensorData<Values>>::value,
+ "it is going to get sent over a socket");
+ data_.count = 0;
+}
+
+template<class Values>
+void SensorBroadcaster<Values>::Start() {
+ notifier_.StartPeriodic(kSensorSendFrequency);
+ if (!notifier_.IsExact()) {
+ LOG(FATAL, "bad choice for kSensorSendFrequency\n");
+ }
+}
+
+template<class Values>
+void SensorBroadcaster<Values>::RegisterControlLoopRunner(
+ SensorSinkInterface<Values> *crio_control_loop_runner) {
+ if (crio_control_loop_runner_ != NULL) {
+ LOG(FATAL, "trying to register loop runner %p but already have %p\n",
+ crio_control_loop_runner, crio_control_loop_runner_);
+ }
+ crio_control_loop_runner_ = crio_control_loop_runner;
+}
+
+template<class Values>
+void SensorBroadcaster<Values>::Notify() {
+ packer_->PackInto(&data_.values);
+ socket_.Send(&data_, sizeof(data_));
+ ++data_.count;
+
+ if ((data_.count % kSendsPerCycle) == 0) {
+ if (crio_control_loop_runner_ != NULL) {
+ crio_control_loop_runner_->Process(&data_);
+ }
+ }
+}
+
+} // namespace sensors
+} // namespace aos
diff --git a/aos/common/sensors/sensor_broadcaster.h b/aos/common/sensors/sensor_broadcaster.h
new file mode 100644
index 0000000..1e61208
--- /dev/null
+++ b/aos/common/sensors/sensor_broadcaster.h
@@ -0,0 +1,57 @@
+#ifndef AOS_COMMON_SENSORS_SENSOR_BROADCASTER_H_
+#define AOS_COMMON_SENSORS_SENSOR_BROADCASTER_H_
+
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/sensors/sensors.h"
+#include "aos/common/sensors/sensor_packer.h"
+#include "aos/common/sensors/sensor_sink.h"
+#include "aos/common/macros.h"
+
+namespace aos {
+namespace crio {
+
+template<class Values>
+class CRIOControlLoopRunner;
+
+} // namespace crio
+namespace sensors {
+
+// A class that handles sending sensor values to the atom.
+// See sensors.h for an overview of where this fits in.
+template<class Values>
+class SensorBroadcaster {
+ public:
+ // Does not take ownership of packer.
+ SensorBroadcaster(SensorPackerInterface<Values> *packer);
+
+ void Start();
+
+ private:
+ // So that it can access RegisterControlLoopRunner.
+ friend class crio::CRIOControlLoopRunner<Values>;
+
+ // Registers an object to get run on control loop timing. Only designed to be
+ // called by crio::CRIOControlLoopRunner.
+ // Does not take ownership of crio_control_loop_runner.
+ void RegisterControlLoopRunner(
+ SensorSinkInterface<Values> *crio_control_loop_runner);
+
+ static void StaticNotify(SensorBroadcaster<Values> *self) { self->Notify(); }
+ void Notify();
+
+ SensorPackerInterface<Values> *const packer_;
+ crio::WDInterruptNotifier<SensorBroadcaster<Values>> notifier_;
+ SendSocket socket_;
+ SensorData<Values> data_;
+ SensorSinkInterface<Values> *crio_control_loop_runner_;
+
+ DISALLOW_COPY_AND_ASSIGN(SensorBroadcaster<Values>);
+};
+
+} // namespace sensors
+} // namespace aos
+
+#include "sensor_broadcaster-tmpl.h"
+
+#endif // AOS_COMMON_SENSORS_SENSOR_BROADCASTER_H_
diff --git a/aos/common/sensors/sensor_packer.h b/aos/common/sensors/sensor_packer.h
new file mode 100644
index 0000000..cfcf045
--- /dev/null
+++ b/aos/common/sensors/sensor_packer.h
@@ -0,0 +1,22 @@
+#ifndef AOS_COMMON_SENSORS_SENSOR_PACKER_H_
+#define AOS_COMMON_SENSORS_SENSOR_PACKER_H_
+
+namespace aos {
+namespace sensors {
+
+// An interface that handles reading input data and putting it into the sensor
+// values struct.
+// See sensors.h for an overview of where this fits in.
+template<class Values>
+class SensorPackerInterface {
+ public:
+ virtual ~SensorPackerInterface() {}
+
+ // Reads the inputs (from WPILib etc) and writes the data into *values.
+ virtual void PackInto(Values *values) = 0;
+};
+
+} // namespace sensors
+} // namespace aos
+
+#endif // AOS_COMMON_SENSORS_SENSOR_PACKER_H_
diff --git a/aos/common/sensors/sensor_receiver-tmpl.h b/aos/common/sensors/sensor_receiver-tmpl.h
new file mode 100644
index 0000000..9d37b8c
--- /dev/null
+++ b/aos/common/sensors/sensor_receiver-tmpl.h
@@ -0,0 +1,154 @@
+#include "aos/common/Configuration.h"
+#include "aos/common/inttypes.h"
+
+namespace aos {
+namespace sensors {
+
+template<class Values>
+const time::Time SensorReceiver<Values>::kJitterDelay =
+ time::Time::InSeconds(0.0025);
+
+template<class Values>
+SensorReceiver<Values>::SensorReceiver(
+ SensorUnpackerInterface<Values> *unpacker)
+ : unpacker_(unpacker),
+ synchronized_(false) {}
+
+template<class Values>
+void SensorReceiver<Values>::RunIteration() {
+ if (synchronized_) {
+ if (ReceiveData()) {
+ LOG(DEBUG, "receive said to try a reset\n");
+ synchronized_ = false;
+ return;
+ }
+ if (GoodPacket()) {
+ unpacker_->UnpackFrom(&data_.values);
+ }
+ } else {
+ LOG(INFO, "resetting to try receiving data\n");
+ Reset();
+ if (Synchronize()) {
+ LOG(INFO, "synchronized successfully\n");
+ synchronized_ = true;
+ }
+ }
+}
+
+template<class Values>
+bool SensorReceiver<Values>::GoodPacket() {
+ // If it's a multiple of kSensorSendFrequency from start_count_.
+ if (((data_.count - start_count_) % kSendsPerCycle) == 0) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+// Looks for when the timestamps transition from before where we want to after
+// and then picks whichever one was closer. After that, reads kTestCycles and
+// makes sure that at most 1 is bad.
+template<class Values>
+bool SensorReceiver<Values>::Synchronize() {
+ time::Time old_received_time(0, 0);
+ time::Time start_time = time::Time::Now();
+ // When we want to send out the next set of values.
+ time::Time goal_time = (start_time / kLoopFrequency.ToNSec()) *
+ kLoopFrequency.ToNSec() +
+ kLoopFrequency - kJitterDelay;
+ while (true) {
+ if (ReceiveData()) return false;
+ time::Time received_time = time::Time::Now();
+ if (received_time > goal_time) {
+ // If this was the very first one we got, try again.
+ if (old_received_time == time::Time(0, 0)) return false;
+
+ assert(old_received_time < goal_time);
+
+ // If the most recent one is closer than the last one.
+ if ((received_time - goal_time).abs() <
+ (old_received_time - goal_time).abs()) {
+ start_count_ = data_.count;
+ } else {
+ start_count_ = data_.count - 1;
+ }
+
+ int bad_count = 0;
+ for (int i = 0; i < kTestCycles;) {
+ ReceiveData();
+ received_time = time::Time::Now();
+ if (GoodPacket()) {
+ LOG(DEBUG, "checking packet count=%"PRId32
+ " received at %"PRId32"s%"PRId32"ns\n",
+ data_.count, received_time.sec(), received_time.nsec());
+ // If |the difference between the goal time for this numbered packet
+ // and the time we actually got this one| is too big.
+ if (((goal_time +
+ kSensorSendFrequency * (data_.count - start_count_)) -
+ received_time).abs() > kSensorSendFrequency) {
+ LOG(INFO, "rejected time of the last good packet\n");
+ ++bad_count;
+ }
+ ++i;
+ }
+ if (bad_count > 1) {
+ LOG(WARNING, "got multiple packets with bad timestamps\n");
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ old_received_time = received_time;
+ }
+}
+
+template<class Values>
+bool SensorReceiver<Values>::ReceiveData() {
+ int old_count = data_.count;
+ DoReceiveData();
+ if (data_.count < 0) {
+ LOG(FATAL, "data count overflowed. currently %"PRId32"\n", data_.count);
+ }
+ if (data_.count < old_count) {
+ LOG(INFO, "count reset. ws %"PRId32", now %"PRId32"\n",
+ old_count, data_.count);
+ return true;
+ }
+ return false;
+}
+
+template<class Values>
+const time::Time NetworkSensorReceiver<Values>::kWarmupTime =
+ time::Time::InSeconds(0.125);
+
+template<class Values>
+NetworkSensorReceiver<Values>::NetworkSensorReceiver(
+ SensorUnpackerInterface<Values> *unpacker)
+ : SensorReceiver<Values>(unpacker),
+ socket_(NetworkPort::kSensors) {}
+
+template<class Values>
+void NetworkSensorReceiver<Values>::Reset() {
+ LOG(INFO, "beginning warm up\n");
+ time::Time start = time::Time::Now();
+ while ((time::Time::Now() - start) < kWarmupTime) {
+ socket_.Receive(this->data(), sizeof(*this->data()));
+ }
+ LOG(INFO, "done warming up\n");
+}
+
+template<class Values>
+void NetworkSensorReceiver<Values>::DoReceiveData() {
+ while (true) {
+ if (socket_.Receive(this->data(), sizeof(*this->data())) ==
+ sizeof(*this->data())) {
+ return;
+ }
+ LOG(WARNING, "received incorrect amount of data\n");
+ }
+}
+
+} // namespace sensors
+} // namespace aos
diff --git a/aos/common/sensors/sensor_receiver.h b/aos/common/sensors/sensor_receiver.h
new file mode 100644
index 0000000..1fe3003
--- /dev/null
+++ b/aos/common/sensors/sensor_receiver.h
@@ -0,0 +1,98 @@
+#ifndef AOS_COMMON_SENSORS_SENSOR_RECEIVER_H_
+#define AOS_COMMON_SENSORS_SENSOR_RECEIVER_H_
+
+#include "aos/common/sensors/sensor_unpacker.h"
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/sensors/sensors.h"
+#include "aos/common/time.h"
+#include "aos/common/gtest_prod.h"
+
+namespace aos {
+namespace sensors {
+namespace testing {
+
+FORWARD_DECLARE_TEST_CASE(SensorReceiverTest, Simple);
+
+} // namespace testing
+
+// A class that handles receiving sensor values from the cRIO.
+// See sensors.h for an overview of where this fits in.
+//
+// Abstract class to make testing the complex logic for choosing which data to
+// use easier.
+template<class Values>
+class SensorReceiver {
+ public:
+ // Does not take ownership of unpacker.
+ SensorReceiver(SensorUnpackerInterface<Values> *unpacker);
+
+ void RunIteration();
+
+ protected:
+ SensorData<Values> *data() { return &data_; }
+
+ private:
+ // How long before the control loops run to aim for receiving sensor data (to
+ // prevent jitter if some packets arrive up to this much later).
+ static const time::Time kJitterDelay;
+ // How many cycles not to send data out to make sure that we're in phase
+ // (during this time, the code verifies that <= 1 cycle is not within 1
+ // cycle's time of kJitterDelay).
+ static const int kTestCycles = 10;
+
+ FRIEND_TEST_NAMESPACE(SensorReceiverTest, Simple, testing);
+
+ // Subclasses need to implement this to read 1 set of data (blocking until it
+ // is available) into data().
+ virtual void DoReceiveData() = 0;
+
+ // Optional: if subclasses can do anything to reinitialize after there are
+ // problems, they should do it here.
+ // This will be called right before calling DoReceiveData() the first time.
+ virtual void Reset() {}
+
+ // Returns whether the current packet looks like a good one to use.
+ bool GoodPacket();
+
+ // Synchronizes with incoming packets and sets start_count_ to where we
+ // started reading.
+ // Returns whether it succeeded in locking on.
+ bool Synchronize();
+ // Receives a set of values and makes sure that it's sane.
+ // Returns whether to start over again with timing.
+ bool ReceiveData();
+
+ SensorUnpackerInterface<Values> *const unpacker_;
+ SensorData<Values> data_;
+ // The count that we started out (all other sent packets will be multiples of
+ // this).
+ int32_t start_count_;
+ bool synchronized_;
+
+ DISALLOW_COPY_AND_ASSIGN(SensorReceiver<Values>);
+};
+
+// A SensorReceiver that receives data from a SensorBroadcaster.
+template<class Values>
+class NetworkSensorReceiver : public SensorReceiver<Values> {
+ public:
+ NetworkSensorReceiver(SensorUnpackerInterface<Values> *unpacker);
+
+ private:
+ // How long to read data as fast as possible for (to clear out buffers etc).
+ static const time::Time kWarmupTime;
+
+ virtual void DoReceiveData();
+ virtual void Reset();
+
+ ReceiveSocket socket_;
+
+ DISALLOW_COPY_AND_ASSIGN(NetworkSensorReceiver<Values>);
+};
+
+} // namespace sensors
+} // namespace aos
+
+#include "aos/common/sensors/sensor_receiver-tmpl.h"
+
+#endif // AOS_COMMON_SENSORS_SENSOR_RECEIVER_H_
diff --git a/aos/common/sensors/sensor_receiver_test.cc b/aos/common/sensors/sensor_receiver_test.cc
new file mode 100644
index 0000000..5a66432
--- /dev/null
+++ b/aos/common/sensors/sensor_receiver_test.cc
@@ -0,0 +1,84 @@
+#include "aos/common/sensors/sensor_receiver.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/common/sensors/sensors.h"
+#include "aos/common/time.h"
+#include "aos/common/queue_testutils.h"
+
+using ::aos::time::Time;
+
+namespace aos {
+namespace sensors {
+namespace testing {
+
+struct TestValues {
+ int count;
+ int more_data;
+};
+class TestSensorReceiver : public SensorReceiver<TestValues>,
+ public SensorUnpackerInterface<TestValues> {
+ public:
+ TestSensorReceiver()
+ : SensorReceiver<TestValues>(this),
+ resets_(0),
+ unpacks_(0) {
+ data()->count = 0;
+ }
+
+ void Reset() {
+ LOG(DEBUG, "reset for the %dth time\n", ++resets_);
+ }
+ void DoReceiveData() {
+ last_received_count_ = ++data()->count;
+ data()->values.count = last_received_count_;
+ Time::IncrementMockTime(kSensorSendFrequency);
+ }
+
+ int resets() { return resets_; }
+ int unpacks() { return unpacks_; }
+ using SensorReceiver<TestValues>::data;
+
+ void UnpackFrom(TestValues *data) {
+ // Make sure that it didn't lost one that we gave it.
+ EXPECT_EQ(last_received_count_, data->count);
+ ++unpacks_;
+ }
+
+ private:
+ int resets_;
+ int unpacks_;
+ int last_received_count_;
+};
+
+class SensorReceiverTest : public ::testing::Test {
+ protected:
+ SensorReceiverTest() {
+ ::aos::common::testing::EnableTestLogging();
+ Time::EnableMockTime(Time(971, 254));
+ }
+
+ TestSensorReceiver &receiver() { return receiver_; }
+
+ private:
+ TestSensorReceiver receiver_;
+};
+
+TEST_F(SensorReceiverTest, Simple) {
+ static const int kIterations = 53;
+ for (int i = 0; i < kIterations; ++i) {
+ receiver().RunIteration();
+ }
+ EXPECT_EQ(1, receiver().resets());
+ // expected value is kIterations/kSendsPerCycle (rounded up) (the number of
+ // times that it should get a good one) - 1 (to compensate for the iteration
+ // when it synced itself up)
+ EXPECT_EQ((kIterations + kSendsPerCycle - 1) / kSendsPerCycle - 1,
+ receiver().unpacks());
+}
+
+// TODO(brians) finish writing tests
+
+} // namespace testing
+} // namespace sensors
+} // namespace aos
diff --git a/aos/common/sensors/sensor_sink.h b/aos/common/sensors/sensor_sink.h
new file mode 100644
index 0000000..81bc491
--- /dev/null
+++ b/aos/common/sensors/sensor_sink.h
@@ -0,0 +1,21 @@
+#ifndef AOS_COMMON_SENSORS_SENSOR_SINK_H_
+#define AOS_COMMON_SENSORS_SENSOR_SINK_H_
+
+#include "aos/common/sensors/sensors.h"
+
+namespace aos {
+namespace sensors {
+
+// Generic class for something that can do something with sensor data.
+template<class Values>
+class SensorSinkInterface {
+ public:
+ virtual ~SensorSinkInterface() {}
+
+ void Process(SensorData<Values> *data);
+};
+
+} // namespace sensors
+} // namespace aos
+
+#endif // AOS_COMMON_SENSORS_SENSOR_SINK_H_
diff --git a/aos/common/sensors/sensor_unpacker.h b/aos/common/sensors/sensor_unpacker.h
new file mode 100644
index 0000000..b19e0c6
--- /dev/null
+++ b/aos/common/sensors/sensor_unpacker.h
@@ -0,0 +1,22 @@
+#ifndef AOS_COMMON_SENSORS_SENSOR_UNPACKER_H_
+#define AOS_COMMON_SENSORS_SENSOR_UNPACKER_H_
+
+namespace aos {
+namespace sensors {
+
+// An interface that handles taking data from the sensor Values struct and
+// putting it into queues (for control loops etc).
+// See sensors.h for an overview of where this fits in.
+template<class Values>
+class SensorUnpackerInterface {
+ public:
+ virtual ~SensorUnpackerInterface() {}
+
+ // Takes the data in *values and writes it out into queues etc.
+ virtual void UnpackFrom(Values *values) = 0;
+};
+
+} // namespace sensors
+} // namespace aos
+
+#endif // AOS_COMMON_SENSORS_SENSOR_UNPACKER_H_
diff --git a/aos/common/sensors/sensors.cc b/aos/common/sensors/sensors.cc
new file mode 100644
index 0000000..8b774d2
--- /dev/null
+++ b/aos/common/sensors/sensors.cc
@@ -0,0 +1,10 @@
+#include "aos/common/sensors/sensors.h"
+
+namespace aos {
+namespace sensors {
+
+const time::Time kSensorSendFrequency =
+ ::aos::control_loops::kLoopFrequency / kSendsPerCycle;
+
+} // namespace sensors
+} // namespace aos
diff --git a/aos/common/sensors/sensors.gyp b/aos/common/sensors/sensors.gyp
new file mode 100644
index 0000000..2c49369
--- /dev/null
+++ b/aos/common/sensors/sensors.gyp
@@ -0,0 +1,87 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'sensor_sink',
+ 'type': 'static_library',
+ 'sources': [
+ ],
+ 'dependencies': [
+ 'sensors',
+ ],
+ 'export_dependent_settings': [
+ 'sensors',
+ ],
+ },
+ {
+ 'target_name': 'sensors',
+ 'type': 'static_library',
+ 'sources': [
+ 'sensors.cc'
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ },
+ {
+ 'target_name': 'sensor_receiver',
+ 'type': 'static_library',
+ 'sources': [
+ #'sensor_receiver-tmpl.h'
+ ],
+ 'dependencies': [
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/common/common.gyp:common',
+ 'sensors',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:gtest_prod',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/common/common.gyp:common',
+ 'sensors',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:gtest_prod',
+ ],
+ },
+ {
+ 'target_name': 'sensor_receiver_test',
+ 'type': 'executable',
+ 'sources': [
+ 'sensor_receiver_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'sensor_receiver',
+ '<(AOS)/common/common.gyp:time',
+ 'sensors',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ ],
+ },
+ {
+ 'target_name': 'sensor_broadcaster',
+ 'type': 'static_library',
+ 'sources': [
+ #'sensor_broadcaster-tmpl.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/common/common.gyp:common',
+ 'sensors',
+ 'sensor_sink',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/common/common.gyp:common',
+ 'sensors',
+ 'sensor_sink',
+ ],
+ },
+ ],
+}
diff --git a/aos/common/sensors/sensors.h b/aos/common/sensors/sensors.h
new file mode 100644
index 0000000..06f5253
--- /dev/null
+++ b/aos/common/sensors/sensors.h
@@ -0,0 +1,48 @@
+#ifndef AOS_COMMON_SENSORS_SENSORS_H_
+#define AOS_COMMON_SENSORS_SENSORS_H_
+
+#include "aos/common/time.h"
+
+#include "aos/common/control_loop/ControlLoop.h"
+
+namespace aos {
+// This namespace contains all of the stuff for dealing with reading sensors and
+// communicating it to everything that needs it. There are 4 main classes whose
+// instances actually process the data. They must all be registered in the
+// appropriate ::aos::crio::ControlsManager hooks.
+//
+// SensorPackers get run on the cRIO to read inputs (from WPILib or elsewhere)
+// and put the values into the Values struct (which is templated for all of the
+// classes that use it).
+// SensorUnpackers get run on both the atom and the cRIO to take the data from
+// the Values struct and put them into queues for control loops etc.
+// SensorBroadcasters (on the cRIO) send the data to a SensorReceiver (on the
+// atom) to pass to its SensorUnpacker there.
+// CRIOControlLoopRunners register with a SensorBroadcaster to get called right
+// after reading the sensor data so that they can immediately pass it so a
+// SensorUnpacker and then run their control loops.
+// The actual SensorPacker and SensorUnpacker classes have the Interface suffix
+// on them.
+namespace sensors {
+
+// How many times per ::aos::control_loops::kLoopFrequency sensor
+// values get sent out by the cRIO.
+// This must evenly divide that frequency into multiples of sysClockRateGet().
+const int kSendsPerCycle = 10;
+// ::aos::control_loops::kLoopFrequency / kSendsPerCycle for
+// convenience.
+extern const time::Time kSensorSendFrequency;
+using ::aos::control_loops::kLoopFrequency;
+
+// This is the struct that actually gets sent over the UDP socket.
+template<class Values>
+struct SensorData {
+ Values values;
+ // Starts at 0 and goes up.
+ int32_t count;
+} __attribute__((packed));
+
+} // namespace sensors
+} // namespace aos
+
+#endif // AOS_COMMON_SENSORS_SENSORS_H_
diff --git a/aos/common/time.h b/aos/common/time.h
index 38238f2..c081e09 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -114,6 +114,10 @@
int ToTicks() const {
return ToNSec() / static_cast<int64_t>(kNSecInSec / sysClkRateGet());
}
+ // Constructs a Time representing ticks.
+ static Time InTicks(int ticks) {
+ return Time::InSeconds(ticks * sysClkRateGet());
+ }
#endif
// Returns the time represented in milliseconds.
@@ -160,12 +164,22 @@
Check();
}
+ // Absolute value.
+ Time abs() const {
+ if (*this > Time(0, 0)) return *this;
+ return Time(-sec_ - 1, kNSecInSec - nsec_);
+ }
+
// Enables returning the mock time value for Now instead of checking the
// system clock. This should only be used when testing things depending on
// time, or many things may/will break.
static void EnableMockTime(const Time now);
// Sets now when time is being mocked.
static void SetMockTime(const Time now);
+ // Convenience function to just increment the mock time by a certain amount.
+ static void IncrementMockTime(const Time amount) {
+ SetMockTime(Now() + amount);
+ }
// Disables mocking time.
static void DisableMockTime();
diff --git a/aos/common/time_test.cc b/aos/common/time_test.cc
index e72e5bd..678c013 100644
--- a/aos/common/time_test.cc
+++ b/aos/common/time_test.cc
@@ -141,6 +141,14 @@
EXPECT_EQ(254971, t.ToMSec());
}
+TEST(TimeTest, Abs) {
+ EXPECT_EQ(MACRO_DARG(Time(971, 1114)), MACRO_DARG(Time(971, 1114).abs()));
+ EXPECT_EQ(MACRO_DARG(Time(253, Time::kNSecInSec * 0.3)),
+ MACRO_DARG(Time(-254, Time::kNSecInSec * 0.7).abs()));
+ EXPECT_EQ(MACRO_DARG(-Time(-971, 973).ToNSec()),
+ MACRO_DARG(Time(970, Time::kNSecInSec - 973).ToNSec()));
+}
+
} // namespace testing
} // namespace time
} // namespace aos
diff --git a/aos/crio/controls/ControlsManager.cpp b/aos/crio/controls/ControlsManager.cpp
index c1f11cd..906394f 100644
--- a/aos/crio/controls/ControlsManager.cpp
+++ b/aos/crio/controls/ControlsManager.cpp
@@ -1,13 +1,13 @@
+#include "aos/crio/controls/ControlsManager.h"
+
#include <stdio.h>
#include <stdlib.h>
#include "WPILib/Compressor.h"
#include "aos/crio/logging/crio_logging.h"
-#include "aos/crio/controls/ControlsManager.h"
#include "aos/common/Configuration.h"
#include "aos/crio/aos_ctdt.h"
-#include "aos/crio/motor_server/CRIOControlLoopRunner.h"
#include "aos/crio/motor_server/MotorServer.h"
namespace aos {
@@ -28,20 +28,19 @@
GetWatchdog().SetEnabled(false);
LOG(INFO, "disabled watchdog\n");
- RegisterControlLoops();
- LOG(INFO, "registered control loops\n");
-
- // CRIOControlLoopRunner calls part of MotorServer, so MotorServer has to get
- // initialized first.
MotorServer::Start();
LOG(INFO, "MotorServer started\n");
- CRIOControlLoopRunner::Start();
- LOG(INFO, "cRIO control loops started\n");
LOG(INFO, "calling init functions\n");
aos_call_init_functions();
LOG(INFO, "initialized\n");
+ RegisterControlLoops();
+ LOG(INFO, "registered control loops\n");
+
+ StartSensorBroadcasters();
+ LOG(INFO, "started sensor broadcasters\n");
+
// Wait forever so that this task doesn't end to avoid confusing any brittle
// FIRST code that might be hiding somewhere.
while (true) {
@@ -50,4 +49,4 @@
}
} // namespace crio
-} // namespace aos
+} // namespace aos
diff --git a/aos/crio/controls/ControlsManager.h b/aos/crio/controls/ControlsManager.h
index 986fe02..aa78587 100644
--- a/aos/crio/controls/ControlsManager.h
+++ b/aos/crio/controls/ControlsManager.h
@@ -4,17 +4,24 @@
namespace aos {
namespace crio {
+// Designed for a subclass (that implements all of the pure virtual methods...)
+// to be passed to START_ROBOT_CLASS (a WPILib macro) to start all of the code.
class ControlsManager : public RobotBase {
public:
- // Gets called when it is time to register all the control loops.
- virtual void RegisterControlLoops() = 0;
virtual void StartCompetition();
- static inline ControlsManager &GetInstance() {
+
+ static ControlsManager &GetInstance() {
return *static_cast<ControlsManager *>(&RobotBase::getInstance());
}
- inline DriverStation *GetDS() {
+ DriverStation *GetDS() {
return m_ds;
}
+
+ private:
+ // Hooks that subclasses have to implement to do the correct things at the
+ // correct times.
+ virtual void RegisterControlLoops() = 0;
+ virtual void StartSensorBroadcasters() = 0;
};
} // namespace crio
diff --git a/aos/crio/controls/controls.gyp b/aos/crio/controls/controls.gyp
new file mode 100644
index 0000000..ad253d0
--- /dev/null
+++ b/aos/crio/controls/controls.gyp
@@ -0,0 +1,22 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'ControlsManager',
+ 'type': 'static_library',
+ 'sources': [
+ 'ControlsManager.cpp',
+ 'JoyStickRead.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:common',
+ '<(AOS)/crio/motor_server/motor_server.gyp:MotorServer',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ ],
+ },
+ ],
+}
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.cpp b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
deleted file mode 100644
index 1971dc1..0000000
--- a/aos/crio/motor_server/CRIOControlLoopRunner.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#include "CRIOControlLoopRunner.h"
-
-#include "aos/aos_core.h"
-#include "aos/crio/shared_libs/interrupt_bridge.h"
-#include "aos/crio/motor_server/MotorOutput.h"
-#include "aos/crio/motor_server/MotorServer.h"
-
-using ::aos::control_loops::SerializableControlLoop;
-
-namespace aos {
-namespace crio {
-
-bool CRIOControlLoopRunner::started_ = false;
-std::vector<SerializableControlLoop *> CRIOControlLoopRunner::loops_;
-Mutex CRIOControlLoopRunner::loops_lock;
-
-void CRIOControlLoopRunner::Start() {
- if (started_) {
- LOG(WARNING, "not going to Start twice!!\n");
- return;
- }
- started_ = true;
-
- // TODO(aschuh): Hold on to a handle to this...
- (new WDInterruptNotifier<void>(Notify))->StartPeriodic(0.01);
-}
-
-void CRIOControlLoopRunner::AddControlLoop(SerializableControlLoop *loop) {
- MutexLocker control_loop_goals_locker(&loops_lock);
- loops_.push_back(loop);
- MotorServer::RegisterControlLoopGoal(loop);
-}
-
-void CRIOControlLoopRunner::Notify(void *) {
- // TODO(aschuh): Too many singletons/static classes!
- SensorOutputs::UpdateAll();
- // sensors get read first so it doesn't really matter if this takes a little bit
- {
- MutexLocker control_loop_goals_locker(
- &MotorServer::control_loop_goals_lock);
- for (auto it = loops_.begin(); it != loops_.end(); ++it) {
- (*it)->Iterate();
- }
- }
- MotorOutput::RunIterationAll();
- MotorServer::WriteOutputs();
-}
-
-} // namespace crio
-} // namespace aos
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.h b/aos/crio/motor_server/CRIOControlLoopRunner.h
deleted file mode 100644
index efed120..0000000
--- a/aos/crio/motor_server/CRIOControlLoopRunner.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
-#define AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
-
-#include <vector>
-#include <semLib.h>
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "aos/common/mutex.h"
-
-namespace aos {
-namespace crio {
-
-// Runs crio-side control loops. Completely static because there is no reason
-// for multiple ones and it gets rid of the problem of passing an instance
-// around.
-class CRIOControlLoopRunner {
- public:
- // Spawns a new Task that loops forever.
- // No other functions should be called before this one returns.
- static void Start();
-
- // Adds a control loop to run.
- // This class takes control of the instance.
- static void AddControlLoop(control_loops::SerializableControlLoop *loop);
-
- private:
- static bool started_;
-
- static std::vector<control_loops::SerializableControlLoop *> loops_;
- static Mutex loops_lock;
-
- // Gets called by a WDInterruptNotifier on 0.01 second intervals.
- static void Notify(void *);
-};
-
-
-} // namespace crio
-} // namespace aos
-
-#endif
diff --git a/aos/crio/motor_server/MotorControllerOutput.cpp b/aos/crio/motor_server/MotorControllerOutput.cc
similarity index 98%
rename from aos/crio/motor_server/MotorControllerOutput.cpp
rename to aos/crio/motor_server/MotorControllerOutput.cc
index a0897b4..7b24892 100644
--- a/aos/crio/motor_server/MotorControllerOutput.cpp
+++ b/aos/crio/motor_server/MotorControllerOutput.cc
@@ -1,6 +1,5 @@
#include "aos/crio/motor_server/MotorControllerOutput.h"
-#include "aos/aos_core.h"
#include "aos/common/byteorder.h"
#include "aos/common/commonmath.h"
diff --git a/aos/crio/motor_server/MotorOutput.cpp b/aos/crio/motor_server/MotorOutput.cc
similarity index 100%
rename from aos/crio/motor_server/MotorOutput.cpp
rename to aos/crio/motor_server/MotorOutput.cc
diff --git a/aos/crio/motor_server/MotorServer.cpp b/aos/crio/motor_server/MotorServer.cc
similarity index 99%
rename from aos/crio/motor_server/MotorServer.cpp
rename to aos/crio/motor_server/MotorServer.cc
index 77a0579..ebb69fd 100644
--- a/aos/crio/motor_server/MotorServer.cpp
+++ b/aos/crio/motor_server/MotorServer.cc
@@ -3,12 +3,11 @@
#include <stdio.h>
#include <stdlib.h>
#include "usrLib.h"
-#include "aos/common/inttypes.h"
#include "WPILib/Timer.h"
#include "WPILib/Task.h"
-#include "aos/aos_core.h"
+#include "aos/common/inttypes.h"
#include "aos/crio/motor_server/MotorControllerOutput.h"
#include "aos/crio/motor_server/SolenoidOutput.h"
#include "aos/common/Configuration.h"
diff --git a/aos/crio/motor_server/MotorServer.h b/aos/crio/motor_server/MotorServer.h
index 0126663..2f7fbde 100644
--- a/aos/crio/motor_server/MotorServer.h
+++ b/aos/crio/motor_server/MotorServer.h
@@ -27,13 +27,13 @@
#include "aos/common/network/SendSocket.h"
#include "aos/crio/motor_server/ControlLoopGoals.h"
#include "aos/crio/motor_server/OutputDevice.h"
-#include "aos/crio/motor_server/SensorSender.h"
#include "aos/crio/shared_libs/ByteBuffer.h"
#include "aos/map_utils.h"
namespace aos {
namespace crio {
+template<class Values>
class CRIOControlLoopRunner;
class MotorServer {
public:
@@ -46,7 +46,9 @@
static const int32_t WORK_PRIORITY = 100;
private:
+ template<class Values>
friend class CRIOControlLoopRunner;
+
// Counter for how many times new values come in. Used to stop all the
// outputs if values stop.
// Would take days to overflow.
diff --git a/aos/crio/motor_server/OutputDevice.h b/aos/crio/motor_server/OutputDevice.h
index 0789a68..7cd4983 100644
--- a/aos/crio/motor_server/OutputDevice.h
+++ b/aos/crio/motor_server/OutputDevice.h
@@ -1,5 +1,5 @@
-#ifndef __CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
-#define __CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
+#ifndef AOS_CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
+#define AOS_CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
#include <stdint.h>
#include "aos/crio/shared_libs/ByteBuffer.h"
@@ -20,7 +20,6 @@
virtual void NoValue() = 0;
};
-} // namespace aos
+} // namespace aos
-#endif
-
+#endif // AOS_CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
diff --git a/aos/crio/motor_server/SensorOutput-tmpl.h b/aos/crio/motor_server/SensorOutput-tmpl.h
deleted file mode 100644
index d6b8b69..0000000
--- a/aos/crio/motor_server/SensorOutput-tmpl.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#include "aos/common/input/SensorInput.h"
-
-namespace aos {
-
-template<class Values> std::vector<SensorOutput<Values> *> SensorOutput<Values>::output_running_;
-template<class Values> void SensorOutput<Values>::Run() {
- semTake(lock_, WAIT_FOREVER);
- output_running_.push_back(this);
- outputs_running_.push_back(this);
- semGive(lock_);
-}
-
-template<class Values> void SensorOutput<Values>::RunIterationAll(Values &vals) {
- semTake(lock_, WAIT_FOREVER);
- for (auto it = output_running_.begin(); it != output_running_.end(); ++it) {
- (*it)->RunIteration(vals);
- }
- semGive(lock_);
-}
-template<class Values> void SensorOutput<Values>::Update() {
- Values vals;
- RunIteration(vals);
- SensorInput<Values>::RunIterationAll(vals);
-}
-
-} // namespace aos
-
diff --git a/aos/crio/motor_server/SensorOutput.cpp b/aos/crio/motor_server/SensorOutput.cpp
deleted file mode 100644
index b887885..0000000
--- a/aos/crio/motor_server/SensorOutput.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "aos/crio/motor_server/SensorOutput.h"
-
-namespace aos {
-
-SEM_ID SensorOutputs::lock_ = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
-std::vector<SensorOutputs *> SensorOutputs::outputs_running_;
-
-void SensorOutputs::UpdateAll() {
- semTake(lock_, WAIT_FOREVER);
- for (auto it = outputs_running_.begin(); it != outputs_running_.end(); ++it) {
- (*it)->Update();
- }
- semGive(lock_);
-}
-
-} // namespace aos
-
diff --git a/aos/crio/motor_server/SensorOutput.h b/aos/crio/motor_server/SensorOutput.h
deleted file mode 100644
index 9e30cb9..0000000
--- a/aos/crio/motor_server/SensorOutput.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#ifndef AOS_CRIO_MOTOR_SERVER_SENSOR_OUTPUT_H_
-#define AOS_CRIO_MOTOR_SERVER_SENSOR_OUTPUT_H_
-
-#include <semLib.h>
-#include <vector>
-
-namespace aos {
-
-// Keeps track of instances of all instantiations.
-class SensorOutputs {
- public:
- // Calls RunIteration on all instances and then runs all SensorInput
- // subclasses for that type.
- static void UpdateAll();
- private:
- static SEM_ID lock_;
- static std::vector<SensorOutputs *> outputs_running_;
- protected:
- // Calls RunIteration with a temporary Values instance and then runs all
- // SensorInput subclasses with the same Values type.
- virtual void Update() = 0;
-};
-
-// Class for implementing crio code that reads sensor values and puts them into
-// the sensor struct.
-template<class Values> class SensorOutput : public SensorOutputs {
- protected:
- // Fills out vals with the current data.
- // May not be called at anything close to consistent intervals and may be
- // called simultaneously with different arguments, so it must be reentrant.
- virtual void RunIteration(Values &vals) = 0;
- public:
- // Sets it up so that RunIteration will get called when appropriate.
- void Run();
-
- // Calls RunIteration on all instances with vals.
- static void RunIterationAll(Values &vals);
- private:
- static std::vector<SensorOutput<Values> *> output_running_;
- virtual void Update();
-};
-
-} // namespace aos
-
-#include "SensorOutput-tmpl.h"
-
-#endif
-
diff --git a/aos/crio/motor_server/SensorSender-tmpl.h b/aos/crio/motor_server/SensorSender-tmpl.h
deleted file mode 100644
index 93fe73d..0000000
--- a/aos/crio/motor_server/SensorSender-tmpl.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "WPILib/Task.h"
-#include "WPILib/Timer.h"
-#include "aos/crio/motor_server/SensorOutput.h"
-#include "aos/common/network/SendSocket.h"
-#include "aos/common/Configuration.h"
-
-namespace aos {
-
-template<class Values> void SensorSender<Values>::Run() {
- SendSocket sock(NetworkPort::kSensors,
- configuration::GetIPAddress(configuration::NetworkDevice::kAtom));
- Values vals;
- while (true) {
- Wait(0.0015);
- SensorOutput<Values>::RunIterationAll(vals);
- sock.Send(&vals, sizeof(vals));
- }
-}
-
-} // namespace aos
-
diff --git a/aos/crio/motor_server/SensorSender.h b/aos/crio/motor_server/SensorSender.h
deleted file mode 100644
index 135e1fa..0000000
--- a/aos/crio/motor_server/SensorSender.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef __CRIO_SENSOR_SENDER_H_
-#define __CRIO_SENSOR_SENDER_H_
-
-namespace aos {
-
-// A class that handles sending all of the sensor values to the atom.
-// Designed for an instantiation (aos::SensorSender<X>) to be AOS_RUN_FORKed,
-// NOT a subclass.
-// Values is the type of the struct that will get sent out over the network.
-// Note: it should the same as the instance of TODO(brians) on the atom and any
-// SensorOutput instances that you want to feed into an instance of this.
-template<class Values> class SensorSender {
- public:
- // Loops forever.
- void Run();
-};
-
-} // namespace aos
-
-#include "SensorSender-tmpl.h"
-
-#endif
-
diff --git a/aos/crio/motor_server/crio_control_loop_runner-tmpl.h b/aos/crio/motor_server/crio_control_loop_runner-tmpl.h
new file mode 100644
index 0000000..4bb448f
--- /dev/null
+++ b/aos/crio/motor_server/crio_control_loop_runner-tmpl.h
@@ -0,0 +1,35 @@
+#include "aos/common/logging/logging.h"
+
+#include "aos/crio/motor_server/MotorOutput.h"
+#include "aos/crio/motor_server/MotorServer.h"
+
+namespace aos {
+namespace crio {
+
+template<class Values>
+CRIOControlLoopRunner<Values>::CRIOControlLoopRunner(
+ sensors::SensorBroadcaster<Values> *broadcaster,
+ sensors::SensorUnpackerInterface<Values> *unpacker)
+ : broadcaster_(broadcaster),
+ unpacker_(unpacker) {}
+
+template<class Values>
+void CRIOControlLoopRunner<Values>::AddControlLoop(
+ ::aos::control_loops::SerializableControlLoop *loop) {
+ loops_.push_back(loop);
+ MotorServer::RegisterControlLoopGoal(loop);
+}
+
+template<class Values>
+void CRIOControlLoopRunner<Values>::Process(sensors::SensorData<Values> *data) {
+ unpacker_->UnpackFrom(&data.values);
+ for (auto it = loops_.begin(); it != loops_.end(); ++it) {
+ (*it)->Iterate();
+ }
+ // TODO(brians): make these nice objects too
+ MotorOutput::RunIterationAll();
+ MotorServer::WriteOutputs();
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/motor_server/crio_control_loop_runner.h b/aos/crio/motor_server/crio_control_loop_runner.h
new file mode 100644
index 0000000..cea0852
--- /dev/null
+++ b/aos/crio/motor_server/crio_control_loop_runner.h
@@ -0,0 +1,47 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
+#define AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
+
+#include <vector>
+#include <semLib.h>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/mutex.h"
+#include "aos/common/sensors/sensor_sink.h"
+#include "aos/common/sensors/sensor_broadcaster.h"
+#include "aos/common/sensors/sensor_unpacker.h"
+#include "aos/common/macros.h"
+
+namespace aos {
+namespace crio {
+
+// Instances can run 0-N control loops on the cRIO.
+// See aos/common/sensors/sensors.h for an overview of where this fits in.
+template<class Values>
+class CRIOControlLoopRunner : public sensors::SensorSinkInterface<Values> {
+ public:
+ // Does not take ownership of broadcaster or unpacker.
+ // *broadcaster must not be started yet.
+ CRIOControlLoopRunner(sensors::SensorBroadcaster<Values> *broadcaster,
+ sensors::SensorUnpackerInterface<Values> *unpacker);
+
+ // Adds a control loop to run.
+ // Must not be called after the broadcaster is started.
+ void AddControlLoop(control_loops::SerializableControlLoop *loop);
+
+ void Process(sensors::SensorData<Values> *data);
+
+ private:
+ std::vector<control_loops::SerializableControlLoop *> loops_;
+ sensors::SensorBroadcaster<Values> *const broadcaster_;
+ sensors::SensorUnpackerInterface<Values> *const unpacker_;
+
+ DISALLOW_COPY_AND_ASSIGN(CRIOControlLoopRunner<Values>);
+};
+
+
+} // namespace crio
+} // namespace aos
+
+#include "aos/crio/motor_server/crio_control_loop_runner-tmpl.h"
+
+#endif // AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
diff --git a/aos/crio/motor_server/motor_server.gyp b/aos/crio/motor_server/motor_server.gyp
new file mode 100644
index 0000000..09de2f3
--- /dev/null
+++ b/aos/crio/motor_server/motor_server.gyp
@@ -0,0 +1,80 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'crio_control_loop_runner',
+ 'type': 'static_library',
+ 'sources': [
+ #'ControlLoopGoals.h'
+ #'crio_control_loop_runner-tmpl.h',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_broadcaster',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_sink',
+ 'output',
+ 'MotorServer',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_broadcaster',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_sink',
+ 'output',
+ 'MotorServer',
+ ],
+ },
+ {
+ 'target_name': 'output',
+ 'type': 'static_library',
+ 'sources': [
+ # 'OutputDevice.h',
+ # 'SolenoidOutput.h'
+ 'MotorControllerOutput.cc',
+ 'MotorOutput.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/common/common.gyp:common',
+ '<(AOS)/common/input/input.gyp:sensor_input',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/common/common.gyp:common',
+ '<(AOS)/common/input/input.gyp:sensor_input',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ ],
+ },
+ {
+ 'target_name': 'MotorServer',
+ 'type': 'static_library',
+ 'sources': [
+ 'MotorServer.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/messages/messages.gyp:QueueHolder',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:ByteBuffer',
+ 'output',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/common.gyp:controls',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/messages/messages.gyp:QueueHolder',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:ByteBuffer',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ },
+ ],
+}
diff --git a/aos/crio/shared_libs/ByteBuffer.h b/aos/crio/shared_libs/ByteBuffer.h
index b5c4902..9a926a7 100644
--- a/aos/crio/shared_libs/ByteBuffer.h
+++ b/aos/crio/shared_libs/ByteBuffer.h
@@ -1,8 +1,9 @@
-#ifndef __CRIO_SHARED_LIBS_BYTE_BUFFER_H_
-#define __CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+#ifndef AOS_CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+#define AOS_CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+
+#include <algorithm>
#include "aos/common/network/ReceiveSocket.h"
-#include <algorithm>
namespace aos {
@@ -13,7 +14,7 @@
int m_i;
char *m_buffer;
bool recv_from_sock(ReceiveSocket *sock) {
- m_length = sock->Recv(m_buffer, m_size, 40000);
+ m_length = sock->Receive(m_buffer, m_size, 40000);
if (m_length < 0) {
m_length = 0;
}
@@ -85,7 +86,6 @@
}
};
-} // namespace aos
+} // namespace aos
-#endif
-
+#endif // AOS_CRIO_SHARED_LIBS_BYTE_BUFFER_H_
diff --git a/aos/crio/shared_libs/interrupt_bridge-tmpl.h b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
index fbd3e98..ef53dce 100644
--- a/aos/crio/shared_libs/interrupt_bridge-tmpl.h
+++ b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
@@ -6,7 +6,6 @@
#include "aos/common/logging/logging.h"
#include "aos/crio/motor_server/MotorServer.h"
-#include "aos/common/time.h"
extern "C" {
// A really simple function implemented in a .c file because the header that
@@ -80,12 +79,14 @@
PeriodicNotifier<T>::PeriodicNotifier(
typename InterruptBridge<T>::Handler handler,
T *param, int priority)
- : InterruptBridge<T>(handler, param, priority) {}
+ : InterruptBridge<T>(handler, param, priority),
+ period_(-1, -1) {}
template<typename T>
-void PeriodicNotifier<T>::StartPeriodic(double period) {
+void PeriodicNotifier<T>::StartPeriodic(time::Time period) {
+ period_ = period;
this->StartTask();
- StartNotifications(period);
+ StartNotifications();
}
template<typename T>
@@ -128,11 +129,11 @@
}
template<typename T>
-void TimerNotifier<T>::StartNotifications(double period) {
+void TimerNotifier<T>::StartNotifications() {
itimerspec timer_spec;
timer_spec.it_value.tv_sec = 0;
timer_spec.it_value.tv_nsec = 1; // 0 would mean to disarm the timer
- timer_spec.it_interval = time::Time::InSeconds(period).ToTimespec();
+ timer_spec.it_interval = this->period().ToTimespec();
if (timer_settime(timer_, 0, &timer_spec, NULL) != OK) {
LOG(FATAL, "timer_settime(%p, 0, %p, NULL) failed with %d: %s\n",
timer_, &timer_spec, errno, strerror(errno));
@@ -163,8 +164,8 @@
}
template<typename T>
-void WDInterruptNotifier<T>::StartNotifications(double period) {
- delay_ = time::Time::InSeconds(period).ToTicks();
+void WDInterruptNotifier<T>::StartNotifications() {
+ delay_ = this->period().ToTicks();
if (wdStart(wd_,
1, // run it really soon
diff --git a/aos/crio/shared_libs/interrupt_bridge.h b/aos/crio/shared_libs/interrupt_bridge.h
index f8f68b4..aedaaea 100644
--- a/aos/crio/shared_libs/interrupt_bridge.h
+++ b/aos/crio/shared_libs/interrupt_bridge.h
@@ -8,6 +8,7 @@
#include "aos/common/scoped_ptr.h"
+#include "aos/common/time.h"
#include "aos/common/macros.h"
class Task;
@@ -61,8 +62,23 @@
template<typename T>
class PeriodicNotifier : public InterruptBridge<T> {
public:
- // Period is how much (in seconds) to wait between running the handler.
- void StartPeriodic(double period);
+ // Period is how much to wait each time between running the handler.
+ void StartPeriodic(time::Time period);
+ // DEPRECATED(brians): use the overload that takes a time::Time
+ void StartPeriodic(double seconds) {
+ StartPeriodic(time::Time::InSeconds(seconds));
+ }
+ // After StartPeriodic is called, returns whether or not the subclass can
+ // actually call the callback exactly on those intervals or whether it will
+ // call it on some rounded amount.
+ //
+ // Default implementation assumes that the subclass has sysClockRateGet()
+ // resolution. Override if this is not true.
+ virtual bool IsExact() {
+ return period_ == time::Time::InTicks(period_.ToTicks());
+ }
+
+ time::Time period() { return period_; }
protected:
PeriodicNotifier(typename InterruptBridge<T>::Handler handler, T *param,
@@ -72,8 +88,10 @@
private:
virtual void StopNotifications() = 0;
// Subclasses should do whatever they have to to start calling Notify() every
- // period seconds.
- virtual void StartNotifications(double period) = 0;
+ // period_.
+ virtual void StartNotifications() = 0;
+
+ time::Time period_;
};
// This one works accurately, but it has the potential to drift over time.
@@ -91,7 +109,7 @@
// an instance. This function calls Notify() on that instance.
static void StaticNotify(void *self_in);
virtual void StopNotifications();
- virtual void StartNotifications(double period);
+ virtual void StartNotifications();
WDOG_ID wd_;
int delay_; // what to pass to wdStart
@@ -122,7 +140,7 @@
// and calls Notify() on that instance.
static void StaticNotify(int signum);
virtual void StopNotifications();
- virtual void StartNotifications(double period);
+ virtual void StartNotifications();
timer_t timer_;
// Which signal timer_ will notify on.
diff --git a/aos/crio/shared_libs/mutex.cpp b/aos/crio/shared_libs/mutex.cpp
index d921492..f7728bd 100644
--- a/aos/crio/shared_libs/mutex.cpp
+++ b/aos/crio/shared_libs/mutex.cpp
@@ -5,6 +5,8 @@
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging.h"
+
namespace aos {
Mutex::Mutex() : impl_(semBCreate(SEM_Q_PRIORITY, SEM_FULL)) {
diff --git a/aos/crio/shared_libs/shared_libs.gyp b/aos/crio/shared_libs/shared_libs.gyp
index 90b3cf0..59fe572 100644
--- a/aos/crio/shared_libs/shared_libs.gyp
+++ b/aos/crio/shared_libs/shared_libs.gyp
@@ -1,13 +1,41 @@
{
'targets': [
{
-# This one includes interrupt_bridge.h too.
+ 'target_name': 'limit_encoder_reader',
+ 'type': 'static_library',
+ 'sources': [
+ 'limit_encoder_reader.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ 'interrupt_notifier',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ 'interrupt_notifier',
+ ],
+ },
+ {
+ 'target_name': 'ByteBuffer',
+ 'type': 'static_library',
+ 'sources': [
+ # 'ByteBuffer.h',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ },
+ {
'target_name': 'interrupt_notifier',
'type': 'static_library',
'sources': [
'interrupt_bridge.cc',
'interrupt_bridge_c.c',
'interrupt_bridge_demo.cc',
+ # 'interrupt_notifier-tmpl.h',
],
'dependencies': [
'<(AOS)/common/common.gyp:time',
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index e9c787e..7c430c2 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -16,7 +16,7 @@
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../control_loops/shooter/shooter.gyp:shooter',
'../input/input.gyp:JoystickReader',
- '../input/input.gyp:SensorReader',
+ '../input/input.gyp:sensor_receiver',
'../input/input.gyp:GyroReader',
'../input/input.gyp:AutoMode',
'../output/output.gyp:MotorWriter',
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index accb087..71c1c3c 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -1,6 +1,6 @@
MotorWriter
JoystickReader
-SensorReader
+sensor_receiver
GyroReader
DriveTrain
AutoMode
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 57e53fa..43c4a53 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -6,6 +6,7 @@
#include "aos/common/inttypes.h"
#include "aos/common/messages/RobotState.q.h"
#include "aos/atom_code/output/MotorOutput.h"
+#include "aos/common/logging/logging.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
diff --git a/frc971/crio/crio.gyp b/frc971/crio/crio.gyp
index b91b32d..cbd7d2d 100644
--- a/frc971/crio/crio.gyp
+++ b/frc971/crio/crio.gyp
@@ -22,13 +22,15 @@
'main.cc',
],
'dependencies': [
- '../input/input.gyp:SensorReader',
- '../input/input.gyp:SensorWriter',
'../output/output.gyp:MotorWriter',
- '../output/output.gyp:SensorSender',
'WPILib_changes',
'<(EXTERNALS):WPILib',
'<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/crio/controls/controls.gyp:ControlsManager',
+ '<(AOS)/crio/motor_server/motor_server.gyp:crio_control_loop_runner',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_broadcaster',
+ '<(DEPTH)/frc971/input/input.gyp:sensor_packer',
+ '<(DEPTH)/frc971/input/input.gyp:sensor_unpacker',
],
},
{
@@ -42,7 +44,6 @@
'target_name': 'FRC_UserProgram_WithTests',
'type': 'shared_library',
'dependencies': [
- 'user_program',
# For testing.
'<(AOS)/build/aos_all.gyp:Crio',
],
diff --git a/frc971/crio/main.cc b/frc971/crio/main.cc
index e814b97..649946b 100644
--- a/frc971/crio/main.cc
+++ b/frc971/crio/main.cc
@@ -1,17 +1,32 @@
#include "aos/crio/controls/ControlsManager.h"
+#include "aos/crio/motor_server/crio_control_loop_runner.h"
+#include "aos/common/sensors/sensor_broadcaster.h"
-#include "aos/crio/motor_server/CRIOControlLoopRunner.h"
+#include "frc971/queues/sensor_values.h"
+#include "frc971/input/sensor_packer.h"
+#include "frc971/input/sensor_unpacker.h"
namespace frc971 {
class MyRobot : public ::aos::crio::ControlsManager {
public:
+ MyRobot()
+ : broadcaster_(&packer_),
+ control_loop_runner_(&broadcaster_, &unpacker_) {}
+
virtual void RegisterControlLoops() {
- //::aos::crio::CRIOControlLoopRunner::AddControlLoop(&shooter_);
+ //control_loop_runner_.AddControlLoop(&shooter_);
}
- private:
- //::frc971::control_loops::ShooterMotor shooter_;
+ virtual void StartSensorBroadcasters() {
+ broadcaster_.Start();
+ }
+
+ ::frc971::SensorPacker packer_;
+ ::frc971::SensorUnpacker unpacker_;
+ ::aos::sensors::SensorBroadcaster< ::frc971::sensor_values> broadcaster_;
+ ::aos::crio::CRIOControlLoopRunner< ::frc971::sensor_values>
+ control_loop_runner_;
};
} // namespace frc971
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index ff43949..89008cf 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -7,7 +7,9 @@
'constants.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/output/output.gyp:motor_output',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
],
}
],
diff --git a/frc971/input/GyroReader.cc b/frc971/input/GyroReader.cc
index 9fbee44..bdcd07e 100644
--- a/frc971/input/GyroReader.cc
+++ b/frc971/input/GyroReader.cc
@@ -4,6 +4,7 @@
#include <fcntl.h>
#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
#include "frc971/queues/GyroAngle.q.h"
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
deleted file mode 100644
index fd14df4..0000000
--- a/frc971/input/SensorReader.cc
+++ /dev/null
@@ -1,44 +0,0 @@
-#define __STDC_LIMIT_MACROS
-
-#include <arpa/inet.h>
-
-#include "aos/aos_core.h"
-#include "aos/common/inttypes.h"
-#include "aos/common/input/SensorInput.h"
-
-#include "frc971/control_loops/DriveTrain.q.h"
-#include "frc971/queues/sensor_values.h"
-
-#define M_PI 3.14159265358979323846
-
-using ::frc971::control_loops::drivetrain;
-
-namespace frc971 {
-
-namespace {
-inline double drivetrain_translate(int32_t in) {
- // TODO(2013) fix the math
- return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
- (3.5 * 2.54 / 100.0 * M_PI);
-}
-} // namespace
-
-class SensorReader : public aos::SensorInput<sensor_values> {
- virtual void RunIteration(sensor_values &sensors) {
- for (size_t i = 0; i < sizeof(sensors.encoders) / sizeof(sensors.encoders[0]); ++i) {
- sensors.encoders[i] = ntohl(sensors.encoders[i]);
- }
-
- // TODO(aschuh): Convert to meters.
- const double left_encoder = drivetrain_translate(sensors.lencoder);
- const double right_encoder = drivetrain_translate(sensors.rencoder);
- drivetrain.position.MakeWithBuilder()
- .left_encoder(left_encoder)
- .right_encoder(right_encoder)
- .Send();
- }
-};
-
-} // namespace frc971
-
-AOS_RUN(frc971::SensorReader)
diff --git a/frc971/input/SensorWriter.cc b/frc971/input/SensorWriter.cc
deleted file mode 100644
index 1fb34db..0000000
--- a/frc971/input/SensorWriter.cc
+++ /dev/null
@@ -1,40 +0,0 @@
-#include <arpa/inet.h>
-
-#include "WPILib/Task.h"
-#include "WPILib/Encoder.h"
-#include "WPILib/DigitalInput.h"
-#include "WPILib/Counter.h"
-
-#include "aos/aos_core.h"
-#include "aos/crio/motor_server/SensorOutput.h"
-#include "aos/common/inttypes.h"
-#include "aos/common/mutex.h"
-#include "aos/crio/shared_libs/interrupt_notifier.h"
-
-#include "frc971/queues/sensor_values.h"
-
-using ::aos::MutexLocker;
-
-namespace frc971 {
-
-class SensorWriter : public aos::SensorOutput<sensor_values> {
- Encoder lencoder;
- Encoder rencoder;
-
- public:
- SensorWriter() : lencoder(1, 2), rencoder(3, 4) {
- lencoder.Start();
- rencoder.Start();
-
- printf("frc971::SensorWriter started\n");
- }
-
- virtual void RunIteration(sensor_values &vals) {
- vals.lencoder = htonl(-lencoder.GetRaw());
- vals.rencoder = -htonl(-rencoder.GetRaw());
- }
-};
-
-} // namespace frc971
-
-AOS_RUN(frc971::SensorWriter)
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index c62365b..27d6ae9 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -8,7 +8,6 @@
'header_path': 'frc971/input',
},
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(AOS)/common/common.gyp:controls',
],
'includes': ['../../aos/build/queues.gypi'],
@@ -20,9 +19,7 @@
'JoystickReader.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(AOS)/atom_code/input/input.gyp:joystick',
- '<(AOS)/common/network/network.gyp:socket',
'actions',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
@@ -30,35 +27,50 @@
],
},
{
- 'target_name': 'SensorReader',
- 'type': '<(aos_target)',
+ 'target_name': 'sensor_unpacker',
+ 'type': 'static_library',
'sources': [
- 'SensorReader.cc',
+ 'sensor_unpacker.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/network/network.gyp:socket',
- ],
- 'conditions': [
- ['OS!="crio"', {
- 'dependencies': [
- '<(AOS)/atom_code/atom_code.gyp:init',
- ],
- }],
],
},
{
- 'target_name': 'SensorWriter',
- 'type': '<(aos_target)',
+ 'target_name': 'sensor_receiver',
+ 'type': 'executable',
'sources': [
- 'SensorWriter.cc',
+ 'sensor_receiver.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'sensor_unpacker',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'sensor_packer',
+ 'type': 'static_library',
+ 'sources': [
+ 'sensor_packer.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/crio/hardware/hardware.gyp:counter',
+ '<(AOS)/crio/hardware/hardware.gyp:digital_source',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/crio/hardware/hardware.gyp:counter',
+ '<(AOS)/crio/hardware/hardware.gyp:digital_source',
],
},
{
diff --git a/frc971/input/sensor_packer.cc b/frc971/input/sensor_packer.cc
new file mode 100644
index 0000000..865b25b
--- /dev/null
+++ b/frc971/input/sensor_packer.cc
@@ -0,0 +1,83 @@
+#include "frc971/input/sensor_packer.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+#include "aos/common/time.h"
+
+#include "frc971/control_loops/index/index.h"
+
+using ::aos::MutexLocker;
+
+namespace frc971 {
+
+SensorPacker::SensorPacker() : lencoder(1, 2), rencoder(3, 4) {
+ lencoder.Start();
+ rencoder.Start();
+
+ printf("frc971::SensorPacker started\n");
+}
+
+void SensorPacker::ReadEncoder(EncoderReadData *data) {
+ int32_t value = data->counter->Get();
+ {
+ ::aos::MutexLocker locker(data->sync);
+ *data->output = value;
+ }
+}
+
+void SensorPacker::TopDiscEdgeReader() {
+ while (true) {
+ top_disc_posedge_output_->source()->
+ WaitForInterrupt(kInterruptTimeout);
+ int32_t position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ top_disc_posedge_position_ = position;
+ ++top_disc_posedge_count_;
+ }
+ top_disc_negedge_output_->source()->
+ WaitForInterrupt(kInterruptTimeout);
+ position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ top_disc_negedge_position_ = position;
+ ++top_disc_negedge_count_;
+ }
+ }
+}
+
+void SensorPacker::BottomDiscEdgeReader() {
+ static const aos::time::Time kBottomDiscNegedgeSleep =
+ aos::time::Time::InSeconds(
+ control_loops::IndexMotor::kBottomDiscIndexDelay);
+ while (true) {
+ bottom_disc_->source()->WaitForInterrupt(kInterruptTimeout);
+ if (bottom_disc_->Get()) {
+ aos::time::Time start = aos::time::Time::Now();
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ ++bottom_disc_negedge_count_;
+ }
+ aos::time::SleepUntil(start + kBottomDiscNegedgeSleep);
+ int32_t position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ bottom_disc_negedge_wait_position_ = position;
+ ++bottom_disc_negedge_wait_count_;
+ }
+ } else {
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ ++bottom_disc_posedge_count_;
+ }
+ }
+ }
+}
+
+void SensorPacker::PackInto(sensor_values *values) {
+ values->lencoder = htonl(-lencoder.GetRaw());
+ values->rencoder = -htonl(-rencoder.GetRaw());
+}
+
+} // namespace frc971
diff --git a/frc971/input/sensor_packer.h b/frc971/input/sensor_packer.h
new file mode 100644
index 0000000..78f6bf6
--- /dev/null
+++ b/frc971/input/sensor_packer.h
@@ -0,0 +1,106 @@
+#ifndef FRC971_INPUT_SENSOR_PACKER_H_
+#define FRC971_INPUT_SENSOR_PACKER_H_
+
+#include "aos/common/libstdc++/memory"
+
+#include "WPILib/Task.h"
+#include "WPILib/Encoder.h"
+#include "WPILib/DigitalInput.h"
+#include "WPILib/Counter.h"
+#include "WPILib/CounterBase.h"
+#include "WPILib/AnalogChannel.h"
+#include "WPILib/AnalogTrigger.h"
+
+#include "aos/common/mutex.h"
+#include "aos/crio/shared_libs/interrupt_notifier.h"
+#include "aos/common/sensors/sensor_packer.h"
+#include "aos/crio/hardware/counter.h"
+#include "aos/crio/hardware/digital_source.h"
+
+#include "frc971/queues/sensor_values.h"
+
+namespace frc971 {
+
+class SensorPacker
+ : public ::aos::sensors::SensorPackerInterface<sensor_values> {
+ public:
+ SensorPacker();
+
+ virtual void PackInto(sensor_values *values);
+
+ private:
+ // Used for passing ReadEncoder all of the information that it needs.
+ struct EncoderReadData {
+ EncoderReadData(::aos::crio::hardware::Counter *counter,
+ int32_t *output,
+ ::aos::Mutex *sync)
+ : counter(counter), output(output), sync(sync) {}
+
+ ::aos::crio::hardware::Counter *const counter;
+ int32_t *const output;
+ ::aos::Mutex *const sync;
+ };
+
+ // Handler function for an ::aos::crio::InterruptNotifier that reads an
+ // encoder and (synchronized on a ::aos::Mutex) writes the value into a
+ // pointer.
+ static void ReadEncoder(EncoderReadData *data);
+
+ // A whole day.
+ // Used for passing to WPILib's WaitForInterrupts.
+ static const float kInterruptTimeout = 60 * 60 * 24;
+
+ static void StaticTopDiscEdgeReader(void *self) {
+ static_cast<SensorPacker *>(self)->TopDiscEdgeReader();
+ }
+ static void StaticBottomDiscEdgeReader(void *self) {
+ static_cast<SensorPacker *>(self)->BottomDiscEdgeReader();
+ }
+
+ void TopDiscEdgeReader();
+ void BottomDiscEdgeReader();
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> drive_left_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> drive_right_counter_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> wrist_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> wrist_hall_effect_;
+ int32_t wrist_edge_position_;
+ ::aos::Mutex wrist_sync_;
+ ::aos::crio::InterruptNotifier<EncoderReadData> wrist_notifier_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> angle_adjust_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> angle_adjust_middle_hall_effect_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> angle_adjust_bottom_hall_effect_;
+ int32_t angle_adjust_middle_edge_position_;
+ int32_t angle_adjust_bottom_edge_position_;
+ ::aos::Mutex angle_adjust_sync_;
+ ::aos::crio::InterruptNotifier<EncoderReadData>
+ angle_adjust_middle_notifier_;
+ ::aos::crio::InterruptNotifier<EncoderReadData>
+ angle_adjust_bottom_notifier_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> shooter_counter_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::CounterCounter> index_counter_;
+ Task top_disc_edge_task_;
+ ::aos::Mutex top_disc_edge_sync_;
+ ::std::unique_ptr< ::AnalogTrigger> top_disc_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> top_disc_posedge_output_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> top_disc_negedge_output_;
+ int32_t top_disc_posedge_count_;
+ int32_t top_disc_negedge_count_;
+ int32_t top_disc_posedge_position_;
+ int32_t top_disc_negedge_position_;
+ Task bottom_disc_edge_task_;
+ ::aos::Mutex bottom_disc_edge_sync_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> bottom_disc_;
+ int32_t bottom_disc_posedge_count_;
+ int32_t bottom_disc_negedge_count_;
+ int32_t bottom_disc_negedge_wait_count_;
+ int32_t bottom_disc_negedge_wait_position_;
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_SENSOR_PACKER_H_
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
new file mode 100644
index 0000000..a0b7f8f
--- /dev/null
+++ b/frc971/input/sensor_receiver.cc
@@ -0,0 +1,16 @@
+#include "aos/common/sensors/sensor_receiver.h"
+#include "aos/atom_code/init.h"
+
+#include "frc971/queues/sensor_values.h"
+#include "frc971/input/sensor_unpacker.h"
+
+int main() {
+ ::aos::Init();
+ ::frc971::SensorUnpacker unpacker;
+ ::aos::sensors::NetworkSensorReceiver< ::frc971::sensor_values>
+ receiver(&unpacker);
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
new file mode 100644
index 0000000..8695785
--- /dev/null
+++ b/frc971/input/sensor_unpacker.cc
@@ -0,0 +1,42 @@
+#include "frc971/input/sensor_unpacker.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+
+#include "frc971/control_loops/DriveTrain.q.h"
+
+#define M_PI 3.14159265358979323846
+
+using ::frc971::control_loops::drivetrain;
+
+namespace frc971 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ // TODO(2013) fix the math
+ return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
+ (3.5 * 2.54 / 100.0 * M_PI);
+}
+
+} // namespace
+
+SensorUnpacker::SensorUnpacker() {}
+
+void SensorUnpacker::UnpackFrom(sensor_values *values) {
+ for (size_t i = 0; i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
+ values->encoders[i] = ntohl(values->encoders[i]);
+ }
+
+ // TODO(aschuh): Convert to meters.
+ const double left_encoder = drivetrain_translate(
+ values->drive_left_encoder);
+ const double right_encoder = drivetrain_translate(
+ values->drive_right_encoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+}
+
+} // namespace frc971
diff --git a/frc971/input/sensor_unpacker.h b/frc971/input/sensor_unpacker.h
new file mode 100644
index 0000000..b15244a
--- /dev/null
+++ b/frc971/input/sensor_unpacker.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_INPUT_SENSOR_UNPACKER_H_
+#define FRC971_INPUT_SENSOR_UNPACKER_H_
+
+#include "aos/common/sensors/sensor_unpacker.h"
+
+#include "frc971/queues/sensor_values.h"
+
+namespace frc971 {
+
+class SensorUnpacker
+ : public ::aos::sensors::SensorUnpackerInterface<sensor_values> {
+ public:
+ SensorUnpacker();
+
+ virtual void UnpackFrom(sensor_values *values);
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_SENSOR_UNPACKER_H_
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/AtomMotorWriter.cc
index 243ac81..37d7591 100644
--- a/frc971/output/AtomMotorWriter.cc
+++ b/frc971/output/AtomMotorWriter.cc
@@ -3,7 +3,6 @@
#include <unistd.h>
#include "aos/aos_core.h"
-#include "aos/common/network/SendSocket.h"
#include "aos/common/control_loop/Timing.h"
#include "aos/common/messages/RobotState.q.h"
#include "aos/atom_code/output/MotorOutput.h"
diff --git a/frc971/output/SensorSender.cc b/frc971/output/SensorSender.cc
deleted file mode 100644
index e2d2e51..0000000
--- a/frc971/output/SensorSender.cc
+++ /dev/null
@@ -1,5 +0,0 @@
-#include "aos/crio/motor_server/SensorSender.h"
-#include "frc971/queues/sensor_values.h"
-
-AOS_RUN_FORK(aos::SensorSender<frc971::sensor_values>, "971SS", 100)
-
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 780eadf..caa4bb8 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -43,18 +43,6 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/network/network.gyp:socket',
- ],
- },
- {
- 'target_name': 'SensorSender',
- 'type': '<(aos_target)',
- 'sources': [
- 'SensorSender.cc',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- '<(AOS)/common/network/network.gyp:socket',
],
},
],