Merge branch 'svn' into HEAD
I fixed dealing with NULL positions in the drivetrain.
diff --git a/aos/atom_code/atom_code.gyp b/aos/atom_code/atom_code.gyp
new file mode 100644
index 0000000..8253588
--- /dev/null
+++ b/aos/atom_code/atom_code.gyp
@@ -0,0 +1,16 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'init',
+ 'type': 'static_library',
+ 'sources': [
+ '<(AOS)/atom_code/init.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/common/common.gyp:die',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/camera/Buffers.cpp b/aos/atom_code/camera/Buffers.cpp
index 22b7337..f580f89 100644
--- a/aos/atom_code/camera/Buffers.cpp
+++ b/aos/atom_code/camera/Buffers.cpp
@@ -1,8 +1,10 @@
-#include "Buffers.h"
-#include "V4L2.h"
+#include "aos/atom_code/camera/Buffers.h"
#include <sys/mman.h>
+#include "aos/atom_code/camera/V4L2.h"
+#include "aos/common/logging/logging.h"
+
namespace aos {
namespace camera {
diff --git a/aos/atom_code/camera/Buffers.h b/aos/atom_code/camera/Buffers.h
index 7f1206d..b72cd3e 100644
--- a/aos/atom_code/camera/Buffers.h
+++ b/aos/atom_code/camera/Buffers.h
@@ -6,7 +6,7 @@
#include <string>
-#include "aos/aos_core.h"
+#include "aos/atom_code/ipc_lib/queue.h"
#include "aos/common/type_traits.h"
namespace aos {
diff --git a/aos/atom_code/camera/camera.gyp b/aos/atom_code/camera/camera.gyp
index e94a6ac..b8e8dd3 100644
--- a/aos/atom_code/camera/camera.gyp
+++ b/aos/atom_code/camera/camera.gyp
@@ -43,13 +43,28 @@
'hard_dependency': 1,
},
{
+ 'target_name': 'buffers',
+ 'type': 'static_library',
+ 'sources': [
+ 'Buffers.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ ],
+ },
+ {
'target_name': 'CameraHTTPStreamer',
'type': 'executable',
'sources': [
'HTTPStreamer.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ 'buffers',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
{
@@ -59,7 +74,8 @@
'Reader.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ 'buffers',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
],
diff --git a/aos/atom_code/camera/jni.cpp b/aos/atom_code/camera/jni.cpp
index 7b7eafa..ad5177a 100644
--- a/aos/atom_code/camera/jni.cpp
+++ b/aos/atom_code/camera/jni.cpp
@@ -3,6 +3,8 @@
#include "jni/aos_Natives.h"
#include "aos/atom_code/camera/Buffers.h"
#include "aos/externals/libjpeg/include/jpeglib.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/atom_code/init.h"
using aos::camera::Buffers;
@@ -248,8 +250,8 @@
} else {
level = DEBUG;
}
- // can't use Get/ReleaseStringCritical because log_do might block waiting to
- // put its message into the queue
+ // Can't use Get/ReleaseStringCritical because log_do might block waiting to
+ // put its message into the queue.
const char *const message_chars = env->GetStringUTFChars(message, NULL);
if (message_chars == NULL) return;
log_do(level, "%s\n", message_chars);
diff --git a/aos/atom_code/core/BinaryLogReader.cpp b/aos/atom_code/core/BinaryLogReader.cpp
index b673c26..e266f24 100644
--- a/aos/atom_code/core/BinaryLogReader.cpp
+++ b/aos/atom_code/core/BinaryLogReader.cpp
@@ -11,16 +11,20 @@
#include <map>
-#include "aos/aos_core.h"
+#include "aos/atom_code/logging/atom_logging.h"
#include "aos/atom_code/core/LogFileCommon.h"
#include "aos/common/Configuration.h"
+#include "aos/atom_code/init.h"
-static const char *const kCRIOName = "CRIO";
+namespace aos {
+namespace logging {
+namespace atom {
+namespace {
-int main() {
- aos::InitNRT();
+int BinaryLogReaderMain() {
+ InitNRT();
- const char *folder = aos::configuration::GetLoggingDirectory();
+ const char *folder = configuration::GetLoggingDirectory();
if (access(folder, R_OK | W_OK) == -1) {
LOG(FATAL, "folder '%s' does not exist. please create it\n", folder);
}
@@ -28,19 +32,24 @@
const time_t t = time(NULL);
char *tmp;
- if (asprintf(&tmp, "%s/aos_log-%jd", folder, static_cast<uintmax_t>(t)) == -1) {
- fprintf(stderr, "BinaryLogReader: couldn't create final name because of %d (%s)."
+ if (asprintf(&tmp, "%s/aos_log-%jd", folder, static_cast<uintmax_t>(t)) ==
+ -1) {
+ fprintf(stderr,
+ "BinaryLogReader: couldn't create final name because of %d (%s)."
" exiting\n", errno, strerror(errno));
return EXIT_FAILURE;
}
char *tmp2;
if (asprintf(&tmp2, "%s/aos_log-current", folder) == -1) {
- fprintf(stderr, "BinaryLogReader: couldn't create symlink name because of %d (%s)."
+ fprintf(stderr,
+ "BinaryLogReader: couldn't create symlink name because of %d (%s)."
" not creating current symlink\n", errno, strerror(errno));
} else {
if (unlink(tmp2) == -1 && (errno != EROFS && errno != ENOENT)) {
- fprintf(stderr, "BinaryLogReader: warning: unlink('%s') failed because of %d (%s)\n",
- tmp2, errno, strerror(errno));
+ fprintf(stderr,
+ "BinaryLogReader: warning: unlink('%s') failed"
+ " because of %d (%s)\n",
+ tmp2, errno, strerror(errno));
}
if (symlink(tmp, tmp2) == -1) {
fprintf(stderr, "BinaryLogReader: warning: symlink('%s', '%s') failed"
@@ -52,11 +61,12 @@
S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
free(tmp);
if (fd == -1) {
- fprintf(stderr, "BinaryLogReader: couldn't open file '%s' because of %d (%s)."
+ fprintf(stderr,
+ "BinaryLogReader: couldn't open file '%s' because of %d (%s)."
" exiting\n", tmp, errno, strerror(errno));
return EXIT_FAILURE;
}
- aos::LogFileAccessor writer(fd, true);
+ LogFileAccessor writer(fd, true);
struct timespec timespec;
time_t last_sec = 0;
@@ -68,47 +78,39 @@
writer.Sync();
}
- const log_message *const msg = log_read_next();
+ const LogMessage *const msg = ReadNext();
if (msg == NULL) continue;
- const log_crio_message *const crio_msg = reinterpret_cast<const log_crio_message *>(
- msg);
- size_t name_size, message_size;
- if (msg->source == -1) {
- name_size = strlen(kCRIOName);
- message_size = strlen(crio_msg->message);
- } else {
- name_size = strlen(msg->name);
- message_size = strlen(msg->message);
- }
- // add on size for terminating '\0'
- name_size += 1;
- message_size += 1;
+ // add 1 for terminating '\0'
+ size_t name_size = strlen(msg->name) + 1;
+ size_t message_size = strlen(msg->message) + 1;
- aos::LogFileMessageHeader *const output = writer.GetWritePosition(
- sizeof(aos::LogFileMessageHeader) + name_size + message_size);
+ LogFileMessageHeader *const output = writer.GetWritePosition(
+ sizeof(LogFileMessageHeader) + name_size + message_size);
char *output_strings = reinterpret_cast<char *>(output) + sizeof(*output);
output->name_size = name_size;
output->message_size = message_size;
output->source = msg->source;
- if (msg->source == -1) {
- output->level = crio_msg->level;
- // TODO(brians) figure out what time to put in
- output->sequence = crio_msg->sequence;
- memcpy(output_strings, kCRIOName, name_size);
- memcpy(output_strings + name_size, crio_msg->message, message_size);
- } else {
- output->level = msg->level;
- output->time_sec = msg->time.tv_sec;
- output->time_nsec = msg->time.tv_nsec;
- output->sequence = msg->sequence;
- memcpy(output_strings, msg->name, name_size);
- memcpy(output_strings + name_size, msg->message, message_size);
- }
+ output->level = msg->level;
+ output->time_sec = msg->seconds;
+ output->time_nsec = msg->nseconds;
+ output->sequence = msg->sequence;
+ memcpy(output_strings, msg->name, name_size);
+ memcpy(output_strings + name_size, msg->message, message_size);
condition_set(&output->marker);
- log_free_message(msg);
+ logging::atom::Free(msg);
}
- aos::Cleanup();
+ Cleanup();
+ return 0;
+}
+
+} // namespace
+} // namespace atom
+} // namespace logging
+} // namespace aos
+
+int main() {
+ return ::aos::logging::atom::BinaryLogReaderMain();
}
diff --git a/aos/atom_code/core/CRIOLogReader.cpp b/aos/atom_code/core/CRIOLogReader.cpp
index f32e21d..73a5350 100644
--- a/aos/atom_code/core/CRIOLogReader.cpp
+++ b/aos/atom_code/core/CRIOLogReader.cpp
@@ -8,44 +8,55 @@
#include <fcntl.h>
#include <arpa/inet.h>
#include <errno.h>
+#include <string.h>
#include <map>
-#include "aos/aos_core.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/atom_code/logging/atom_logging.h"
#include "aos/common/Configuration.h"
#include "aos/common/byteorder.h"
+#include "aos/atom_code/init.h"
+
+namespace aos {
+namespace logging {
+namespace atom {
+namespace {
struct log_buffer {
- log_crio_message msg;
+ LogMessage *msg;
size_t used;
- log_buffer() {
+ log_buffer() : msg(NULL) {
Clear();
}
void Clear() {
+ logging::atom::Free(msg);
+ msg = logging::atom::Get();
used = 0;
}
- // returns whether msg is now complete
+
+ // Returns whether msg is now complete.
bool ReceiveFrom(int sock) {
- const ssize_t ret = recv(sock, reinterpret_cast<uint8_t *>(&msg) + used,
- sizeof(msg) - used, 0);
+ const ssize_t ret = recv(sock, reinterpret_cast<uint8_t *>(msg) + used,
+ sizeof(*msg) - used, 0);
if (ret == -1) {
LOG(ERROR, "recv(%d, %p, %d) failed because of %d: %s\n",
- sock, reinterpret_cast<uint8_t *>(&msg) + used, sizeof(msg) - used,
+ sock, reinterpret_cast<uint8_t *>(msg) + used, sizeof(*msg) - used,
errno, strerror(errno));
return false;
} else {
used += ret;
- if (used > sizeof(msg)) {
- LOG(WARNING, "used(%zd) is > sizeof(msg)(%zd)\n", used, sizeof(msg));
+ if (used > sizeof(*msg)) {
+ LOG(WARNING, "used(%zd) is > sizeof(*msg)(%zd)\n", used, sizeof(*msg));
}
- return used >= sizeof(msg);
+ return used >= sizeof(*msg);
}
}
};
-int main() {
- aos::InitNRT();
+int CRIOLogReaderMain() {
+ InitNRT();
const int sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock == -1) {
@@ -59,11 +70,11 @@
} bind_sockaddr;
memset(&bind_sockaddr, 0, sizeof(bind_sockaddr));
bind_sockaddr.in.sin_family = AF_INET;
- bind_sockaddr.in.sin_port = htons(static_cast<uint16_t>(aos::NetworkPort::kLogs));
+ bind_sockaddr.in.sin_port = htons(static_cast<uint16_t>(NetworkPort::kLogs));
bind_sockaddr.in.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(sock, &bind_sockaddr.addr, sizeof(bind_sockaddr.addr)) == -1) {
- LOG(ERROR, "bind(%d, %p) failed because of %d: %s\n", sock, &bind_sockaddr.addr,
- errno, strerror(errno));
+ LOG(ERROR, "bind(%d, %p) failed because of %d: %s\n", sock,
+ &bind_sockaddr.addr, errno, strerror(errno));
return EXIT_FAILURE;
}
if (listen(sock, 10) == -1) {
@@ -109,21 +120,35 @@
"because of %d: %s\n",
sock, SOCK_NONBLOCK, errno, strerror(errno));
} else {
- socks[new_sock]; // creates using value's default constructor
+ socks[new_sock]; // creates using value's default constructor
}
}
for (auto it = socks.begin(); it != socks.end(); ++it) {
if (FD_ISSET(it->first, &read_fds)) {
if (it->second.ReceiveFrom(it->first)) {
- it->second.msg.identifier = -1;
- it->second.msg.time = aos::ntoh(it->second.msg.time);
- log_crio_message_send(it->second.msg);
+ it->second.msg->source = ntoh(it->second.msg->source);
+ it->second.msg->sequence = ntoh(it->second.msg->sequence);
+ it->second.msg->level = ntoh(it->second.msg->level);
+ it->second.msg->seconds = ntoh(it->second.msg->seconds);
+ it->second.msg->nseconds = ntoh(it->second.msg->nseconds);
+
+ logging::atom::Write(it->second.msg);
+ it->second.msg = NULL;
it->second.Clear();
}
}
}
}
- aos::Cleanup();
+ Cleanup();
+}
+
+} // namespace
+} // namespace atom
+} // namespace logging
+} // namespace aos
+
+int main() {
+ return ::aos::logging::atom::CRIOLogReaderMain();
}
diff --git a/aos/atom_code/core/LogDisplayer.cpp b/aos/atom_code/core/LogDisplayer.cpp
index 9dee08f..d106119 100644
--- a/aos/atom_code/core/LogDisplayer.cpp
+++ b/aos/atom_code/core/LogDisplayer.cpp
@@ -9,6 +9,7 @@
#include "aos/atom_code/core/LogFileCommon.h"
#include "aos/aos_core.h"
+#include "aos/common/logging/logging_impl.h"
namespace {
@@ -28,6 +29,7 @@
" -s, --skip NUMBER skip NUMBER matching logs\n"
" // -m, --max NUMBER only display up to NUMBER logs\n"
" // -o, --format FORMAT use FORMAT to display log entries\n"
+ " -h, --help display this help and exit\n"
"\n"
"LEVEL must be DEBUG, INFO, WARNING, ERROR, or FATAL.\n"
" It defaults to INFO.\n"
@@ -81,7 +83,7 @@
filter_name = optarg;
break;
case 'l':
- filter_level = str_log(optarg);
+ filter_level = ::aos::logging::str_log(optarg);
if (filter_level == LOG_UNKNOWN) {
fprintf(stderr, "LogDisplayer: unknown log level '%s'\n", optarg);
exit(EXIT_FAILURE);
@@ -115,13 +117,14 @@
case '?':
break;
default:
- fprintf(stderr, "LogDisplayer: in a bad spot (%s: %d)\n", __FILE__, __LINE__);
+ fprintf(stderr, "LogDisplayer: in a bad spot (%s: %d)\n",
+ __FILE__, __LINE__);
abort();
}
}
fprintf(stderr, "displaying down to level %s from file '%s'\n",
- log_str(filter_level), filename);
+ ::aos::logging::log_str(filter_level), filename);
if (optind < argc) {
fprintf(stderr, "non-option ARGV-elements: ");
while (optind < argc) {
@@ -135,23 +138,34 @@
filename, strerror(errno));
exit(EXIT_FAILURE);
}
- aos::LogFileAccessor accessor(fd, false);
+ ::aos::logging::LogFileAccessor accessor(fd, false);
if (!start_at_beginning) {
accessor.MoveToEnd();
}
- const aos::LogFileMessageHeader *msg;
+ const ::aos::logging::LogFileMessageHeader *msg;
+ ::aos::logging::LogMessage log_message;
do {
msg = accessor.ReadNextMessage(follow);
if (msg == NULL) continue;
- if (log_gt_important(filter_level, msg->level)) continue;
+ if (::aos::logging::log_gt_important(filter_level, msg->level)) continue;
if (filter_name != NULL &&
- strcmp(filter_name, reinterpret_cast<const char *>(msg) + sizeof(*msg)) != 0) {
+ strcmp(filter_name,
+ reinterpret_cast<const char *>(msg) + sizeof(*msg)) != 0) {
continue;
}
- printf("%s(%"PRId32")(%03"PRId8"): %s at %010"PRIu64"s%09"PRIu64"ns: %s",
- reinterpret_cast<const char *>(msg) + sizeof(*msg), msg->source,
- msg->sequence, log_str(msg->level), msg->time_sec, msg->time_nsec,
- reinterpret_cast<const char *>(msg) + sizeof(*msg) + msg->name_size);
+
+ log_message.source = msg->source;
+ log_message.sequence = msg->sequence;
+ log_message.level = msg->level;
+ log_message.seconds = msg->time_sec;
+ log_message.nseconds = msg->time_nsec;
+ strncpy(log_message.name,
+ reinterpret_cast<const char *>(msg) + sizeof(*msg),
+ sizeof(log_message.name));
+ strncpy(log_message.message,
+ reinterpret_cast<const char *>(msg) + sizeof(*msg) +
+ msg->name_size,
+ sizeof(log_message.message));
+ ::aos::logging::internal::PrintMessage(stdout, log_message);
} while (msg != NULL);
}
-
diff --git a/aos/atom_code/core/LogFileCommon.h b/aos/atom_code/core/LogFileCommon.h
index 235c55b..afb86b0 100644
--- a/aos/atom_code/core/LogFileCommon.h
+++ b/aos/atom_code/core/LogFileCommon.h
@@ -11,18 +11,22 @@
#include <algorithm>
-#include "aos/aos_core.h"
+#include "aos/common/logging/logging_impl.h"
namespace aos {
+namespace logging {
// File format: {
// LogFileMessageHeader header;
-// char *name; // of the process that wrote the message
+// char *name; // of the process that wrote the message
// char *message;
// } not crossing kPageSize boundaries into the file.
//
-// Field sizes designed to fit the various values from log_message even on
+// Field sizes designed to fit the various values from LogMessage even on
// other machines (hopefully) because they're baked into the files.
+//
+// A lot of the fields don't have comments because they're the same as the
+// identically named fields in LogMessage.
struct __attribute__((aligned)) LogFileMessageHeader {
// gets condition_set once this one has been written
// for readers keeping up with a live writer
@@ -36,18 +40,17 @@
log_level level;
static_assert(sizeof(level) == 1, "log_level changed size!");
- uint64_t time_sec;
- static_assert(sizeof(time_sec) >= sizeof(log_message::time.tv_sec), "tv_sec won't fit");
- uint64_t time_nsec;
- static_assert(sizeof(time_nsec) >= sizeof(log_message::time.tv_nsec),
+ uint32_t time_sec;
+ static_assert(sizeof(time_sec) >= sizeof(LogMessage::seconds),
+ "tv_sec won't fit");
+ uint32_t time_nsec;
+ static_assert(sizeof(time_nsec) >= sizeof(LogMessage::nseconds),
"tv_nsec won't fit");
- int32_t source; // pid or -1 for crio
- static_assert(sizeof(source) >= sizeof(log_message::source), "PIDs won't fit");
- uint8_t sequence;
- static_assert(sizeof(sequence) == sizeof(log_crio_message::sequence),
- "something changed");
- static_assert(sizeof(sequence) == sizeof(log_message::sequence),
+ int32_t source;
+ static_assert(sizeof(source) >= sizeof(LogMessage::source), "PIDs won't fit");
+ uint16_t sequence;
+ static_assert(sizeof(sequence) == sizeof(LogMessage::sequence),
"something changed");
// both including the terminating '\0'
@@ -129,7 +132,7 @@
sizeof(mutex) > kPageSize) {
char *const temp = current_;
MapNextPage();
- if (condition_set_value(reinterpret_cast<mutex *>(&temp[position_]), 2) != 0) {
+ if (condition_set_value(reinterpret_cast<mutex *>(&temp[position_]), 2) == -1) {
fprintf(stderr, "LogFileCommon: condition_set_value(%p, 2) failed with %d: %s."
" readers will hang\n", &temp[position_], errno, strerror(errno));
}
@@ -186,7 +189,7 @@
}
};
-};
+} // namespace logging
+} // namespace aos
#endif
-
diff --git a/aos/atom_code/core/LogStreamer.cpp b/aos/atom_code/core/LogStreamer.cpp
index ceec18d..a7151ad 100644
--- a/aos/atom_code/core/LogStreamer.cpp
+++ b/aos/atom_code/core/LogStreamer.cpp
@@ -10,35 +10,44 @@
#include <fcntl.h>
#include <inttypes.h>
-#include "aos/aos_core.h"
+#include "aos/atom_code/logging/atom_logging.h"
#include "aos/atom_code/core/LogFileCommon.h"
+#include "aos/atom_code/init.h"
+#include "aos/atom_code/ipc_lib/queue.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/common/time.h"
-static const char *const kCRIOName = "CRIO";
+namespace aos {
+namespace logging {
+namespace atom {
+namespace {
-int main() {
- aos::InitNRT();
+int LogStreamerMain() {
+ InitNRT();
- const time_t t = time(NULL);
- printf("starting at %jd----------------------------------\n", static_cast<uintmax_t>(t));
+ const time::Time now = time::Time::Now();
+ printf("starting at %"PRId32"s%"PRId32"ns---------------------------------\n",
+ now.sec(), now.nsec());
int index = 0;
while (true) {
- const log_message *const msg = log_read_next2(BLOCK, &index);
+ const LogMessage *const msg = ReadNext(BLOCK, &index);
if (msg == NULL) continue;
- const log_crio_message *const crio_msg = reinterpret_cast<const log_crio_message *>(
- msg);
- if (msg->source == -1) {
- printf("CRIO(%03"PRId8"): %s at %f: %s", crio_msg->sequence,
- log_str(crio_msg->level), crio_msg->time, crio_msg->message);
- } else {
- printf("%s(%"PRId32")(%03"PRId8"): %s at %010jus%09luns: %s",
- msg->name, msg->source, msg->sequence, log_str(msg->level),
- static_cast<uintmax_t>(msg->time.tv_sec), msg->time.tv_nsec, msg->message);
- }
+ internal::PrintMessage(stdout, *msg);
- log_free_message(msg);
+ logging::atom::Free(msg);
}
- aos::Cleanup();
+ Cleanup();
+ return 0;
+}
+
+} // namespace
+} // namespace atom
+} // namespace logging
+} // namespace aos
+
+int main() {
+ return ::aos::logging::atom::LogStreamerMain();
}
diff --git a/aos/atom_code/core/core.cc b/aos/atom_code/core/core.cc
index b34169b..4179a0a 100644
--- a/aos/atom_code/core/core.cc
+++ b/aos/atom_code/core/core.cc
@@ -3,10 +3,10 @@
// Purposes: All shared memory gets allocated here.
//
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-#include "aos/aos_core.h"
+#include <sys/select.h>
+#include <stdlib.h>
+
+#include "aos/atom_code/init.h"
int main() {
aos::InitCreate();
diff --git a/aos/atom_code/core/core.gyp b/aos/atom_code/core/core.gyp
index 04d3cc8..3b327da 100644
--- a/aos/atom_code/core/core.gyp
+++ b/aos/atom_code/core/core.gyp
@@ -7,7 +7,7 @@
'core.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
{
@@ -17,7 +17,9 @@
'BinaryLogReader.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/common/common.gyp:common',
],
},
{
@@ -27,7 +29,9 @@
'LogStreamer.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/common/common.gyp:time',
],
},
{
@@ -37,7 +41,8 @@
'LogDisplayer.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
{
@@ -47,7 +52,8 @@
'CRIOLogReader.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
],
diff --git a/aos/atom_code/init.cc b/aos/atom_code/init.cc
index 4776261..7eecde3 100644
--- a/aos/atom_code/init.cc
+++ b/aos/atom_code/init.cc
@@ -12,8 +12,9 @@
#include <stdlib.h>
#include <stdint.h>
-#include "aos/aos_core.h"
#include "aos/common/die.h"
+#include "aos/atom_code/logging/atom_logging.h"
+#include "aos/atom_code/ipc_lib/shared_mem.h"
namespace aos {
@@ -69,10 +70,7 @@
Die("%s-init: creating shared memory reference failed\n",
program_invocation_short_name);
}
- if (log_init(program_invocation_short_name) != 0) {
- Die("%s-init: initializing logging failed\n",
- program_invocation_short_name);
- }
+ logging::atom::Register();
}
const char *const kNoRealtimeEnvironmentVariable = "AOS_NO_REALTIME";
diff --git a/aos/atom_code/ipc_lib/mutex.cpp b/aos/atom_code/ipc_lib/mutex.cpp
index d1f0ef2..4753e78 100644
--- a/aos/atom_code/ipc_lib/mutex.cpp
+++ b/aos/atom_code/ipc_lib/mutex.cpp
@@ -2,6 +2,8 @@
#include <inttypes.h>
#include <errno.h>
+#include <stdio.h>
+#include <string.h>
#include "aos/aos_core.h"
#include "aos/common/type_traits.h"
diff --git a/aos/atom_code/ipc_lib/queue.c b/aos/atom_code/ipc_lib/queue.c
index 5cfd2ac..2e45326 100644
--- a/aos/atom_code/ipc_lib/queue.c
+++ b/aos/atom_code/ipc_lib/queue.c
@@ -6,6 +6,8 @@
#include <errno.h>
#include <assert.h>
+#include "aos/common/logging/logging.h"
+
#define READ_DEBUG 0
#define WRITE_DEBUG 0
#define REF_DEBUG 0
@@ -36,7 +38,7 @@
abort();
}
#if REF_DEBUG
- printf("ref_free_count: %p\n", msg);
+ printf("ref free: %p\n", msg);
#endif
--pool->used;
@@ -484,10 +486,7 @@
msg = pool->pool[pool->used];
} else {
if (pool->length >= pool->mem_length) {
- //TODO(brians) log this if it isn't the log queue
- fprintf(stderr, "queue: overused_pool\n");
- msg = NULL;
- goto exit;
+ LOG(FATAL, "overused pool %p\n", pool);
}
msg = pool->pool[pool->length] = aos_alloc_msg(pool);
++pool->length;
@@ -500,7 +499,6 @@
#endif
header->index = pool->used;
++pool->used;
-exit:
mutex_unlock(&pool->pool_lock);
return msg;
}
diff --git a/aos/atom_code/logging/atom_logging.cc b/aos/atom_code/logging/atom_logging.cc
new file mode 100644
index 0000000..08a65cb
--- /dev/null
+++ b/aos/atom_code/logging/atom_logging.cc
@@ -0,0 +1,144 @@
+#include "aos/atom_code/logging/atom_logging.h"
+
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <sys/types.h>
+#include <errno.h>
+#include <unistd.h>
+#include <limits.h>
+#include <sys/prctl.h>
+
+#include <algorithm>
+
+#include "aos/common/die.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/atom_code/thread_local.h"
+#include "aos/atom_code/ipc_lib/queue.h"
+
+namespace aos {
+namespace logging {
+namespace {
+
+using internal::Context;
+
+AOS_THREAD_LOCAL Context *my_context(NULL);
+
+::std::string GetMyName() {
+ // The maximum number of characters that can make up a thread name.
+ // The docs are unclear if it can be 16 characters with no '\0', so we'll be
+ // safe by adding our own where necessary.
+ static const size_t kThreadNameLength = 16;
+
+ ::std::string process_name(program_invocation_short_name);
+
+ char thread_name_array[kThreadNameLength + 1];
+ if (prctl(PR_GET_NAME, thread_name_array) != 0) {
+ Die("prctl(PR_GET_NAME, %p) failed with %d: %s\n",
+ thread_name_array, errno, strerror(errno));
+ }
+ thread_name_array[sizeof(thread_name_array) - 1] = '\0';
+ ::std::string thread_name(thread_name_array);
+
+ // If the first bunch of characters are the same.
+ // We cut off comparing at the shorter of the 2 strings because one or the
+ // other often ends up cut off.
+ if (strncmp(thread_name.c_str(), process_name.c_str(),
+ ::std::min(thread_name.length(), process_name.length())) == 0) {
+ // This thread doesn't have an actual name.
+ return process_name;
+ }
+
+ return process_name + '.' + thread_name;
+}
+
+static const aos_type_sig message_sig = {sizeof(LogMessage), 1323, 1500};
+static aos_queue *queue;
+
+} // namespace
+namespace internal {
+
+Context *Context::Get() {
+ if (my_context == NULL) {
+ my_context = new Context();
+ my_context->name = GetMyName();
+ if (my_context->name.size() + 1 > sizeof(LogMessage::name)) {
+ Die("logging: process/thread name '%s' is too long\n",
+ my_context->name.c_str());
+ }
+ my_context->source = getpid();
+ }
+ return my_context;
+}
+
+void Context::Delete() {
+ delete my_context;
+ my_context = NULL;
+}
+
+} // namespace internal
+namespace atom {
+namespace {
+
+class AtomQueueLogImplementation : public LogImplementation {
+ virtual void DoLog(log_level level, const char *format, va_list ap) {
+ LogMessage *message = static_cast<LogMessage *>(aos_queue_get_msg(queue));
+ if (message == NULL) {
+ LOG(FATAL, "queue get message failed\n");
+ }
+
+ internal::FillInMessage(level, format, ap, message);
+
+ Write(message);
+ }
+};
+
+} // namespace
+
+void Register() {
+ Init();
+
+ queue = aos_fetch_queue("LoggingQueue", &message_sig);
+ if (queue == NULL) {
+ Die("logging: couldn't fetch queue\n");
+ }
+
+ AddImplementation(new AtomQueueLogImplementation());
+}
+
+const LogMessage *ReadNext(int flags, int *index) {
+ return static_cast<const LogMessage *>(
+ aos_queue_read_msg_index(queue, flags, index));
+}
+
+const LogMessage *ReadNext() {
+ return ReadNext(BLOCK);
+}
+
+const LogMessage *ReadNext(int flags) {
+ const LogMessage *r = NULL;
+ do {
+ r = static_cast<const LogMessage *>(aos_queue_read_msg(queue, flags));
+ // not blocking means return a NULL if that's what it gets
+ } while ((flags & BLOCK) && r == NULL);
+ return r;
+}
+
+LogMessage *Get() {
+ return static_cast<LogMessage *>(aos_queue_get_msg(queue));
+}
+
+void Free(const LogMessage *msg) {
+ aos_queue_free_msg(queue, msg);
+}
+
+void Write(LogMessage *msg) {
+ if (aos_queue_write_msg_free(queue, msg, OVERRIDE) < 0) {
+ LOG(FATAL, "writing failed");
+ }
+}
+
+} // namespace atom
+} // namespace logging
+} // namespace aos
diff --git a/aos/atom_code/logging/atom_logging.cpp b/aos/atom_code/logging/atom_logging.cpp
deleted file mode 100644
index e98078b..0000000
--- a/aos/atom_code/logging/atom_logging.cpp
+++ /dev/null
@@ -1,259 +0,0 @@
-#include <stdarg.h>
-#include <stdio.h>
-#include <string.h>
-#include <time.h>
-#include <sys/types.h>
-#include <errno.h>
-#include <unistd.h>
-#include <limits.h>
-
-#include <algorithm>
-
-#include "aos/aos_core.h"
-#include "aos/common/die.h"
-
-#define DECL_LEVEL(name, value) const log_level name = value;
-DECL_LEVELS
-#undef DECL_LEVEL
-
-log_level log_min = 0;
-
-static const aos_type_sig message_sig = {sizeof(log_queue_message), 1234, 1500};
-static const char *name;
-static size_t name_size;
-static aos_queue *queue;
-static log_message corked_message;
-static int cork_line_min, cork_line_max;
-bool log_initted = false;
-
-static inline void cork_init() {
- corked_message.message[0] = '\0'; // make strlen of it 0
- cork_line_min = INT_MAX;
- cork_line_max = -1;
-}
-int log_init(const char *name_in){
- if (log_initted) {
- return 1;
- }
-
- const size_t name_in_len = strlen(name_in);
- const char *last_slash = static_cast<const char *>(memrchr(name_in,
- '/', name_in_len));
- if (last_slash == NULL) {
- name_size = name_in_len;
- last_slash = name_in - 1;
- } else {
- name_size = name_in + name_in_len - last_slash;
- }
- if (name_size >= sizeof(log_message::name)) {
- fprintf(stderr, "logging: error: name '%s' (going to use %zu bytes) is too long\n",
- name_in, name_size);
- return -1;
- }
- char *const tmp = static_cast<char *>(malloc(name_size + 1));
- if (tmp == NULL) {
- fprintf(stderr, "logging: error: couldn't malloc(%zd)\n", name_size + 1);
- return -1;
- }
- name = tmp;
- memcpy(tmp, last_slash + 1, name_size);
- tmp[name_size] = 0;
- queue = aos_fetch_queue("LoggingQueue", &message_sig);
- if (queue == NULL) {
- fprintf(stderr, "logging: error: couldn't fetch queue\n");
- return -1;
- }
-
- cork_init();
-
- log_initted = true;
- return 0;
-}
-void log_uninit() {
- free(const_cast<char *>(name));
- name = NULL;
- name_size = 0;
- queue = NULL;
- log_initted = false;
-}
-
-static inline void check_init() {
- if (!log_initted) {
- fprintf(stderr, "logging: warning: not initialized in %jd."
- " initializing using \"<null>\" as name\n", static_cast<intmax_t>(getpid()));
- log_init("<null>");
- }
-}
-
-const log_message *log_read_next2(int flags, int *index) {
- check_init();
- return static_cast<const log_message *>(aos_queue_read_msg_index(queue, flags, index));
-}
-const log_message *log_read_next1(int flags) {
- check_init();
- const log_message *r = NULL;
- do {
- r = static_cast<const log_message *>(aos_queue_read_msg(queue, flags));
- } while ((flags & BLOCK) && r == NULL); // not blocking means return a NULL if that's what it gets
- return r;
-}
-void log_free_message(const log_message *msg) {
- check_init();
- aos_queue_free_msg(queue, msg);
-}
-
-int log_crio_message_send(log_crio_message &to_send) {
- check_init();
-
- log_crio_message *msg = static_cast<log_crio_message *>(aos_queue_get_msg(queue));
- if (msg == NULL) {
- fprintf(stderr, "logging: error: couldn't get a message to send '%s'\n",
- to_send.message);
- return -1;
- }
- //*msg = to_send;
- static_assert(sizeof(to_send) == sizeof(*msg), "something is very wrong here");
- memcpy(msg, &to_send, sizeof(to_send));
- if (aos_queue_write_msg(queue, msg, OVERRIDE) < 0) {
- fprintf(stderr, "logging: error: writing crio message '%s' failed\n", msg->message);
- aos_queue_free_msg(queue, msg);
- return -1;
- }
-
- return 0;
-}
-
-// Prints format (with ap) into output and correctly deals with the message
-// being too long etc.
-// Returns whether it succeeded or not.
-static inline bool vsprintf_in(char *output, size_t output_size,
- const char *format, va_list ap) {
- static const char *continued = "...\n";
- const size_t size = output_size - strlen(continued);
- const int ret = vsnprintf(output, size, format, ap);
- if (ret < 0) {
- fprintf(stderr, "logging: error: vsnprintf failed with %d (%s)\n",
- errno, strerror(errno));
- return false;
- } else if (static_cast<uintmax_t>(ret) >= static_cast<uintmax_t>(size)) {
- // overwrite the NULL at the end of the existing one and
- // copy in the one on the end of continued
- memcpy(&output[size - 1], continued, strlen(continued) + 1);
- }
- return true;
-}
-static inline bool write_message(log_message *msg, log_level level) {
- msg->level = level;
- msg->source = getpid();
- memcpy(msg->name, name, name_size + 1);
- if (clock_gettime(CLOCK_REALTIME, &msg->time) == -1) {
- fprintf(stderr, "logging: warning: couldn't get the current time "
- "because of %d (%s)\n", errno, strerror(errno));
- msg->time.tv_sec = 0;
- msg->time.tv_nsec = 0;
- }
-
- static uint8_t local_sequence = -1;
- msg->sequence = ++local_sequence;
-
- if (aos_queue_write_msg(queue, msg, OVERRIDE) < 0) {
- fprintf(stderr, "logging: error: writing message '%s' failed\n", msg->message);
- aos_queue_free_msg(queue, msg);
- return false;
- }
- return true;
-}
-static inline int vlog_do(log_level level, const char *format, va_list ap) {
- log_message *msg = static_cast<log_message *>(aos_queue_get_msg(queue));
- if (msg == NULL) {
- fprintf(stderr, "logging: error: couldn't get a message to send '%s'\n", format);
- return -1;
- }
-
- if (!vsprintf_in(msg->message, sizeof(msg->message), format, ap)) {
- return -1;
- }
-
- if (!write_message(msg, level)) {
- return -1;
- }
-
- if (level == FATAL) {
- aos::Die("%s", msg->message);
- }
-
- return 0;
-}
-int log_do(log_level level, const char *format, ...) {
- check_init();
- va_list ap;
- va_start(ap, format);
- const int ret = vlog_do(level, format, ap);
- va_end(ap);
- return ret;
-}
-
-static inline int vlog_cork(int line, const char *format, va_list ap) {
- const size_t message_length = strlen(corked_message.message);
- if (line > cork_line_max) cork_line_max = line;
- if (line < cork_line_min) cork_line_min = line;
- return vsprintf_in(corked_message.message + message_length,
- sizeof(corked_message.message) - message_length, format, ap) ? 0 : -1;
-}
-int log_cork(int line, const char *format, ...) {
- check_init();
- va_list ap;
- va_start(ap, format);
- const int ret = vlog_cork(line, format, ap);
- va_end(ap);
- return ret;
-}
-static inline bool log_uncork_helper(char *output, size_t output_size,
- const char *format, ...) {
- check_init();
- va_list ap;
- va_start(ap, format);
- const bool ret = vsprintf_in(output, output_size, format, ap);
- va_end(ap);
- return ret;
-}
-int log_uncork(int line, log_level level, const char *begin_format,
- const char *format, ...) {
- check_init();
- va_list ap;
- va_start(ap, format);
- const int ret = vlog_cork(line, format, ap);
- va_end(ap);
- if (ret != 0) {
- return ret;
- }
-
- log_message *msg = static_cast<log_message *>(aos_queue_get_msg(queue));
- if (msg == NULL) {
- fprintf(stderr, "logging: error: couldn't get a message to send '%s'\n", format);
- cork_init();
- return -1;
- }
-
- static char new_format[LOG_MESSAGE_LEN];
- if (!log_uncork_helper(new_format, sizeof(new_format), begin_format,
- cork_line_min, cork_line_max)) {
- cork_init();
- return -1;
- }
- const size_t new_length = strlen(new_format);
- memcpy(msg->message, new_format, new_length);
- memcpy(msg->message + new_length, corked_message.message,
- std::min(strlen(corked_message.message) + 1,
- sizeof(msg->message) - new_length));
- // in case corked_message.message was too long, it'll still be NULL-terminated
- msg->message[sizeof(msg->message) - 1] = '\0';
- cork_init();
-
- if (!write_message(msg, level)) {
- return -1;
- }
-
- return 0;
-}
-
diff --git a/aos/atom_code/logging/atom_logging.h b/aos/atom_code/logging/atom_logging.h
index 6211e22..45f2a07 100644
--- a/aos/atom_code/logging/atom_logging.h
+++ b/aos/atom_code/logging/atom_logging.h
@@ -1,109 +1,30 @@
#ifndef AOS_ATOM_CODE_LOGGING_LOGGING_H_
#define AOS_ATOM_CODE_LOGGING_LOGGING_H_
-// IWYU pragma: private, include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging_impl.h"
-#ifndef AOS_COMMON_LOGGING_LOGGING_H_
-#error This file may only be #included through common/logging/logging.h!!!
-#endif
+namespace aos {
+namespace logging {
+namespace atom {
-#include "aos/aos_core.h"
+// Calls AddImplementation to register the usual atom logging implementation
+// which sends the messages through a queue. This implementation relies on
+// another process(es) to read the log messages that it puts into the queue.
+// It gets called by aos::Init*.
+void Register();
-#ifdef __cplusplus
-extern "C" {
-#endif
+// Fairly simple wrappers around the raw queue calls.
-int log_init(const char *name);
-// WARNING: THIS LEAKS MEMORY AND SHARED MEMORY
-void log_uninit(void);
+// This one never returns NULL if flags contains BLOCK.
+const LogMessage *ReadNext(int flags);
+const LogMessage *ReadNext(int flags, int *index);
+const LogMessage *ReadNext();
+LogMessage *Get();
+void Free(const LogMessage *msg);
+void Write(LogMessage *msg);
-extern log_level log_min;
-
-// The basic structure that goes into the shared memory queue.
-// It is packed so the pid_t at the front is at the same location as
-// the one in log_crio_message.
-typedef struct log_message_t_ {
- pid_t source;
- log_level level;
- char message[LOG_MESSAGE_LEN];
- char name[40];
- struct timespec time;
- uint8_t sequence; // per process
-} __attribute__((packed)) log_message;
-
-#ifdef __cplusplus
-#define LOG_BOOL bool
-#else
-#define LOG_BOOL uint8_t
-#endif
-extern LOG_BOOL log_initted;
-#undef LOG_BOOL
-
-// Unless explicitly stated otherwise, format must always be a string constant
-// and args are printf-style arguments for format.
-// The validitiy of format and args together will be checked at compile time
-// using a gcc function attribute.
-
-// Logs the specified thing.
-#define LOG(level, format, args...) do { \
- if (level >= log_min) { \
- log_do(level, LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " format, ##args); \
- } \
-} while (0)
-// Allows "bottling up" multiple log fragments which can then all be logged in
-// one message with LOG_UNCORK.
-// format does not have to be a constant
-#define LOG_CORK(format, args...) do { \
- log_cork(__LINE__, format, ##args); \
-} while (0)
-// Actually logs all of the saved up log fragments (including format and args on
-// the end).
-#define LOG_UNCORK(level, format, args...) do { \
- log_uncork(__LINE__, level, LOG_SOURCENAME ": %d-%d: ", format, ##args); \
-} while (0)
-// Makes a normal logging call if possible or just prints it out on stderr.
-#define LOG_IFINIT(level, format, args...) do{ \
- if(log_initted) { \
- LOG(level, format, args); \
- } else { \
- fprintf(stderr, "%s-noinit: " format, log_str(level), ##args); \
- } \
-}while(0)
-
-// All functions return 0 for success and - for error.
-
-// Actually implements the basic logging call.
-// Does not check that level is valid.
-// TODO(brians): Fix this so it works with clang.
-int log_do(log_level level, const char *format, ...)
- __attribute__((format(gnu_printf, 2, 3)));
-
-// TODO(brians): Fix this so it works with clang.
-int log_cork(int line, const char *format, ...)
- __attribute__((format(gnu_printf, 2, 3)));
-// Implements the uncork logging call.
-// IMPORTANT: begin_format must have 2 %d formats as its only 2 format specifiers
-// which will get passed the minimum and maximum line numbers that have been
-// corked into this call.
-// TODO(brians): Fix this so it works with clang.
-int log_uncork(int line, log_level level, const char *begin_format,
- const char *format, ...)
- __attribute__((format(gnu_printf, 4, 5)));
-
-const log_message *log_read_next1(int flags);
-const log_message *log_read_next2(int flags, int *index);
-inline const log_message *log_read_next(void) { return log_read_next1(BLOCK); }
-void log_free_message(const log_message *msg);
-
-// The structure that is actually in the shared memory queue.
-union log_queue_message {
- log_message atom;
- log_crio_message crio;
-};
-
-#ifdef __cplusplus
-}
-#endif
+} // namespace atom
+} // namespace logging
+} // namespace aos
#endif
-
diff --git a/aos/atom_code/logging/atom_logging_test.cpp b/aos/atom_code/logging/atom_logging_test.cpp
deleted file mode 100644
index a97d1e9..0000000
--- a/aos/atom_code/logging/atom_logging_test.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-#include <string>
-
-#include "gtest/gtest.h"
-
-#include "aos/aos_core.h"
-#include "aos/atom_code/ipc_lib/sharedmem_test_setup.h"
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/inttypes.h"
-#include "aos/common/time.h"
-
-using ::aos::time::Time;
-using ::testing::AssertionResult;
-using ::testing::AssertionSuccess;
-using ::testing::AssertionFailure;
-
-namespace aos {
-namespace testing {
-
-static const std::string kLoggingName = "LoggingTestName";
-
-class LoggingTest : public SharedMemTestSetup {
- virtual void SetUp() {
- SharedMemTestSetup::SetUp();
- ASSERT_EQ(0, log_init(kLoggingName.c_str()));
- }
- virtual void TearDown() {
- log_uninit();
- SharedMemTestSetup::TearDown();
- }
-
- public:
- AssertionResult WasAnythingLogged() {
- const log_message *msg = log_read_next1(NON_BLOCK);
- if (msg != NULL) {
- char bad_msg[LOG_MESSAGE_LEN];
- memcpy(bad_msg, msg->message, sizeof(bad_msg));
- log_free_message(msg);
- return AssertionSuccess() << "read message '" << bad_msg << "'";
- }
- return AssertionFailure();
- }
- AssertionResult WasLogged(log_level level, const std::string value) {
- const log_message *msg = NULL;
- char bad_msg[LOG_MESSAGE_LEN];
- bad_msg[0] = '\0';
- const pid_t owner = getpid();
- while (true) {
- if (msg != NULL) {
- static_assert(sizeof(bad_msg) == sizeof(msg->message),
- "something is wrong");
- if (bad_msg[0] != '\0') {
- printf("read bad message: %s", bad_msg);
- }
- memcpy(bad_msg, msg->message, sizeof(bad_msg));
- log_free_message(msg);
- msg = NULL;
- }
- msg = log_read_next1(NON_BLOCK);
- if (msg == NULL) {
- return AssertionFailure() << "last message read was '" << bad_msg << "'"
- " instead of '" << value << "'";
- }
- if (msg->source != owner) continue;
- if (msg->level != level) continue;
- if (strcmp(msg->name, kLoggingName.c_str()) != 0) continue;
- if (strcmp(msg->message + strlen(msg->message) - value.length(),
- value.c_str()) != 0) continue;
-
- // if it's gotten this far, then the message is correct
- log_free_message(msg);
- return AssertionSuccess();
- }
- }
-};
-typedef LoggingTest LoggingDeathTest;
-
-// Tests both basic logging functionality and that the test setup works
-// correctly.
-TEST_F(LoggingTest, Basic) {
- EXPECT_FALSE(WasAnythingLogged());
- LOG(INFO, "test log 1\n");
- EXPECT_TRUE(WasLogged(INFO, "test log 1\n"));
- LOG(INFO, "test log 1.5\n");
- // there's a subtle typo on purpose...
- EXPECT_FALSE(WasLogged(INFO, "test log 15\n"));
- LOG(ERROR, "test log 2=%d\n", 55);
- EXPECT_TRUE(WasLogged(ERROR, "test log 2=55\n"));
- LOG(ERROR, "test log 3\n");
- EXPECT_FALSE(WasLogged(WARNING, "test log 3\n"));
- LOG(WARNING, "test log 4\n");
- EXPECT_TRUE(WasAnythingLogged());
-}
-TEST_F(LoggingTest, Cork) {
- static const int begin_line = __LINE__;
- LOG_CORK("first part ");
- LOG_CORK("second part (=%d) ", 19);
- EXPECT_FALSE(WasAnythingLogged());
- LOG_CORK("third part ");
- static const int end_line = __LINE__;
- LOG_UNCORK(WARNING, "last part %d\n", 5);
- char *expected;
- ASSERT_GT(asprintf(&expected, "atom_logging_test.cpp: %d-%d: "
- "first part second part (=19) third part last part 5\n",
- begin_line + 1, end_line + 1), 0);
- EXPECT_TRUE(WasLogged(WARNING, std::string(expected)));
-}
-
-TEST_F(LoggingDeathTest, Fatal) {
- ASSERT_EXIT(LOG(FATAL, "this should crash it\n"),
- ::testing::KilledBySignal(SIGABRT),
- "this should crash it");
-}
-
-TEST_F(LoggingTest, PrintfDirectives) {
- LOG(INFO, "test log %%1 %%d\n");
- EXPECT_TRUE(WasLogged(INFO, "test log %1 %d\n"));
- LOG_DYNAMIC(WARNING, "test log %%2 %%f\n");
- EXPECT_TRUE(WasLogged(WARNING, "test log %2 %f\n"));
- LOG_CORK("log 3 part %%1 %%d ");
- LOG_UNCORK(DEBUG, "log 3 part %%2 %%f\n");
- EXPECT_TRUE(WasLogged(DEBUG, "log 3 part %1 %d log 3 part %2 %f\n"));
-}
-
-// For writing only.
-TEST_F(LoggingTest, Timing) {
- static const long kTimingCycles = 5000;
- Time start = Time::Now();
- for (long i = 0; i < kTimingCycles; ++i) {
- LOG(INFO, "a\n");
- }
- Time elapsed = Time::Now() - start;
-
- printf("short message took %"PRId64" nsec for %ld\n", elapsed.ToNSec(),
- kTimingCycles);
-
- start = Time::Now();
- for (long i = 0; i < kTimingCycles; ++i) {
- LOG(INFO, "something longer than just \"a\" to log to test timing\n");
- }
- elapsed = Time::Now() - start;
- printf("long message took %"PRId64" nsec for %ld\n", elapsed.ToNSec(),
- kTimingCycles);
-}
-
-} // namespace testing
-} // namespace aos
diff --git a/aos/atom_code/logging/logging.gyp b/aos/atom_code/logging/logging.gyp
index 5d29da8..dfb189c 100644
--- a/aos/atom_code/logging/logging.gyp
+++ b/aos/atom_code/logging/logging.gyp
@@ -1,15 +1,4 @@
{
'targets': [
- {
- 'target_name': 'atom_logging_test',
- 'type': 'executable',
- 'sources': [
- 'atom_logging_test.cpp',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- '<(AOS)/build/aos.gyp:libaos',
- ],
- },
],
}
diff --git a/aos/atom_code/output/HTTPServer.cpp b/aos/atom_code/output/HTTPServer.cpp
index 88e3c6f..b0d6b93 100644
--- a/aos/atom_code/output/HTTPServer.cpp
+++ b/aos/atom_code/output/HTTPServer.cpp
@@ -4,6 +4,7 @@
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
+#include <string.h>
#include <memory>
diff --git a/aos/atom_code/queue-tmpl.h b/aos/atom_code/queue-tmpl.h
index 3a8eea8..f9761bd 100644
--- a/aos/atom_code/queue-tmpl.h
+++ b/aos/atom_code/queue-tmpl.h
@@ -179,6 +179,15 @@
}
template <class T>
+void Queue<T>::Clear() {
+ if (queue_ != NULL) {
+ queue_msg_.reset();
+ queue_ = NULL;
+ queue_msg_.set_queue(NULL);
+ }
+}
+
+template <class T>
bool Queue<T>::FetchNext() {
Init();
// TODO(aschuh): Use aos_queue_read_msg_index so that multiple readers
diff --git a/aos/atom_code/starter/starter.cpp b/aos/atom_code/starter/starter.cpp
index 0cfddd5..e47e213 100644
--- a/aos/atom_code/starter/starter.cpp
+++ b/aos/atom_code/starter/starter.cpp
@@ -8,54 +8,56 @@
#include <sys/inotify.h>
#include <sys/stat.h>
-#include "aos/aos_core.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/atom_code/init.h"
void niceexit(int status);
pid_t start(const char *cmd, uint8_t times) {
char *which_cmd, *which_cmd_stm;
if (asprintf(&which_cmd, "which %s", cmd) == -1) {
- LOG_IFINIT(ERROR, "creating \"which %s\" failed with %d: %s\n",
- cmd, errno, strerror(errno));
+ LOG(ERROR, "creating \"which %s\" failed with %d: %s\n",
+ cmd, errno, strerror(errno));
niceexit(EXIT_FAILURE);
}
if (asprintf(&which_cmd_stm, "which %s.stm", cmd) == -1) {
- LOG_IFINIT(ERROR, "creating \"which %s.stm\" failed with %d: %s\n",
- cmd, errno, strerror(errno));
+ LOG(ERROR, "creating \"which %s.stm\" failed with %d: %s\n",
+ cmd, errno, strerror(errno));
niceexit(EXIT_FAILURE);
}
FILE *which = popen(which_cmd, "r");
char exe[CMDLEN + 5], orig_exe[CMDLEN];
size_t ret;
if ((ret = fread(orig_exe, 1, sizeof(orig_exe), which)) == CMDLEN) {
- LOG_IFINIT(ERROR, "`which %s` was too long. not starting '%s'\n", cmd, cmd);
+ LOG(ERROR, "`which %s` was too long. not starting '%s'\n", cmd, cmd);
return 0;
}
orig_exe[ret] = '\0';
if (pclose(which) == -1) {
- LOG_IFINIT(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
+ LOG(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
}
free(which_cmd);
if (strlen(orig_exe) == 0) { // which returned nothing; check if stm exists
- LOG_IFINIT(INFO, "%s didn't exist. trying %s.stm\n", cmd, cmd);
+ LOG(INFO, "%s didn't exist. trying %s.stm\n", cmd, cmd);
FILE *which_stm = popen(which_cmd_stm, "r");
if ((ret = fread(orig_exe, 1, sizeof(orig_exe), which_stm)) == CMDLEN) {
- LOG_IFINIT(ERROR, "`which %s.stm` was too long. not starting %s\n", cmd, cmd);
+ LOG(ERROR, "`which %s.stm` was too long. not starting %s\n", cmd, cmd);
return 0;
}
orig_exe[ret] = '\0';
if (pclose(which) == -1) {
- LOG_IFINIT(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
+ LOG(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
}
}
if (strlen(orig_exe) == 0) {
- LOG_IFINIT(WARNING, "couldn't find file '%s[.stm]'. not going to start it\n",
- cmd);
+ LOG(WARNING, "couldn't find file '%s[.stm]'. not going to start it\n",
+ cmd);
return 0;
}
if (orig_exe[strlen(orig_exe) - 1] != '\n') {
- LOG_IFINIT(WARNING, "no \\n on the end of `which %s[.stm]` output ('%s')\n",
- cmd, orig_exe);
+ LOG(WARNING, "no \\n on the end of `which %s[.stm]` output ('%s')\n",
+ cmd, orig_exe);
} else {
orig_exe[strlen(orig_exe) - 1] = '\0'; // get rid of the \n
}
@@ -65,11 +67,11 @@
struct stat st;
errno = 0;
if (stat(orig_exe, &st) != 0 && errno != ENOENT) {
- LOG_IFINIT(ERROR, "killing everything because stat('%s') failed with %d: %s\n",
- orig_exe, errno, strerror(errno));
+ LOG(ERROR, "killing everything because stat('%s') failed with %d: %s\n",
+ orig_exe, errno, strerror(errno));
niceexit(EXIT_FAILURE);
} else if (errno == ENOENT) {
- LOG_IFINIT(WARNING, "binary '%s' doesn't exist. not starting it\n", orig_exe);
+ LOG(WARNING, "binary '%s' doesn't exist. not starting it\n", orig_exe);
return 0;
}
struct stat st2;
@@ -78,38 +80,38 @@
if (!orig_zero) {
// if it failed and it wasn't because it was missing
if (unlink(exe) != 0 && (errno != EROFS && errno != ENOENT)) {
- LOG_IFINIT(ERROR,
- "killing everything because unlink('%s') failed with %d: %s\n",
- exe, errno, strerror(errno));
+ LOG(ERROR,
+ "killing everything because unlink('%s') failed with %d: %s\n",
+ exe, errno, strerror(errno));
niceexit(EXIT_FAILURE);
}
if (link(orig_exe, exe) != 0) {
- LOG_IFINIT(ERROR,
- "killing everything because link('%s', '%s') failed with %d: %s\n",
- orig_exe, exe, errno, strerror(errno));
+ LOG(ERROR,
+ "killing everything because link('%s', '%s') failed with %d: %s\n",
+ orig_exe, exe, errno, strerror(errno));
niceexit(EXIT_FAILURE);
}
}
if (errno == EEXIST) {
- LOG_IFINIT(INFO, "exe ('%s') already existed\n", exe);
+ LOG(INFO, "exe ('%s') already existed\n", exe);
}
pid_t child;
if ((child = fork()) == 0) {
execlp(exe, orig_exe, static_cast<char *>(NULL));
- LOG_IFINIT(ERROR,
- "killing everything because execlp('%s', '%s', NULL) "
- "failed with %d: %s\n",
- exe, cmd, errno, strerror(errno));
+ LOG(ERROR,
+ "killing everything because execlp('%s', '%s', NULL) "
+ "failed with %d: %s\n",
+ exe, cmd, errno, strerror(errno));
_exit(EXIT_FAILURE); // don't niceexit or anything because this is the child!!
}
if (child == -1) {
- LOG_IFINIT(WARNING, "fork on '%s' failed with %d: %s",
- cmd, errno, strerror(errno));
+ LOG(WARNING, "fork on '%s' failed with %d: %s",
+ cmd, errno, strerror(errno));
if (times < 100) {
return start(cmd, times + 1);
} else {
- LOG_IFINIT(ERROR, "tried to start '%s' too many times. giving up\n", cmd);
+ LOG(ERROR, "tried to start '%s' too many times. giving up\n", cmd);
return 0;
}
} else {
@@ -117,9 +119,9 @@
files[child] = orig_exe;
int ret = inotify_add_watch(notifyfd, orig_exe, IN_ATTRIB | IN_MODIFY);
if (ret < 0) {
- LOG_IFINIT(WARNING, "inotify_add_watch('%s') failed: "
- "not going to watch for changes to it because of %d: %s\n",
- orig_exe, errno, strerror(errno));
+ LOG(WARNING, "inotify_add_watch('%s') failed: "
+ "not going to watch for changes to it because of %d: %s\n",
+ orig_exe, errno, strerror(errno));
} else {
watches[ret] = child;
mtimes[ret] = st2.st_mtime;
@@ -164,6 +166,8 @@
atexit(exit_handler);
+ aos::logging::Init();
+
notifyfd = inotify_init1(IN_NONBLOCK);
pid_t core = start("core", 0);
@@ -330,18 +334,6 @@
niceexit(EXIT_FAILURE);
}
- /*// remove all of the watches assigned to the pid that just died
- for (auto it = watches.begin(); it != watches.end(); ++it) {
- if (it->second == infop.si_pid) {
- watches_to_ignore.insert(it->first);
- }
- }
- for (auto it = watches_to_ignore.begin();
- it != watches_to_ignore.end(); ++it) {
- LOG(DEBUG, "watch id %d was on PID %d\n", *it, infop.si_pid);
- watches.erase(*it);
- }*/
-
start(children[infop.si_pid], 0);
LOG(DEBUG, "erasing %d from children\n", infop.si_pid);
children.erase(infop.si_pid);
diff --git a/aos/atom_code/starter/starter.gyp b/aos/atom_code/starter/starter.gyp
index 44e4b08..cff6d07 100644
--- a/aos/atom_code/starter/starter.gyp
+++ b/aos/atom_code/starter/starter.gyp
@@ -7,7 +7,7 @@
'starter.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
'copies': [
{
diff --git a/aos/atom_code/thread_local.h b/aos/atom_code/thread_local.h
new file mode 100644
index 0000000..9563a87
--- /dev/null
+++ b/aos/atom_code/thread_local.h
@@ -0,0 +1,13 @@
+#ifndef AOS_ATOM_CODE_THREAD_LOCAL_H_
+#define AOS_ATOM_CODE_THREAD_LOCAL_H_
+
+// The storage class to use when declaring thread-local variables. This provides
+// a single place to change it if/when we want to switch to something standard.
+//
+// Example: AOS_THREAD_LOCAL void *bla; // at namespace (aka global) scope
+//
+// C++11 has thread_local, but it's not clear whether Clang supports that as of
+// 12/18/12.
+#define AOS_THREAD_LOCAL __thread
+
+#endif // AOS_ATOM_CODE_THREAD_LOCAL_H_
diff --git a/aos/build/aos.gyp b/aos/build/aos.gyp
index b6bd3ee..0c1506c 100644
--- a/aos/build/aos.gyp
+++ b/aos/build/aos.gyp
@@ -8,17 +8,12 @@
'conditions': [
['OS=="crio"', {
'libaos_source_files': [
- '<!@(find <(AOS)/crio/controls <(AOS)/crio/messages <(AOS)/crio/motor_server <(AOS)/crio/shared_libs -name *.c -or -name *.cpp -or -name *.cc)',
+ '<!@(find <(AOS)/crio/controls <(AOS)/crio/messages <(AOS)/crio/motor_server -name *.c -or -name *.cpp -or -name *.cc)',
'<(AOS)/crio/Talon.cpp',
- '<(AOS)/common/die.cc',
],
}, {
'libaos_source_files': [
- '<(AOS)/atom_code/camera/Buffers.cpp',
'<(AOS)/atom_code/async_action/AsyncAction_real.cpp',
- '<(AOS)/atom_code/init.cc',
- '<(AOS)/atom_code/ipc_lib/mutex.cpp',
- '<(AOS)/common/die.cc',
],
}
],
@@ -28,17 +23,20 @@
{
'target_name': 'logging',
'type': 'static_library',
+ 'sources': [
+ '<(AOS)/common/logging/logging_impl.cc',
+ ],
'conditions': [
['OS=="crio"', {
'sources': [
- '<(AOS)/crio/logging/crio_logging.cpp',
+ '<(AOS)/crio/logging/crio_logging.cc',
],
'dependencies': [
'<(EXTERNALS):WPILib',
]
}, {
'sources': [
- '<(AOS)/atom_code/logging/atom_logging.cpp'
+ '<(AOS)/atom_code/logging/atom_logging.cc',
],
'dependencies': [
'<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
@@ -50,6 +48,9 @@
],
'dependencies': [
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:once',
+ '<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/common.gyp:die',
],
},
{
diff --git a/aos/build/aos_all.gyp b/aos/build/aos_all.gyp
index 68439e3..d1f446a 100644
--- a/aos/build/aos_all.gyp
+++ b/aos/build/aos_all.gyp
@@ -19,7 +19,6 @@
'../crio/crio.gyp:unsafe_queue_test',
'../common/common.gyp:queue_test',
#'../common/messages/messages.gyp:*', # TODO(brians) did this test ever exist?
- '../atom_code/logging/logging.gyp:*',
'../common/common.gyp:die_test',
'../common/util/util.gyp:trapezoid_profile_test',
'Common',
@@ -44,6 +43,7 @@
'<(AOS)/common/common.gyp:time_test',
'<(AOS)/common/common.gyp:mutex_test',
'<(AOS)/common/common.gyp:once_test',
+ '<(AOS)/common/logging/logging.gyp:logging_impl_test',
],
},
],
diff --git a/aos/common/byteorder.h b/aos/common/byteorder.h
index 3444f98..87ef39a 100644
--- a/aos/common/byteorder.h
+++ b/aos/common/byteorder.h
@@ -4,6 +4,7 @@
#ifndef __VXWORKS__
#include <endian.h> // endian(3)
#endif
+#include <string.h>
// Contains functions for converting between host and network byte order for
// things other than 16/32 bit integers (those are provided by byteorder(3)).
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index 967eb51..e4b64b5 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -21,7 +21,10 @@
'queue_testutils.cc',
],
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib'
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ 'once',
+ '<(EXTERNALS):gtest',
],
},
{
@@ -132,12 +135,19 @@
{
'target_name': 'controls',
'type': 'static_library',
- 'sources': [],
+ 'sources': [
+ # 'control_loop/ControlLoop-tmpl.h',
+ ],
'dependencies': [
'<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/build/aos.gyp:logging',
'timing',
],
+ 'export_dependent_settings': [
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/build/aos.gyp:logging',
+ 'timing',
+ ],
},
{
'target_name': 'queue_test',
@@ -208,6 +218,34 @@
],
},
{
+ 'target_name': 'die',
+ 'type': 'static_library',
+ 'sources': [
+ 'die.cc',
+ ],
+ },
+ {
+ 'target_name': 'mutex',
+ 'type': 'static_library',
+ 'conditions': [
+ ['OS=="crio"', {
+ 'sources': [
+ '<(AOS)/crio/shared_libs/mutex.cpp',
+ ],
+ }, {
+ 'sources': [
+ '<(AOS)/atom_code/ipc_lib/mutex.cpp',
+ ],
+ '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': 'mutex_test',
'type': '<(aos_target)',
'sources': [
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 80d592a..a15d909 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -64,8 +64,10 @@
return;
}
}
- position->Print(state, sizeof(state));
- LOG(DEBUG, "position={%s}\n", state);
+ if (position) {
+ position->Print(state, sizeof(state));
+ LOG(DEBUG, "position={%s}\n", state);
+ }
}
bool outputs_enabled = false;
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index b69cf01..ac88c98 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -5,8 +5,8 @@
#include "aos/aos_core.h"
#include "aos/common/control_loop/Timing.h"
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/type_traits.h"
+#include "aos/common/queue.h"
namespace aos {
namespace control_loops {
@@ -49,9 +49,9 @@
public:
// Maximum age of position packets before the loop will be disabled due to
// invalid position data.
- static const int kPositionTimeoutMs = 100;
+ static const int kPositionTimeoutMs = 1000;
// Maximum age of driver station packets before the loop will be disabled.
- static const int kDSPacketTimeoutMs = 100;
+ static const int kDSPacketTimeoutMs = 500;
ControlLoop(T *control_loop) : control_loop_(control_loop) {}
diff --git a/aos/common/control_loop/Timing.cpp b/aos/common/control_loop/Timing.cpp
index 63fda44..6db179c 100644
--- a/aos/common/control_loop/Timing.cpp
+++ b/aos/common/control_loop/Timing.cpp
@@ -1,6 +1,8 @@
-#include "aos/common/logging/logging.h"
#include "aos/common/control_loop/Timing.h"
+#include <string.h>
+
+#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
namespace aos {
diff --git a/aos/common/control_loop/control_loops.q b/aos/common/control_loop/control_loops.q
index 5ba30ec..823005e 100644
--- a/aos/common/control_loop/control_loops.q
+++ b/aos/common/control_loop/control_loops.q
@@ -20,7 +20,7 @@
};
message Output {
- double pwm;
+ double voltage;
};
message Status {
diff --git a/aos/common/inttypes.h b/aos/common/inttypes.h
index e10a33b..d243860 100644
--- a/aos/common/inttypes.h
+++ b/aos/common/inttypes.h
@@ -8,12 +8,15 @@
#ifndef __VXWORKS__
#include <inttypes.h>
#else
+#define PRId64 "lld"
+
// It warns about just "d", but not this, which is kind of weird because
// sizeof(int) == sizeof(long) == sizeof(int32_t) == 4, but oh well.
#define PRId32 "ld"
#define PRIx32 "lx"
-#define PRId64 "lld"
-#define PRIu16 "u"
+#define PRIu32 "lu"
+
+#define PRIu16 "hu"
#endif
#endif // AOS_COMMON_INTTYPES_H_
diff --git a/aos/common/logging/logging.gyp b/aos/common/logging/logging.gyp
new file mode 100644
index 0000000..52d2527
--- /dev/null
+++ b/aos/common/logging/logging.gyp
@@ -0,0 +1,15 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'logging_impl_test',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'logging_impl_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/common/logging/logging.h b/aos/common/logging/logging.h
index e3ff839..392fc5b 100644
--- a/aos/common/logging/logging.h
+++ b/aos/common/logging/logging.h
@@ -1,102 +1,114 @@
#ifndef AOS_COMMON_LOGGING_LOGGING_H_
-// must be kept in sync with crio/logging/crio_logging.h and atom_code/logging/atom_logging.h
#define AOS_COMMON_LOGGING_LOGGING_H_
+// This file contains the logging client interface. It works with both C and C++
+// code.
+
+#include <stdio.h>
#include <stdint.h>
-#include <sys/types.h>
-#include <unistd.h>
-#include <string.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef uint8_t log_level;
+
#define DECL_LEVELS \
DECL_LEVEL(DEBUG, 0); /* stuff that gets printed out every cycle */ \
DECL_LEVEL(INFO, 1); /* things like PosEdge/NegEdge */ \
-/* things that might still work if they happen occasionally but should be watched */ \
+/* things that might still work if they happen occasionally */ \
DECL_LEVEL(WARNING, 2); \
/*-1 so that vxworks macro of same name will have same effect if used*/ \
DECL_LEVEL(ERROR, -1); /* errors */ \
-DECL_LEVEL(FATAL, 4); /* serious errors. the logging code will terminate the process/task */ \
+/* serious errors. the logging code will terminate the process/task */ \
+DECL_LEVEL(FATAL, 4); \
DECL_LEVEL(LOG_UNKNOWN, 5); /* unknown logging level */
-#define DECL_LEVEL(name, value) extern const log_level name;
+#define DECL_LEVEL(name, value) static const log_level name = value;
#undef ERROR
-DECL_LEVELS
+DECL_LEVELS;
#undef DECL_LEVEL
#define STRINGIFY(x) TO_STRING(x)
#define TO_STRING(x) #x
//not static const size_t for c code
-#define LOG_MESSAGE_LEN 300
+#define LOG_MESSAGE_LEN 500
+
+#ifdef __VXWORKS__
+// We're using ancient glibc, so sticking to just what the syscall can handle is
+// probably safer.
+#define LOG_PRINTF_FORMAT_TYPE printf
+#else
+#define LOG_PRINTF_FORMAT_TYPE gnu_printf
+#endif
+#ifdef __cplusplus
+extern "C" {
+#endif
+// Actually implements the basic logging call.
+// Does not check that level is valid.
+void log_do(log_level level, const char *format, ...)
+ __attribute__((format(LOG_PRINTF_FORMAT_TYPE, 2, 3)));
+
+void log_cork(int line, const char *function, const char *format, ...)
+ __attribute__((format(LOG_PRINTF_FORMAT_TYPE, 3, 4)));
+// Implements the uncork logging call.
+void log_uncork(int line, const char *function, log_level level,
+ const char *file, const char *format, ...)
+ __attribute__((format(LOG_PRINTF_FORMAT_TYPE, 5, 6)));
+#ifdef __cplusplus
+}
+#endif
+
+// A magical static const char[] or string literal that communicates the name
+// of the enclosing function.
+// It's currently using __PRETTY_FUNCTION__ because both GCC and Clang support
+// that and it gives nicer results in C++ than the standard __func__ (which
+// would also work).
+#define LOG_CURRENT_FUNCTION __PRETTY_FUNCTION__
+
+// The basic logging call.
+#define LOG(level, format, args...) do {\
+ log_do(level, LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": %s: " format, \
+ LOG_CURRENT_FUNCTION, ##args); \
+ /* so that GCC knows that it won't return */ \
+ if (level == FATAL) { \
+ fprintf(stderr, "log_do(FATAL) fell through!!!!!\n"); \
+ printf("see stderr\n"); \
+ abort(); \
+ } \
+} while (0)
// Allows format to not be a string constant.
-#define LOG_DYNAMIC(level, format, args...) do{ \
+#define LOG_DYNAMIC(level, format, args...) do { \
static char log_buf[LOG_MESSAGE_LEN]; \
int ret = snprintf(log_buf, sizeof(log_buf), format, ##args); \
- if(ret < 0 || (uintmax_t)ret >= LOG_MESSAGE_LEN){ \
- LOG(WARNING, "next message was too long so not subbing in args\n"); \
+ if (ret < 0 || (uintmax_t)ret >= LOG_MESSAGE_LEN) { \
+ LOG(ERROR, "next message was too long so not subbing in args\n"); \
LOG(level, "%s", format); \
}else{ \
LOG(level, "%s", log_buf); \
} \
-}while(0)
+} while (0)
-// The struct that the crio-side code uses for making logging calls.
-// Packed so it's the same on the atom and the crio.
-typedef struct {
- // pid_t here at the front like in log_message
- pid_t identifier; // must ALWAYS be -1 to identify that this is a crio log message
- log_level level;
- // still has to fit in LOG_MESSAGE_LEN on the atom side
- char message[LOG_MESSAGE_LEN - 50];
- double time;
- uint8_t sequence;
-} __attribute__((packed)) log_crio_message;
+// Allows "bottling up" multiple log fragments which can then all be logged in
+// one message with LOG_UNCORK.
+// Calls from a given thread/task will be grouped together.
+#define LOG_CORK(format, args...) do { \
+ log_cork(__LINE__, LOG_CURRENT_FUNCTION, format, ##args); \
+} while (0)
+// Actually logs all of the saved up log fragments (including format and args on
+// the end).
+#define LOG_UNCORK(level, format, args...) do { \
+ log_uncork(__LINE__, LOG_CURRENT_FUNCTION, level, LOG_SOURCENAME, \
+ format, ##args); \
+} while (0)
-#ifdef __cplusplus
-// Just sticks the message into the shared memory queue.
-int log_crio_message_send(log_crio_message &to_send);
-// Returns left > right. LOG_UNKNOWN is most important.
-static inline bool log_gt_important(log_level left, log_level right) {
- log_level l = left, r = right;
- if (l == ERROR) l = 3;
- if (r == ERROR) r = 3;
- return left > right;
-}
-#endif
-
-// Returns a string representing level or "unknown".
-static inline const char *log_str(log_level level) {
- // c doesn't really have const variables so they don't work in case statements
- if (level == DEBUG) return "DEBUG";
- if (level == INFO) return "INFO";
- if (level == WARNING) return "WARNING";
- if (level == ERROR) return "ERROR";
- if (level == FATAL) return "FATAL";
- return "unknown";
-}
-// Returns the log level represented by str or LOG_UNKNOWN.
-static inline log_level str_log(const char *str) {
- if (!strcmp(str, "DEBUG")) return DEBUG;
- if (!strcmp(str, "INFO")) return INFO;
- if (!strcmp(str, "WARNING")) return WARNING;
- if (!strcmp(str, "ERROR")) return ERROR;
- if (!strcmp(str, "FATAL")) return FATAL;
- return LOG_UNKNOWN;
-}
+// TODO(brians) add CHECK macros like glog
+// (<http://google-glog.googlecode.com/svn/trunk/doc/glog.html>)
+// and replace assert with one
#ifdef __cplusplus
}
#endif
-#ifdef __unix
-#include "aos/atom_code/logging/atom_logging.h" // IWYU pragma: export
-#else
-#include "aos/crio/logging/crio_logging.h" // IWYU pragma: export
#endif
-
-#endif
-
diff --git a/aos/common/logging/logging_impl.cc b/aos/common/logging/logging_impl.cc
new file mode 100644
index 0000000..64696dc
--- /dev/null
+++ b/aos/common/logging/logging_impl.cc
@@ -0,0 +1,243 @@
+#include "aos/common/logging/logging_impl.h"
+
+#include <assert.h>
+
+#include "aos/common/die.h"
+#include "aos/common/time.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/once.h"
+
+namespace aos {
+namespace logging {
+namespace {
+
+using internal::Context;
+
+LogImplementation *global_top_implementation(NULL);
+// Just going to use a mutex instead of getting fancy because speed doesn't
+// really matter when accessing global_top_implementation.
+Mutex global_top_implementation_mutex;
+LogImplementation *get_global_top_implementation() {
+ MutexLocker locker(&global_top_implementation_mutex);
+ return global_top_implementation;
+}
+
+// The root LogImplementation. It only logs to stderr/stdout.
+// Some of the things specified in the LogImplementation documentation doesn't
+// apply here (mostly the parts about being able to use LOG) because this is the
+// root one.
+class RootLogImplementation : public LogImplementation {
+ virtual void set_next(LogImplementation *) {
+ LOG(FATAL, "can't have a next logger from here\n");
+ }
+
+ virtual void DoLog(log_level level, const char *format, va_list ap) {
+ LogMessage message;
+ internal::FillInMessage(level, format, ap, &message);
+ internal::PrintMessage(stderr, message);
+ fputs("root logger got used, see stderr for message\n", stdout);
+ }
+};
+
+void SetGlobalImplementation(LogImplementation *implementation) {
+ Context *context = Context::Get();
+
+ context->implementation = implementation;
+ {
+ MutexLocker locker(&global_top_implementation_mutex);
+ global_top_implementation = implementation;
+ }
+}
+
+// Prints format (with ap) into output and correctly deals with the result
+// being too long etc.
+void ExecuteFormat(char *output, size_t output_size,
+ const char *format, va_list ap) {
+ static const char *continued = "...\n";
+ const size_t size = output_size - strlen(continued);
+ const int ret = vsnprintf(output, size, format, ap);
+ if (ret < 0) {
+ LOG(FATAL, "vsnprintf(%p, %zd, %s, %p) failed with %d (%s)\n",
+ output, size, format, ap, errno, strerror(errno));
+ } else if (static_cast<uintmax_t>(ret) >= static_cast<uintmax_t>(size)) {
+ // Overwrite the '\0' at the end of the existing data and
+ // copy in the one on the end of continued.
+ memcpy(&output[size - 1], continued, strlen(continued) + 1);
+ }
+}
+
+void NewContext() {
+ Context::Delete();
+}
+
+void *DoInit() {
+ SetGlobalImplementation(new RootLogImplementation());
+
+#ifndef __VXWORKS__
+ if (pthread_atfork(NULL /*prepare*/, NULL /*parent*/,
+ NewContext /*child*/) != 0) {
+ LOG(FATAL, "pthread_atfork(NULL, NULL, %p) failed\n",
+ NewContext);
+ }
+#endif
+
+ return NULL;
+}
+
+} // namespace
+namespace internal {
+
+Context::Context()
+ : implementation(get_global_top_implementation()),
+ sequence(0) {
+ cork_data.Reset();
+}
+
+void FillInMessage(log_level level, const char *format, va_list ap,
+ LogMessage *message) {
+ Context *context = Context::Get();
+
+ ExecuteFormat(message->message, sizeof(message->message), format, ap);
+
+ message->level = level;
+ message->source = context->source;
+ memcpy(message->name, context->name.c_str(), context->name.size() + 1);
+
+ time::Time now = time::Time::Now();
+ message->seconds = now.sec();
+ message->nseconds = now.nsec();
+
+ message->sequence = context->sequence++;
+}
+
+void PrintMessage(FILE *output, const LogMessage &message) {
+ fprintf(output, "%s(%"PRId32")(%05"PRIu16"): %s at"
+ " %010"PRId32".%09"PRId32"s: %s",
+ message.name, message.source, message.sequence,
+ log_str(message.level), message.seconds, message.nseconds,
+ message.message);
+}
+
+} // namespace internal
+
+void LogImplementation::DoVLog(log_level level, const char *format, va_list ap,
+ int levels) {
+ Context *context = Context::Get();
+
+ LogImplementation *top_implementation = context->implementation;
+ LogImplementation *new_implementation = top_implementation;
+ LogImplementation *implementation = NULL;
+ assert(levels >= 1);
+ for (int i = 0; i < levels; ++i) {
+ implementation = new_implementation;
+ if (new_implementation == NULL) {
+ Die("no logging implementation to use\n");
+ }
+ new_implementation = new_implementation->next();
+ }
+ context->implementation = new_implementation;
+ implementation->DoLog(level, format, ap);
+ context->implementation = top_implementation;
+
+ if (level == FATAL) {
+ VDie(format, ap);
+ }
+}
+
+void VLog(log_level level, const char *format, va_list ap) {
+ LogImplementation::DoVLog(level, format, ap, 1);
+}
+
+void VCork(int line, const char *function, const char *format, va_list ap) {
+ Context *context = Context::Get();
+
+ const size_t message_length = strlen(context->cork_data.message);
+ if (line > context->cork_data.line_max) context->cork_data.line_max = line;
+ if (line < context->cork_data.line_min) context->cork_data.line_min = line;
+
+ if (context->cork_data.function == NULL) {
+ context->cork_data.function = function;
+ } else {
+ if (strcmp(context->cork_data.function, function) != 0) {
+ LOG(FATAL, "started corking data in function %s but then moved to %s\n",
+ context->cork_data.function, function);
+ }
+ }
+
+ ExecuteFormat(context->cork_data.message + message_length,
+ sizeof(context->cork_data.message) - message_length,
+ format, ap);
+}
+
+void VUnCork(int line, const char *function, log_level level, const char *file,
+ const char *format, va_list ap) {
+ Context *context = Context::Get();
+
+ VCork(line, function, format, ap);
+
+ log_do(level, "%s: %d-%d: %s: %s", file,
+ context->cork_data.line_min, context->cork_data.line_max, function,
+ context->cork_data.message);
+
+ context->cork_data.Reset();
+}
+
+void LogNext(log_level level, const char *format, ...) {
+ va_list ap;
+ va_start(ap, format);
+ LogImplementation::DoVLog(level, format, ap, 2);
+ va_end(ap);
+}
+
+void AddImplementation(LogImplementation *implementation) {
+ Context *context = Context::Get();
+
+ if (implementation->next() != NULL) {
+ LOG(FATAL, "%p already has a next implementation, but it's not"
+ " being used yet\n", implementation);
+ }
+
+ LogImplementation *old = context->implementation;
+ if (old != NULL) {
+ implementation->set_next(old);
+ }
+ SetGlobalImplementation(implementation);
+}
+
+void Init() {
+ static Once<void> once(DoInit);
+ once.Get();
+}
+
+void Load() {
+ Context::Get();
+}
+
+void Cleanup() {
+ Context::Delete();
+}
+
+} // namespace logging
+} // namespace aos
+
+void log_do(log_level level, const char *format, ...) {
+ va_list ap;
+ va_start(ap, format);
+ aos::logging::VLog(level, format, ap);
+ va_end(ap);
+}
+
+void log_cork(int line, const char *function, const char *format, ...) {
+ va_list ap;
+ va_start(ap, format);
+ aos::logging::VCork(line, function, format, ap);
+ va_end(ap);
+}
+
+void log_uncork(int line, const char *function, log_level level,
+ const char *file, const char *format, ...) {
+ va_list ap;
+ va_start(ap, format);
+ aos::logging::VUnCork(line, function, level, file, format, ap);
+ va_end(ap);
+}
diff --git a/aos/common/logging/logging_impl.h b/aos/common/logging/logging_impl.h
new file mode 100644
index 0000000..1416fe1
--- /dev/null
+++ b/aos/common/logging/logging_impl.h
@@ -0,0 +1,206 @@
+#ifndef AOS_COMMON_LOGGING_LOGGING_IMPL_H_
+#define AOS_COMMON_LOGGING_LOGGING_IMPL_H_
+
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <limits.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <string>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/type_traits.h"
+#include "aos/common/mutex.h"
+
+// This file has all of the logging implementation. It can't be #included by C
+// code like logging.h can.
+// It is useful for the rest of the logging implementation and other C++ code
+// that needs to do special things with logging.
+
+namespace aos {
+namespace logging {
+
+// Unless explicitly stated otherwise, format must always be a string constant,
+// args are printf-style arguments for format, and ap is a va_list of args.
+// The validity of format and args together will be checked at compile time
+// using a gcc function attribute.
+
+// The struct that the code uses for making logging calls.
+// Packed so that it ends up the same under both linux and vxworks.
+struct __attribute__((packed)) LogMessage {
+#ifdef __VXWORKS__
+ static_assert(sizeof(pid_t) == sizeof(int),
+ "we use task IDs (aka ints) and pid_t interchangeably");
+#endif
+ // Actually the task ID (aka a pointer to the TCB) on the cRIO.
+ pid_t source;
+ static_assert(sizeof(source) == 4, "that's how they get printed");
+ // Per task/thread.
+ uint16_t sequence;
+ log_level level;
+ int32_t seconds, nseconds;
+ char name[100];
+ char message[LOG_MESSAGE_LEN];
+};
+static_assert(shm_ok<LogMessage>::value, "it's going in a queue");
+
+// Returns left > right. LOG_UNKNOWN is most important.
+static inline bool log_gt_important(log_level left, log_level right) {
+ if (left == ERROR) left = 3;
+ if (right == ERROR) right = 3;
+ return left > right;
+}
+
+// Returns a string representing level or "unknown".
+static inline const char *log_str(log_level level) {
+#define DECL_LEVEL(name, value) if (level == name) return #name;
+ DECL_LEVELS;
+#undef DECL_LEVEL
+ return "unknown";
+}
+// Returns the log level represented by str or LOG_UNKNOWN.
+static inline log_level str_log(const char *str) {
+#define DECL_LEVEL(name, value) if (!strcmp(str, #name)) return name;
+ DECL_LEVELS;
+#undef DECL_LEVEL
+ return LOG_UNKNOWN;
+}
+
+// Takes a message and logs it. It will set everything up and then call DoLog
+// for the current LogImplementation.
+void VLog(log_level level, const char *format, va_list ap);
+// Adds to the saved up message.
+void VCork(int line, const char *format, va_list ap);
+// Actually logs the saved up message.
+void VUnCork(int line, log_level level, const char *file,
+ const char *format, va_list ap);
+
+// Will call VLog with the given arguments for the next logger in the chain.
+void LogNext(log_level level, const char *format, ...)
+ __attribute__((format(LOG_PRINTF_FORMAT_TYPE, 2, 3)));
+
+// Represents a system that can actually take log messages and do something
+// useful with them.
+// All of the code (transitively too!) in the DoLog here can make
+// normal LOG and LOG_DYNAMIC calls but can NOT call LOG_CORK/LOG_UNCORK. These
+// calls will not result in DoLog recursing. However, implementations must be
+// safe to call from multiple threads/tasks at the same time. Also, any other
+// overriden methods may end up logging through a given implementation's DoLog.
+class LogImplementation {
+ public:
+ LogImplementation() : next_(NULL) {}
+
+ // The one that this one's implementation logs to.
+ // NULL means that there is no next one.
+ LogImplementation *next() { return next_; }
+ // Virtual in case a subclass wants to perform checks. There will be a valid
+ // logger other than this one available while this is called.
+ virtual void set_next(LogImplementation *next) { next_ = next; }
+
+ private:
+ // Actually logs the given message. Implementations should somehow create a
+ // LogMessage and then call internal::FillInMessage.
+ virtual void DoLog(log_level level, const char *format, va_list ap) = 0;
+
+ // Function of this class so that it can access DoLog.
+ // Levels is how many LogImplementations to not use off the stack.
+ static void DoVLog(log_level, const char *format, va_list ap, int levels);
+ // Friends so that they can access DoVLog.
+ friend void VLog(log_level, const char *, va_list);
+ friend void LogNext(log_level, const char *, ...);
+
+ LogImplementation *next_;
+};
+
+// Adds another implementation to the stack of implementations in this
+// task/thread.
+// Any tasks/threads created after this call will also use this implementation.
+// The cutoff is when the state in a given task/thread is created (either lazily
+// when needed or by calling Load()).
+// The logging system takes ownership of implementation. It will delete it if
+// necessary, so it must be created with new.
+void AddImplementation(LogImplementation *implementation);
+
+// Must be called at least once per process/load before anything else is
+// called. This function is safe to call multiple times from multiple
+// tasks/threads.
+void Init();
+
+// Forces all of the state that is usually lazily created when first needed to
+// be created when called. Cleanup() will delete it.
+void Load();
+// Resets all information in this task/thread to its initial state.
+// NOTE: This is not the opposite of Init(). The state that this deletes is
+// lazily created when needed. It is actually the opposite of Load().
+void Cleanup();
+
+// This is where all of the code that is only used by actual LogImplementations
+// goes.
+namespace internal {
+
+// An separate instance of this class is accessible from each task/thread.
+// NOTE: It will get deleted in the child of a fork.
+struct Context {
+ Context();
+
+ // Gets the Context object for this task/thread. Will create one the first
+ // time it is called.
+ //
+ // The implementation for each platform will lazily instantiate a new instance
+ // and then initialize name the first time.
+ // IMPORTANT: The implementation of this can not use logging.
+ static Context *Get();
+ // Deletes the Context object for this task/thread so that the next Get() is
+ // called it will create a new one.
+ // It is valid to call this when Get() has never been called.
+ static void Delete();
+
+ // Which one to log to right now.
+ // Will be NULL if there is no logging implementation to use right now.
+ LogImplementation *implementation;
+
+ // A name representing this task/(process and thread).
+ // strlen(name.c_str()) must be <= sizeof(LogMessage::name).
+ ::std::string name;
+
+ // What to assign LogMessage::source to in this task/thread.
+ pid_t source;
+
+ // The sequence value to send out with the next message.
+ uint16_t sequence;
+
+ // Contains all of the information related to implementing LOG_CORK and
+ // LOG_UNCORK.
+ struct {
+ char message[LOG_MESSAGE_LEN];
+ int line_min, line_max;
+ // Sets the data up to record a new series of corked logs.
+ void Reset() {
+ message[0] = '\0'; // make strlen of it 0
+ line_min = INT_MAX;
+ line_max = -1;
+ function = NULL;
+ }
+ // The function that the calls are in.
+ // REMEMBER: While the compiler/linker will probably optimize all of the
+ // identical strings to point to the same data, it might not, so using == to
+ // compare this with another value is a bad idea.
+ const char *function;
+ } cork_data;
+};
+
+// Fills in *message according to the given inputs. Used for implementing
+// LogImplementation::DoLog.
+void FillInMessage(log_level level, const char *format, va_list ap,
+ LogMessage *message);
+
+// Prints message to output.
+void PrintMessage(FILE *output, const LogMessage &message);
+
+} // namespace internal
+} // namespace logging
+} // namespace aos
+
+#endif // AOS_COMMON_LOGGING_LOGGING_IMPL_H_
diff --git a/aos/common/logging/logging_impl_test.cc b/aos/common/logging/logging_impl_test.cc
new file mode 100644
index 0000000..f64f3d6
--- /dev/null
+++ b/aos/common/logging/logging_impl_test.cc
@@ -0,0 +1,177 @@
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "aos/common/logging/logging_impl.h"
+#include "aos/common/time.h"
+#include "aos/common/die.h"
+#include "aos/common/inttypes.h"
+
+using ::testing::AssertionResult;
+using ::testing::AssertionSuccess;
+using ::testing::AssertionFailure;
+
+namespace aos {
+namespace logging {
+namespace testing {
+
+class TestLogImplementation : public LogImplementation {
+ virtual void DoLog(log_level level, const char *format, va_list ap) {
+ internal::FillInMessage(level, format, ap, &message_);
+
+ used_ = true;
+ }
+
+ LogMessage message_;
+
+ public:
+ const LogMessage &message() { return message_; }
+ bool used() { return used_; }
+ void reset_used() { used_ = false; }
+
+ TestLogImplementation() : used_(false) {}
+
+ bool used_;
+};
+class LoggingTest : public ::testing::Test {
+ protected:
+ AssertionResult WasAnythingLogged() {
+ if (log_implementation->used()) {
+ return AssertionSuccess() << "read message '" <<
+ log_implementation->message().message << "'";
+ }
+ return AssertionFailure();
+ }
+ AssertionResult WasLogged(log_level level, const std::string message) {
+ if (!log_implementation->used()) {
+ return AssertionFailure() << "nothing was logged";
+ }
+ if (log_implementation->message().level != level) {
+ return AssertionFailure() << "a message with level " <<
+ log_str(log_implementation->message().level) <<
+ " was logged instead of " << log_str(level);
+ }
+ internal::Context *context = internal::Context::Get();
+ if (log_implementation->message().source != context->source) {
+ Die("got a message from %"PRIu32", but we're %"PRIu32"\n",
+ static_cast<uint32_t>(log_implementation->message().source),
+ static_cast<uint32_t>(context->source));
+ }
+ if (strcmp(log_implementation->message().name,
+ context->name.c_str()) != 0) {
+ Die("got a message from %s, but we're %s\n",
+ log_implementation->message().name, context->name.c_str());
+ }
+ if (strstr(log_implementation->message().message, message.c_str())
+ == NULL) {
+ return AssertionFailure() << "got a message of '" <<
+ log_implementation->message().message <<
+ "' but expected it to contain '" << message << "'";
+ }
+
+ return AssertionSuccess() << log_implementation->message().message;
+ }
+
+ private:
+ virtual void SetUp() {
+ static bool first = true;
+ if (first) {
+ first = false;
+
+ Init();
+ // We'll end up with several of them stacked up, but it really doesn't
+ // matter.
+ AddImplementation(log_implementation = new TestLogImplementation());
+ }
+
+ log_implementation->reset_used();
+ }
+ virtual void TearDown() {
+ Cleanup();
+ }
+
+ static TestLogImplementation *log_implementation;
+};
+TestLogImplementation *LoggingTest::log_implementation(NULL);
+typedef LoggingTest LoggingDeathTest;
+
+// Tests both basic logging functionality and that the test setup works
+// correctly.
+TEST_F(LoggingTest, Basic) {
+ EXPECT_FALSE(WasAnythingLogged());
+ LOG(INFO, "test log 1\n");
+ EXPECT_TRUE(WasLogged(INFO, "test log 1\n"));
+ LOG(INFO, "test log 1.5\n");
+ // there's a subtle typo on purpose...
+ EXPECT_FALSE(WasLogged(INFO, "test log 15\n"));
+ LOG(ERROR, "test log 2=%d\n", 55);
+ EXPECT_TRUE(WasLogged(ERROR, "test log 2=55\n"));
+ LOG(ERROR, "test log 3\n");
+ EXPECT_FALSE(WasLogged(WARNING, "test log 3\n"));
+ LOG(WARNING, "test log 4\n");
+ EXPECT_TRUE(WasAnythingLogged());
+}
+TEST_F(LoggingTest, Cork) {
+ static const int begin_line = __LINE__;
+ LOG_CORK("first part ");
+ LOG_CORK("second part (=%d) ", 19);
+ EXPECT_FALSE(WasAnythingLogged());
+ LOG_CORK("third part ");
+ static const int end_line = __LINE__;
+ LOG_UNCORK(WARNING, "last part %d\n", 5);
+ std::stringstream expected;
+ expected << "logging_impl_test.cc: ";
+ expected << (begin_line + 1);
+ expected << "-";
+ expected << (end_line + 1);
+ expected << ": ";
+ expected << __PRETTY_FUNCTION__;
+ expected << ": first part second part (=19) third part last part 5\n";
+ EXPECT_TRUE(WasLogged(WARNING, expected.str()));
+}
+
+#ifndef __VXWORKS__
+TEST_F(LoggingDeathTest, Fatal) {
+ ASSERT_EXIT(LOG(FATAL, "this should crash it\n"),
+ ::testing::KilledBySignal(SIGABRT),
+ "this should crash it");
+}
+#endif
+
+TEST_F(LoggingTest, PrintfDirectives) {
+ LOG(INFO, "test log %%1 %%d\n");
+ EXPECT_TRUE(WasLogged(INFO, "test log %1 %d\n"));
+ LOG_DYNAMIC(WARNING, "test log %%2 %%f\n");
+ EXPECT_TRUE(WasLogged(WARNING, "test log %2 %f\n"));
+ LOG_CORK("log 3 part %%1 %%d ");
+ LOG_UNCORK(DEBUG, "log 3 part %%2 %%f\n");
+ EXPECT_TRUE(WasLogged(DEBUG, "log 3 part %1 %d log 3 part %2 %f\n"));
+}
+
+TEST_F(LoggingTest, Timing) {
+ // For writing only.
+ //static const long kTimingCycles = 5000000;
+ static const long kTimingCycles = 5000;
+
+ time::Time start = time::Time::Now();
+ for (long i = 0; i < kTimingCycles; ++i) {
+ LOG(INFO, "a\n");
+ }
+ time::Time end = time::Time::Now();
+ time::Time diff = end - start;
+ printf("short message took %lld nsec for %ld\n",
+ diff.ToNSec(), kTimingCycles);
+
+ start = time::Time::Now();
+ for (long i = 0; i < kTimingCycles; ++i) {
+ LOG(INFO, "something longer than just \"a\" to log to test timing\n");
+ }
+ end = time::Time::Now();
+ diff = end - start;
+ printf("long message took %lld nsec for %ld\n",
+ diff.ToNSec(), kTimingCycles);
+}
+
+} // namespace testing
+} // namespace logging
+} // namespace aos
diff --git a/aos/common/messages/QueueHolder.h b/aos/common/messages/QueueHolder.h
index 6cf2835..2b7c0d7 100644
--- a/aos/common/messages/QueueHolder.h
+++ b/aos/common/messages/QueueHolder.h
@@ -2,6 +2,7 @@
#define __AOS_MESSAGES_QUEUE_HOLDER_H_
#include <stddef.h>
+#include <string.h>
#include <algorithm>
diff --git a/aos/common/mutex.h b/aos/common/mutex.h
index c1d6f95..035889b 100644
--- a/aos/common/mutex.h
+++ b/aos/common/mutex.h
@@ -5,9 +5,9 @@
#include <semLib.h>
#endif
-#include "aos/aos_core.h"
#include "aos/common/macros.h"
#include "aos/common/type_traits.h"
+#include "aos/atom_code/ipc_lib/aos_sync.h"
namespace aos {
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 7b38e67..2ae2f0a 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -202,6 +202,10 @@
// take a different amount of time the first cycle.
void Init();
+ // Removes all internal references to shared memory so shared memory can be
+ // restarted safely. This should only be used in testing.
+ void Clear();
+
// Fetches the next message from the queue.
// Returns true if there was a new message available and we successfully
// fetched it. This removes the message from the queue for all readers.
diff --git a/aos/common/queue_testutils.cc b/aos/common/queue_testutils.cc
index 6cce012..7db5a6e 100644
--- a/aos/common/queue_testutils.cc
+++ b/aos/common/queue_testutils.cc
@@ -1,9 +1,93 @@
#include "aos/common/queue_testutils.h"
+
+#include <string.h>
+
+#include "gtest/gtest.h"
+
#include "aos/common/queue.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/common/once.h"
+
+using ::aos::logging::LogMessage;
namespace aos {
namespace common {
namespace testing {
+namespace {
+
+class TestLogImplementation : public logging::LogImplementation {
+ public:
+ const ::std::vector<LogMessage> &messages() { return messages_; }
+
+ // This class has to be a singleton so that everybody can get access to the
+ // same instance to read out the messages etc.
+ static TestLogImplementation *GetInstance() {
+ static Once<TestLogImplementation> once(CreateInstance);
+ return once.Get();
+ }
+
+ // Clears out all of the messages already recorded.
+ void ClearMessages() {
+ messages_.clear();
+ }
+
+ // Prints out all of the messages (like when a test fails).
+ void PrintAllMessages() {
+ for (auto it = messages_.begin(); it != messages_.end(); ++it) {
+ logging::internal::PrintMessage(stdout, *it);
+ }
+ }
+
+ private:
+ TestLogImplementation() {}
+
+ static TestLogImplementation *CreateInstance() {
+ return new TestLogImplementation();
+ }
+
+ virtual void DoLog(log_level level, const char *format, va_list ap) {
+ LogMessage message;
+
+ logging::internal::FillInMessage(level, format, ap, &message);
+
+ if (!logging::log_gt_important(WARNING, level)) {
+ logging::internal::PrintMessage(stdout, message);
+ }
+
+ messages_.push_back(message);
+ }
+
+ ::std::vector<LogMessage> messages_;
+};
+
+class MyTestEventListener : public ::testing::EmptyTestEventListener {
+ virtual void OnTestStart(const ::testing::TestInfo &/*test_info*/) {
+ TestLogImplementation::GetInstance()->ClearMessages();
+ }
+ virtual void OnTestEnd(const ::testing::TestInfo &test_info) {
+ if (test_info.result()->Failed()) {
+ printf("Test %s failed. Printing out all log messages.\n",
+ test_info.name());
+ fputs("\tThis will include already printed WARNING and up messages.\n",
+ stdout);
+ TestLogImplementation::GetInstance()->PrintAllMessages();
+ }
+ }
+};
+
+void *DoEnableTestLogging() {
+ logging::Init();
+ logging::AddImplementation(TestLogImplementation::GetInstance());
+
+ ::testing::UnitTest::GetInstance()->listeners().Append(
+ new MyTestEventListener());
+
+ return NULL;
+}
+
+Once<void> enable_test_logging_once(DoEnableTestLogging);
+
+} // namespace
GlobalCoreInstance::GlobalCoreInstance() {
const size_t kCoreSize = 0x100000;
@@ -14,6 +98,8 @@
memset(memory, 0, kCoreSize);
assert(aos_core_use_address_as_shared_mem(memory, kCoreSize) == 0);
+
+ EnableTestLogging();
}
GlobalCoreInstance::~GlobalCoreInstance() {
@@ -21,6 +107,10 @@
global_core = NULL;
}
+void EnableTestLogging() {
+ enable_test_logging_once.Get();
+}
+
} // namespace testing
} // namespace common
} // namespace aos
diff --git a/aos/common/queue_testutils.h b/aos/common/queue_testutils.h
index 81f1efb..aabdd2d 100644
--- a/aos/common/queue_testutils.h
+++ b/aos/common/queue_testutils.h
@@ -13,6 +13,13 @@
struct aos_core global_core_data_;
};
+// Enables the logging framework for use during a gtest test.
+// It will print out all WARNING and above messages all of the time. It will
+// also print out all log messages when a test fails.
+// This function only needs to be called once in each process (after gtest is
+// initialized), however it can be called more than that.
+void EnableTestLogging();
+
} // namespace testing
} // namespace common
} // namespace aos
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 1d97b80..995011a 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -3,24 +3,71 @@
#ifdef __VXWORKS__
#include <taskLib.h>
#endif
+#include <errno.h>
+#include <string.h>
#include "aos/common/logging/logging.h"
#include "aos/common/inttypes.h"
+#include "aos/common/mutex.h"
namespace aos {
namespace time {
-Time Time::Now(clockid_t clock) {
- timespec temp;
- if (clock_gettime(clock, &temp) != 0) {
- // TODO(aschuh): There needs to be a pluggable low level logging interface
- // so we can break this dependency loop. This also would help during
- // startup.
- LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
- static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
- }
- return Time(temp);
+// State required to enable and use mock time.
+namespace {
+// True if mock time is enabled.
+// This does not need to be checked with the mutex held because setting time to
+// be enabled or disabled is atomic, and all future operations are atomic
+// anyways. If there is a race condition setting or clearing whether time is
+// enabled or not, it will still be a race condition if current_mock_time is
+// also set atomically with enabled.
+bool mock_time_enabled = false;
+// Mutex to make time reads and writes thread safe.
+Mutex time_mutex;
+// Current time when time is mocked.
+Time current_mock_time(0, 0);
+
+// TODO(aschuh): This doesn't include SleepFor and SleepUntil.
+// TODO(aschuh): Create a clock source object and change the default?
+// That would let me create a MockTime clock source.
}
+
+void Time::EnableMockTime(const Time now) {
+ mock_time_enabled = true;
+ MutexLocker time_mutex_locker(&time_mutex);
+ current_mock_time = now;
+}
+
+void Time::DisableMockTime() {
+ MutexLocker time_mutex_locker(&time_mutex);
+ mock_time_enabled = false;
+}
+
+void Time::SetMockTime(const Time now) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ if (!mock_time_enabled) {
+ LOG(FATAL, "Tried to set mock time and mock time is not enabled\n");
+ }
+ current_mock_time = now;
+}
+
+Time Time::Now(clockid_t clock) {
+ if (mock_time_enabled) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ return current_mock_time;
+ } else {
+ timespec temp;
+ if (clock_gettime(clock, &temp) != 0) {
+ // TODO(aschuh): There needs to be a pluggable low level logging interface
+ // so we can break this dependency loop. This also would help during
+ // startup.
+ LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
+ static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
+ }
+ return Time(temp);
+ }
+}
+
void Time::Check() {
if (nsec_ >= kNSecInSec || nsec_ < 0) {
LOG(FATAL, "0 <= nsec_(%"PRId32") < %"PRId32" isn't true.\n",
diff --git a/aos/common/time.h b/aos/common/time.h
index 1d7ccae..38238f2 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -60,6 +60,7 @@
return ans;
}
#endif // SWIG
+
// CLOCK_MONOTONIC on the fitpc and CLOCK_REALTIME on the cRIO because the
// cRIO doesn't have any others.
// CLOCK_REALTIME is the default realtime clock and CLOCK_MONOTONIC doesn't
@@ -159,6 +160,15 @@
Check();
}
+ // 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);
+ // Disables mocking time.
+ static void DisableMockTime();
+
private:
int32_t sec_, nsec_;
// LOG(FATAL)s if nsec_ is >= kNSecInSec.
diff --git a/aos/crio/controls/ControlsManager.cpp b/aos/crio/controls/ControlsManager.cpp
index a37ec6f..c1f11cd 100644
--- a/aos/crio/controls/ControlsManager.cpp
+++ b/aos/crio/controls/ControlsManager.cpp
@@ -3,7 +3,7 @@
#include "WPILib/Compressor.h"
-#include "aos/aos_core.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"
@@ -22,7 +22,7 @@
printf("aos::ControlsManager::RobotMain\n");
(new Compressor(14, 1))->Start();
- logging::Start();
+ logging::crio::Register();
LOG(INFO, "logging started\n");
GetWatchdog().SetEnabled(false);
diff --git a/aos/crio/logging/crio_logging.cc b/aos/crio/logging/crio_logging.cc
new file mode 100644
index 0000000..2d643c3
--- /dev/null
+++ b/aos/crio/logging/crio_logging.cc
@@ -0,0 +1,190 @@
+#include "aos/crio/logging/crio_logging.h"
+
+#include <string.h>
+#include <msgQLib.h>
+#include <stdint.h>
+#include <taskLib.h>
+
+#include "WPILib/Timer.h"
+#include "WPILib/Task.h"
+
+#include "aos/common/logging/logging_impl.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/Configuration.h"
+#include "aos/common/die.h"
+
+namespace aos {
+namespace logging {
+namespace {
+
+using internal::Context;
+
+MSG_Q_ID queue;
+static const int kQueueLength = 150;
+
+// This gets run in a low-priority task to take the logs from the queue and send
+// them to the atom over a TCP socket.
+void DoTask() {
+ SendSocket sock;
+ LogMessage msg;
+ while (true) {
+ const int ret = msgQReceive(queue, reinterpret_cast<char *>(&msg),
+ sizeof(msg), WAIT_FOREVER);
+ if (ret == ERROR) {
+ LOG(WARNING, "receiving a message failed with %d (%s)",
+ errno, strerror(errno));
+ continue;
+ }
+ if (ret != sizeof(msg)) {
+ LOG(WARNING, "received a message of size %d instead of %d\n",
+ ret, sizeof(msg));
+ continue;
+ }
+
+ if (sock.LastStatus() != 0) {
+ int numMsgs = msgQNumMsgs(queue);
+ if (numMsgs == ERROR) {
+ LOG(FATAL, "msgQNumMsgs(%p) failed with %d: %s\n", queue,
+ errno, strerror(errno));
+ }
+ // If the queue is more than a little bit full.
+ // Otherwise, without a connection on the other end, the queue just fills
+ // up and then all the senders start failing.
+ if (numMsgs > kQueueLength * 3 / 10) {
+ // DEBUG and INFO aren't worth recording here.
+ if (log_gt_important(msg.level, INFO)) {
+ LogNext(WARNING, "log_sender: dropping %s", msg.message);
+ }
+ continue;
+ } else {
+ if (sock.Connect(NetworkPort::kLogs,
+ configuration::GetIPAddress(
+ configuration::NetworkDevice::kAtom),
+ SOCK_STREAM) != 0) {
+ LOG(WARNING, "connecting failed because of %d: %s\n",
+ errno, strerror(errno));
+ LogNext(WARNING, "log_sender: couldn't send %s", msg.message);
+ continue;
+ }
+ }
+ }
+ sock.Send(&msg, sizeof(msg));
+ if (sock.LastStatus() != 0) {
+ LOG(WARNING, "sending '%s' failed because of %d: %s\n",
+ msg.message, errno, strerror(errno));
+ }
+ }
+}
+
+// A simple linked list of Contexts.
+struct ContextHolder {
+ static ContextHolder *head;
+ // Which task this one belongs to.
+ const int task;
+ ContextHolder *next;
+ Context context;
+
+ ContextHolder() : task(taskIdSelf()) {
+ MutexLocker locker(&list_mutex);
+ next = head;
+ head = this;
+ }
+ ~ContextHolder() {
+ MutexLocker locker(&list_mutex);
+ head = next;
+ }
+
+ static ContextHolder *Get() {
+ int thisTask = taskIdSelf();
+ for (ContextHolder *c = head; c != NULL; c = c->next) {
+ if (c->task == thisTask) {
+ return c;
+ }
+ }
+ return NULL;
+ }
+
+ private:
+ static Mutex list_mutex;
+};
+ContextHolder *ContextHolder::head(NULL);
+Mutex ContextHolder::list_mutex;
+
+} // namespace
+namespace internal {
+
+Context *Context::Get() {
+ ContextHolder *holder = ContextHolder::Get();
+
+ if (holder == NULL) {
+ holder = new ContextHolder();
+
+ errno = 0; // in case taskName decides not to set it
+ const char *my_name = taskName(0);
+ if (my_name == NULL) {
+ Die("logging: taskName(0) failed with %d: %s\n",
+ errno, strerror(errno));
+ }
+ holder->context.name = std::string(my_name);
+ if (holder->context.name.size() + 1 > sizeof(LogMessage::name)) {
+ Die("logging: somebody chose a task name ('%s') that's too long\n",
+ my_name);
+ }
+ holder->context.source = taskIdSelf();
+ }
+
+ return &holder->context;
+}
+
+void Context::Delete() {
+ delete ContextHolder::Get();
+}
+
+} // namespace internal
+
+namespace crio {
+namespace {
+
+class CrioLogImplementation : public LogImplementation {
+ virtual void DoLog(log_level level, const char *format, va_list ap) {
+ LogMessage message;
+
+ internal::FillInMessage(level, format, ap, &message);
+
+ if (msgQSend(queue, reinterpret_cast<char *>(&message), sizeof(message),
+ NO_WAIT, MSG_PRI_NORMAL) != 0) {
+ if (errno == S_objLib_OBJ_UNAVAILABLE) {
+ LOG(WARNING, "no space for sending %s", message.message);
+ } else {
+ LOG(FATAL, "msgQSend(%p, %p, %d, NO_WAIT, MSG_PRI_NORMAL) failed"
+ " with %d: %s\n", queue, &message, sizeof(message),
+ errno, strerror(errno));
+ }
+ }
+ }
+};
+
+} // namespace
+
+void Register() {
+ queue = msgQCreate(kQueueLength,
+ sizeof(LogMessage),
+ MSG_Q_PRIORITY);
+ if (queue == NULL) {
+ Die("msgQCreate(%d, %d, MSG_Q_PRIORITY) failed with %d: %s\n",
+ kQueueLength, sizeof(LogMessage), errno, strerror(errno));
+ }
+
+ Task *task = new Task("LogSender",
+ (FUNCPTR)(DoTask),
+ 150); // low priority
+ task->Start();
+
+ Init();
+
+ AddImplementation(new CrioLogImplementation());
+}
+
+} // namespace crio
+} // namespace logging
+} // namespace aos
diff --git a/aos/crio/logging/crio_logging.cpp b/aos/crio/logging/crio_logging.cpp
deleted file mode 100644
index 8ebafc0..0000000
--- a/aos/crio/logging/crio_logging.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-#include <string.h>
-
-#include "WPILib/Timer.h"
-#include "WPILib/Task.h"
-
-#include "aos/aos_core.h"
-#include "aos/common/network/SendSocket.h"
-#include "aos/common/Configuration.h"
-#include "aos/common/die.h"
-
-#undef ERROR
-#define DECL_LEVEL(name, value) const log_level name = value;
-DECL_LEVELS
-#undef DECL_LEVEL
-
-//#define fprintf(...)
-
-namespace aos {
-namespace logging {
-namespace {
-
-MSG_Q_ID queue;
-uint8_t sequence = 0;
-
-// This gets run in a low-priority task to take the logs from the queue and send
-// them to the atom over a TCP socket.
-void DoTask() {
- SendSocket sock;
- log_crio_message msg;
- while (true) {
- const int ret = msgQReceive(queue, reinterpret_cast<char *>(&msg),
- sizeof(msg), WAIT_FOREVER);
- if (ret == ERROR) {
- fprintf(stderr, "logging: warning: receiving a message failed"
- " with %d (%s)", errno, strerror(errno));
- continue;
- }
- if (ret != sizeof(msg)) {
- fprintf(stderr, "logging: warning: received a message of size %d "
- "instead of %zd\n", ret, sizeof(msg));
- continue;
- }
-
- if (sock.LastStatus() != 0) {
- if (sock.Connect(NetworkPort::kLogs,
- configuration::GetIPAddress(
- configuration::NetworkDevice::kAtom),
- SOCK_STREAM) != 0) {
- fprintf(stderr, "logging: warning: connecting failed"
- " because of %d: %s\n", errno, strerror(errno));
- }
- }
- sock.Send(&msg, sizeof(msg));
- if (sock.LastStatus() != 0) {
- fprintf(stderr, "logging: warning: sending '%s' failed"
- " because of %d: %s\n", msg.message, errno, strerror(errno));
- }
- }
-}
-
-} // namespace
-
-void Start() {
- queue = msgQCreate(100, // max messages
- sizeof(log_crio_message),
- MSG_Q_PRIORITY);
- Task *task = new Task("LogSender",
- (FUNCPTR)(DoTask),
- 150); // priority
- task->Start();
-}
-
-int Do(log_level level, const char *format, ...) {
- log_crio_message msg;
- msg.time = Timer::GetFPGATimestamp();
- msg.level = level;
- msg.sequence = __sync_fetch_and_add(&sequence, 1);
-
- const char *continued = "...";
- const size_t size = sizeof(msg.message) - strlen(continued);
- va_list ap;
- va_start(ap, format);
- const int ret = vsnprintf(msg.message, size, format, ap);
- va_end(ap);
-
- if (ret < 0) {
- fprintf(stderr, "logging: error: vsnprintf failed with %d (%s)\n",
- errno, strerror(errno));
- return -1;
- } else if (static_cast<uintmax_t>(ret) >= static_cast<uintmax_t>(size)) {
- // overwrite the NULL at the end of the existing one and
- // copy in the one on the end of continued
- memcpy(&msg.message[size - 1], continued, strlen(continued) + 1);
- }
- if (msgQSend(queue, reinterpret_cast<char *>(&msg), sizeof(msg),
- NO_WAIT, MSG_PRI_NORMAL) == ERROR) {
- fprintf(stderr, "logging: warning: sending message '%s'"
- " failed with %d (%s)", msg.message, errno, strerror(errno));
- return -1;
- }
-
- if (level == FATAL) {
- aos::Die("%s", msg.message);
- }
-
- return 0;
-}
-
-} // namespace logging
-} // namespace aos
diff --git a/aos/crio/logging/crio_logging.h b/aos/crio/logging/crio_logging.h
index c3dbf2a..3fad750 100644
--- a/aos/crio/logging/crio_logging.h
+++ b/aos/crio/logging/crio_logging.h
@@ -1,35 +1,17 @@
#ifndef AOS_CRIO_CRIO_LOGGING_LOGGING_H_
#define AOS_CRIO_CRIO_LOGGING_LOGGING_H_
-#ifndef AOS_COMMON_LOGGING_LOGGING_H_
-#error This file may only be #included through common/logging/logging.h!!!
-#endif
-
-#undef extern
-#undef const
-#undef ERROR
-
-#include <msgQLib.h>
-#include <stdint.h>
-
-//#define LOG(level, fmt, args...) printf(STRINGIFY(level) ": " fmt, ##args)
-#define LOG(level, fmt, args...) \
- ::aos::logging::Do(level, \
- LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " fmt, \
- ##args)
-//#define LOG(...)
-
namespace aos {
namespace logging {
+namespace crio {
-// Initialize internal variables and start up the task that sends the logs to
-// the atom. Must be called before Do.
-void Start();
-// The function that the LOG macro actually calls. Queues up a message for the
-// task to send. Start must be called before this function is.
-int Do(log_level level, const char *format, ...)
- __attribute__((format(printf, 2, 3)));
+// Calls AddImplementation to register the usual crio logging implementation
+// which sends the messages to the atom. This function will start the task that
+// does the actual sending. It relies on a process being running on the atom to
+// receive them.
+void Register();
+} // namespace crio
} // namespace logging
} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_bridge-tmpl.h b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
index cc00ab8..fbd3e98 100644
--- a/aos/crio/shared_libs/interrupt_bridge-tmpl.h
+++ b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
@@ -5,6 +5,7 @@
#include "WPILib/Task.h"
#include "aos/common/logging/logging.h"
+#include "aos/crio/motor_server/MotorServer.h"
#include "aos/common/time.h"
extern "C" {
diff --git a/aos/crio/shared_libs/interrupt_bridge.h b/aos/crio/shared_libs/interrupt_bridge.h
index 85ecd0f..f8f68b4 100644
--- a/aos/crio/shared_libs/interrupt_bridge.h
+++ b/aos/crio/shared_libs/interrupt_bridge.h
@@ -8,7 +8,7 @@
#include "aos/common/scoped_ptr.h"
-#include "aos/aos_core.h"
+#include "aos/common/macros.h"
class Task;
diff --git a/aos/crio/shared_libs/interrupt_bridge_demo.cc b/aos/crio/shared_libs/interrupt_bridge_demo.cc
index d7e66be..0755c97 100644
--- a/aos/crio/shared_libs/interrupt_bridge_demo.cc
+++ b/aos/crio/shared_libs/interrupt_bridge_demo.cc
@@ -4,7 +4,6 @@
#include "WPILib/Timer.h"
-#include "aos/aos_core.h"
#include "aos/common/time.h"
using aos::time::Time;
diff --git a/aos/crio/shared_libs/mutex.cpp b/aos/crio/shared_libs/mutex.cpp
index 35ac4e3..d921492 100644
--- a/aos/crio/shared_libs/mutex.cpp
+++ b/aos/crio/shared_libs/mutex.cpp
@@ -1,6 +1,9 @@
#include "aos/common/mutex.h"
#include <semLib.h>
+#include <string.h>
+
+#include "aos/common/logging/logging.h"
namespace aos {
diff --git a/aos/crio/shared_libs/shared_libs.gyp b/aos/crio/shared_libs/shared_libs.gyp
new file mode 100644
index 0000000..90b3cf0
--- /dev/null
+++ b/aos/crio/shared_libs/shared_libs.gyp
@@ -0,0 +1,25 @@
+{
+ 'targets': [
+ {
+# This one includes interrupt_bridge.h too.
+ 'target_name': 'interrupt_notifier',
+ 'type': 'static_library',
+ 'sources': [
+ 'interrupt_bridge.cc',
+ 'interrupt_bridge_c.c',
+ 'interrupt_bridge_demo.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(EXTERNALS):WPILib',
+ ],
+ },
+ ],
+}
diff --git a/frc971/atom_code/.gitignore b/frc971/atom_code/.gitignore
new file mode 100644
index 0000000..b1eea9c
--- /dev/null
+++ b/frc971/atom_code/.gitignore
@@ -0,0 +1,2 @@
+/shooter.csv
+/wrist.csv
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index d727036..e9c787e 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -6,6 +6,15 @@
'dependencies': [
'<(AOS)/build/aos_all.gyp:Atom',
'../control_loops/control_loops.gyp:DriveTrain',
+ '../control_loops/wrist/wrist.gyp:wrist',
+ '../control_loops/wrist/wrist.gyp:wrist_lib_test',
+ '../control_loops/index/index.gyp:index',
+ '../control_loops/index/index.gyp:index_lib_test',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_lib_test',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_csv',
+ '../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:GyroReader',
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 0edca32..1e01ca7 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -9,3 +9,7 @@
CameraReader
CameraServer
CameraHTTPStreamer
+angle_adjust
+wrist
+index
+shooter
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index bd2f078..e5af1a9 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -1,41 +1,154 @@
#include "frc971/constants.h"
#include <stddef.h>
-#include <inttypes.h>
+#include <math.h>
+#include "aos/common/inttypes.h"
#include "aos/common/messages/RobotState.q.h"
#include "aos/atom_code/output/MotorOutput.h"
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
namespace frc971 {
namespace constants {
namespace {
-const double kCompHorizontal = -1.77635 + 0.180;
-const double kPracticeHorizontal = -1.77635 + -0.073631;
+// It has about 0.029043 of gearbox slop.
+const double kCompWristHallEffectStartAngle = 1.0872860614359172;
+const double kPracticeWristHallEffectStartAngle = 1.0872860614359172;
+
+const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
+const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
+
+const double kPracticeWristUpperPhysicalLimit = 1.677562;
+const double kCompWristUpperPhysicalLimit = 1.677562;
+
+const double kPracticeWristLowerPhysicalLimit = -0.746128;
+const double kCompWristLowerPhysicalLimit = -0.746128;
+
+const double kPracticeWristUpperLimit = 1.615385;
+const double kCompWristUpperLimit = 1.615385;
+
+const double kPracticeWristLowerLimit = -0.746128;
+const double kCompWristLowerLimit = -0.746128;
+
+const double kWristZeroingSpeed = 0.25;
+const double kWristZeroingOffSpeed = 1.0;
+
+const int kAngleAdjustHallEffect = 2;
+
+const double kCompAngleAdjustHallEffectStartAngle[2] = {0.305432, 1.5};
+const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.305432, 1.5};
+
+const double kCompAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
+const double kPracticeAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
+
+const double kPracticeAngleAdjustUpperPhysicalLimit = 0.904737;
+const double kCompAngleAdjustUpperPhysicalLimit = 0.904737;
+
+const double kPracticeAngleAdjustLowerPhysicalLimit = 0.272451;
+const double kCompAngleAdjustLowerPhysicalLimit = 0.272451;
+
+const double kPracticeAngleAdjustUpperLimit = 0.87;
+const double kCompAngleAdjustUpperLimit = 0.87;
+
+const double kPracticeAngleAdjustLowerLimit = 0.30;
+const double kCompAngleAdjustLowerLimit = 0.30;
+
+const double kAngleAdjustZeroingSpeed = -0.2;
+const double kAngleAdjustZeroingOffSpeed = -0.5;
+
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
struct Values {
- // what horizontal_offset returns
- double horizontal;
+ // Wrist hall effect positive and negative edges.
+ double wrist_hall_effect_start_angle;
+ double wrist_hall_effect_stop_angle;
+
+ // Upper and lower extreme limits of travel for the wrist.
+ double wrist_upper_limit;
+ double wrist_lower_limit;
+
+ // Physical limits. These are here for testing.
+ double wrist_upper_physical_limit;
+ double wrist_lower_physical_limit;
+
+ // Zeroing speed.
+ double wrist_zeroing_speed;
+ // Zeroing off speed.
+ double wrist_zeroing_off_speed;
+
+ // AngleAdjust hall effect positive and negative edges.
+ const double *angle_adjust_hall_effect_start_angle;
+ const double *angle_adjust_hall_effect_stop_angle;
+
+ // Upper and lower extreme limits of travel for the angle adjust.
+ double angle_adjust_upper_limit;
+ double angle_adjust_lower_limit;
+ // Physical limits. These are here for testing.
+ double angle_adjust_upper_physical_limit;
+ double angle_adjust_lower_physical_limit;
+
+ // Zeroing speed.
+ double angle_adjust_zeroing_speed;
+ // Zeroing off speed.
+ double angle_adjust_zeroing_off_speed;
+
// what camera_center returns
int camera_center;
};
+
Values *values = NULL;
// Attempts to retrieve a new Values instance and stores it in values if
// necessary.
// Returns a valid Values instance or NULL.
const Values *GetValues() {
+ // TODO(brians): Make this use the new Once construct.
if (values == NULL) {
LOG(INFO, "creating a Constants for team %"PRIu16"\n",
- aos::robot_state->team_id);
- switch (aos::robot_state->team_id) {
+ ::aos::robot_state->team_id);
+ switch (::aos::robot_state->team_id) {
case kCompTeamNumber:
- values = new Values{kCompHorizontal, kCompCameraCenter};
+ values = new Values{kCompWristHallEffectStartAngle,
+ kCompWristHallEffectStopAngle,
+ kCompWristUpperLimit,
+ kCompWristLowerLimit,
+ kCompWristUpperPhysicalLimit,
+ kCompWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
+ kCompAngleAdjustHallEffectStartAngle,
+ kCompAngleAdjustHallEffectStopAngle,
+ kCompAngleAdjustUpperLimit,
+ kCompAngleAdjustLowerLimit,
+ kCompAngleAdjustUpperPhysicalLimit,
+ kCompAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
+ kCompCameraCenter};
break;
case kPracticeTeamNumber:
- values = new Values{kPracticeHorizontal, kPracticeCameraCenter};
+ values = new Values{kPracticeWristHallEffectStartAngle,
+ kPracticeWristHallEffectStopAngle,
+ kPracticeWristUpperLimit,
+ kPracticeWristLowerLimit,
+ kPracticeWristUpperPhysicalLimit,
+ kPracticeWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
+ kPracticeAngleAdjustHallEffectStartAngle,
+ kPracticeAngleAdjustHallEffectStopAngle,
+ kPracticeAngleAdjustUpperLimit,
+ kPracticeAngleAdjustLowerLimit,
+ kPracticeAngleAdjustUpperPhysicalLimit,
+ kPracticeAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
+ kPracticeCameraCenter};
break;
default:
LOG(ERROR, "unknown team #%"PRIu16"\n",
@@ -48,12 +161,116 @@
} // namespace
-bool horizontal_offset(double *horizontal) {
+bool wrist_hall_effect_start_angle(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *horizontal = values->horizontal;
+ *angle = values->wrist_hall_effect_start_angle;
return true;
}
+bool wrist_hall_effect_stop_angle(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->wrist_hall_effect_stop_angle;
+ return true;
+}
+bool wrist_upper_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->wrist_upper_limit;
+ return true;
+}
+
+bool wrist_lower_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->wrist_lower_limit;
+ return true;
+}
+
+bool wrist_upper_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->wrist_upper_physical_limit;
+ return true;
+}
+
+bool wrist_lower_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->wrist_lower_physical_limit;
+ return true;
+}
+
+bool wrist_zeroing_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->wrist_zeroing_speed;
+ return true;
+}
+
+bool wrist_zeroing_off_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->wrist_zeroing_off_speed;
+ return true;
+}
+
+bool angle_adjust_hall_effect_start_angle(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ angle[0] = values->angle_adjust_hall_effect_start_angle[0];
+ angle[1] = values->angle_adjust_hall_effect_start_angle[1];
+ return true;
+}
+bool angle_adjust_hall_effect_stop_angle(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ angle[0] = values->angle_adjust_hall_effect_stop_angle[0];
+ angle[1] = values->angle_adjust_hall_effect_stop_angle[1];
+ return true;
+}
+bool angle_adjust_upper_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_upper_limit;
+ return true;
+}
+
+bool angle_adjust_lower_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_lower_limit;
+ return true;
+}
+
+bool angle_adjust_upper_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_upper_physical_limit;
+ return true;
+}
+
+bool angle_adjust_lower_physical_limit(double *angle) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *angle = values->angle_adjust_lower_physical_limit;
+ return true;
+}
+
+bool angle_adjust_zeroing_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->angle_adjust_zeroing_speed;
+ return true;
+}
+
+bool angle_adjust_zeroing_off_speed(double *speed) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *speed = values->angle_adjust_zeroing_off_speed;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;
diff --git a/frc971/constants.h b/frc971/constants.h
index 1138538..296a4f4 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -13,9 +13,33 @@
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 5971;
-// Sets *horizontal to how many radians from the hall effect transition point
-// to horizontal for the wrist.
-bool horizontal_offset(double *horizontal);
+// Sets *angle to how many radians from horizontal to the location of interest.
+bool wrist_hall_effect_start_angle(double *angle);
+bool wrist_hall_effect_stop_angle(double *angle);
+// These are the soft stops for up and down.
+bool wrist_lower_limit(double *angle);
+bool wrist_upper_limit(double *angle);
+// These are the hard stops. Don't use these for anything but testing.
+bool wrist_lower_physical_limit(double *angle);
+bool wrist_upper_physical_limit(double *angle);
+
+// Returns the speed to move the wrist at when zeroing in rad/sec
+bool wrist_zeroing_speed(double *speed);
+bool wrist_zeroing_off_speed(double *speed);
+
+bool angle_adjust_hall_effect_start_angle(double *angle);
+bool angle_adjust_hall_effect_stop_angle(double *angle);
+// These are the soft stops for up and down.
+bool angle_adjust_lower_limit(double *angle);
+bool angle_adjust_upper_limit(double *angle);
+// These are the hard stops. Don't use these for anything but testing.
+bool angle_adjust_lower_physical_limit(double *angle);
+bool angle_adjust_upper_physical_limit(double *angle);
+
+// Returns speed to move the angle adjust when zeroing, in rad/sec
+bool angle_adjust_zeroing_speed(double *speed);
+bool angle_adjust_zeroing_off_speed(double *speed);
+
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
bool camera_center(int *center);
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
new file mode 100644
index 0000000..b4117e2
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -0,0 +1,112 @@
+#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+AngleAdjustMotor::AngleAdjustMotor(
+ control_loops::AngleAdjustLoop *my_angle_adjust)
+ : aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
+ my_angle_adjust),
+ zeroed_joint_(MakeAngleAdjustLoop()) {
+}
+
+bool AngleAdjustMotor::FetchConstants(
+ ZeroedJoint<2>::ConfigurationData *config_data) {
+ if (!constants::angle_adjust_lower_limit(
+ &config_data->lower_limit)) {
+ LOG(ERROR, "Failed to fetch the angle adjust lower limit constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_upper_limit(
+ &config_data->upper_limit)) {
+ LOG(ERROR, "Failed to fetch the angle adjust upper limit constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_hall_effect_start_angle(
+ config_data->hall_effect_start_angle)) {
+ LOG(ERROR, "Failed to fetch the hall effect start angle constants.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_zeroing_off_speed(
+ &config_data->zeroing_off_speed)) {
+ LOG(ERROR,
+ "Failed to fetch the angle adjust zeroing off speed constant.\n");
+ return false;
+ }
+ if (!constants::angle_adjust_zeroing_speed(
+ &config_data->zeroing_speed)) {
+ LOG(ERROR, "Failed to fetch the angle adjust zeroing speed constant.\n");
+ return false;
+ }
+
+ config_data->max_zeroing_voltage = 4.0;
+ return true;
+}
+
+// Positive angle is up, and positive power is up.
+void AngleAdjustMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::AngleAdjustLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * /*status*/) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->voltage = 0;
+ }
+
+ // Cache the constants to avoid error handling down below.
+ ZeroedJoint<2>::ConfigurationData config_data;
+ if (!FetchConstants(&config_data)) {
+ LOG(WARNING, "Failed to fetch constants.\n");
+ return;
+ } else {
+ zeroed_joint_.set_config_data(config_data);
+ }
+
+ ZeroedJoint<2>::PositionData transformed_position;
+ ZeroedJoint<2>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->angle;
+ transformed_position.hall_effects[0] = position->bottom_hall_effect;
+ transformed_position.hall_effect_positions[0] =
+ position->bottom_calibration;
+ transformed_position.hall_effects[1] = position->middle_hall_effect;
+ transformed_position.hall_effect_positions[1] =
+ position->middle_calibration;
+ }
+
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
+
+ if (position) {
+ LOG(DEBUG, "pos: %f bottom_hall: %s middle_hall: %s absolute: %f\n",
+ position->angle,
+ position->bottom_hall_effect ? "true" : "false",
+ position->middle_hall_effect ? "true" : "false",
+ zeroed_joint_.absolute_position());
+ }
+
+ if (output) {
+ output->voltage = voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.gyp b/frc971/control_loops/angle_adjust/angle_adjust.gyp
new file mode 100644
index 0000000..fefb832
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.gyp
@@ -0,0 +1,82 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'angle_adjust_loop',
+ 'type': 'static_library',
+ 'sources': ['angle_adjust_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/angle_adjust',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'angle_adjust_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'angle_adjust.cc',
+ 'angle_adjust_motor_plant.cc',
+ 'unaugmented_angle_adjust_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ 'angle_adjust_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ 'angle_adjust_loop',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ 'angle_adjust_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ 'angle_adjust_loop',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'angle_adjust_loop',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_main.cc',
+ ],
+ 'dependencies': [
+ 'angle_adjust_lib',
+ 'angle_adjust_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
new file mode 100644
index 0000000..dfa8ad2
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.h
@@ -0,0 +1,68 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
+
+#include <array>
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Allows the control loop to add the tests to access private members.
+namespace testing {
+class AngleAdjustTest_RezeroWithMissingPos_Test;
+class AngleAdjustTest_DisableGoesUninitialized_Test;
+}
+
+class AngleAdjustMotor
+ : public aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop> {
+ public:
+ explicit AngleAdjustMotor(
+ control_loops::AngleAdjustLoop *my_angle_adjust =
+ &control_loops::angle_adjust);
+ protected:
+ virtual void RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::AngleAdjustLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status *status);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the wrist is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
+ private:
+ // Allows the testing code to access some of private members.
+ friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
+ friend class testing::AngleAdjustTest_DisableGoesUninitialized_Test;
+
+ // Fetches and locally caches the latest set of constants.
+ // Returns whether it succeeded or not.
+ bool FetchConstants(ZeroedJoint<2>::ConfigurationData *config_data);
+
+ // The zeroed joint to use.
+ ZeroedJoint<2> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(AngleAdjustMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_csv.cc b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
new file mode 100644
index 0000000..109af56
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
@@ -0,0 +1,52 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/atom_code/init.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+
+using ::frc971::control_loops::angle_adjust;
+using ::aos::time::Time;
+
+// Records data from the queue and stores it in a .csv file which can then
+// be plotted/processed with relative ease.
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Not saving to a CSV file.\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ angle_adjust.goal.FetchLatest();
+ angle_adjust.status.FetchLatest();
+ angle_adjust.position.FetchLatest();
+ angle_adjust.output.FetchLatest();
+ if (angle_adjust.output.get() &&
+ angle_adjust.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (angle_adjust.position->sent_time - start_time).ToSeconds(),
+ angle_adjust.output->voltage,
+ angle_adjust.position->angle);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_data.csv b/frc971/control_loops/angle_adjust/angle_adjust_data.csv
new file mode 100644
index 0000000..3e3a9fb
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_data.csv
@@ -0,0 +1,273 @@
+0.007086, 0.696383, -0.000596
+0.016561, 0.696383, -0.000596
+0.026648, 0.696383, -0.000596
+0.036501, 0.696383, -0.000596
+0.046475, 0.696383, -0.000596
+0.056429, 0.696383, -0.000596
+0.066661, 0.696383, -0.000596
+0.076538, 0.696383, -0.000596
+0.086562, 0.696383, -0.000596
+0.096496, 0.696383, -0.000596
+0.106453, 0.696383, -0.000596
+0.116673, 0.696383, -0.000596
+0.126529, 0.696383, -0.000596
+0.136500, 0.696383, -0.000596
+0.146462, 0.696383, -0.000596
+0.156442, 0.696383, -0.000596
+0.166658, 0.696383, -0.000596
+0.176508, 0.696383, -0.000596
+0.186466, 0.696383, -0.000596
+0.196514, 0.696383, -0.000596
+0.206469, 0.696383, -0.000596
+0.216679, 0.696383, -0.000596
+0.226464, 0.696383, -0.000596
+0.236401, 0.696383, -0.000596
+0.246476, 0.696383, -0.000596
+0.256391, 0.696383, -0.000596
+0.266650, 0.696383, -0.000596
+0.276517, 0.696383, -0.000596
+0.286565, 0.696383, -0.000596
+0.296526, 0.696383, -0.000596
+0.306469, 0.696383, -0.000596
+0.316703, 0.696383, -0.000596
+0.326546, 0.696383, -0.000596
+0.336500, 0.696383, -0.000596
+0.346466, 0.696383, -0.000596
+0.356397, 0.696383, -0.000596
+0.366658, 0.696383, -0.000596
+0.376517, 0.696383, -0.000596
+0.386461, 0.696383, -0.000596
+0.396497, 0.696383, -0.000596
+0.406516, 0.696383, -0.000596
+0.416662, 0.696383, -0.000596
+0.426498, 0.696383, -0.000596
+0.436506, 0.696383, -0.000596
+0.446560, 0.696383, -0.000596
+0.456494, 0.696383, -0.000596
+0.466635, 0.696383, -0.000596
+0.476505, 0.696383, -0.000596
+0.486529, 0.696383, -0.000596
+0.496494, 0.696383, -0.000596
+0.506479, 0.696383, -0.000596
+0.516679, 0.696383, -0.000596
+0.526550, 0.696383, -0.000596
+0.536505, 0.696383, -0.000596
+0.546452, 0.696383, -0.000596
+0.556481, 0.696383, -0.000596
+0.566531, 0.696383, -0.000596
+0.576513, 0.696383, -0.000596
+0.586463, 0.696383, -0.000596
+0.596771, 0.696383, -0.000596
+0.606503, 0.696383, -0.000596
+0.616679, 0.696383, -0.000596
+0.626483, 0.696383, -0.000596
+0.636496, 0.696383, -0.000596
+0.646654, 0.696383, -0.000596
+0.656423, 0.696383, -0.000596
+0.666661, 0.696383, -0.000596
+0.676531, 0.696383, -0.000596
+0.686526, 0.696383, -0.000596
+0.696670, 0.696383, -0.000596
+0.706628, 0.696383, -0.000596
+0.716675, 0.696383, -0.000596
+0.726719, 0.696383, -0.000596
+0.736498, 0.696383, -0.000596
+0.746484, 0.696383, -0.000596
+0.756468, 0.696383, -0.000596
+0.766526, 0.696383, -0.000596
+0.776498, 0.696383, -0.000596
+0.786469, 0.696383, -0.000596
+0.796251, 0.696383, -0.000596
+0.806312, 0.696383, -0.000596
+0.816579, 0.696383, -0.000596
+0.826352, 0.696383, -0.000596
+0.836395, 0.696383, -0.000596
+0.846460, 0.696383, -0.000596
+0.856333, 0.696383, -0.000596
+0.866312, 0.696383, -0.000596
+0.876541, 0.696383, -0.000596
+0.886231, 0.696383, -0.000596
+0.896378, 0.696383, -0.000596
+0.906549, 0.696383, -0.000596
+0.916219, 0.696383, -0.000596
+0.926300, 0.696383, -0.000596
+0.936212, 0.696383, -0.000596
+0.946392, 12.000000, -0.000596
+0.956205, 12.000000, -0.000596
+0.966382, 12.000000, 0.001192
+0.976384, 12.000000, 0.005962
+0.988371, 12.000000, 0.014906
+0.996207, 12.000000, 0.022657
+1.006429, 12.000000, 0.032794
+1.016788, 12.000000, 0.043526
+1.026528, 12.000000, 0.055451
+1.036616, 12.000000, 0.067376
+1.049418, 12.000000, 0.084071
+1.056874, 12.000000, 0.093015
+1.066581, 12.000000, 0.105536
+1.076708, 12.000000, 0.118653
+1.086697, 12.000000, 0.131771
+1.096754, 12.000000, 0.144888
+1.106694, 12.000000, 0.158006
+1.116795, 12.000000, 0.171123
+1.126945, 12.000000, 0.184241
+1.136491, 12.000000, 0.197358
+1.146490, 12.000000, 0.211072
+1.156720, 8.838210, 0.224189
+1.166497, 4.659384, 0.237307
+1.176550, 1.566925, 0.250424
+1.186490, -1.463977, 0.262945
+1.196706, -4.497083, 0.273081
+1.206413, -6.664959, 0.280833
+1.216504, -8.191987, 0.285603
+1.226718, -8.899323, 0.285603
+1.238720, -8.265740, 0.282025
+1.246502, -7.344013, 0.277851
+1.256515, -5.283271, 0.270696
+1.266504, -3.292020, 0.262945
+1.276639, -1.703019, 0.254598
+1.286435, 0.722641, 0.247443
+1.296401, 2.376863, 0.240884
+1.306274, 3.669941, 0.235518
+1.316431, 4.798566, 0.231940
+1.326280, 5.585790, 0.230748
+1.336257, 5.131469, 0.231344
+1.346265, 5.052962, 0.233133
+1.356246, 4.237080, 0.236710
+1.366233, 3.588899, 0.240288
+1.376252, 2.132660, 0.244462
+1.386339, 1.550053, 0.248039
+1.396224, 0.328756, 0.252213
+1.406339, -0.379540, 0.255194
+1.416340, -1.273041, 0.256983
+1.426200, -1.551323, 0.258175
+1.436332, -1.593706, 0.258772
+1.446348, -1.695210, 0.258175
+1.456324, -1.569522, 0.257579
+1.466365, -0.979101, 0.256387
+1.476513, -0.794073, 0.255194
+1.486723, -0.508791, 0.254002
+1.496495, -0.189683, 0.252809
+1.506430, 0.093002, 0.251617
+1.516565, 0.386294, 0.250424
+1.526518, 0.681860, 0.249828
+1.536477, 0.659148, 0.249828
+1.546440, 0.541822, 0.249828
+1.556459, 0.602564, 0.249828
+1.566511, 0.597167, 0.249828
+1.576515, 0.587492, 0.249828
+1.586463, 0.593212, 0.249828
+1.596495, 0.592427, 0.249828
+1.606519, 0.591648, 0.249828
+1.616647, 0.592179, 0.249828
+1.626482, 0.592082, 0.249828
+1.636498, 0.592021, 0.249828
+1.646521, 0.592070, 0.249828
+1.656480, 0.592059, 0.249828
+1.666552, 0.592055, 0.249828
+1.676484, 0.592059, 0.249828
+1.686525, 0.592058, 0.249828
+1.696670, 0.592057, 0.249828
+1.706459, 0.592058, 0.249828
+1.716735, 0.592058, 0.249828
+1.726493, 0.592058, 0.249828
+1.736508, 0.592058, 0.249828
+1.746452, 0.592058, 0.249828
+1.756477, 0.592058, 0.249828
+1.766815, 0.592058, 0.249828
+1.776500, 0.592058, 0.249828
+1.786441, 0.592058, 0.249828
+1.796462, 0.592058, 0.249828
+1.806512, 0.592058, 0.249828
+1.816550, 0.592058, 0.249828
+1.826504, 0.592058, 0.249828
+1.836496, 0.592058, 0.249828
+1.846519, 0.592058, 0.249828
+1.856477, 0.592058, 0.249828
+1.866533, 0.592058, 0.249828
+1.876513, 0.592058, 0.249828
+1.886541, 0.592058, 0.249828
+1.896490, 0.592058, 0.249828
+1.906479, -12.000000, 0.249828
+1.916446, -12.000000, 0.249828
+1.926530, -12.000000, 0.248635
+1.936520, -12.000000, 0.244462
+1.946463, -12.000000, 0.236710
+1.956469, -12.000000, 0.226574
+1.966515, -12.000000, 0.215842
+1.976560, -12.000000, 0.203917
+1.986476, -12.000000, 0.191396
+1.996480, -12.000000, 0.177682
+2.006460, -12.000000, 0.163968
+2.016545, -12.000000, 0.150254
+2.026495, -12.000000, 0.135944
+2.036471, -12.000000, 0.121635
+2.046473, -12.000000, 0.107325
+2.056469, -12.000000, 0.093015
+2.066524, -12.000000, 0.078705
+2.076516, -12.000000, 0.064395
+2.086512, -12.000000, 0.049489
+2.096561, -11.491993, 0.034582
+2.106457, -4.953303, 0.020272
+2.116661, -2.354450, 0.005366
+2.126490, 1.313071, -0.008347
+2.136461, 4.880571, -0.020272
+2.146526, 7.088633, -0.029216
+2.156469, 9.296628, -0.034582
+2.166524, 9.779309, -0.036371
+2.176504, 9.677380, -0.033986
+2.186457, 8.409415, -0.029216
+2.196463, 7.037205, -0.022657
+2.206499, 4.964211, -0.014906
+2.216552, 3.206835, -0.007155
+2.226456, 0.878265, 0.000596
+2.236471, -0.788450, 0.007751
+2.246517, -2.095142, 0.012521
+2.256472, -3.529778, 0.016099
+2.266416, -3.784184, 0.017887
+2.276485, -3.906740, 0.017291
+2.286520, -3.591089, 0.015502
+2.296497, -2.772296, 0.012521
+2.306461, -2.173367, 0.008944
+2.316561, -1.324239, 0.005962
+2.326547, -0.303999, 0.002385
+2.336490, 0.515297, -0.000596
+2.346453, 1.081194, -0.002981
+2.356491, 1.877482, -0.005366
+2.366526, 1.984534, -0.005962
+2.376468, 2.197044, -0.005962
+2.386411, 1.936623, -0.005962
+2.396484, 2.018387, -0.005366
+2.406517, 1.716757, -0.004770
+2.416569, 1.598133, -0.003577
+2.426463, 1.469285, -0.002981
+2.436457, 0.993349, -0.001789
+2.446504, 0.754160, -0.001192
+2.456498, 0.786747, -0.000596
+2.466418, 0.572920, -0.000596
+2.476482, 0.737242, -0.000596
+2.486521, 0.701741, -0.000596
+2.496489, 0.685572, -0.000596
+2.506441, 0.700476, -0.000596
+2.516718, 0.696605, -0.000596
+2.526498, 0.695450, -0.000596
+2.536473, 0.696784, -0.000596
+2.546448, 0.696379, -0.000596
+2.556479, 0.696304, -0.000596
+2.566517, 0.696421, -0.000596
+2.576491, 0.696380, -0.000596
+2.586447, 0.696376, -0.000596
+2.596495, 0.696386, -0.000596
+2.606515, 0.696382, -0.000596
+2.616555, 0.696382, -0.000596
+2.626465, 0.696383, -0.000596
+2.636463, 0.696383, -0.000596
+2.646513, 0.696383, -0.000596
+2.656499, 0.696383, -0.000596
+2.666488, 0.696383, -0.000596
+2.676556, 0.696383, -0.000596
+2.686505, 0.696383, -0.000596
+2.696457, 0.696383, -0.000596
+2.706448, 0.696383, -0.000596
+2.716562, 0.696383, -0.000596
+2.726498, 0.696383, -0.000596
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
new file mode 100644
index 0000000..620d784
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -0,0 +1,315 @@
+#include <unistd.h>
+
+#include <memory>
+#include <array>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+#include "frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the angle_adjust and
+// sends out queue messages containing the position.
+class AngleAdjustMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // angle_adjust, which will be treated as 0 by the encoder.
+ explicit AngleAdjustMotorSimulation(double initial_position)
+ : angle_adjust_plant_(
+ new StateFeedbackPlant<2, 1, 1>(MakeRawAngleAdjustPlant())),
+ my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status") {
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ initial_position_ = initial_position;
+ angle_adjust_plant_->X(0, 0) = initial_position_;
+ angle_adjust_plant_->X(1, 0) = 0.0;
+ angle_adjust_plant_->Y = angle_adjust_plant_->C() * angle_adjust_plant_->X;
+ last_position_ = angle_adjust_plant_->Y(0, 0);
+ last_voltage_ = 0.0;
+ calibration_value_[0] = 0.0;
+ calibration_value_[1] = 0.0;
+ }
+
+ // Returns the absolute angle of the angle_adjust.
+ double GetAbsolutePosition() const {
+ return angle_adjust_plant_->Y(0, 0);
+ }
+
+ // Returns the adjusted angle of the angle_adjust.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double angle = GetPosition();
+
+ double hall_effect_start_angle[2];
+ ASSERT_TRUE(constants::angle_adjust_hall_effect_start_angle(
+ hall_effect_start_angle));
+ double hall_effect_stop_angle[2];
+ ASSERT_TRUE(constants::angle_adjust_hall_effect_stop_angle(
+ hall_effect_stop_angle));
+
+ ::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
+ my_angle_adjust_loop_.position.MakeMessage();
+ position->angle = angle;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position <= hall_effect_start_angle[0] &&
+ abs_position >= hall_effect_stop_angle[0]) {
+ position->bottom_hall_effect = true;
+ } else {
+ position->bottom_hall_effect = false;
+ }
+ if (abs_position <= hall_effect_start_angle[1] &&
+ abs_position >= hall_effect_stop_angle[1]) {
+ position->middle_hall_effect = true;
+ } else {
+ position->middle_hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ // TODO(aschuh): This won't deal with both edges correctly.
+ if ((last_position_ < hall_effect_start_angle[0] ||
+ last_position_ > hall_effect_stop_angle[0]) &&
+ (position->bottom_hall_effect)) {
+ calibration_value_[0] = hall_effect_start_angle[0] - initial_position_;
+ }
+ if ((last_position_ < hall_effect_start_angle[1] ||
+ last_position_ > hall_effect_stop_angle[1]) &&
+ (position->middle_hall_effect)) {
+ calibration_value_[1] = hall_effect_start_angle[1] - initial_position_;
+ }
+
+ position->bottom_calibration = calibration_value_[0];
+ position->middle_calibration = calibration_value_[1];
+ position.Send();
+ }
+
+ // Simulates the angle_adjust moving for one timestep.
+ void Simulate() {
+ last_position_ = angle_adjust_plant_->Y(0, 0);
+ EXPECT_TRUE(my_angle_adjust_loop_.output.FetchLatest());
+ angle_adjust_plant_->U << last_voltage_;
+ angle_adjust_plant_->Update();
+
+ // Assert that we are in the right physical range.
+ double upper_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_upper_physical_limit(
+ &upper_physical_limit));
+ double lower_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_lower_physical_limit(
+ &lower_physical_limit));
+
+ EXPECT_GE(upper_physical_limit, angle_adjust_plant_->Y(0, 0));
+ EXPECT_LE(lower_physical_limit, angle_adjust_plant_->Y(0, 0));
+ last_voltage_ = my_angle_adjust_loop_.output->voltage;
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
+
+ private:
+ AngleAdjustLoop my_angle_adjust_loop_;
+ double initial_position_;
+ double last_position_;
+ double last_voltage_;
+ double calibration_value_[2];
+};
+
+class AngleAdjustTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ AngleAdjustLoop my_angle_adjust_loop_;
+
+ // Create a loop and simulation plant.
+ AngleAdjustMotor angle_adjust_motor_;
+ AngleAdjustMotorSimulation angle_adjust_motor_plant_;
+
+ AngleAdjustTest() :
+ my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status"),
+ angle_adjust_motor_(&my_angle_adjust_loop_),
+ angle_adjust_motor_plant_(0.75) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_angle_adjust_loop_.goal.FetchLatest();
+ my_angle_adjust_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_angle_adjust_loop_.goal->goal,
+ angle_adjust_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~AngleAdjustTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the angle_adjust zeros correctly and goes to a position.
+TEST_F(AngleAdjustTest, ZerosCorrectly) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
+ for (int i = 0; i < 400; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly starting on the sensor.
+TEST_F(AngleAdjustTest, ZerosStartingOn) {
+ angle_adjust_motor_plant_.Reinitialize(0.30);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
+ for (int i = 0; i < 500; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_F(AngleAdjustTest, HandleMissingPosition) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ }
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(AngleAdjustTest, RezeroWithMissingPos) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
+ for (int i = 0; i < 800; ++i) {
+ // After 3 seconds, simulate the encoder going missing.
+ // This should trigger a re-zero. To make sure it works, change the goal as
+ // well.
+ if (i < 300 || i > 400) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
+ }
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.5).Send();
+ }
+ if (i == 430) {
+ EXPECT_TRUE(angle_adjust_motor_.is_zeroing() ||
+ angle_adjust_motor_.is_moving_off());
+ }
+
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(AngleAdjustTest, DisableGoesUninitialized) {
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.4).Send();
+ for (int i = 0; i < 800; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ // After 0.5 seconds, disable the robot.
+ if (i > 50 && i < 200) {
+ SendDSPacket(false);
+ if (i > 100) {
+ // Give the loop a couple cycled to get the message and then verify that
+ // it is in the correct state.
+ EXPECT_TRUE(angle_adjust_motor_.is_uninitialized());
+ }
+ } else {
+ SendDSPacket(true);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_TRUE(angle_adjust_motor_.is_zeroing());
+ }
+
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+
+/*
+// TODO(aschuh): Enable these tests if we install a second hall effect sensor.
+// Tests that the angle_adjust zeros correctly from above the second sensor.
+TEST_F(AngleAdjustTest, ZerosCorrectlyAboveSecond) {
+ angle_adjust_motor_plant_.Reinitialize(1.75);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(1.0).Send();
+ for (int i = 0; i < 400; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the angle_adjust zeros correctly starting on
+// the second hall effect sensor.
+TEST_F(AngleAdjustTest, ZerosStartingOnSecond) {
+ angle_adjust_motor_plant_.Reinitialize(1.25);
+ my_angle_adjust_loop_.goal.MakeWithBuilder().goal(1.0).Send();
+ for (int i = 0; i < 500; ++i) {
+ angle_adjust_motor_plant_.SendPositionMessage();
+ angle_adjust_motor_.Iterate();
+ angle_adjust_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+*/
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_main.cc b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
new file mode 100644
index 0000000..b9f8189
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/angle_adjust/angle_adjust.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ ::frc971::control_loops::AngleAdjustMotor angle_adjust;
+ angle_adjust.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor.q b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
new file mode 100644
index 0000000..a98419a
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
@@ -0,0 +1,24 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group AngleAdjustLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Position {
+ // Angle of the height adjust.
+ double angle;
+ bool bottom_hall_effect;
+ bool middle_hall_effect;
+ // The exact position when the corresponding hall_effect changed.
+ double bottom_calibration;
+ double middle_calibration;
+ };
+
+ queue aos.control_loops.Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group AngleAdjustLoop angle_adjust;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
new file mode 100644
index 0000000..185030f
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeAngleAdjustPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00844804908295, 0.000186726546509, 0.0, 0.706562970689, 0.0353055515475, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeAngleAdjustController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 2.45656297069, 164.64938515, 2172.97100986;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 147.285618609, 4.58304321916, 0.956562970689;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeAngleAdjustPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeAngleAdjustPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeAngleAdjustPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeAngleAdjustLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeAngleAdjustController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
new file mode 100644
index 0000000..7141fa9
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeAngleAdjustPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeAngleAdjustController();
+
+StateFeedbackPlant<3, 1, 1> MakeAngleAdjustPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeAngleAdjustLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
new file mode 100644
index 0000000..ea84dce
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustRawPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00844804908295, 0.0, 0.706562970689;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000186726546509, 0.0353055515475;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeAngleAdjustRawController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.60656297069, 51.0341417582;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 311.565731672, 11.2839301509;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeAngleAdjustRawPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawAngleAdjustPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeAngleAdjustRawPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawAngleAdjustLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeAngleAdjustRawController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h
new file mode 100644
index 0000000..c066e0a
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/unaugmented_angle_adjust_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustRawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeAngleAdjustRawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawAngleAdjustPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawAngleAdjustLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_UNAUGMENTED_ANGLE_ADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index a9ddb12..18df792 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -6,6 +6,20 @@
},
'targets': [
{
+ 'target_name': 'state_feedback_loop',
+ 'type': 'static_library',
+ 'sources': [
+ #'state_feedback_loop.h'
+ #'StateFeedbackLoop.h'
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):eigen',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
'target_name': 'control_loops',
'type': 'static_library',
'sources': ['<@(loop_files)'],
@@ -13,12 +27,10 @@
'header_path': 'frc971/control_loops',
},
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(AOS)/common/common.gyp:control_loop_queues',
'<(AOS)/common/common.gyp:queues',
],
'export_dependent_settings': [
- '<(AOS)/build/aos.gyp:libaos',
'<(AOS)/common/common.gyp:control_loop_queues',
'<(AOS)/common/common.gyp:queues',
],
@@ -33,10 +45,10 @@
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:controls',
- '<(AOS)/build/aos.gyp:libaos',
'control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(EXTERNALS):eigen',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'state_feedback_loop',
],
},
],
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
new file mode 100644
index 0000000..8a8d540
--- /dev/null
+++ b/frc971/control_loops/index/index.cc
@@ -0,0 +1,935 @@
+#include "frc971/control_loops/index/index.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/inttypes.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+
+double IndexMotor::Frisbee::ObserveNoTopDiscSensor(double index_position) {
+ // The absolute disc position in meters.
+ double disc_position = absolute_position(index_position);
+ if (IndexMotor::kTopDiscDetectStart <= disc_position &&
+ disc_position <= IndexMotor::kTopDiscDetectStop) {
+ // Whoops, this shouldn't be happening.
+ // Move the disc off the way that makes most sense.
+ double distance_to_above = IndexMotor::ConvertDiscPositionToIndex(
+ ::std::abs(disc_position - IndexMotor::kTopDiscDetectStop));
+ double distance_to_below = IndexMotor::ConvertDiscPositionToIndex(
+ ::std::abs(disc_position - IndexMotor::kTopDiscDetectStart));
+ if (distance_to_above < distance_to_below) {
+ LOG(INFO, "Moving disc to top slow.\n");
+ // Move it up.
+ index_start_position_ -= distance_to_above;
+ return -distance_to_above;
+ } else {
+ LOG(INFO, "Moving disc to bottom slow.\n");
+ index_start_position_ += distance_to_below;
+ return distance_to_below;
+ }
+ }
+ return 0.0;
+}
+
+IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
+ : aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
+ wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
+ hopper_disc_count_(0),
+ total_disc_count_(0),
+ shot_disc_count_(0),
+ safe_goal_(Goal::HOLD),
+ loader_goal_(LoaderGoal::READY),
+ loader_state_(LoaderState::READY),
+ loader_up_(false),
+ disc_clamped_(false),
+ disc_ejected_(false),
+ last_bottom_disc_detect_(false),
+ last_top_disc_detect_(false),
+ no_prior_position_(true),
+ missing_position_count_(0) {
+}
+
+/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
+/*static*/ const double IndexMotor::kIndexStartPosition = 0.2159;
+/*static*/ const double IndexMotor::kIndexFreeLength =
+ IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
+/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
+ kIndexStartPosition + kIndexFreeLength;
+/*static*/ const double IndexMotor::kReadyToPreload =
+ kLoaderFreeStopPosition - ConvertDiscAngleToDiscPosition(M_PI / 6.0);
+/*static*/ const double IndexMotor::kReadyToLiftPosition =
+ kLoaderFreeStopPosition + 0.2921;
+/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
+/*static*/ const double IndexMotor::kGrabberStartPosition =
+ kReadyToLiftPosition - kGrabberLength;
+/*static*/ const double IndexMotor::kGrabberMovementVelocity = 0.7;
+/*static*/ const double IndexMotor::kLifterStopPosition =
+ kReadyToLiftPosition + 0.161925;
+/*static*/ const double IndexMotor::kLifterMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kEjectorStopPosition =
+ kLifterStopPosition + 0.01;
+/*static*/ const double IndexMotor::kEjectorMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kBottomDiscDetectStart = 0.00;
+/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.13;
+/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.032;
+/*static*/ const ::aos::time::Time IndexMotor::kTransferOffDelay =
+ ::aos::time::Time::InSeconds(0.3);
+
+// TODO(aschuh): Verify these with the sensor actually on.
+/*static*/ const double IndexMotor::kTopDiscDetectStart =
+ (IndexMotor::kLoaderFreeStopPosition -
+ IndexMotor::ConvertDiscAngleToDiscPosition(49 * M_PI / 180));
+/*static*/ const double IndexMotor::kTopDiscDetectStop =
+ (IndexMotor::kLoaderFreeStopPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(19 * M_PI / 180));
+
+// I measured the angle between 2 discs. That then gives me the distance
+// between 2 posedges (or negedges). Then subtract off the width of the
+// positive pulse, and that gives the width of the negative pulse.
+/*static*/ const double IndexMotor::kTopDiscDetectMinSeperation =
+ (IndexMotor::ConvertDiscAngleToDiscPosition(120 * M_PI / 180) -
+ (IndexMotor::kTopDiscDetectStop - IndexMotor::kTopDiscDetectStart));
+
+const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
+
+/*static*/ const int IndexMotor::kGrabbingDelay = 5;
+/*static*/ const int IndexMotor::kLiftingDelay = 30;
+/*static*/ const int IndexMotor::kShootingDelay = 5;
+/*static*/ const int IndexMotor::kLoweringDelay = 20;
+
+// TODO(aschuh): Tune these.
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kMinMotionVoltage = 11.0;
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 20;
+
+/*static*/ double IndexMotor::ConvertDiscAngleToIndex(const double angle) {
+ return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(
+ const double angle) {
+ return angle * (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToDiscAngle(
+ const double position) {
+ return position / (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
+ return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscPosition(const double angle) {
+ return IndexMotor::ConvertDiscAngleToDiscPosition(
+ ConvertIndexToDiscAngle(angle));
+}
+
+/*static*/ double IndexMotor::ConvertTransferToDiscPosition(
+ const double angle) {
+ const double gear_ratio = (1 + (kDiscRadius * 2 + kTransferRollerRadius) /
+ kTransferRollerRadius);
+ return angle / gear_ratio * (kDiscRadius + kTransferRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToIndex(
+ const double position) {
+ return IndexMotor::ConvertDiscAngleToIndex(
+ ConvertDiscPositionToDiscAngle(position));
+}
+
+bool IndexMotor::MinDiscPosition(double *disc_position, Frisbee **found_disc) {
+ bool found_start = false;
+ for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+ Frisbee &frisbee = frisbees_[i];
+ if (!found_start) {
+ if (frisbee.has_position()) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ found_start = true;
+ }
+ } else {
+ if (frisbee.position() <= *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
+ }
+ }
+ return found_start;
+}
+
+bool IndexMotor::MaxDiscPosition(double *disc_position, Frisbee **found_disc) {
+ bool found_start = false;
+ for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+ Frisbee &frisbee = frisbees_[i];
+ if (!found_start) {
+ if (frisbee.has_position()) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ found_start = true;
+ }
+ } else {
+ if (frisbee.position() > *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
+ }
+ }
+ return found_start;
+}
+
+void IndexMotor::IndexStateFeedbackLoop::CapU() {
+ // If the voltage has been low for a large number of cycles, cut the motor
+ // power. This is generally very bad controls practice since this isn't LTI,
+ // but we don't really care about tracking anything other than large step
+ // inputs, and the loader doesn't need to be that accurate.
+ if (::std::abs(U(0, 0)) < kMinMotionVoltage) {
+ ++low_voltage_count_;
+ if (low_voltage_count_ > kNoMotionCuttoffCount) {
+ U(0, 0) = 0.0;
+ }
+ } else {
+ low_voltage_count_ = 0;
+ }
+
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U(i, 0) > U_max(i, 0)) {
+ U(i, 0) = U_max(i, 0);
+ } else if (U(i, 0) < U_min(i, 0)) {
+ U(i, 0) = U_min(i, 0);
+ }
+ }
+}
+
+
+// Positive angle is towards the shooter, and positive power is towards the
+// shooter.
+void IndexMotor::RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status) {
+ Time now = Time::Now();
+ // Make goal easy to work with and sanity check it.
+ Goal goal_enum = static_cast<Goal>(goal->goal_state);
+ if (goal->goal_state < 0 || goal->goal_state > 4) {
+ LOG(ERROR, "Goal state is %"PRId32" which is out of range. Going to HOLD.\n",
+ goal->goal_state);
+ goal_enum = Goal::HOLD;
+ }
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ double intake_voltage = 0.0;
+ double transfer_voltage = 0.0;
+ if (output) {
+ output->intake_voltage = 0.0;
+ output->transfer_voltage = 0.0;
+ output->index_voltage = 0.0;
+ }
+
+ status->ready_to_intake = false;
+
+ // Set the controller to use to be the one designed for the current number of
+ // discs in the hopper. This is safe since the controller prevents the index
+ // from being set out of bounds and picks the closest controller.
+ wrist_loop_->set_controller_index(frisbees_.size());
+
+ // Compute a safe index position that we can use.
+ if (position) {
+ wrist_loop_->Y << position->index_position;
+ // Set the goal to be the current position if this is the first time through
+ // so we don't always spin the indexer to the 0 position before starting.
+ if (no_prior_position_) {
+ wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
+ wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
+ no_prior_position_ = false;
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // The open positions for the upper is right here and isn't a hard edge.
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
+ }
+
+ // If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
+ if (missing_position_count_ > 50) {
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // We can't really trust the open range any more if the crio rebooted.
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
+ // Adjust the disc positions so that they don't have to move.
+ const double disc_offset =
+ position->index_position - wrist_loop_->X_hat(0, 0);
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ frisbee->OffsetDisc(disc_offset);
+ }
+ wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
+ }
+ missing_position_count_ = 0;
+ } else {
+ ++missing_position_count_;
+ }
+ const double index_position = wrist_loop_->X_hat(0, 0);
+
+ if (position) {
+ // Reset the open region if we saw a negedge.
+ if (position->bottom_disc_negedge_wait_count !=
+ last_bottom_disc_negedge_wait_count_) {
+ // Saw a negedge, must be a new region.
+ lower_open_region_.Restart(position->bottom_disc_negedge_wait_position);
+ }
+ // Reset the open region if we saw a negedge.
+ if (position->top_disc_negedge_count != last_top_disc_negedge_count_) {
+ // Saw a negedge, must be a new region.
+ upper_open_region_.Restart(position->top_disc_negedge_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->bottom_disc_detect) {
+ lower_open_region_.Expand(index_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->top_disc_detect) {
+ upper_open_region_.Expand(index_position);
+ }
+
+ if (!position->top_disc_detect) {
+ // We don't see a disc. Verify that there are no discs that we should be
+ // seeing.
+ // Assume that discs will move slow enough that we won't miss one as it
+ // goes by. They will either pile up above or below the sensor.
+
+ double cumulative_offset = 0.0;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ frisbee->OffsetDisc(cumulative_offset);
+ double amount_moved = frisbee->ObserveNoTopDiscSensor(
+ wrist_loop_->X_hat(0, 0));
+ cumulative_offset += amount_moved;
+ }
+ }
+
+ if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
+ const double index_position = wrist_loop_->X_hat(0, 0) -
+ position->index_position + position->top_disc_posedge_position;
+ // TODO(aschuh): Sanity check this number...
+ // Requires storing when the disc was last seen with the sensor off, and
+ // figuring out what to do if things go south.
+
+ // 1 if discs are going up, 0 if we have no clue, and -1 if they are going
+ // down.
+ int disc_direction = 0;
+ if (wrist_loop_->X_hat(1, 0) > 50.0) {
+ disc_direction = 1;
+ } else if (wrist_loop_->X_hat(1, 0) < -50.0) {
+ disc_direction = -1;
+ } else {
+ // Save the upper and lower positions that we last saw a disc at.
+ // If there is a big buffer above, must be a disc from below.
+ // If there is a big buffer below, must be a disc from above.
+ // This should work to replace the velocity threshold above.
+
+ const double open_width = upper_open_region_.width();
+ const double relative_upper_open_precentage =
+ (upper_open_region_.upper_bound() - index_position) / open_width;
+ const double relative_lower_open_precentage =
+ (index_position - upper_open_region_.lower_bound()) / open_width;
+
+ if (ConvertIndexToDiscPosition(open_width) <
+ kTopDiscDetectMinSeperation * 0.9) {
+ LOG(ERROR, "Discs are way too close to each other. Doing nothing\n");
+ } else if (relative_upper_open_precentage > 0.75) {
+ // Looks like it is a disc going down from above since we are near
+ // the upper edge.
+ disc_direction = -1;
+ LOG(INFO, "Disc edge going down\n");
+ } else if (relative_lower_open_precentage > 0.75) {
+ // Looks like it is a disc going up from below since we are near
+ // the lower edge.
+ disc_direction = 1;
+ LOG(INFO, "Disc edge going up\n");
+ } else {
+ LOG(ERROR,
+ "Got an edge in the middle of what should be an open region.\n");
+ LOG(ERROR, "Open width: %f upper precentage %f %%\n",
+ open_width, relative_upper_open_precentage);
+ }
+ }
+
+ if (disc_direction > 0) {
+ // Moving up at a reasonable clip.
+ // Find the highest disc that is below the top disc sensor.
+ // While we are at it, count the number above and log an error if there
+ // are too many.
+ if (frisbees_.size() == 0) {
+ Frisbee new_frisbee;
+ new_frisbee.has_been_indexed_ = true;
+ new_frisbee.index_start_position_ = index_position -
+ ConvertDiscPositionToIndex(kTopDiscDetectStart -
+ kIndexStartPosition);
+ ++hopper_disc_count_;
+ ++total_disc_count_;
+ frisbees_.push_front(new_frisbee);
+ LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
+ }
+
+ int above_disc_count = 0;
+ double highest_position = 0;
+ Frisbee *highest_frisbee_below_sensor = NULL;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ // It is save to use the top position for the cuttoff, since the
+ // sensor being low will result in discs being pushed off of it.
+ if (disc_position >= kTopDiscDetectStop) {
+ ++above_disc_count;
+ } else if (!highest_frisbee_below_sensor ||
+ disc_position > highest_position) {
+ highest_frisbee_below_sensor = &*frisbee;
+ highest_position = disc_position;
+ }
+ }
+ if (above_disc_count > 1) {
+ LOG(ERROR, "We have 2 discs above the top sensor.\n");
+ }
+
+ // We now have the disc. Shift all the ones below the sensor up by the
+ // computed delta.
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ highest_position - kTopDiscDetectStart);
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ if (disc_position < kTopDiscDetectStop) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ LOG(INFO, "Currently have %d discs, saw posedge moving up. "
+ "Moving down by %f to %f\n", frisbees_.size(),
+ ConvertIndexToDiscPosition(disc_delta),
+ highest_frisbee_below_sensor->absolute_position(
+ wrist_loop_->X_hat(0, 0)));
+ } else if (disc_direction < 0) {
+ // Moving down at a reasonable clip.
+ // There can only be 1 disc up top that would give us a posedge.
+ // Find it and place it at the one spot that it can be.
+ double min_disc_position = 0;
+ Frisbee *min_frisbee = NULL;
+ MinDiscPosition(&min_disc_position, &min_frisbee);
+ if (!min_frisbee) {
+ // Uh, oh, we see a disc but there isn't one...
+ LOG(ERROR, "Saw a disc up top but there isn't one in the hopper\n");
+ } else {
+ const double disc_position = min_frisbee->absolute_position(
+ index_position);
+
+ const double disc_delta_meters = disc_position - kTopDiscDetectStop;
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ disc_delta_meters);
+ LOG(INFO, "Posedge going down. Moving top disc down by %f\n",
+ disc_delta_meters);
+ for (auto frisbee = frisbees_.begin(), end = frisbees_.end();
+ frisbee != end; ++frisbee) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ } else {
+ LOG(ERROR, "Not sure how to handle the upper posedge, doing nothing\n");
+ }
+ }
+ }
+
+ // Bool to track if it is safe for the goal to change yet.
+ bool safe_to_change_state = true;
+ switch (safe_goal_) {
+ case Goal::HOLD:
+ // The goal should already be good, so sit tight with everything the same
+ // as it was.
+ break;
+ case Goal::READY_LOWER:
+ case Goal::INTAKE:
+ {
+ if (position) {
+ // Posedge of the disc entering the beam break.
+ if (position->bottom_disc_posedge_count !=
+ last_bottom_disc_posedge_count_) {
+ transfer_frisbee_.Reset();
+ transfer_frisbee_.bottom_posedge_time_ = now;
+ LOG(INFO, "Posedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_posedge_time_.ToSeconds());
+ ++hopper_disc_count_;
+ ++total_disc_count_;
+ }
+
+ // Disc exited the beam break now.
+ if (position->bottom_disc_negedge_count !=
+ last_bottom_disc_negedge_count_) {
+ transfer_frisbee_.bottom_negedge_time_ = now;
+ LOG(INFO, "Negedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_negedge_time_.ToSeconds());
+ frisbees_.push_front(transfer_frisbee_);
+ }
+
+ if (position->bottom_disc_detect) {
+ intake_voltage = transfer_voltage = 12.0;
+ // Must wait until the disc gets out before we can change state.
+ safe_to_change_state = false;
+
+ // TODO(aschuh): A disc on the way through needs to start moving
+ // the indexer if it isn't already moving. Maybe?
+
+ Time elapsed_posedge_time = now -
+ transfer_frisbee_.bottom_posedge_time_;
+ if (elapsed_posedge_time >= Time::InSeconds(0.3)) {
+ // It has been too long. The disc must be jammed.
+ LOG(ERROR, "Been way too long. Jammed disc?\n");
+ }
+ }
+
+ // Check all non-indexed discs and see if they should be indexed.
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ if (!frisbee->has_been_indexed_) {
+ intake_voltage = transfer_voltage = 12.0;
+
+ if (last_bottom_disc_negedge_wait_count_ !=
+ position->bottom_disc_negedge_wait_count) {
+ // We have an index difference.
+ // Save the indexer position, and the time.
+ if (last_bottom_disc_negedge_wait_count_ + 1 !=
+ position->bottom_disc_negedge_wait_count) {
+ LOG(ERROR, "Funny, we got 2 edges since we last checked.\n");
+ }
+
+ // Save the captured position as the position at which the disc
+ // touched the indexer.
+ LOG(INFO, "Grabbed on the index now at %f\n", index_position);
+ frisbee->has_been_indexed_ = true;
+ frisbee->index_start_position_ =
+ position->bottom_disc_negedge_wait_position;
+ }
+ }
+ if (!frisbee->has_been_indexed_) {
+ // All discs must be indexed before it is safe to stop indexing.
+ safe_to_change_state = false;
+ }
+ }
+
+ // Figure out where the indexer should be to move the discs down to
+ // the right position.
+ double max_disc_position = 0;
+ if (MaxDiscPosition(&max_disc_position, NULL)) {
+ LOG(DEBUG, "There is a disc down here!\n");
+ // TODO(aschuh): Figure out what to do if grabbing the next one
+ // would cause things to jam into the loader.
+ // Say we aren't ready any more. Undefined behavior will result if
+ // that isn't observed.
+ double bottom_disc_position =
+ max_disc_position + ConvertDiscAngleToIndex(M_PI);
+ wrist_loop_->R << bottom_disc_position, 0.0;
+
+ // Verify that we are close enough to the goal so that we should be
+ // fine accepting the next disc.
+ double disc_error_meters = ConvertIndexToDiscPosition(
+ wrist_loop_->X_hat(0, 0) - bottom_disc_position);
+ // We are ready for the next disc if the first one is in the first
+ // half circle of the indexer. It will take time for the disc to
+ // come into the indexer, so we will be able to move it out of the
+ // way in time.
+ // This choice also makes sure that we don't claim that we aren't
+ // ready between full speed intaking.
+ if (-ConvertDiscAngleToIndex(M_PI) < disc_error_meters &&
+ disc_error_meters < 0.04) {
+ // We are only ready if we aren't being asked to change state or
+ // are full.
+ status->ready_to_intake =
+ (safe_goal_ == goal_enum) && hopper_disc_count_ < 4;
+ } else {
+ status->ready_to_intake = false;
+ }
+ } else {
+ // No discs! We are always ready for more if we aren't being
+ // asked to change state.
+ status->ready_to_intake = (safe_goal_ == goal_enum);
+ }
+
+ // Turn on the transfer roller if we are ready.
+ if (status->ready_to_intake && hopper_disc_count_ < 4 &&
+ safe_goal_ == Goal::INTAKE) {
+ intake_voltage = transfer_voltage = 12.0;
+ }
+ }
+ LOG(DEBUG, "INTAKE\n");
+ }
+ break;
+ case Goal::READY_SHOOTER:
+ case Goal::SHOOT:
+ // Don't let us leave the shoot or preload state if there are 4 discs in
+ // the hopper.
+ if (hopper_disc_count_ >= 4 && goal_enum != Goal::SHOOT) {
+ safe_to_change_state = false;
+ }
+ // Check if we have any discs to shoot or load and handle them.
+ double min_disc_position = 0;
+ if (MinDiscPosition(&min_disc_position, NULL)) {
+ const double ready_disc_position = min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToPreload - kIndexStartPosition);
+
+ const double grabbed_disc_position =
+ min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToLiftPosition -
+ kIndexStartPosition + 0.10);
+
+ // Check the state of the loader FSM.
+ // If it is ready to load discs, position the disc so that it is ready
+ // to be grabbed.
+ // If it isn't ready, there is a disc in there. It needs to finish it's
+ // cycle first.
+ if (loader_state_ != LoaderState::READY) {
+ // We already have a disc in the loader.
+ // Stage the discs back a bit.
+ wrist_loop_->R << ready_disc_position, 0.0;
+
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+
+ // Must wait until it has been grabbed to continue.
+ if (loader_state_ == LoaderState::GRABBING) {
+ safe_to_change_state = false;
+ }
+ } else {
+ // No disc up top right now.
+ wrist_loop_->R << grabbed_disc_position, 0.0;
+
+ // See if the disc has gotten pretty far up yet.
+ if (wrist_loop_->X_hat(0, 0) > ready_disc_position) {
+ // Point of no return. We are committing to grabbing it now.
+ safe_to_change_state = false;
+ const double robust_grabbed_disc_position =
+ (grabbed_disc_position -
+ ConvertDiscPositionToIndex(kGrabberLength));
+
+ // If close, start grabbing and/or shooting.
+ if (wrist_loop_->X_hat(0, 0) > robust_grabbed_disc_position) {
+ // Start the state machine.
+ if (safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ } else {
+ loader_goal_ = LoaderGoal::GRAB;
+ }
+ // This frisbee is now gone. Take it out of the queue.
+ frisbees_.pop_back();
+ }
+ }
+ }
+ } else {
+ if (loader_state_ != LoaderState::READY) {
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+ } else {
+ // Ok, no discs in sight. Spin the hopper up by 150% of it's full
+ // range and verify that we don't see anything.
+ const double hopper_clear_verification_position =
+ ::std::max(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
+
+ wrist_loop_->R << hopper_clear_verification_position, 0.0;
+ if (::std::abs(wrist_loop_->X_hat(0, 0) -
+ hopper_clear_verification_position) <
+ ConvertDiscPositionToIndex(0.05)) {
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ LOG(ERROR, "Upper is [%f %f]\n",
+ upper_open_region_.upper_bound(),
+ upper_open_region_.lower_bound());
+ LOG(ERROR, "Lower is [%f %f]\n",
+ lower_open_region_.upper_bound(),
+ lower_open_region_.lower_bound());
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
+ }
+ }
+
+ {
+ const double hopper_clear_verification_position =
+ ::std::max(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
+
+ if (wrist_loop_->X_hat(0, 0) >
+ hopper_clear_verification_position +
+ ConvertDiscPositionToIndex(0.05)) {
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ LOG(ERROR, "Upper is [%f %f]\n",
+ upper_open_region_.upper_bound(),
+ upper_open_region_.lower_bound());
+ LOG(ERROR, "Lower is [%f %f]\n",
+ lower_open_region_.upper_bound(),
+ lower_open_region_.lower_bound());
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ > 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still %"PRId32" discs there\n",
+ hopper_disc_count_);
+ }
+ }
+ }
+
+ LOG(DEBUG, "READY_SHOOTER or SHOOT\n");
+ break;
+ }
+
+ // Wait for a period of time to make sure that the disc gets sucked
+ // in properly. We need to do this regardless of what the indexer is doing.
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ if (now - frisbee->bottom_negedge_time_ < kTransferOffDelay) {
+ transfer_voltage = 12.0;
+ }
+ }
+
+ // If we have 4 discs, it is time to preload.
+ if (safe_to_change_state && hopper_disc_count_ >= 4) {
+ switch (safe_goal_) {
+ case Goal::HOLD:
+ case Goal::READY_LOWER:
+ case Goal::INTAKE:
+ safe_goal_ = Goal::READY_SHOOTER;
+ safe_to_change_state = false;
+ LOG(INFO, "We have %"PRId32" discs, time to preload automatically\n",
+ hopper_disc_count_);
+ break;
+ case Goal::READY_SHOOTER:
+ case Goal::SHOOT:
+ break;
+ }
+ }
+
+ // The only way out of the loader is to shoot the disc. The FSM can only go
+ // forwards.
+ switch (loader_state_) {
+ case LoaderState::READY:
+ LOG(DEBUG, "Loader READY\n");
+ // Open and down, ready to accept a disc.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+ if (loader_goal_ == LoaderGoal::GRAB ||
+ loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+ if (loader_goal_ == LoaderGoal::GRAB) {
+ LOG(INFO, "Told to GRAB, moving on\n");
+ } else {
+ LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
+ }
+ loader_state_ = LoaderState::GRABBING;
+ loader_countdown_ = kGrabbingDelay;
+ } else {
+ break;
+ }
+ case LoaderState::GRABBING:
+ LOG(DEBUG, "Loader GRABBING %d\n", loader_countdown_);
+ // Closing the grabber.
+ loader_up_ = false;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::GRABBED;
+ }
+ case LoaderState::GRABBED:
+ LOG(DEBUG, "Loader GRABBED\n");
+ // Grabber closed.
+ loader_up_ = false;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+ shooter.status.FetchLatest();
+ if (shooter.status.get()) {
+ // TODO(aschuh): If we aren't shooting nicely, wait until the shooter
+ // is up to speed rather than just spinning.
+ if (shooter.status->average_velocity > 130 && shooter.status->ready) {
+ loader_state_ = LoaderState::LIFTING;
+ loader_countdown_ = kLiftingDelay;
+ LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
+ } else {
+ LOG(WARNING, "Told to SHOOT_AND_RESET, shooter too slow at %f\n",
+ shooter.status->average_velocity);
+ break;
+ }
+ } else {
+ LOG(ERROR, "Told to SHOOT_AND_RESET, no shooter data, moving on.\n");
+ loader_state_ = LoaderState::LIFTING;
+ loader_countdown_ = kLiftingDelay;
+ }
+ } else if (loader_goal_ == LoaderGoal::READY) {
+ LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
+ break;
+ } else {
+ break;
+ }
+ case LoaderState::LIFTING:
+ LOG(DEBUG, "Loader LIFTING %d\n", loader_countdown_);
+ // Lifting the disc.
+ loader_up_ = true;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::LIFTED;
+ }
+ case LoaderState::LIFTED:
+ LOG(DEBUG, "Loader LIFTED\n");
+ // Disc lifted. Time to eject it out.
+ loader_up_ = true;
+ disc_clamped_ = true;
+ disc_ejected_ = false;
+ loader_state_ = LoaderState::SHOOTING;
+ loader_countdown_ = kShootingDelay;
+ case LoaderState::SHOOTING:
+ LOG(DEBUG, "Loader SHOOTING %d\n", loader_countdown_);
+ // Ejecting the disc into the shooter.
+ loader_up_ = true;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::SHOOT;
+ }
+ case LoaderState::SHOOT:
+ LOG(DEBUG, "Loader SHOOT\n");
+ // The disc has been shot.
+ loader_up_ = true;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ loader_state_ = LoaderState::LOWERING;
+ loader_countdown_ = kLoweringDelay;
+ --hopper_disc_count_;
+ ++shot_disc_count_;
+ case LoaderState::LOWERING:
+ LOG(DEBUG, "Loader LOWERING %d\n", loader_countdown_);
+ // Lowering the loader back down.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = true;
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ }
+ case LoaderState::LOWERED:
+ LOG(DEBUG, "Loader LOWERED\n");
+ // The indexer is lowered.
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+ loader_state_ = LoaderState::READY;
+ // Once we have shot, we need to hang out in READY until otherwise
+ // notified.
+ loader_goal_ = LoaderGoal::READY;
+ break;
+ }
+
+ // Update the observer.
+ wrist_loop_->Update(position != NULL, output == NULL);
+
+ if (position) {
+ LOG(DEBUG, "pos=%f\n", position->index_position);
+ last_bottom_disc_detect_ = position->bottom_disc_detect;
+ last_top_disc_detect_ = position->top_disc_detect;
+ last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
+ last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
+ last_bottom_disc_negedge_wait_count_ =
+ position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ }
+
+ status->hopper_disc_count = hopper_disc_count_;
+ status->total_disc_count = total_disc_count_;
+ status->shot_disc_count = shot_disc_count_;
+ status->preloaded = (loader_state_ != LoaderState::READY);
+
+ if (output) {
+ output->intake_voltage = intake_voltage;
+ output->transfer_voltage = transfer_voltage;
+ output->index_voltage = wrist_loop_->U(0, 0);
+ output->loader_up = loader_up_;
+ output->disc_clamped = disc_clamped_;
+ output->disc_ejected = disc_ejected_;
+ }
+
+ if (safe_to_change_state) {
+ safe_goal_ = goal_enum;
+ }
+ if (hopper_disc_count_ < 0) {
+ LOG(ERROR, "NEGATIVE DISCS. VERY VERY BAD\n");
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index.gyp b/frc971/control_loops/index/index.gyp
new file mode 100644
index 0000000..f61da8c
--- /dev/null
+++ b/frc971/control_loops/index/index.gyp
@@ -0,0 +1,73 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'index_loop',
+ 'type': 'static_library',
+ 'sources': ['index_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/index',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'index_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'index.cc',
+ 'index_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'index_loop',
+ ],
+ },
+ {
+ 'target_name': 'index_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'index_lib_test.cc',
+ 'transfer_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_loop',
+ 'index_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'index',
+ 'type': 'executable',
+ 'sources': [
+ 'index_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'index_lib',
+ 'index_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
new file mode 100644
index 0000000..6b55ec4
--- /dev/null
+++ b/frc971/control_loops/index/index.h
@@ -0,0 +1,346 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_H_
+
+#include <deque>
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class IndexTest_InvalidStateTest_Test;
+class IndexTest_LostDisc_Test;
+}
+
+// This class represents a region of space.
+class Region {
+ public:
+ Region () : upper_bound_(0.0), lower_bound_(0.0) {}
+
+ // Restarts the region tracking by starting over with a 0 width region with
+ // the bounds at [edge, edge].
+ void Restart(double edge) {
+ upper_bound_ = edge;
+ lower_bound_ = edge;
+ }
+
+ // Expands the region to include the new point.
+ void Expand(double new_point) {
+ if (new_point > upper_bound_) {
+ upper_bound_ = new_point;
+ } else if (new_point < lower_bound_) {
+ lower_bound_ = new_point;
+ }
+ }
+
+ // Returns the width of the region.
+ double width() const { return upper_bound_ - lower_bound_; }
+ // Returns the upper and lower bounds.
+ double upper_bound() const { return upper_bound_; }
+ double lower_bound() const { return lower_bound_; }
+
+ private:
+ // Upper bound of the region.
+ double upper_bound_;
+ // Lower bound of the region.
+ double lower_bound_;
+};
+
+class IndexMotor
+ : public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
+ public:
+ explicit IndexMotor(
+ control_loops::IndexLoop *my_index = &control_loops::index_loop);
+
+ static const double kTransferStartPosition;
+ static const double kIndexStartPosition;
+ // The distance from where the disc first grabs on the indexer to where it
+ // just bairly clears the loader.
+ static const double kIndexFreeLength;
+ // The distance to where the disc just starts to enter the loader.
+ static const double kLoaderFreeStopPosition;
+ // The distance to where the next disc gets positioned while the current disc
+ // is shooting.
+ static const double kReadyToPreload;
+
+ // Distance that the grabber pulls the disc in by.
+ static const double kGrabberLength;
+ // Distance to where the grabber takes over.
+ static const double kGrabberStartPosition;
+
+ // The distance to where the disc hits the back of the loader and is ready to
+ // lift.
+ static const double kReadyToLiftPosition;
+
+ static const double kGrabberMovementVelocity;
+ // TODO(aschuh): This depends on the shooter angle...
+ // Distance to where the shooter is up and ready to shoot.
+ static const double kLifterStopPosition;
+ static const double kLifterMovementVelocity;
+
+ // Distance to where the disc has been launched.
+ // TODO(aschuh): This depends on the shooter angle...
+ static const double kEjectorStopPosition;
+ static const double kEjectorMovementVelocity;
+
+ // Start and stop position of the bottom disc detect sensor in meters.
+ static const double kBottomDiscDetectStart;
+ static const double kBottomDiscDetectStop;
+ // Delay between the negedge of the disc detect and when it engages on the
+ // indexer.
+ static const double kBottomDiscIndexDelay;
+
+ // Time after seeing the fourth disc that we need to wait before turning the
+ // transfer roller off.
+ static const ::aos::time::Time kTransferOffDelay;
+
+ static const double kTopDiscDetectStart;
+ static const double kTopDiscDetectStop;
+
+ // Minimum distance between 2 frisbees as seen by the top disc detect sensor.
+ static const double kTopDiscDetectMinSeperation;
+
+ // Converts the angle of the indexer to the angle of the disc.
+ static double ConvertIndexToDiscAngle(const double angle);
+ // Converts the angle of the indexer to the position that the center of the
+ // disc has traveled.
+ static double ConvertIndexToDiscPosition(const double angle);
+
+ // Converts the angle of the transfer roller to the position that the center
+ // of the disc has traveled.
+ static double ConvertTransferToDiscPosition(const double angle);
+
+ // Converts the distance around the indexer to the position of
+ // the index roller.
+ static double ConvertDiscPositionToIndex(const double position);
+ // Converts the angle around the indexer to the position of the index roller.
+ static double ConvertDiscAngleToIndex(const double angle);
+ // Converts the angle around the indexer to the position of the disc in the
+ // indexer.
+ static double ConvertDiscAngleToDiscPosition(const double angle);
+ // Converts the distance around the indexer to the angle of the disc around
+ // the indexer.
+ static double ConvertDiscPositionToDiscAngle(const double position);
+
+ // Disc radius in meters.
+ static const double kDiscRadius;
+ // Roller radius in meters.
+ static const double kRollerRadius;
+ // Transfer roller radius in meters.
+ static const double kTransferRollerRadius;
+
+ // Time that it takes to grab the disc in cycles.
+ static const int kGrabbingDelay;
+ // Time that it takes to lift the loader in cycles.
+ static const int kLiftingDelay;
+ // Time that it takes to shoot the disc in cycles.
+ static const int kShootingDelay;
+ // Time that it takes to lower the loader in cycles.
+ static const int kLoweringDelay;
+
+ // Object representing a Frisbee tracked by the indexer.
+ class Frisbee {
+ public:
+ Frisbee()
+ : bottom_posedge_time_(0, 0),
+ bottom_negedge_time_(0, 0) {
+ Reset();
+ }
+
+ // Resets a Frisbee so it can be reused.
+ void Reset() {
+ bottom_posedge_time_ = ::aos::time::Time(0, 0);
+ bottom_negedge_time_ = ::aos::time::Time(0, 0);
+ has_been_indexed_ = false;
+ index_start_position_ = 0.0;
+ }
+
+ // Returns true if the position is valid.
+ bool has_position() const {
+ return has_been_indexed_;
+ }
+
+ // Returns the most up to date and accurate position that we have for the
+ // disc. This is the indexer position that the disc grabbed at.
+ double position() const {
+ return index_start_position_;
+ }
+
+ // Returns the absolute position of the disc in meters in the hopper given
+ // that the indexer is at the provided position.
+ double absolute_position(const double index_position) const {
+ return IndexMotor::ConvertIndexToDiscPosition(
+ index_position - index_start_position_) +
+ IndexMotor::kIndexStartPosition;
+ }
+
+ // Shifts the disc down the indexer by the provided offset. This is to
+ // handle when the cRIO reboots.
+ void OffsetDisc(double offset) {
+ index_start_position_ += offset;
+ }
+
+ // Potentially offsets the position with the knowledge that no discs are
+ // currently blocking the top sensor. This knowledge can be used to move
+ // this disc if it is believed to be blocking the top sensor.
+ // Returns the amount that the disc moved due to this observation.
+ double ObserveNoTopDiscSensor(double index_position);
+
+ // Posedge and negedge disc times.
+ ::aos::time::Time bottom_posedge_time_;
+ ::aos::time::Time bottom_negedge_time_;
+
+ // True if the disc has a valid index position.
+ bool has_been_indexed_;
+ // Location of the index when the disc first contacted it.
+ double index_start_position_;
+ };
+
+ // Returns where the indexer thinks the frisbees are.
+ const ::std::deque<Frisbee> &frisbees() const { return frisbees_; }
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status);
+
+ private:
+ friend class testing::IndexTest_InvalidStateTest_Test;
+ friend class testing::IndexTest_LostDisc_Test;
+
+ // This class implements the CapU function correctly given all the extra
+ // information that we know about from the wrist motor.
+ class IndexStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ IndexStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ low_voltage_count_(0) {
+ }
+
+ // Voltage below which the indexer won't move with a disc in it.
+ static const double kMinMotionVoltage;
+ // Maximum number of cycles to apply a low voltage to the motor.
+ static const double kNoMotionCuttoffCount;
+
+ // Caps U, but disables the motor after a number of cycles of inactivity.
+ virtual void CapU();
+ private:
+ // Number of cycles that we have seen a small voltage being applied.
+ uint32_t low_voltage_count_;
+ };
+
+ // Sets disc_position to the minimum or maximum disc position.
+ // Sets found_disc to point to the frisbee that was found, and ignores it if
+ // found_disc is NULL.
+ // Returns true if there were discs, and false if there weren't.
+ // On false, disc_position is left unmodified.
+ bool MinDiscPosition(double *disc_position, Frisbee **found_disc);
+ bool MaxDiscPosition(double *disc_position, Frisbee **found_disc);
+
+ // The state feedback control loop to talk to for the index.
+ ::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
+
+ // Count of the number of discs that we have collected.
+ int32_t hopper_disc_count_;
+ int32_t total_disc_count_;
+ int32_t shot_disc_count_;
+
+ enum class Goal {
+ // Hold position, in a low power state.
+ HOLD = 0,
+ // Get ready to load discs by shifting the discs down.
+ READY_LOWER = 1,
+ // Ready the discs, spin up the transfer roller, and accept discs.
+ INTAKE = 2,
+ // Get ready to shoot, and place a disc in the loader.
+ READY_SHOOTER = 3,
+ // Shoot at will.
+ SHOOT = 4
+ };
+
+ // These two enums command and track the loader loading discs into the
+ // shooter.
+ enum class LoaderState {
+ // Open and down, ready to accept a disc.
+ READY,
+ // Closing the grabber.
+ GRABBING,
+ // Grabber closed.
+ GRABBED,
+ // Lifting the disc.
+ LIFTING,
+ // Disc lifted.
+ LIFTED,
+ // Ejecting the disc into the shooter.
+ SHOOTING,
+ // The disc has been shot.
+ SHOOT,
+ // Lowering the loader back down.
+ LOWERING,
+ // The indexer is lowered.
+ LOWERED
+ };
+
+ // TODO(aschuh): If we are grabbed and asked to be ready, now what?
+ // LOG ?
+ enum class LoaderGoal {
+ // Get the loader ready to accept another disc.
+ READY,
+ // Grab a disc now.
+ GRAB,
+ // Lift it up, shoot, and reset.
+ // Waits to shoot until the shooter is stable.
+ // Resets the goal to READY once one disc has been shot.
+ SHOOT_AND_RESET
+ };
+
+ // The current goal
+ Goal safe_goal_;
+
+ // Loader goal, state, and counter.
+ LoaderGoal loader_goal_;
+ LoaderState loader_state_;
+ int loader_countdown_;
+
+ // Current state of the pistons.
+ bool loader_up_;
+ bool disc_clamped_;
+ bool disc_ejected_;
+
+ // The frisbee that is flying through the transfer rollers.
+ Frisbee transfer_frisbee_;
+
+ // Bottom disc detect from the last valid packet for detecting edges.
+ bool last_bottom_disc_detect_;
+ bool last_top_disc_detect_;
+ int32_t last_bottom_disc_posedge_count_;
+ int32_t last_bottom_disc_negedge_count_;
+ int32_t last_bottom_disc_negedge_wait_count_;
+ int32_t last_top_disc_posedge_count_;
+ int32_t last_top_disc_negedge_count_;
+
+ // Frisbees are in order such that the newest frisbee is on the front.
+ ::std::deque<Frisbee> frisbees_;
+
+ // True if we haven't seen a position before.
+ bool no_prior_position_;
+ // Number of position messages that we have missed in a row.
+ uint32_t missing_position_count_;
+
+ // The no-disc regions for both the bottom and top beam break sensors.
+ Region upper_open_region_;
+ Region lower_open_region_;
+
+ DISALLOW_COPY_AND_ASSIGN(IndexMotor);
+};
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
new file mode 100644
index 0000000..4bc22c6
--- /dev/null
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -0,0 +1,1320 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/index/index.h"
+#include "frc971/control_loops/index/index_motor_plant.h"
+#include "frc971/control_loops/index/transfer_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+class Frisbee {
+ public:
+ // Creates a frisbee starting at the specified position in the frisbee path,
+ // and with the transfer and index rollers at the specified positions.
+ Frisbee(double transfer_roller_position,
+ double index_roller_position,
+ double position = IndexMotor::kBottomDiscDetectStart - 0.001)
+ : transfer_roller_position_(transfer_roller_position),
+ index_roller_position_(index_roller_position),
+ position_(position),
+ has_been_shot_(false),
+ has_bottom_disc_negedge_wait_position_(false),
+ bottom_disc_negedge_wait_position_(0.0),
+ after_negedge_time_left_(IndexMotor::kBottomDiscIndexDelay),
+ counted_negedge_wait_(false),
+ has_top_disc_posedge_position_(false),
+ top_disc_posedge_position_(0.0),
+ has_top_disc_negedge_position_(false),
+ top_disc_negedge_position_(0.0) {
+ }
+
+ // Returns true if the frisbee is controlled by the transfer roller.
+ bool IsTouchingTransfer(double position) const {
+ return (position >= IndexMotor::kBottomDiscDetectStart &&
+ position <= IndexMotor::kIndexStartPosition);
+ }
+ bool IsTouchingTransfer() const { return IsTouchingTransfer(position_); }
+
+ // Returns true if the frisbee is in a place where it is unsafe to grab.
+ bool IsUnsafeToGrab() const {
+ return (position_ > (IndexMotor::kLoaderFreeStopPosition) &&
+ position_ < IndexMotor::kGrabberStartPosition);
+ }
+
+ // Returns true if the frisbee is controlled by the indexing roller.
+ bool IsTouchingIndex(double position) const {
+ return (position >= IndexMotor::kIndexStartPosition &&
+ position < IndexMotor::kGrabberStartPosition);
+ }
+ bool IsTouchingIndex() const { return IsTouchingIndex(position_); }
+
+ // Returns true if the frisbee is in a position such that the disc can be
+ // lifted.
+ bool IsUnsafeToLift() const {
+ return (position_ >= IndexMotor::kLoaderFreeStopPosition &&
+ position_ <= IndexMotor::kReadyToLiftPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the grabber will
+ // pull it into the loader.
+ bool IsTouchingGrabber() const {
+ return (position_ >= IndexMotor::kGrabberStartPosition &&
+ position_ < IndexMotor::kReadyToLiftPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the disc can be
+ // lifted.
+ bool IsTouchingLoader() const {
+ return (position_ >= IndexMotor::kReadyToLiftPosition &&
+ position_ < IndexMotor::kLifterStopPosition);
+ }
+
+ // Returns true if the frisbee is touching the ejector.
+ bool IsTouchingEjector() const {
+ return (position_ >= IndexMotor::kLifterStopPosition &&
+ position_ < IndexMotor::kEjectorStopPosition);
+ }
+
+ // Returns true if the disc is triggering the bottom disc detect sensor.
+ bool bottom_disc_detect(double position) const {
+ return (position >= IndexMotor::kBottomDiscDetectStart &&
+ position <= IndexMotor::kBottomDiscDetectStop);
+ }
+ bool bottom_disc_detect() const { return bottom_disc_detect(position_); }
+
+ // Returns true if the disc is triggering the top disc detect sensor.
+ bool top_disc_detect(double position) const {
+ return (position >= IndexMotor::kTopDiscDetectStart &&
+ position <= IndexMotor::kTopDiscDetectStop);
+ }
+ bool top_disc_detect() const { return top_disc_detect(position_); }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_bottom_disc_detect(double disc_dx) {
+ if (bottom_disc_detect()) {
+ return !bottom_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Returns true if the bottom disc sensor will posedge after the disc moves
+ // by dx.
+ bool will_posedge_top_disc_detect(double disc_dx) {
+ if (!top_disc_detect()) {
+ return top_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_top_disc_detect(double disc_dx) {
+ if (top_disc_detect()) {
+ return !top_disc_detect(position_ + disc_dx);
+ }
+ return false;
+ }
+
+ // Handles potentially dealing with the delayed negedge.
+ // Computes the index position when time expires using the cached old indexer
+ // position, the elapsed time, and the average velocity.
+ void HandleAfterNegedge(
+ double index_velocity, double elapsed_time, double time_left) {
+ if (!has_bottom_disc_negedge_wait_position_) {
+ if (after_negedge_time_left_ < time_left) {
+ // Assume constant velocity and compute the position.
+ bottom_disc_negedge_wait_position_ =
+ index_roller_position_ +
+ index_velocity * (elapsed_time + after_negedge_time_left_);
+ after_negedge_time_left_ = 0.0;
+ has_bottom_disc_negedge_wait_position_ = true;
+ } else {
+ after_negedge_time_left_ -= time_left;
+ }
+ }
+ }
+
+ // Updates the position of the disc assuming that it has started on the
+ // transfer. The elapsed time is the simulated amount of time that has
+ // elapsed since the simulation timestep started and this method was called.
+ // time_left is the amount of time left to spend during this timestep.
+ double UpdateTransferPositionForTime(double transfer_roller_velocity,
+ double index_roller_velocity,
+ double elapsed_time,
+ double time_left) {
+ double disc_dx = IndexMotor::ConvertTransferToDiscPosition(
+ transfer_roller_velocity * time_left);
+ bool shrunk_time = false;
+ if (!IsTouchingTransfer(position_ + disc_dx)) {
+ shrunk_time = true;
+ time_left = (IndexMotor::kIndexStartPosition - position_) /
+ transfer_roller_velocity;
+ disc_dx = IndexMotor::ConvertTransferToDiscPosition(
+ transfer_roller_velocity * time_left);
+ }
+
+ if (will_negedge_bottom_disc_detect(disc_dx)) {
+ // Compute the time from the negedge to the end of the cycle assuming
+ // constant velocity.
+ const double elapsed_time =
+ (position_ + disc_dx - IndexMotor::kBottomDiscDetectStop) /
+ disc_dx * time_left;
+
+ // I am not implementing very short delays until this fails.
+ assert(elapsed_time <= after_negedge_time_left_);
+ after_negedge_time_left_ -= elapsed_time;
+ } else if (position_ >= IndexMotor::kBottomDiscDetectStop) {
+ HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
+ }
+
+ if (shrunk_time) {
+ EXPECT_LT(0, transfer_roller_velocity);
+ position_ = IndexMotor::kIndexStartPosition;
+ } else {
+ position_ += disc_dx;
+ }
+ LOG(DEBUG, "Transfer Roller: Disc is at %f\n", position_);
+ return time_left;
+ }
+
+ // Updates the position of the disc assuming that it has started on the
+ // indexer. The elapsed time is the simulated amount of time that has
+ // elapsed since the simulation timestep started and this method was called.
+ // time_left is the amount of time left to spend during this timestep.
+ double UpdateIndexPositionForTime(double index_roller_velocity,
+ double elapsed_time,
+ double time_left) {
+ double index_dx = IndexMotor::ConvertIndexToDiscPosition(
+ index_roller_velocity * time_left);
+ bool shrunk_time = false;
+ if (!IsTouchingIndex(position_ + index_dx)) {
+ shrunk_time = true;
+ time_left = (IndexMotor::kGrabberStartPosition - position_) /
+ index_roller_velocity;
+ index_dx = IndexMotor::ConvertTransferToDiscPosition(
+ index_roller_velocity * time_left);
+ }
+
+ if (position_ >= IndexMotor::kBottomDiscDetectStop) {
+ HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
+ }
+
+ if (will_posedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ double edge_position = index_roller_velocity > 0 ?
+ IndexMotor::kTopDiscDetectStart : IndexMotor::kTopDiscDetectStop;
+ const double disc_time =
+ (edge_position - position_) / index_roller_velocity;
+ top_disc_posedge_position_ = index_roller_position_ +
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
+ has_top_disc_posedge_position_ = true;
+ LOG(INFO, "Posedge on top sensor at %f\n", top_disc_posedge_position_);
+ }
+
+ if (will_negedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ double edge_position = index_roller_velocity > 0 ?
+ IndexMotor::kTopDiscDetectStop : IndexMotor::kTopDiscDetectStart;
+ const double disc_time =
+ (edge_position - position_) / index_roller_velocity;
+ top_disc_negedge_position_ = index_roller_position_ +
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
+ has_top_disc_negedge_position_ = true;
+ LOG(INFO, "Negedge on top sensor at %f\n", top_disc_negedge_position_);
+ }
+
+ if (shrunk_time) {
+ if (index_roller_velocity > 0) {
+ position_ = IndexMotor::kGrabberStartPosition;
+ } else {
+ position_ = IndexMotor::kIndexStartPosition;
+ }
+ } else {
+ position_ += index_dx;
+ }
+ LOG(DEBUG, "Index: Disc is at %f\n", position_);
+ return time_left;
+ }
+
+ // Updates the position given velocities, piston comands, and the time left in
+ // the simulation cycle.
+ void UpdatePositionForTime(double transfer_roller_velocity,
+ double index_roller_velocity,
+ bool clamped,
+ bool lifted,
+ bool ejected,
+ double time_left) {
+ double elapsed_time = 0.0;
+ // We are making this assumption below
+ ASSERT_LE(IndexMotor::kBottomDiscDetectStop,
+ IndexMotor::kIndexStartPosition);
+ if (IsTouchingTransfer() || position() < 0.0) {
+ double deltat = UpdateTransferPositionForTime(
+ transfer_roller_velocity, index_roller_velocity,
+ elapsed_time, time_left);
+ time_left -= deltat;
+ elapsed_time += deltat;
+ }
+
+ if (IsTouchingIndex() && time_left >= 0) {
+ // Verify that we aren't trying to grab or lift when it isn't safe.
+ EXPECT_FALSE(clamped && IsUnsafeToGrab());
+ EXPECT_FALSE(lifted && IsUnsafeToLift());
+
+ double deltat = UpdateIndexPositionForTime(
+ index_roller_velocity, elapsed_time, time_left);
+ time_left -= deltat;
+ elapsed_time += deltat;
+ }
+ if (IsTouchingGrabber()) {
+ if (clamped) {
+ const double grabber_dx =
+ IndexMotor::kGrabberMovementVelocity * time_left;
+ position_ = ::std::min(position_ + grabber_dx,
+ IndexMotor::kReadyToLiftPosition);
+ }
+ EXPECT_FALSE(lifted) << "Can't lift while in grabber";
+ EXPECT_FALSE(ejected) << "Can't eject while in grabber";
+ LOG(DEBUG, "Grabber: Disc is at %f\n", position_);
+ } else if (IsTouchingLoader()) {
+ if (lifted) {
+ const double lifter_dx =
+ IndexMotor::kLifterMovementVelocity * time_left;
+ position_ = ::std::min(position_ + lifter_dx,
+ IndexMotor::kLifterStopPosition);
+ }
+ EXPECT_TRUE(clamped);
+ EXPECT_FALSE(ejected);
+ LOG(DEBUG, "Loader: Disc is at %f\n", position_);
+ } else if (IsTouchingEjector()) {
+ EXPECT_TRUE(lifted);
+ if (ejected) {
+ const double ejector_dx =
+ IndexMotor::kEjectorMovementVelocity * time_left;
+ position_ = ::std::min(position_ + ejector_dx,
+ IndexMotor::kEjectorStopPosition);
+ EXPECT_FALSE(clamped);
+ }
+ LOG(DEBUG, "Ejector: Disc is at %f\n", position_);
+ } else if (position_ == IndexMotor::kEjectorStopPosition) {
+ LOG(DEBUG, "Shot: Disc is at %f\n", position_);
+ has_been_shot_ = true;
+ }
+ }
+
+ // Updates the position of the frisbee in the frisbee path.
+ void UpdatePosition(double transfer_roller_position,
+ double index_roller_position,
+ bool clamped,
+ bool lifted,
+ bool ejected) {
+ const double transfer_roller_velocity =
+ (transfer_roller_position - transfer_roller_position_) / 0.01;
+ const double index_roller_velocity =
+ (index_roller_position - index_roller_position_) / 0.01;
+ UpdatePositionForTime(transfer_roller_velocity,
+ index_roller_velocity,
+ clamped,
+ lifted,
+ ejected,
+ 0.01);
+ transfer_roller_position_ = transfer_roller_position;
+ index_roller_position_ = index_roller_position;
+ }
+
+ // Returns if the disc has been shot and can be removed from the robot.
+ bool has_been_shot() const {
+ return has_been_shot_;
+ }
+
+ // Returns the position of the disc in the system.
+ double position() const {
+ return position_;
+ }
+
+ // Sets whether or not we have counted the delayed negedge.
+ void set_counted_negedge_wait(bool counted_negedge_wait) {
+ counted_negedge_wait_ = counted_negedge_wait;
+ }
+
+ // Returns if we have counted the delayed negedge.
+ bool counted_negedge_wait() { return counted_negedge_wait_; }
+
+ // Returns true if the negedge wait position is valid.
+ bool has_bottom_disc_negedge_wait_position() {
+ return has_bottom_disc_negedge_wait_position_;
+ }
+
+ // Returns the negedge wait position.
+ double bottom_disc_negedge_wait_position() {
+ return bottom_disc_negedge_wait_position_;
+ }
+
+ // Returns the last position where a posedge was seen.
+ double top_disc_posedge_position() { return top_disc_posedge_position_; }
+
+ // Returns the last position where a negedge was seen.
+ double top_disc_negedge_position() { return top_disc_negedge_position_; }
+
+ // True if the top disc has seen a posedge.
+ // Reading this flag clears it.
+ bool has_top_disc_posedge_position() {
+ bool prev = has_top_disc_posedge_position_;
+ has_top_disc_posedge_position_ = false;
+ return prev;
+ }
+
+ // True if the top disc has seen a negedge.
+ // Reading this flag clears it.
+ bool has_top_disc_negedge_position() {
+ bool prev = has_top_disc_negedge_position_;
+ has_top_disc_negedge_position_ = false;
+ return prev;
+ }
+
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndex(double offset) {
+ index_roller_position_ += offset;
+ }
+
+ private:
+ // Previous transfer roller position for computing deltas.
+ double transfer_roller_position_;
+ // Previous index roller position for computing deltas.
+ double index_roller_position_;
+ // Position in the robot.
+ double position_;
+ // True if the disc has been shot.
+ bool has_been_shot_;
+ // True if the delay after the negedge of the beam break has occured.
+ bool has_bottom_disc_negedge_wait_position_;
+ // Posiiton of the indexer when the delayed negedge occures.
+ double bottom_disc_negedge_wait_position_;
+ // Time left after the negedge before we need to sample the indexer position.
+ double after_negedge_time_left_;
+ // Bool for the user to record if they have counted the negedge from this
+ // disc.
+ bool counted_negedge_wait_;
+ // True if the top disc sensor posedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_posedge_position_;
+ // The position at which the posedge occured.
+ double top_disc_posedge_position_;
+ // True if the top disc sensor negedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_negedge_position_;
+ // The position at which the negedge occured.
+ double top_disc_negedge_position_;
+};
+
+
+// Class which simulates the index and sends out queue messages containing the
+// position.
+class IndexMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // index, which will be treated as 0 by the encoder.
+ IndexMotorSimulation()
+ : index_plant_(new StateFeedbackPlant<2, 1, 1>(MakeIndexPlant())),
+ transfer_plant_(new StateFeedbackPlant<2, 1, 1>(MakeTransferPlant())),
+ bottom_disc_posedge_count_(0),
+ bottom_disc_negedge_count_(0),
+ bottom_disc_negedge_wait_count_(0),
+ bottom_disc_negedge_wait_position_(0),
+ top_disc_posedge_count_(0),
+ top_disc_posedge_position_(0.0),
+ top_disc_negedge_count_(0),
+ top_disc_negedge_position_(0.0),
+ my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status") {
+ }
+
+ // Starts a disc offset from the start of the index.
+ void InsertDisc(double offset = IndexMotor::kBottomDiscDetectStart - 0.001) {
+ Frisbee new_frisbee(transfer_roller_position(),
+ index_roller_position(),
+ offset);
+ ASSERT_FALSE(new_frisbee.bottom_disc_detect());
+ ASSERT_FALSE(new_frisbee.top_disc_detect());
+ frisbees.push_back(new_frisbee);
+ }
+
+ // Returns true if the bottom disc sensor is triggered.
+ bool BottomDiscDetect() const {
+ bool bottom_disc_detect = false;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ bottom_disc_detect |= frisbee->bottom_disc_detect();
+ }
+ return bottom_disc_detect;
+ }
+
+ // Returns true if the top disc sensor is triggered.
+ bool TopDiscDetect() const {
+ bool top_disc_detect = false;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ top_disc_detect |= frisbee->top_disc_detect();
+ }
+ return top_disc_detect;
+ }
+
+ // Updates all discs, and verifies that the state of the system is sane.
+ void UpdateDiscs(bool clamped, bool lifted, bool ejected) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ const bool old_bottom_disc_detect = frisbee->bottom_disc_detect();
+ frisbee->UpdatePosition(transfer_roller_position(),
+ index_roller_position(),
+ clamped,
+ lifted,
+ ejected);
+
+ // Look for disc detect edges and report them.
+ const bool bottom_disc_detect = frisbee->bottom_disc_detect();
+ if (old_bottom_disc_detect && !bottom_disc_detect) {
+ LOG(INFO, "Negedge of disc\n");
+ ++bottom_disc_negedge_count_;
+ }
+
+ if (!old_bottom_disc_detect && frisbee->bottom_disc_detect()) {
+ LOG(INFO, "Posedge of disc\n");
+ ++bottom_disc_posedge_count_;
+ }
+
+ // See if the frisbee has a delayed negedge and encoder value to report
+ // back.
+ if (frisbee->has_bottom_disc_negedge_wait_position()) {
+ if (!frisbee->counted_negedge_wait()) {
+ bottom_disc_negedge_wait_position_ =
+ frisbee->bottom_disc_negedge_wait_position();
+ ++bottom_disc_negedge_wait_count_;
+ frisbee->set_counted_negedge_wait(true);
+ }
+ }
+ if (frisbee->has_top_disc_posedge_position()) {
+ ++top_disc_posedge_count_;
+ top_disc_posedge_position_ = frisbee->top_disc_posedge_position();
+ }
+ if (frisbee->has_top_disc_negedge_position()) {
+ ++top_disc_negedge_count_;
+ top_disc_negedge_position_ = frisbee->top_disc_negedge_position();
+ }
+ }
+
+ // Make sure nobody is too close to anybody else.
+ Frisbee *last_frisbee = NULL;
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ if (last_frisbee) {
+ const double distance = frisbee->position() - last_frisbee->position();
+ double min_distance;
+ if (frisbee->IsTouchingTransfer() ||
+ last_frisbee->IsTouchingTransfer()) {
+ min_distance = 0.3;
+ } else {
+ min_distance =
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI * 2.0 / 3.0);
+ }
+
+ EXPECT_LT(min_distance, ::std::abs(distance)) << "Discs too close";
+ }
+ last_frisbee = &*frisbee;
+ }
+
+ // Remove any shot frisbees.
+ for (int i = 0; i < static_cast<int>(frisbees.size()); ++i) {
+ if (frisbees[i].has_been_shot()) {
+ shot_frisbees.push_back(frisbees[i]);
+ frisbees.erase(frisbees.begin() + i);
+ --i;
+ }
+ }
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::IndexLoop::Position> position =
+ my_index_loop_.position.MakeMessage();
+ position->index_position = index_roller_position();
+ position->bottom_disc_detect = BottomDiscDetect();
+ position->top_disc_detect = TopDiscDetect();
+ position->bottom_disc_posedge_count = bottom_disc_posedge_count_;
+ position->bottom_disc_negedge_count = bottom_disc_negedge_count_;
+ position->bottom_disc_negedge_wait_count = bottom_disc_negedge_wait_count_;
+ position->bottom_disc_negedge_wait_position =
+ bottom_disc_negedge_wait_position_;
+ position->top_disc_posedge_count = top_disc_posedge_count_;
+ position->top_disc_posedge_position = top_disc_posedge_position_;
+ position->top_disc_negedge_count = top_disc_negedge_count_;
+ position->top_disc_negedge_position = top_disc_negedge_position_;
+ LOG(DEBUG,
+ "bdd: %x tdd: %x posedge %d negedge %d "
+ "delaycount %d delaypos %f topcount %d toppos %f "
+ "topcount %d toppos %f\n",
+ position->bottom_disc_detect,
+ position->top_disc_detect,
+ position->bottom_disc_posedge_count,
+ position->bottom_disc_negedge_count,
+ position->bottom_disc_negedge_wait_count,
+ position->bottom_disc_negedge_wait_position,
+ position->top_disc_posedge_count,
+ position->top_disc_posedge_position,
+ position->top_disc_negedge_count,
+ position->top_disc_negedge_position);
+ position.Send();
+ }
+
+ // Simulates the index moving for one timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+
+ index_plant_->set_plant_index(frisbees.size());
+ index_plant_->U << my_index_loop_.output->index_voltage;
+ index_plant_->Update();
+
+ transfer_plant_->U << my_index_loop_.output->transfer_voltage;
+ transfer_plant_->Update();
+ LOG(DEBUG, "tv: %f iv: %f tp : %f ip: %f\n",
+ my_index_loop_.output->transfer_voltage,
+ my_index_loop_.output->index_voltage,
+ transfer_roller_position(), index_roller_position());
+
+ UpdateDiscs(my_index_loop_.output->disc_clamped,
+ my_index_loop_.output->loader_up,
+ my_index_loop_.output->disc_ejected);
+ }
+
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndices(double offset) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ frisbee->OffsetIndex(offset);
+ }
+ }
+
+ // Plants for the index and transfer rollers.
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> index_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> transfer_plant_;
+
+ // Posedge and negedge counts for the beam break.
+ int32_t bottom_disc_posedge_count_;
+ int32_t bottom_disc_negedge_count_;
+
+ // Delayed negedge count and corrisponding position.
+ int32_t bottom_disc_negedge_wait_count_;
+ int32_t bottom_disc_negedge_wait_position_;
+
+ // Posedge count and position for the upper disc sensor.
+ int32_t top_disc_posedge_count_;
+ double top_disc_posedge_position_;
+
+ // Negedge count and position for the upper disc sensor.
+ int32_t top_disc_negedge_count_;
+ double top_disc_negedge_position_;
+
+ // Returns the absolute angle of the index.
+ double index_roller_position() const {
+ return index_plant_->Y(0, 0);
+ }
+
+ // Returns the absolute angle of the index.
+ double transfer_roller_position() const {
+ return transfer_plant_->Y(0, 0);
+ }
+
+ // Frisbees being tracked in the robot.
+ ::std::vector<Frisbee> frisbees;
+ // Frisbees that have been shot.
+ ::std::vector<Frisbee> shot_frisbees;
+
+ private:
+ // Control loop for the indexer.
+ IndexLoop my_index_loop_;
+};
+
+
+class IndexTest : public ::testing::Test {
+ protected:
+ IndexTest() : my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status"),
+ index_motor_(&my_index_loop_),
+ index_motor_plant_(),
+ loop_count_(0) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ ::frc971::control_loops::shooter.goal.Clear();
+ ::frc971::control_loops::shooter.status.Clear();
+ ::frc971::control_loops::shooter.output.Clear();
+ ::frc971::control_loops::shooter.position.Clear();
+ SendDSPacket(true);
+ Time::EnableMockTime(Time(0, 0));
+ }
+
+ virtual ~IndexTest() {
+ ::aos::robot_state.Clear();
+ ::frc971::control_loops::shooter.goal.Clear();
+ ::frc971::control_loops::shooter.status.Clear();
+ ::frc971::control_loops::shooter.output.Clear();
+ ::frc971::control_loops::shooter.position.Clear();
+ Time::DisableMockTime();
+ }
+
+ // Sends a DS packet with the enable bit set to enabled.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ // Updates the current mock time.
+ void UpdateTime() {
+ loop_count_ += 1;
+ Time::SetMockTime(Time::InMS(10 * loop_count_));
+ }
+
+ // Lets N cycles of time pass.
+ void SimulateNCycles(int cycles) {
+ for (int i = 0; i < cycles; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+ }
+
+ // Loads n discs into the indexer at the bottom.
+ void LoadNDiscs(int n) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ // Spin it up.
+ SimulateNCycles(100);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+
+ // Stuff N discs in, waiting between each one for a tiny bit of time so they
+ // don't get too close.
+ int num_grabbed = 0;
+ int wait_counter = 0;
+ while (num_grabbed < n) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (!index_motor_plant_.BottomDiscDetect()) {
+ if (wait_counter > 0) {
+ --wait_counter;
+ } else {
+ index_motor_plant_.InsertDisc();
+ ++num_grabbed;
+ wait_counter = 9;
+ }
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ // Settle.
+ int settling_time =
+ static_cast<int>(IndexMotor::kTransferOffDelay.ToSeconds() * 100.0) + 2;
+ for (int i = 0; i < 100; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ my_index_loop_.output.FetchLatest();
+ my_index_loop_.status.FetchLatest();
+ if (i <= settling_time || my_index_loop_.status->hopper_disc_count < 4) {
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ } else {
+ EXPECT_EQ(0.0, my_index_loop_.output->transfer_voltage);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+ }
+
+ // Loads 2 discs, and then offsets them. We then send the first disc to the
+ // grabber, and the second disc back down to the bottom. Verify that both
+ // discs get found correctly. Positive numbers shift the discs up.
+ void TestDualLostDiscs(double top_disc_offset, double bottom_disc_offset) {
+ LoadNDiscs(2);
+
+ // Move them in the indexer so they need to be re-found.
+ // The top one is moved further than the bottom one so that both edges need to
+ // be inspected.
+ index_motor_plant_.frisbees[0].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(top_disc_offset));
+ index_motor_plant_.frisbees[1].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(bottom_disc_offset));
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that the disc has been grabbed.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+ // And that we are preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Pull the disc back down.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(300);
+
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.02);
+
+ // Verify that we found the disc as accurately as the FPGA allows.
+ my_index_loop_.position.FetchLatest();
+ EXPECT_NEAR(
+ index_motor_.frisbees()[0].absolute_position(
+ my_index_loop_.position->index_position),
+ index_motor_plant_.frisbees[1].position(), 0.0001);
+ }
+
+ // Copy of core that works in this process only.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ IndexLoop my_index_loop_;
+
+ // Create a loop and simulation plant.
+ IndexMotor index_motor_;
+ IndexMotorSimulation index_motor_plant_;
+
+ // Number of loop cycles that have been executed for tracking the current
+ // time.
+ int loop_count_;
+};
+
+// Tests that the index grabs 1 disc and places it at the correct position.
+TEST_F(IndexTest, GrabSingleDisc) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 250; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ }
+ if (i > 0) {
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[0].position(), 0.05);
+}
+
+// Tests that the index grabs 1 disc and places it at the correct position when
+// told to hold immediately after the disc starts into the bot.
+TEST_F(IndexTest, GrabAndHold) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ // The disc has been seen. Tell the indexer to now hold.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(0).Send();
+ } else if (i > 102) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(0, my_index_loop_.status->shot_disc_count);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that the index grabs two discs and places them at the correct
+// positions.
+TEST_F(IndexTest, GrabTwoDiscs) {
+ LoadNDiscs(2);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+}
+
+// Tests that the index grabs 2 discs, and loads one up into the loader to get
+// ready to shoot. It then pulls the second disc back down to be ready to
+// intake more.
+TEST_F(IndexTest, ReadyGrabsOneDisc) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that the disc has been grabbed.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+ // And that we are preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Pull the disc back down and verify that the transfer roller doesn't turn on
+ // until we are ready.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 100; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ if (!my_index_loop_.status->ready_to_intake) {
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0)
+ << "Transfer should be off until indexer is ready";
+ }
+
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ LOG(INFO, "Top disc error is %f\n",
+ IndexMotor::kReadyToLiftPosition -
+ index_motor_plant_.frisbees[0].position());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.02);
+ LOG(INFO, "Bottom disc error is %f\n",
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI))-
+ index_motor_plant_.frisbees[1].position());
+}
+
+// Tests that the index grabs 1 disc and continues to pull it in correctly when
+// in the READY_LOWER state. The transfer roller should be disabled then.
+TEST_F(IndexTest, GrabAndReady) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ } else if (i > 150) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that grabbing 4 discs ends up with 4 discs in the bot and us no longer
+// ready.
+TEST_F(IndexTest, GrabFourDiscs) {
+ LoadNDiscs(4);
+
+ my_index_loop_.output.FetchLatest();
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 4);
+ EXPECT_FALSE(my_index_loop_.status->ready_to_intake);
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.001);
+
+ EXPECT_NEAR(
+ IndexMotor::kReadyToPreload,
+ index_motor_plant_.frisbees[1].position(), 0.01);
+
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[1].position() -
+ index_motor_plant_.frisbees[2].position()), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[2].position() -
+ index_motor_plant_.frisbees[3].position()), 0.10);
+}
+
+// Tests that shooting 4 discs works.
+TEST_F(IndexTest, ShootFourDiscs) {
+ LoadNDiscs(4);
+
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size());
+
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+
+ // Lifting and shooting takes a while...
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 4);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_FALSE(my_index_loop_.output->disc_clamped);
+ EXPECT_FALSE(my_index_loop_.output->loader_up);
+ EXPECT_FALSE(my_index_loop_.output->disc_ejected);
+
+ EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.shot_frisbees.size());
+}
+
+// Tests that discs aren't pulled out of the loader half way through being
+// grabbed when being asked to index.
+TEST_F(IndexTest, PreloadToIndexEarlyTransition) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ for (int i = 0; i < 300; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ // Drop out of the loop as soon as it enters the loader.
+ // This will require it to finish the job before intaking more.
+ my_index_loop_.status.FetchLatest();
+ if (index_motor_plant_.frisbees[0].position() >
+ IndexMotor::kLoaderFreeStopPosition) {
+ break;
+ }
+ }
+
+ // Pull the disc back down and verify that the transfer roller doesn't turn on
+ // until we are ready.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ SimulateNCycles(100);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.output->disc_clamped);
+
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
+ index_motor_plant_.frisbees[0].position(), 0.01);
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+}
+
+// Tests that disabling while grabbing a disc doesn't cause problems.
+TEST_F(IndexTest, HandleDisable) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 200; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ } else if (i == 102) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send();
+ } else if (i > 150) {
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0);
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(i < 102 || i > 110);
+ UpdateTime();
+ }
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[0].position(), 0.04);
+}
+
+// Tests that we can shoot after grabbing in the loader.
+TEST_F(IndexTest, GrabbedToShoot) {
+ LoadNDiscs(2);
+
+ // Lift the discs up to the top. Wait a while to let the system settle and
+ // verify that they don't collide.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(300);
+
+ // Verify that it is preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Shoot them all.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(200);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ EXPECT_FALSE(my_index_loop_.status->preloaded);
+}
+
+// Tests that the cRIO can reboot and we don't loose discs.
+TEST_F(IndexTest, cRIOReboot) {
+ LoadNDiscs(2);
+
+ SimulateNCycles(100);
+ for (int i = 0; i < 100; ++i) {
+ // No position for a while is a cRIO reboot.
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(false);
+ UpdateTime();
+ }
+
+ // Shift the plant.
+ const double kPlantOffset = 5000.0;
+ index_motor_plant_.index_plant_->Y(0, 0) += kPlantOffset;
+ index_motor_plant_.index_plant_->X(0, 0) += kPlantOffset;
+ index_motor_plant_.bottom_disc_posedge_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_wait_count_ = 971;
+ index_motor_plant_.bottom_disc_negedge_wait_position_ = -1502;
+
+ // Shift the discs
+ index_motor_plant_.OffsetIndices(kPlantOffset);
+ // Let time elapse to see if the loop wants to move the discs or not.
+ SimulateNCycles(1000);
+
+ // Verify that 2 discs are at the bottom of the hopper.
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
+ EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
+ index_motor_plant_.frisbees[1].position(), 0.10);
+ EXPECT_NEAR(
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ (index_motor_plant_.frisbees[0].position() -
+ index_motor_plant_.frisbees[1].position()), 0.10);
+}
+
+// Tests that invalid states are rejected.
+TEST_F(IndexTest, InvalidStateTest) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(10).Send();
+ SimulateNCycles(2);
+ // Verify that the goal is correct.
+ EXPECT_GE(4, static_cast<int>(index_motor_.safe_goal_));
+ EXPECT_LE(0, static_cast<int>(index_motor_.safe_goal_));
+}
+
+// Tests that the motor is turned off after a number of cycles of low power.
+TEST_F(IndexTest, ZeroPowerAfterTimeout) {
+ LoadNDiscs(4);
+ SimulateNCycles(100);
+
+ // Verify that the motor is hard off. This relies on floating point math
+ // never really getting to 0 unless you set it explicitly.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
+}
+
+// Tests that preloading 2 discs relocates the discs if they shift on the
+// indexer. Test shifting all 4 ways.
+TEST_F(IndexTest, ShiftedDiscsAreRefound) {
+ TestDualLostDiscs(0.10, 0.15);
+}
+
+TEST_F(IndexTest, ShiftedDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(0.15, 0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefound) {
+ TestDualLostDiscs(-0.15, -0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(-0.10, -0.15);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, IntakingAfterLoading) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CanShootOneDiscAfterReady) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(100);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(1, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, GotExtraDisc) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.1);
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.6);
+ SimulateNCycles(100);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(3, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(4 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, LostDisc) {
+ LoadNDiscs(3);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ index_motor_plant_.frisbees.erase(
+ index_motor_plant_.frisbees.begin() + 1);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(2, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(3 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+ EXPECT_EQ(0u, index_motor_.frisbees_.size());
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(0.0, my_index_loop_.output->index_voltage);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CRIOReboot) {
+ index_motor_plant_.index_plant_->Y(0, 0) = 5000.0;
+ index_motor_plant_.index_plant_->X(0, 0) = 5000.0;
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+ EXPECT_EQ(1, my_index_loop_.status->hopper_disc_count);
+}
+
+// Verifies that the indexer can shoot a disc and then intake and shoot another
+// one. This verifies that the code that forgets discs works correctly.
+TEST_F(IndexTest, CanShootIntakeAndShoot) {
+ for (int i = 1; i < 4; ++i) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(500);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(i, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_EQ(i, my_index_loop_.status->shot_disc_count);
+ }
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index_main.cc b/frc971/control_loops/index/index_main.cc
new file mode 100644
index 0000000..9ca3290
--- /dev/null
+++ b/frc971/control_loops/index/index_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/index/index.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::IndexMotor indexer;
+ indexer.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
new file mode 100644
index 0000000..61e7a61
--- /dev/null
+++ b/frc971/control_loops/index/index_motor.q
@@ -0,0 +1,76 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group IndexLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The state for the indexer to be in.
+ // 0 means hold position, in a low power state.
+ // 1 means get ready to load discs by shifting the discs down.
+ // 2 means ready the discs, spin up the transfer roller, and accept discs.
+ // 3 means get ready to shoot, and place a disc grabbed in the loader.
+ // 4 means shoot at will.
+ int32_t goal_state;
+ };
+
+ message Position {
+ // Index position
+ double index_position;
+
+ // Current values of both sensors.
+ bool top_disc_detect;
+ bool bottom_disc_detect;
+ // Counts for positive and negative edges on the bottom sensor.
+ int32_t bottom_disc_posedge_count;
+ int32_t bottom_disc_negedge_count;
+ // The most recent encoder position read after the most recent
+ // negedge and a count of how many times it's been updated.
+ double bottom_disc_negedge_wait_position;
+ int32_t bottom_disc_negedge_wait_count;
+ // The most recent index position at the posedge of the top disc detect
+ // and a count of how many edges have been seen.
+ int32_t top_disc_posedge_count;
+ double top_disc_posedge_position;
+ // The most recent index position at the negedge of the top disc detect
+ // and a count of how many edges have been seen.
+ int32_t top_disc_negedge_count;
+ double top_disc_negedge_position;
+ };
+
+ message Output {
+ // Intake roller(s) output voltage.
+ // Positive means into the robot.
+ double intake_voltage;
+ // Transfer roller output voltage.
+ // Positive means into the robot.
+ double transfer_voltage;
+ // Index roller. Positive means up towards the shooter.
+ double index_voltage;
+ // Loader pistons.
+ bool disc_clamped;
+ bool loader_up;
+ bool disc_ejected;
+ };
+
+ message Status {
+ // Number of discs in the hopper
+ int32_t hopper_disc_count;
+ // Number of discs seen by the hopper.
+ int32_t total_disc_count;
+ // Number of discs shot by the shooter.
+ int32_t shot_disc_count;
+ // Disc ready in the loader.
+ bool preloaded;
+ // Indexer ready to accept more discs.
+ bool ready_to_intake;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group IndexLoop index_loop;
diff --git a/frc971/control_loops/index/index_motor_plant.cc b/frc971/control_loops/index/index_motor_plant.cc
new file mode 100644
index 0000000..10fa60c
--- /dev/null
+++ b/frc971/control_loops/index/index_motor_plant.cc
@@ -0,0 +1,151 @@
+#include "frc971/control_loops/index/index_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00832470485812, 0.0, 0.68478614982;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.06201698456, 11.6687573378;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00901822957243, 0.0, 0.810292182273;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0363437103863, 7.02270693014;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00927953099869, 0.0, 0.859452713637;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0266707124098, 5.20285570613;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex0DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.58478614982, 48.4122215588;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.90251621122, 0.0460029989298;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex0DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex1DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.37331047876, 0.0642434141389;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex1DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex2DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.37331047876, 0.0642434141389;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex2DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex3DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71029218227, 64.1044007344;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 3.16117420545, 0.0947502706704;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex3DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex4DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.75945271364, 70.6153894746;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 4.26688750446, 0.137549804289;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex4DiscPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(5);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex0DiscPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex1DiscPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex2DiscPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex3DiscPlantCoefficients());
+ plants[4] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex4DiscPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(5);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeIndex0DiscController());
+ controllers[1] = new StateFeedbackController<2, 1, 1>(MakeIndex1DiscController());
+ controllers[2] = new StateFeedbackController<2, 1, 1>(MakeIndex2DiscController());
+ controllers[3] = new StateFeedbackController<2, 1, 1>(MakeIndex3DiscController());
+ controllers[4] = new StateFeedbackController<2, 1, 1>(MakeIndex4DiscController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index_motor_plant.h b/frc971/control_loops/index/index_motor_plant.h
new file mode 100644
index 0000000..e82db6a
--- /dev/null
+++ b/frc971/control_loops/index/index_motor_plant.h
@@ -0,0 +1,36 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex0DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex1DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex2DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex3DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex4DiscController();
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/transfer_motor_plant.cc b/frc971/control_loops/index/transfer_motor_plant.cc
new file mode 100644
index 0000000..0852b26
--- /dev/null
+++ b/frc971/control_loops/index/transfer_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/index/transfer_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeTransferController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.06905877421, 0.0368709177253;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeTransferPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeTransferPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeTransferController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/transfer_motor_plant.h b/frc971/control_loops/index/transfer_motor_plant.h
new file mode 100644
index 0000000..596f9b3
--- /dev/null
+++ b/frc971/control_loops/index/transfer_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeTransferController();
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
new file mode 100755
index 0000000..3c87e86
--- /dev/null
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -0,0 +1,153 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class AngleAdjust(control_loop.ControlLoop):
+ def __init__(self, name="AngleAdjustRaw"):
+ super(AngleAdjust, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = .428
+ # Stall Current in Amps
+ self.stall_current = 63.8
+ # Free Speed in RPM
+ self.free_speed = 14900.0
+ # Free Current in Amps
+ self.free_current = 1.2
+ # Moment of inertia of the angle adjust about the shooter's pivot in kg m^2
+ self.J = 9.4
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio of the gearbox multiplied by the ratio of the radii of
+ # the output and the angle adjust curve, which is essentially another gear.
+ self.G = (1.0 / 50.0) * (0.01905 / 0.41964)
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.45, .8])
+
+ print "Unaugmented controller poles at"
+ print self.K
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+class AngleAdjustDeltaU(AngleAdjust):
+ def __init__(self, name="AngleAdjust"):
+ super(AngleAdjustDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+
+ self.A = numpy.matrix([[0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
+
+ self.B = numpy.matrix([[0.0],
+ [0.0],
+ [1.0]])
+
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
+
+ self.PlaceControllerPoles([0.60, 0.35, 0.80])
+
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, 0.15])
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ angle_adjust_data = numpy.genfromtxt(
+ 'angle_adjust/angle_adjust_data.csv', delimiter=',')
+ angle_adjust = AngleAdjust()
+ simulated_x = []
+ real_x = []
+ initial_x = angle_adjust_data[0, 2]
+ for i in xrange(angle_adjust_data.shape[0]):
+ angle_adjust.Update(numpy.matrix([[angle_adjust_data[i, 1] - 0.7]]))
+ simulated_x.append(angle_adjust.X[0, 0])
+ x_offset = angle_adjust_data[i, 2] - initial_x
+ real_x.append(x_offset)
+
+ sim_delay = 2
+ pylab.plot(range(sim_delay, angle_adjust_data.shape[0] + sim_delay),
+ simulated_x, label='Simulation')
+ pylab.plot(range(angle_adjust_data.shape[0]), real_x, label='Reality')
+ pylab.legend()
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ angle_adjust = AngleAdjustDeltaU()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(angle_adjust.K * (R - angle_adjust.X_hat), angle_adjust.U_min, angle_adjust.U_max)
+ angle_adjust.UpdateObserver(U)
+ angle_adjust.Update(U)
+ close_loop_x.append(angle_adjust.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ print "Expected .cc file name and .h file name"
+ else:
+ loop_writer = control_loop.ControlLoopWriter("RawAngleAdjust",
+ [AngleAdjust()])
+ if argv[3][-3:] == '.cc':
+ loop_writer.Write(argv[4], argv[3])
+ else:
+ loop_writer.Write(argv[3], argv[4])
+
+ loop_writer = control_loop.ControlLoopWriter("AngleAdjust", [angle_adjust])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
new file mode 100644
index 0000000..754ba62
--- /dev/null
+++ b/frc971/control_loops/python/control_loop.py
@@ -0,0 +1,295 @@
+import controls
+import numpy
+
+class ControlLoopWriter(object):
+ def __init__(self, gain_schedule_name, loops, namespaces=None):
+ """Constructs a control loop writer.
+
+ Args:
+ gain_schedule_name: string, Name of the overall controller.
+ loops: array[ControlLoop], a list of control loops to gain schedule
+ in order.
+ namespaces: array[string], a list of names of namespaces to nest in
+ order. If None, the default will be used.
+ """
+ self._gain_schedule_name = gain_schedule_name
+ self._loops = loops
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'control_loops']
+
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
+
+ self._namespace_end = '\n'.join(
+ ['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ def _HeaderGuard(self, header_file):
+ return ('FRC971_CONTROL_LOOPS_' +
+ header_file.upper().replace('.', '_').replace('/', '_') +
+ '_')
+
+ def Write(self, header_file, cc_file):
+ """Writes the loops to the specified files."""
+ self.WriteHeader(header_file)
+ self.WriteCC(header_file, cc_file)
+
+ def _GenericType(self, typename):
+ """Returns a loop template using typename for the type."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ return '%s<%d, %d, %d>' % (
+ typename, num_states, num_inputs, num_outputs)
+
+ def _ControllerType(self):
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
+
+ def _LoopType(self):
+ """Returns a template name for StateFeedbackLoop."""
+ return self._GenericType('StateFeedbackLoop')
+
+ def _PlantType(self):
+ """Returns a template name for StateFeedbackPlant."""
+ return self._GenericType('StateFeedbackPlant')
+
+ def _CoeffType(self):
+ """Returns a template name for StateFeedbackPlantCoefficients."""
+ return self._GenericType('StateFeedbackPlantCoefficients')
+
+ def WriteHeader(self, header_file):
+ """Writes the header file to the file named header_file."""
+ with open(header_file, 'w') as fd:
+ header_guard = self._HeaderGuard(header_file)
+ fd.write('#ifndef %s\n'
+ '#define %s\n\n' % (header_guard, header_guard))
+ fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlantHeader())
+ fd.write('\n')
+ fd.write(loop.DumpControllerHeader())
+ fd.write('\n')
+
+ fd.write('%s Make%sPlant();\n\n' %
+ (self._PlantType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sLoop();\n\n' %
+ (self._LoopType(), self._gain_schedule_name))
+
+ fd.write(self._namespace_end)
+ fd.write('\n\n')
+ fd.write("#endif // %s\n" % header_guard)
+
+ def WriteCC(self, header_file_name, cc_file):
+ """Writes the cc file to the file named cc_file."""
+ with open(cc_file, 'w') as fd:
+ fd.write('#include \"frc971/control_loops/%s\"\n' % header_file_name)
+ fd.write('\n')
+ fd.write('#include <vector>\n')
+ fd.write('\n')
+ fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlant())
+ fd.write('\n')
+
+ for loop in self._loops:
+ fd.write(loop.DumpController())
+ fd.write('\n')
+
+ fd.write('%s Make%sPlant() {\n' %
+ (self._PlantType(), self._gain_schedule_name))
+ fd.write(' ::std::vector<%s *> plants(%d);\n' % (
+ self._CoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' plants[%d] = new %s(%s);\n' %
+ (index, self._CoeffType(),
+ loop.PlantFunction()))
+ fd.write(' return %s(plants);\n' % self._PlantType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ fd.write(' ::std::vector<%s *> controllers(%d);\n' % (
+ self._ControllerType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' controllers[%d] = new %s(%s);\n' %
+ (index, self._ControllerType(),
+ loop.ControllerFunction()))
+ fd.write(' return %s(controllers);\n' % self._LoopType())
+ fd.write('}\n\n')
+
+ fd.write(self._namespace_end)
+ fd.write('\n')
+
+
+class ControlLoop(object):
+ def __init__(self, name):
+ """Constructs a control loop object.
+
+ Args:
+ name: string, The name of the loop to use when writing the C++ files.
+ """
+ self._name = name
+
+ def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+ """Calculates the discrete time values for A and B.
+
+ Args:
+ A_continuous: numpy.matrix, The continuous time A matrix
+ B_continuous: numpy.matrix, The continuous time B matrix
+ dt: float, The time step of the control loop
+
+ Returns:
+ (A, B), numpy.matrix, the control matricies.
+ """
+ return controls.c2d(A_continuous, B_continuous, dt)
+
+ def InitializeState(self):
+ """Sets X, Y, and X_hat to zero defaults."""
+ self.X = numpy.zeros((self.A.shape[0], 1))
+ self.Y = self.C * self.X
+ self.X_hat = numpy.zeros((self.A.shape[0], 1))
+
+ def PlaceControllerPoles(self, poles):
+ """Places the controller poles.
+
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.K = controls.dplace(self.A, self.B, poles)
+
+ def PlaceObserverPoles(self, poles):
+ """Places the observer poles.
+
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.L = controls.dplace(self.A.T, self.C.T, poles).T
+
+ def Update(self, U):
+ """Simulates one time step with the provided U."""
+ U = numpy.clip(U, self.U_min, self.U_max)
+ self.X = self.A * self.X + self.B * U
+ self.Y = self.C * self.X + self.D * U
+
+ def UpdateObserver(self, U):
+ """Updates the observer given the provided U."""
+ self.X_hat = (self.A * self.X_hat + self.B * U +
+ self.L * (self.Y - self.C * self.X_hat - self.D * U))
+
+ def _DumpMatrix(self, matrix_name, matrix):
+ """Dumps the provided matrix into a variable called matrix_name.
+
+ Args:
+ matrix_name: string, The variable name to save the matrix to.
+ matrix: The matrix to dump.
+
+ Returns:
+ string, The C++ commands required to populate a variable named matrix_name
+ with the contents of matrix.
+ """
+ ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
+ matrix.shape[0], matrix.shape[1], matrix_name)]
+ first = True
+ for x in xrange(matrix.shape[0]):
+ for y in xrange(matrix.shape[1]):
+ element = matrix[x, y]
+ if first:
+ ans.append(' %s << ' % matrix_name)
+ first = False
+ else:
+ ans.append(', ')
+ ans.append(str(element))
+
+ ans.append(';\n')
+ return ''.join(ans)
+
+ def DumpPlantHeader(self):
+ """Writes out a c++ header declaration which will create a Plant object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
+ num_states, num_inputs, num_outputs, self._name)
+
+ def DumpPlant(self):
+ """Writes out a c++ function which will create a PlantCoefficients object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
+ ' Make%sPlantCoefficients() {\n' % (
+ num_states, num_inputs, num_outputs, self._name)]
+
+ ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('C', self.C))
+ ans.append(self._DumpMatrix('D', self.D))
+ ans.append(self._DumpMatrix('U_max', self.U_max))
+ ans.append(self._DumpMatrix('U_min', self.U_min))
+
+ ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
+ '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
+ num_outputs))
+ ans.append('}\n')
+ return ''.join(ans)
+
+ def PlantFunction(self):
+ """Returns the name of the plant coefficient function."""
+ return 'Make%sPlantCoefficients()' % self._name
+
+ def ControllerFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sController()' % self._name
+
+ def DumpControllerHeader(self):
+ """Writes out a c++ header declaration which will create a Controller object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
+ num_states, num_inputs, num_outputs, self.ControllerFunction())
+
+ def DumpController(self):
+ """Returns a c++ function which will create a Controller object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
+ num_states, num_inputs, num_outputs, self.ControllerFunction())]
+
+ ans.append(self._DumpMatrix('L', self.L))
+ ans.append(self._DumpMatrix('K', self.K))
+
+ ans.append(' return StateFeedbackController<%d, %d, %d>'
+ '(L, K, Make%sPlantCoefficients());\n' % (num_states, num_inputs,
+ num_outputs, self._name))
+ ans.append('}\n')
+ return ''.join(ans)
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 7d34a85..a40bfe2 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -81,3 +81,21 @@
num_uncontrollable_eigenvalues)
return K
+
+
+def c2d(A, B, dt):
+ """Converts from continuous time state space representation to discrete time.
+ Evaluates e^(A dt) for the discrete time version of A, and
+ integral(e^(A t) * B, 0, dt).
+ Returns (A, B). C and D are unchanged."""
+ e, P = numpy.linalg.eig(A)
+ diag = numpy.matrix(numpy.eye(A.shape[0]))
+ diage = numpy.matrix(numpy.eye(A.shape[0]))
+ for eig, count in zip(e, range(0, A.shape[0])):
+ diag[count, count] = numpy.exp(eig * dt)
+ if abs(eig) < 1.0e-16:
+ diage[count, count] = dt
+ else:
+ diage[count, count] = (numpy.exp(eig * dt) - 1.0) / eig
+
+ return (P * diag * numpy.linalg.inv(P), P * diage * numpy.linalg.inv(P) * B)
diff --git a/frc971/control_loops/python/index.py b/frc971/control_loops/python/index.py
new file mode 100755
index 0000000..f165afe
--- /dev/null
+++ b/frc971/control_loops/python/index.py
@@ -0,0 +1,104 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Index(control_loop.ControlLoop):
+ def __init__(self, J=0.00013, name="Index"):
+ super(Index, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the index in kg m^2
+ self.J = J
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.40, .63])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ index = Index()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ index.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(index.X[0, 0])
+ simulated_v.append(index.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ index = Index()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(index.K * (R - index.X_hat), index.U_min, index.U_max)
+ index.UpdateObserver(U)
+ index.Update(U)
+ close_loop_x.append(index.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Set the constants for the number of discs that we expect to see.
+ # The c++ code expects that the index in the array will be the number of
+ # discs.
+ index0 = Index(0.00010, "Index0Disc")
+ index1 = Index(0.00013, "Index1Disc")
+ index2 = Index(0.00013, "Index2Disc")
+ index3 = Index(0.00018, "Index3Disc")
+ index4 = Index(0.00025, "Index4Disc")
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .c file name"
+ else:
+ loop_writer = control_loop.ControlLoopWriter(
+ "Index", [index0, index1, index2, index3, index4])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
new file mode 100755
index 0000000..83beb90
--- /dev/null
+++ b/frc971/control_loops/python/shooter.py
@@ -0,0 +1,140 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from matplotlib import pylab
+import control_loop
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 0.49819248
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0 - 1500.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0032
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current / 2
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 11.0 / 34.0
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.6, .981])
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ shooter_data = numpy.genfromtxt('shooter/shooter_data.csv', delimiter=',')
+ shooter = Shooter()
+ simulated_x = []
+ real_x = []
+ x_vel = []
+ initial_x = shooter_data[0, 2]
+ last_x = initial_x
+ for i in xrange(shooter_data.shape[0]):
+ shooter.Update(numpy.matrix([[shooter_data[i, 1]]]))
+ simulated_x.append(shooter.X[0, 0])
+ x_offset = shooter_data[i, 2] - initial_x
+ real_x.append(x_offset)
+ x_vel.append((shooter_data[i, 2] - last_x) * 100.0)
+ last_x = shooter_data[i, 2]
+
+ sim_delay = 1
+ pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+ simulated_x, label='Simulation')
+ pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+ pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+ pylab.legend()
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ shooter = Shooter()
+ close_loop_x = []
+ close_loop_U = []
+ velocity_goal = 300
+ R = numpy.matrix([[0.0], [velocity_goal]])
+ for _ in pylab.linspace(0,1.99,200):
+ # Iterate the position up.
+ R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+ # Prevents the position goal from going beyond what is necessary.
+ velocity_weight_scalar = 0.35
+ max_reference = (
+ (shooter.U_max[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ min_reference = (
+ (shooter.U_min[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ R[0, 0] = numpy.clip(R[0, 0], min_reference, max_reference)
+ U = numpy.clip(shooter.K * (R - shooter.X_hat),
+ shooter.U_min, shooter.U_max)
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ close_loop_x.append(shooter.X[1, 0])
+ close_loop_U.append(U[0, 0])
+
+ #pylab.plotfile("shooter.csv", (0,1))
+ #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+ #pylab.plotfile("shooter.csv", (0,2))
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
+ pylab.show()
+
+ # Simulate spin down.
+ spin_down_x = [];
+ R = numpy.matrix([[50.0], [0.0]])
+ for _ in xrange(150):
+ U = 0
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ spin_down_x.append(shooter.X[1, 0])
+
+ #pylab.plot(range(150), spin_down_x)
+ #pylab.show()
+
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/transfer.py b/frc971/control_loops/python/transfer.py
new file mode 100755
index 0000000..d7818a3
--- /dev/null
+++ b/frc971/control_loops/python/transfer.py
@@ -0,0 +1,94 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Transfer(control_loop.ControlLoop):
+ def __init__(self):
+ super(Transfer, self).__init__("Transfer")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the transfer in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.75, .6])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ transfer = Transfer()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ transfer.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(transfer.X[0, 0])
+ simulated_v.append(transfer.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ transfer = Transfer()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(transfer.K * (R - transfer.X_hat), transfer.U_min, transfer.U_max)
+ transfer.UpdateObserver(U)
+ transfer.Update(U)
+ close_loop_x.append(transfer.X[0, 0])
+
+ #pylab.plot(range(100), close_loop_x)
+ #pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ loop_writer = control_loop.ControlLoopWriter("Transfer", [transfer])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
new file mode 100755
index 0000000..f21ed03
--- /dev/null
+++ b/frc971/control_loops/python/wrist.py
@@ -0,0 +1,154 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Wrist(control_loop.ControlLoop):
+ def __init__(self, name="RawWrist"):
+ super(Wrist, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 1.4
+ # Stall Current in Amps
+ self.stall_current = 86
+ # Free Speed in RPM
+ self.free_speed = 6200.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the wrist in kg m^2
+ # TODO(aschuh): Measure this in reality. It doesn't seem high enough.
+ # James measured 0.51, but that can't be right given what I am seeing.
+ self.J = 2.0
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((84.0 / 20.0) * (50.0 / 14.0) * (40.0 / 14.0) * (40.0 / 12.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([0.85, 0.45])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+class WristDeltaU(Wrist):
+ def __init__(self, name="Wrist"):
+ super(WristDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+
+ self.A = numpy.matrix([[0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
+
+ self.B = numpy.matrix([[0.0],
+ [0.0],
+ [1.0]])
+
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
+
+ self.PlaceControllerPoles([0.60, 0.37, 0.80])
+
+ print "K"
+ print self.K
+ print "Placed controller poles are"
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, 0.15])
+ print "Placed observer poles are"
+ print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+def ClipDeltaU(wrist, delta_u):
+ old_u = numpy.matrix([[wrist.X[2, 0]]])
+ new_u = numpy.clip(old_u + delta_u, wrist.U_min, wrist.U_max)
+ return new_u - old_u
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ wrist = WristDeltaU()
+ simulated_x = []
+ for _ in xrange(100):
+ wrist.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(wrist.X[0, 0])
+
+ pylab.plot(range(100), simulated_x)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ wrist = WristDeltaU()
+ close_loop_x = []
+ close_loop_u = []
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ wrist.X[2, 0] = -5
+ for _ in xrange(100):
+ U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
+ U = ClipDeltaU(wrist, U)
+ wrist.UpdateObserver(U)
+ wrist.Update(U)
+ close_loop_x.append(wrist.X[0, 0] * 10)
+ close_loop_u.append(wrist.X[2, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.plot(range(100), close_loop_u)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name for"
+ print "both the plant and unaugmented plant"
+ else:
+ unaug_wrist = Wrist("RawWrist")
+ unaug_loop_writer = control_loop.ControlLoopWriter("RawWrist",
+ [unaug_wrist])
+ if argv[3][-3:] == '.cc':
+ unaug_loop_writer.Write(argv[4], argv[3])
+ else:
+ unaug_loop_writer.Write(argv[3], argv[4])
+
+ loop_writer = control_loop.ControlLoopWriter("Wrist", [wrist])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..ec08ec8
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -0,0 +1,109 @@
+#include "frc971/control_loops/shooter/shooter.h"
+
+#include "aos/aos_core.h"
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
+ : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+ loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
+ history_position_(0),
+ position_goal_(0.0),
+ last_position_(0.0) {
+ memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double ShooterMotor::dt = 0.01;
+/*static*/ const double ShooterMotor::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
+void ShooterMotor::RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status) {
+ const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat(0, 0) : position->position);
+ double output_voltage = 0.0;
+
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
+ }
+
+ loop_->Y << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ // Prevents integral windup by limiting the position error such that the
+ // error can't produce much more than full power.
+ const double kVelocityWeightScalar = 0.35;
+ const double max_reference =
+ (loop_->U_max(0, 0) - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
+ / loop_->K(0, 0) + loop_->X_hat(0, 0);
+ const double min_reference =
+ (loop_->U_min(0, 0) - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
+ / loop_->K(0, 0) + loop_->X_hat(0, 0);
+
+ position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
+ min_reference);
+ loop_->R << position_goal_, velocity_goal;
+ position_goal_ += velocity_goal * dt;
+
+ loop_->Update(position, output == NULL);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->U[0] = 0.0;
+ } else {
+ output_voltage = loop_->U[0];
+ }
+
+ LOG(DEBUG,
+ "PWM: %f, raw_pos: %f rotations: %f "
+ "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
+ output_voltage, current_position,
+ current_position / (2 * M_PI),
+ (current_position - last_position_) / dt,
+ loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+
+ // Calculates the velocity over the last kHistoryLength * .01 seconds
+ // by taking the difference between the current and next history positions.
+ int old_history_position = ((history_position_ == 0) ?
+ kHistoryLength : history_position_) - 1;
+ average_velocity_ = (history_[old_history_position] -
+ history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+
+ status->average_velocity = average_velocity_;
+
+ // Determine if the velocity is close enough to the goal to be ready.
+ if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
+ velocity_goal != 0.0) {
+ LOG(DEBUG, "Steady: ");
+ status->ready = true;
+ } else {
+ LOG(DEBUG, "Not ready: ");
+ status->ready = false;
+ }
+ LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ last_position_ = current_position;
+
+ if (output) {
+ output->voltage = output_voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..bf1cd67
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -0,0 +1,83 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/shooter',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'shooter_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shooter.cc',
+ 'shooter_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'shooter_lib',
+ 'shooter_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..77c605b
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.h
@@ -0,0 +1,54 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class ShooterMotor
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+ explicit ShooterMotor(
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+ // Control loop time step.
+ static const double dt;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for in
+ // rad/sec.
+ static const double kMaxSpeed;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status);
+
+ private:
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+ // History array and stuff for determining average velocity and whether
+ // we are ready to shoot.
+ static const int kHistoryLength = 5;
+ double history_[kHistoryLength];
+ ptrdiff_t history_position_;
+ double average_velocity_;
+
+ double position_goal_;
+ double last_position_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/frc971/control_loops/shooter/shooter_csv.cc b/frc971/control_loops/shooter/shooter_csv.cc
new file mode 100644
index 0000000..de2b4ee
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_csv.cc
@@ -0,0 +1,51 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+
+using ::frc971::control_loops::shooter;
+using ::aos::time::Time;
+
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Logging to stdout instead\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ shooter.goal.FetchLatest();
+ shooter.status.FetchLatest();
+ shooter.position.FetchLatest();
+ shooter.output.FetchLatest();
+ if (shooter.output.get() &&
+ shooter.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (shooter.position->sent_time - start_time).ToSeconds(),
+ shooter.output->voltage,
+ shooter.position->position);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/control_loops/shooter/shooter_data.csv b/frc971/control_loops/shooter/shooter_data.csv
new file mode 100644
index 0000000..3515070
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_data.csv
@@ -0,0 +1,638 @@
+0.009404, 0.000000, 1484.878965
+0.019423, 0.000000, 1484.878965
+0.029389, 0.000000, 1484.878965
+0.039354, 0.000000, 1484.878965
+0.049887, 0.000000, 1484.878965
+0.059522, 0.000000, 1484.878965
+0.069479, 0.000000, 1484.878965
+0.079381, 0.000000, 1484.878965
+0.089338, 0.000000, 1484.878965
+0.099357, 0.000000, 1484.878965
+0.109409, 0.000000, 1484.878965
+0.119355, 0.000000, 1484.878965
+0.129358, 0.000000, 1484.878965
+0.139354, 0.000000, 1484.878965
+0.149480, 0.000000, 1484.878965
+0.159363, 0.000000, 1484.878965
+0.169341, 0.000000, 1484.878965
+0.179367, 0.000000, 1484.878965
+0.189636, 0.000000, 1484.878965
+0.199875, 0.000000, 1484.878965
+0.210070, 0.000000, 1484.878965
+0.219349, 0.000000, 1484.878965
+0.229544, 0.000000, 1484.878965
+0.239404, 0.000000, 1484.878965
+0.249410, 0.000000, 1484.878965
+0.259839, 0.000000, 1484.878965
+0.269492, 0.000000, 1484.878965
+0.279847, 0.000000, 1484.878965
+0.290056, 0.000000, 1484.878965
+0.299362, 0.000000, 1484.878965
+0.309457, 0.000000, 1484.878965
+0.319829, 0.000000, 1484.878965
+0.329446, 0.000000, 1484.878965
+0.339818, 0.000000, 1484.878965
+0.349444, 0.000000, 1484.878965
+0.359899, 0.000000, 1484.878965
+0.370053, 0.000000, 1484.878965
+0.379510, 0.000000, 1484.878965
+0.390136, 0.000000, 1484.878965
+0.399366, 0.000000, 1484.878965
+0.409472, 0.000000, 1484.878965
+0.419898, 0.000000, 1484.878965
+0.430131, 0.000000, 1484.878965
+0.439363, 0.000000, 1484.878965
+0.449459, 0.000000, 1484.878965
+0.459840, 0.000000, 1484.878965
+0.469382, 0.000000, 1484.878965
+0.479846, 0.000000, 1484.878965
+0.489432, 0.000000, 1484.878965
+0.499342, 0.000000, 1484.878965
+0.509350, 0.000000, 1484.878965
+0.519406, 0.000000, 1484.878965
+0.530084, 0.000000, 1484.878965
+0.539341, 0.000000, 1484.878965
+0.549406, 0.000000, 1484.878965
+0.559401, 0.000000, 1484.878965
+0.569409, 0.000000, 1484.878965
+0.579831, 0.000000, 1484.878965
+0.589469, 0.000000, 1484.878965
+0.599356, 0.000000, 1484.878965
+0.610099, 0.000000, 1484.878965
+0.619333, 0.000000, 1484.878965
+0.629479, 0.000000, 1484.878965
+0.639805, 0.000000, 1484.878965
+0.650053, 0.000000, 1484.878965
+0.659423, 0.000000, 1484.878965
+0.669413, 0.000000, 1484.878965
+0.679822, 0.000000, 1484.878965
+0.690045, 0.000000, 1484.878965
+0.699411, 0.000000, 1484.878965
+0.709584, 0.000000, 1484.878965
+0.719866, 0.000000, 1484.878965
+0.729475, 0.000000, 1484.878965
+0.739328, 0.000000, 1484.878965
+0.749396, 0.000000, 1484.878965
+0.759836, 0.000000, 1484.878965
+0.769492, 0.000000, 1484.878965
+0.779340, 0.000000, 1484.878965
+0.789401, 0.000000, 1484.878965
+0.799405, 0.000000, 1484.878965
+0.809407, 0.000000, 1484.878965
+0.819830, 0.000000, 1484.878965
+0.829411, 0.000000, 1484.878965
+0.839413, 0.000000, 1484.878965
+0.849409, 0.000000, 1484.878965
+0.859349, 0.000000, 1484.878965
+0.869453, 0.000000, 1484.878965
+0.879831, 0.000000, 1484.878965
+0.889426, 0.000000, 1484.878965
+0.899332, 0.000000, 1484.878965
+0.909439, 0.000000, 1484.878965
+0.919830, 0.000000, 1484.878965
+0.929464, 0.000000, 1484.878965
+0.939328, 0.000000, 1484.878965
+0.949440, 0.000000, 1484.878965
+0.959871, 0.000000, 1484.878965
+0.969393, 0.000000, 1484.878965
+0.980080, 0.000000, 1484.878965
+0.989440, 0.000000, 1484.878965
+0.999370, 0.000000, 1484.878965
+1.009338, 0.000000, 1484.878965
+1.019368, 0.000000, 1484.878965
+1.029492, 0.000000, 1484.878965
+1.039816, 0.000000, 1484.878965
+1.049430, 0.000000, 1484.878965
+1.059324, 0.000000, 1484.878965
+1.069351, 0.000000, 1484.878965
+1.079867, 0.000000, 1484.878965
+1.089417, 0.000000, 1484.878965
+1.099324, 0.000000, 1484.878965
+1.109348, 0.000000, 1484.878965
+1.119389, 0.000000, 1484.878965
+1.129331, 0.000000, 1484.878965
+1.139306, 0.000000, 1484.878965
+1.149394, 0.000000, 1484.878965
+1.159374, 0.000000, 1484.878965
+1.169335, 0.000000, 1484.878965
+1.179817, 0.000000, 1484.878965
+1.189415, 0.000000, 1484.878965
+1.199338, 0.000000, 1484.878965
+1.209349, 0.000000, 1484.878965
+1.219333, 0.000000, 1484.878965
+1.229518, 0.000000, 1484.878965
+1.239329, 0.000000, 1484.878965
+1.249334, 0.000000, 1484.878965
+1.259316, 0.000000, 1484.878965
+1.269388, 0.000000, 1484.878965
+1.279357, 0.000000, 1484.878965
+1.289451, 0.000000, 1484.878965
+1.299350, 0.000000, 1484.878965
+1.309350, 0.000000, 1484.878965
+1.319848, 0.000000, 1484.878965
+1.329384, 0.000000, 1484.878965
+1.339375, 0.000000, 1484.878965
+1.349359, 0.000000, 1484.878965
+1.359384, 0.000000, 1484.878965
+1.369428, 0.000000, 1484.878965
+1.379443, 0.000000, 1484.878965
+1.389498, 0.000000, 1484.878965
+1.399332, 0.000000, 1484.878965
+1.409393, 0.000000, 1484.878965
+1.419325, 0.000000, 1484.878965
+1.430129, 0.000000, 1484.878965
+1.439419, 0.000000, 1484.878965
+1.449510, 0.000000, 1484.878965
+1.459828, 0.000000, 1484.878965
+1.469377, 0.000000, 1484.878965
+1.479834, 0.000000, 1484.878965
+1.489367, 0.000000, 1484.878965
+1.499316, 0.000000, 1484.878965
+1.509405, 0.000000, 1484.878965
+1.519341, 0.000000, 1484.878965
+1.529334, 0.000000, 1484.878965
+1.539305, 0.000000, 1484.878965
+1.550118, 0.000000, 1484.878965
+1.559386, 0.000000, 1484.878965
+1.569647, 0.000000, 1484.878965
+1.579395, 0.000000, 1484.878965
+1.589381, 0.000000, 1484.878965
+1.599819, 0.000000, 1484.878965
+1.609401, 0.000000, 1484.878965
+1.619404, 0.000000, 1484.878965
+1.629335, 0.000000, 1484.878965
+1.639327, 0.000000, 1484.878965
+1.649334, 0.000000, 1484.878965
+1.659341, 0.000000, 1484.878965
+1.669328, 0.000000, 1484.878965
+1.679850, 0.000000, 1484.878965
+1.689423, 0.000000, 1484.878965
+1.699320, 0.000000, 1484.878965
+1.710128, 0.000000, 1484.878965
+1.719388, 0.000000, 1484.878965
+1.730042, 0.000000, 1484.878965
+1.739338, 0.000000, 1484.878965
+1.749483, 0.000000, 1484.878965
+1.759420, 0.000000, 1484.878965
+1.769334, 0.000000, 1484.878965
+1.779289, 0.000000, 1484.878965
+1.789325, 0.000000, 1484.878965
+1.799395, 0.000000, 1484.878965
+1.809493, 0.000000, 1484.878965
+1.819312, 0.000000, 1484.878965
+1.829402, 0.000000, 1484.878965
+1.839317, 0.000000, 1484.878965
+1.849330, 0.000000, 1484.878965
+1.859354, 0.000000, 1484.878965
+1.869394, 0.000000, 1484.878965
+1.879816, 0.000000, 1484.878965
+1.889374, 0.000000, 1484.878965
+1.899381, 0.000000, 1484.878965
+1.909332, 0.000000, 1484.878965
+1.919359, 0.000000, 1484.878965
+1.929338, 0.000000, 1484.878965
+1.939359, 0.000000, 1484.878965
+1.949332, 0.000000, 1484.878965
+1.959325, 0.000000, 1484.878965
+1.969341, 0.000000, 1484.878965
+1.979362, 0.000000, 1484.878965
+1.989330, 0.000000, 1484.878965
+1.999479, 0.000000, 1484.878965
+2.009392, 0.000000, 1484.878965
+2.019318, 0.000000, 1484.878965
+2.029320, 0.000000, 1484.878965
+2.039323, 0.000000, 1484.878965
+2.049387, 0.000000, 1484.878965
+2.059818, 0.000000, 1484.878965
+2.069766, 0.000000, 1484.878965
+2.079835, 0.000000, 1484.878965
+2.089372, 0.000000, 1484.878965
+2.099322, 0.000000, 1484.878965
+2.109357, 0.000000, 1484.878965
+2.119387, 0.000000, 1484.878965
+2.129327, 0.000000, 1484.878965
+2.139458, 0.000000, 1484.878965
+2.149392, 0.000000, 1484.878965
+2.159826, 0.000000, 1484.878965
+2.169591, 0.000000, 1484.878965
+2.179656, 0.000000, 1484.878965
+2.189392, 0.000000, 1484.878965
+2.199491, 0.000000, 1484.878965
+2.209541, 0.000000, 1484.878965
+2.219287, 0.000000, 1484.878965
+2.229123, 0.000000, 1484.878965
+2.239347, 0.000000, 1484.878965
+2.249390, 0.000000, 1484.878965
+2.259407, 0.000000, 1484.878965
+2.269393, 0.000000, 1484.878965
+2.279375, 0.000000, 1484.878965
+2.289416, 0.000000, 1484.878965
+2.299368, 0.000000, 1484.878965
+2.309379, 0.000000, 1484.878965
+2.319382, 0.000000, 1484.878965
+2.329435, 0.000000, 1484.878965
+2.339329, 0.000000, 1484.878965
+2.349389, 0.000000, 1484.878965
+2.359454, 0.000000, 1484.878965
+2.369832, 0.000000, 1484.878965
+2.379390, 0.000000, 1484.878965
+2.389381, 0.000000, 1484.878965
+2.399429, 0.000000, 1484.878965
+2.409394, 0.000000, 1484.878965
+2.419367, 0.000000, 1484.878965
+2.429384, 0.000000, 1484.878965
+2.439408, 0.000000, 1484.878965
+2.449391, 0.000000, 1484.878965
+2.459343, 0.000000, 1484.878965
+2.469424, 0.000000, 1484.878965
+2.479357, 0.000000, 1484.878965
+2.489388, 0.000000, 1484.878965
+2.499413, 0.000000, 1484.878965
+2.510081, 0.000000, 1484.878965
+2.519397, 0.000000, 1484.878965
+2.529342, 0.000000, 1484.878965
+2.539372, 0.000000, 1484.878965
+2.549674, 0.000000, 1484.878965
+2.559586, 0.000000, 1484.878965
+2.569807, 0.000000, 1484.878965
+2.579362, 0.000000, 1484.878965
+2.589325, 0.000000, 1484.878965
+2.599300, 0.000000, 1484.878965
+2.609436, 0.000000, 1484.878965
+2.619476, 0.000000, 1484.878965
+2.629668, 0.000000, 1484.878965
+2.639301, 0.000000, 1484.878965
+2.649411, 0.000000, 1484.878965
+2.659301, 0.000000, 1484.878965
+2.669336, 0.000000, 1484.878965
+2.679460, 0.000000, 1484.878965
+2.689691, 0.000000, 1484.878965
+2.699310, 0.000000, 1484.878965
+2.710046, 0.000000, 1484.878965
+2.719584, 0.000000, 1484.878965
+2.729333, 0.000000, 1484.878965
+2.739288, 0.000000, 1484.878965
+2.749320, 0.000000, 1484.878965
+2.759517, 0.000000, 1484.878965
+2.769811, 0.000000, 1484.878965
+2.779463, 0.000000, 1484.878965
+2.789708, 0.000000, 1484.878965
+2.799310, 12.000000, 1484.878965
+2.809361, 12.000000, 1484.878965
+2.819345, 12.000000, 1484.943934
+2.829470, 12.000000, 1485.117183
+2.839666, 12.000000, 1485.355402
+2.849432, 12.000000, 1485.680245
+2.859547, 12.000000, 1486.091712
+2.869422, 12.000000, 1486.589805
+2.879352, 12.000000, 1487.152866
+2.889431, 12.000000, 1487.802552
+2.899538, 12.000000, 1488.538863
+2.909416, 12.000000, 1489.340142
+2.919676, 12.000000, 1490.163078
+2.929470, 11.856977, 1491.072638
+2.939341, 10.941776, 1492.090480
+2.949422, 9.709468, 1493.086665
+2.959343, 9.484298, 1494.212787
+2.969480, 9.024482, 1495.360566
+2.979482, 8.408468, 1496.616625
+2.989402, 7.584528, 1497.851029
+2.999839, 7.318006, 1499.193713
+3.009487, 6.726255, 1500.601366
+3.019345, 5.691274, 1502.030675
+3.029433, 5.445505, 1503.503297
+3.039661, 5.201068, 1505.019231
+3.049419, 4.805405, 1506.556821
+3.059323, 4.164102, 1508.116067
+3.069465, 3.901448, 1509.696970
+3.079658, 3.715078, 1511.321185
+3.089408, 3.656437, 1512.902087
+3.099346, 3.325052, 1514.504646
+3.109498, 3.310343, 1516.128861
+3.119381, 3.348580, 1517.753076
+3.129434, 3.031678, 1519.377291
+3.139461, 3.165136, 1520.979850
+3.149456, 3.276186, 1522.604064
+3.159650, 3.130954, 1524.228279
+3.169436, 3.017931, 1525.852494
+3.179542, 2.968543, 1527.455053
+3.189425, 3.107515, 1529.079268
+3.199654, 3.010200, 1530.681827
+3.209442, 3.078322, 1532.306042
+3.219518, 2.953745, 1533.908601
+3.229434, 3.021034, 1535.511159
+3.239303, 4.196084, 1537.113718
+3.249474, 2.523334, 1538.716277
+3.259481, 2.549791, 1540.318836
+3.269395, 2.856174, 1541.943051
+3.279668, 2.830169, 1543.545609
+3.289491, 2.903769, 1545.148168
+3.299343, 2.930722, 1546.729071
+3.309430, 2.915555, 1548.331629
+3.319847, 2.887528, 1549.934188
+3.329444, 3.022588, 1551.515091
+3.339468, 2.922583, 1553.095993
+3.349700, 3.155171, 1554.676896
+3.359307, 2.959473, 1556.257798
+3.369439, 2.963462, 1557.817045
+3.379865, 3.151337, 1559.397947
+3.389482, 3.237455, 1560.957194
+3.399670, 3.075969, 1562.538096
+3.409431, 3.127616, 1564.097342
+3.419355, 3.012946, 1565.656589
+3.429472, 3.094794, 1567.237491
+3.439645, 2.986884, 1568.796738
+3.449433, 3.228489, 1570.355984
+3.459540, 3.042079, 1571.915230
+3.469425, 3.052375, 1573.474477
+3.479338, 3.083112, 1575.055379
+3.489491, 2.926137, 1576.614626
+3.499576, 2.990285, 1578.152216
+3.509412, 3.204911, 1579.711462
+3.519368, 3.134930, 1581.270709
+3.529436, 3.050871, 1582.829955
+3.539512, 3.012085, 1584.389201
+3.549482, 3.160836, 1585.948448
+3.559848, 3.076263, 1587.486038
+3.569480, 2.996910, 1589.045284
+3.579466, 2.963210, 1590.604530
+3.589392, 3.113684, 1592.142121
+3.599645, 3.029053, 1593.679711
+3.609466, 3.111230, 1595.238957
+3.619478, 3.001191, 1596.776547
+3.629414, 3.083010, 1598.335794
+3.639639, 2.977469, 1599.873384
+3.649431, 3.061646, 1601.410974
+3.659343, 2.956478, 1602.970220
+3.669437, 3.040410, 1604.507810
+3.679660, 3.096927, 1606.067057
+3.689414, 3.104633, 1607.582991
+3.699466, 3.092523, 1609.120581
+3.709581, 3.079314, 1610.658171
+3.719844, 3.069195, 1612.195761
+3.729455, 3.060854, 1613.755008
+3.739454, 2.890971, 1615.270942
+3.749456, 3.120861, 1616.808532
+3.759652, 2.943141, 1618.367778
+3.769565, 2.963262, 1619.905368
+3.779406, 3.162518, 1621.442958
+3.789419, 3.096059, 1622.958892
+3.799647, 2.857986, 1624.496482
+3.809451, 3.063847, 1626.034073
+3.819535, 3.048330, 1627.550007
+3.829423, 3.158748, 1629.087597
+3.839290, 3.053598, 1630.625187
+3.849534, 2.974064, 1632.162777
+3.859585, 2.947301, 1633.678711
+3.869342, 3.104581, 1635.194645
+3.879651, 3.025376, 1636.753891
+3.889461, 3.112302, 1638.269825
+3.899471, 3.006920, 1639.785759
+3.909503, 3.093474, 1641.323349
+3.919650, 2.992726, 1642.860940
+3.929493, 3.081710, 1644.376873
+3.939457, 2.981346, 1645.914464
+3.949675, 2.908199, 1647.430398
+3.959566, 3.045687, 1648.946332
+3.969420, 3.126825, 1650.505578
+3.979850, 2.816686, 1652.021512
+3.989429, 3.120514, 1653.537446
+3.999650, 3.009544, 1655.053380
+4.009434, 3.053283, 1656.590970
+4.019490, 2.769875, 1658.106904
+4.029473, 2.932334, 1659.644494
+4.039639, 2.903060, 1661.182084
+4.049524, 3.176347, 1662.698018
+4.059463, 2.998063, 1664.213952
+4.069518, 3.013137, 1665.729886
+4.079855, 3.051287, 1667.267476
+4.089451, 3.065561, 1668.783410
+4.099514, 2.901543, 1670.299344
+4.109450, 2.971613, 1671.793622
+4.119835, 3.035151, 1673.331212
+4.129459, 3.052225, 1674.847146
+4.139393, 2.884954, 1676.363080
+4.149517, 3.114730, 1677.879014
+4.159643, 3.101587, 1679.373292
+4.169483, 3.212047, 1680.889226
+4.179478, 2.947840, 1682.405160
+4.189417, 3.111024, 1683.921094
+4.199352, 3.081820, 1685.415371
+4.209534, 3.196368, 1686.931305
+4.219472, 2.937203, 1688.447239
+4.229358, 3.102203, 1689.963173
+4.239581, 4.044265, 1691.479107
+4.249471, 2.568287, 1692.995041
+4.259564, 2.592233, 1694.510975
+4.269489, 2.874029, 1696.048565
+4.279649, 2.848913, 1697.564499
+4.289504, 3.102328, 1699.058777
+4.299467, 2.913154, 1700.574711
+4.309396, 2.926342, 1702.090645
+4.319635, 3.127416, 1703.584923
+4.329463, 3.066398, 1705.122513
+4.339462, 3.157757, 1706.616790
+4.349523, 3.054520, 1708.132724
+4.359685, 2.982710, 1709.627002
+4.369520, 3.124646, 1711.142936
+4.379481, 2.887285, 1712.658870
+4.389476, 3.058346, 1714.174804
+4.399662, 3.028207, 1715.647426
+4.409523, 3.140828, 1717.163360
+4.419532, 3.042732, 1718.679293
+4.429478, 3.131501, 1720.195227
+4.439668, 3.033777, 1721.667849
+4.449451, 3.127131, 1723.183783
+4.459519, 3.031674, 1724.699717
+4.469529, 3.125338, 1726.193995
+4.479652, 3.029621, 1727.709929
+4.489541, 3.123096, 1729.204206
+4.499285, 2.865452, 1730.720140
+4.509567, 3.035110, 1732.236074
+4.519358, 3.169872, 1733.752008
+4.529256, 3.046239, 1735.246286
+4.539423, 2.956041, 1736.762220
+4.549418, 3.092368, 1738.278154
+4.559287, 3.017417, 1739.772432
+4.569392, 3.113609, 1741.266709
+4.579389, 3.015401, 1742.804300
+4.589391, 3.107141, 1744.298577
+4.599271, 3.010950, 1745.792855
+4.609237, 3.104538, 1747.308789
+4.619397, 2.847040, 1748.803067
+4.629258, 3.016751, 1750.340657
+4.639391, 3.151513, 1751.834935
+4.649382, 3.027871, 1753.329212
+4.659249, 2.937667, 1754.845146
+4.669717, 3.073994, 1756.361080
+4.679709, 2.837164, 1757.877014
+4.689305, 3.009530, 1759.371292
+4.699414, 2.979957, 1760.865570
+4.709265, 3.254504, 1762.381504
+4.719267, 2.918277, 1763.875782
+4.729387, 3.014535, 1765.391715
+4.739263, 2.964507, 1766.885993
+4.749394, 3.242657, 1768.401927
+4.759241, 3.073913, 1769.917861
+4.769396, 2.934311, 1771.412139
+4.779436, 2.891905, 1772.928073
+4.789376, 3.055522, 1774.444007
+4.799259, 2.823787, 1775.938285
+4.809430, 2.993720, 1777.454219
+4.819715, 2.961969, 1778.948496
+4.829635, 3.235854, 1780.442774
+4.839637, 3.061570, 1781.958708
+4.849506, 3.081810, 1783.474642
+4.859505, 2.801335, 1784.968920
+4.869352, 3.134735, 1786.484854
+4.879650, 3.036602, 1787.979131
+4.889446, 3.084670, 1789.495065
+4.899459, 2.966217, 1790.989343
+4.909516, 3.056512, 1792.483621
+4.919642, 2.963315, 1793.999555
+4.929515, 3.058695, 1795.493833
+4.939540, 2.963436, 1796.988110
+4.949486, 3.056827, 1798.504044
+4.959635, 2.960935, 1800.019978
+4.969264, 3.054371, 1801.514256
+4.979559, 3.120496, 1803.030190
+4.989369, 2.975943, 1804.502812
+4.999639, 3.049612, 1806.018745
+5.009522, 3.114635, 1807.513023
+5.019539, 2.811284, 1809.028957
+5.029502, 3.124778, 1810.523235
+5.039639, 3.020720, 1812.017513
+5.049531, 3.069436, 1813.511790
+5.059571, 2.952407, 1815.006068
+5.069509, 3.205185, 1816.522002
+5.079646, 3.035861, 1818.016280
+5.089525, 3.224397, 1819.532214
+5.099572, 2.870103, 1821.026492
+5.109284, 3.134886, 1822.542426
+5.119546, 3.015399, 1824.036703
+5.129816, 3.065232, 1825.530981
+5.139566, 2.951614, 1827.025259
+5.149507, 3.044046, 1828.541193
+5.159648, 2.951048, 1830.035471
+5.169497, 3.208013, 1831.551405
+5.179545, 2.874516, 1833.045682
+5.189501, 3.137284, 1834.539960
+5.199633, 3.012938, 1836.034238
+5.209521, 3.222545, 1837.528516
+5.219560, 3.032578, 1839.044450
+5.229503, 3.056672, 1840.538727
+5.239594, 4.237957, 1842.033005
+5.249532, 2.592533, 1843.527283
+5.259542, 2.486074, 1845.043217
+5.269566, 2.894539, 1846.515838
+5.279650, 3.123100, 1848.010116
+5.289252, 3.165125, 1849.504394
+5.299546, 3.307151, 1850.998672
+5.309525, 3.213976, 1852.471293
+5.319632, 3.141194, 1853.943915
+5.329458, 3.284019, 1855.459849
+5.339575, 3.050528, 1856.932470
+5.349266, 3.226635, 1858.448404
+5.359682, 3.201591, 1859.921026
+5.369278, 3.319115, 1861.436960
+5.379549, 3.063938, 1862.931237
+5.389254, 3.071781, 1864.447171
+5.399651, 2.961775, 1865.919793
+5.409641, 3.063785, 1867.435727
+5.419564, 0.000000, 1868.930005
+5.429518, 0.000000, 1870.424282
+5.439596, 0.000000, 1871.918560
+5.449485, 0.000000, 1873.347869
+5.459469, 0.000000, 1874.733866
+5.469640, 0.000000, 1876.098207
+5.479575, 0.000000, 1877.419235
+5.489492, 0.000000, 1878.675294
+5.499454, 0.000000, 1879.909698
+5.509441, 0.000000, 1881.100789
+5.519669, 0.000000, 1882.270223
+5.529268, 0.000000, 1883.396346
+5.539531, 0.000000, 1884.479156
+5.549491, 0.000000, 1885.518653
+5.559591, 0.000000, 1886.536495
+5.569479, 0.000000, 1887.532680
+5.579553, 0.000000, 1888.485553
+5.589263, 0.000000, 1889.416769
+5.599628, 0.000000, 1890.283017
+5.609531, 0.000000, 1891.149265
+5.619452, 0.000000, 1891.993857
+5.629254, 0.000000, 1892.795136
+5.639595, 0.000000, 1893.574759
+5.649382, 0.000000, 1894.332726
+5.659518, 0.000000, 1895.047381
+5.669510, 0.000000, 1895.740379
+5.679630, 0.000000, 1896.433378
+5.689513, 0.000000, 1897.083064
+5.699559, 0.000000, 1897.689437
+5.709897, 0.000000, 1898.295811
+5.719663, 0.000000, 1898.880528
+5.729785, 0.000000, 1899.443590
+5.739550, 0.000000, 1899.984995
+5.749477, 0.000000, 1900.504743
+5.759631, 0.000000, 1901.002836
+5.769499, 0.000000, 1901.500928
+5.779545, 0.000000, 1901.955709
+5.789477, 0.000000, 1902.388833
+5.799588, 0.000000, 1902.821957
+5.809530, 0.000000, 1903.233424
+5.819550, 0.000000, 1903.623236
+5.829288, 0.000000, 1903.991391
+5.839583, 0.000000, 1904.359547
+5.849261, 0.000000, 1904.706046
+5.859488, 0.000000, 1905.030889
+5.869375, 0.000000, 1905.334076
+5.879665, 0.000000, 1905.658919
+5.889507, 0.000000, 1905.918793
+5.899530, 0.000000, 1906.200324
+5.909520, 0.000000, 1906.460198
+5.919633, 0.000000, 1906.720073
+5.929467, 0.000000, 1906.958291
+5.939535, 0.000000, 1907.174853
+5.949423, 0.000000, 1907.391415
+5.959583, 0.000000, 1907.586320
+5.969500, 0.000000, 1907.781226
+5.979548, 0.000000, 1907.954476
+5.989236, 0.000000, 1908.106069
+5.999631, 0.000000, 1908.279319
+6.009506, 0.000000, 1908.409256
+6.019548, 0.000000, 1908.560849
+6.029525, 0.000000, 1908.669130
+6.039582, 0.000000, 1908.799068
+6.049527, 0.000000, 1908.907349
+6.059538, 0.000000, 1909.015630
+6.069537, 0.000000, 1909.102254
+6.079650, 0.000000, 1909.188879
+6.089474, 0.000000, 1909.253848
+6.099533, 0.000000, 1909.318816
+6.109497, 0.000000, 1909.383785
+6.119587, 0.000000, 1909.448754
+6.129255, 0.000000, 1909.492066
+6.139524, 0.000000, 1909.513722
+6.149479, 0.000000, 1909.557035
+6.159584, 0.000000, 1909.578691
+6.169260, 0.000000, 1909.600347
+6.179536, 0.000000, 1909.600347
+6.189396, 0.000000, 1909.600347
+6.199574, 0.000000, 1909.600347
+6.209506, 0.000000, 1909.600347
+6.219521, 0.000000, 1909.600347
+6.229482, 0.000000, 1909.600347
+6.239565, 0.000000, 1909.600347
+6.249254, 0.000000, 1909.600347
+6.259521, 0.000000, 1909.600347
+6.269506, 0.000000, 1909.600347
+6.279589, 0.000000, 1909.600347
+6.289535, 0.000000, 1909.600347
+6.299542, 0.000000, 1909.600347
+6.309460, 0.000000, 1909.600347
+6.319858, 0.000000, 1909.600347
+6.329537, 0.000000, 1909.600347
+6.339535, 0.000000, 1909.600347
+6.349231, 0.000000, 1909.600347
+6.359649, 0.000000, 1909.600347
+6.369534, 0.000000, 1909.600347
+6.379535, 0.000000, 1909.600347
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..beb62f1
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,193 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/control_loops/shooter/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (my_shooter_loop_) actually works (same format as wrist).
+ ShooterMotorSimulation()
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372, ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status") {
+ }
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->position = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << my_shooter_loop_.output->voltage;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372,
+ ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+ my_shooter_loop_.status->average_velocity,
+ 10.0);
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ SendDSPacket(true);
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ VerifyNearGoal();
+ my_shooter_loop_.output.FetchLatest();
+ EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ VerifyNearGoal();
+
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+ }
+}
+
+// Test to make sure that it does not exceed the encoder's rated speed.
+TEST_F(ShooterTest, RateLimitTest) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(1000.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_GT(shooter_motor_.kMaxSpeed,
+ shooter_motor_plant_.shooter_plant_->X(1, 0));
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(i > 50);
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..72b820e
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/shooter/shooter.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/shooter/shooter_motor.q b/frc971/control_loops/shooter/shooter_motor.q
new file mode 100644
index 0000000..f2abf25
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor.q
@@ -0,0 +1,31 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Goal velocity in rad/sec
+ double velocity;
+ };
+
+ message Status {
+ // True if the shooter is up to speed.
+ bool ready;
+ // The average velocity over the last 0.1 seconds.
+ double average_velocity;
+ };
+
+ message Position {
+ // The angle of the shooter wheel measured in rad/sec.
+ double position;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue Status status;
+};
+
+queue_group ShooterLoop shooter;
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..1a623f3
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00992127884628, 0.0, 0.984297191531;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00398899915072, 0.795700859132;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.08429719153, 29.2677479765;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 0.955132813139, 0.50205697652;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..3588605
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeShooterController();
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
new file mode 100644
index 0000000..811c875
--- /dev/null
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -0,0 +1,335 @@
+#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+
+#include <assert.h>
+
+#include <vector>
+
+// Stupid vxworks system headers define it which blows up Eigen...
+#undef m_data
+
+#include "Eigen/Dense"
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackPlantCoefficients {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> A;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+
+ StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
+ : A(other.A),
+ B(other.B),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {
+ }
+
+ StateFeedbackPlantCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<double, number_of_outputs, 1> &U_max,
+ const Eigen::Matrix<double, number_of_outputs, 1> &U_min)
+ : A(A),
+ B(B),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max) {
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int kNumStates = number_of_states;
+ static const int kNumOutputs = number_of_outputs;
+ static const int kNumInputs = number_of_inputs;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackPlant {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ ::std::vector<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ return coefficients().A;
+ }
+ double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ return coefficients().B;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return coefficients().C;
+ }
+ double C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ return coefficients().D;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return coefficients().U_min;
+ }
+ double U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return coefficients().U_max;
+ }
+ double U_max(int i, int j) const { return U_max()(i, j); }
+
+ const StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[plant_index_];
+ }
+
+ int plant_index() const { return plant_index_; }
+ void set_plant_index(int plant_index) {
+ if (plant_index < 0) {
+ plant_index_ = 0;
+ } else if (plant_index >= static_cast<int>(coefficients_.size())) {
+ plant_index_ = static_cast<int>(coefficients_.size());
+ } else {
+ plant_index_ = plant_index;
+ }
+ }
+
+ void Reset() {
+ X.setZero();
+ Y.setZero();
+ U.setZero();
+ }
+
+ Eigen::Matrix<double, number_of_states, 1> X;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+ Eigen::Matrix<double, number_of_inputs, 1> U;
+
+ StateFeedbackPlant(
+ const ::std::vector<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs,
+ number_of_outputs> *> &coefficients)
+ : coefficients_(coefficients),
+ plant_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackPlant(StateFeedbackPlant &&other)
+ : plant_index_(0) {
+ Reset();
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ virtual ~StateFeedbackPlant() {}
+
+ // Assert that U is within the hardware range.
+ virtual void CheckU() {
+ for (int i = 0; i < kNumOutputs; ++i) {
+ assert(U(i, 0) <= U_max(i, 0));
+ assert(U(i, 0) >= U_min(i, 0));
+ }
+ }
+
+ // Computes the new X and Y given the control input.
+ void Update() {
+ // Powers outside of the range are more likely controller bugs than things
+ // that the plant should deal with.
+ CheckU();
+ X = A() * X + B() * U;
+ Y = C() * X + D() * U;
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int kNumStates = number_of_states;
+ static const int kNumOutputs = number_of_outputs;
+ static const int kNumInputs = number_of_inputs;
+
+ private:
+ int plant_index_;
+};
+
+// A Controller is a structure which holds a plant and the K and L matrices.
+// This is designed such that multiple controllers can share one set of state to
+// support gain scheduling easily.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackController {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+ StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs> plant;
+
+ StateFeedbackController(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : L(L),
+ K(K),
+ plant(plant) {
+ }
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackLoop {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ return controller().plant.A;
+ }
+ double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ return controller().plant.B;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return controller().plant.C;
+ }
+ double C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ return controller().plant.D;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
+ return controller().K;
+ }
+ double K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ return controller().L;
+ }
+ double L(int i, int j) const { return L()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return controller().plant.U_min;
+ }
+ double U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return controller().plant.U_max;
+ }
+ double U_max(int i, int j) const { return U_max()(i, j); }
+
+ Eigen::Matrix<double, number_of_states, 1> X_hat;
+ Eigen::Matrix<double, number_of_states, 1> R;
+ Eigen::Matrix<double, number_of_inputs, 1> U;
+ Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
+ Eigen::Matrix<double, number_of_outputs, 1> U_ff;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+
+ ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> *> controllers_;
+
+ const StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>
+ &controller() const {
+ return *controllers_[controller_index_];
+ }
+
+ void Reset() {
+ X_hat.setZero();
+ R.setZero();
+ U.setZero();
+ U_uncapped.setZero();
+ U_ff.setZero();
+ Y.setZero();
+ }
+
+ StateFeedbackLoop(
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &controller)
+ : controller_index_(0) {
+ controllers_.push_back(
+ new StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>(controller));
+ Reset();
+ }
+
+ StateFeedbackLoop(
+ const ::std::vector<StateFeedbackController<
+ number_of_states, number_of_inputs,
+ number_of_outputs> *> &controllers)
+ : controllers_(controllers),
+ controller_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackLoop(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
+ : controller_index_(0) {
+ controllers_.push_back(
+ new StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>(L, K, plant));
+
+ Reset();
+ }
+ virtual ~StateFeedbackLoop() {}
+
+ virtual void FeedForward() {
+ for (int i = 0; i < number_of_outputs; ++i) {
+ U_ff[i] = 0.0;
+ }
+ }
+
+ // If U is outside the hardware range, limit it before the plant tries to use
+ // it.
+ virtual void CapU() {
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U(i, 0) > U_max(i, 0)) {
+ U(i, 0) = U_max(i, 0);
+ } else if (U(i, 0) < U_min(i, 0)) {
+ U(i, 0) = U_min(i, 0);
+ }
+ }
+ }
+
+ // update_observer is whether or not to use the values in Y.
+ // stop_motors is whether or not to output all 0s.
+ void Update(bool update_observer, bool stop_motors) {
+ if (stop_motors) {
+ U.setZero();
+ } else {
+ U = U_uncapped = K() * (R - X_hat);
+ CapU();
+ }
+
+ if (update_observer) {
+ X_hat = (A() - L() * C()) * X_hat + L() * Y + B() * U;
+ } else {
+ X_hat = A() * X_hat + B() * U;
+ }
+ }
+
+ // Sets the current controller to be index and verifies that it isn't out of
+ // range.
+ void set_controller_index(int index) {
+ if (index < 0) {
+ controller_index_ = 0;
+ } else if (index >= static_cast<int>(controllers_.size())) {
+ controller_index_ = static_cast<int>(controllers_.size());
+ } else {
+ controller_index_ = index;
+ }
+ }
+
+ void controller_index() const { return controller_index_; }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int kNumStates = number_of_states;
+ static const int kNumOutputs = number_of_outputs;
+ static const int kNumInputs = number_of_inputs;
+
+ int controller_index_;
+};
+
+#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
diff --git a/frc971/control_loops/update_angle_adjust.sh b/frc971/control_loops/update_angle_adjust.sh
new file mode 100755
index 0000000..096f95e
--- /dev/null
+++ b/frc971/control_loops/update_angle_adjust.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the angle adjust controller.
+
+./python/angle_adjust.py angle_adjust/angle_adjust_motor_plant.h \
+ angle_adjust/angle_adjust_motor_plant.cc \
+ angle_adjust/unaugmented_angle_adjust_motor_plant.h \
+ angle_adjust/unaugmented_angle_adjust_motor_plant.cc \
diff --git a/frc971/control_loops/update_index.sh b/frc971/control_loops/update_index.sh
new file mode 100755
index 0000000..3b819b4
--- /dev/null
+++ b/frc971/control_loops/update_index.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the index controller.
+
+./python/index.py index/index_motor_plant.h index/index_motor_plant.cc
diff --git a/frc971/control_loops/update_shooter.sh b/frc971/control_loops/update_shooter.sh
new file mode 100755
index 0000000..db98547
--- /dev/null
+++ b/frc971/control_loops/update_shooter.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the shooter controller.
+
+./python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
diff --git a/frc971/control_loops/update_transfer.sh b/frc971/control_loops/update_transfer.sh
new file mode 100755
index 0000000..d7820fa
--- /dev/null
+++ b/frc971/control_loops/update_transfer.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the transfer controller.
+
+./python/transfer.py index/transfer_motor_plant.h index/transfer_motor_plant.cc
diff --git a/frc971/control_loops/update_wrist.sh b/frc971/control_loops/update_wrist.sh
new file mode 100755
index 0000000..1a3b54c
--- /dev/null
+++ b/frc971/control_loops/update_wrist.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the wrist controller.
+
+./python/wrist.py wrist/wrist_motor_plant.h \
+ wrist/wrist_motor_plant.cc \
+ wrist/unaugmented_wrist_motor_plant.h \
+ wrist/unaugmented_wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc
new file mode 100644
index 0000000..a9871c4
--- /dev/null
+++ b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawWristController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h
new file mode 100644
index 0000000..c049420
--- /dev/null
+++ b/frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawWristController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawWristPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawWristLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_UNAUGMENTED_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
new file mode 100644
index 0000000..8c4ae1a
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -0,0 +1,102 @@
+#include "frc971/control_loops/wrist/wrist.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
+ : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
+ zeroed_joint_(MakeWristLoop()) {
+}
+
+bool WristMotor::FetchConstants(
+ ZeroedJoint<1>::ConfigurationData *config_data) {
+ if (!constants::wrist_lower_limit(&config_data->lower_limit)) {
+ LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
+ return false;
+ }
+ if (!constants::wrist_upper_limit(&config_data->upper_limit)) {
+ LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
+ return false;
+ }
+ if (!constants::wrist_hall_effect_start_angle(
+ &config_data->hall_effect_start_angle[0])) {
+ LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
+ return false;
+ }
+ if (!constants::wrist_zeroing_off_speed(&config_data->zeroing_off_speed)) {
+ LOG(ERROR, "Failed to fetch the wrist zeroing off speed constant.\n");
+ return false;
+ }
+
+ if (!constants::wrist_zeroing_speed(&config_data->zeroing_speed)) {
+ LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
+ return false;
+ }
+
+ config_data->max_zeroing_voltage = 5.0;
+ return true;
+}
+
+// Positive angle is up, and positive power is up.
+void WristMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::WristLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * /*status*/) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->voltage = 0;
+ }
+
+ // Cache the constants to avoid error handling down below.
+ ZeroedJoint<1>::ConfigurationData config_data;
+ if (!FetchConstants(&config_data)) {
+ return;
+ } else {
+ zeroed_joint_.set_config_data(config_data);
+ }
+
+ ZeroedJoint<1>::PositionData transformed_position;
+ ZeroedJoint<1>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->pos;
+ transformed_position.hall_effects[0] = position->hall_effect;
+ transformed_position.hall_effect_positions[0] = position->calibration;
+ }
+
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
+
+ if (position) {
+ LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
+ position->pos,
+ position->hall_effect ? "true" : "false",
+ zeroed_joint_.absolute_position());
+ }
+
+ if (output) {
+ output->voltage = voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
new file mode 100644
index 0000000..6b50fad
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -0,0 +1,70 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'wrist_loop',
+ 'type': 'static_library',
+ 'sources': ['wrist_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/wrist',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'wrist_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'wrist.cc',
+ 'wrist_motor_plant.cc',
+ 'unaugmented_wrist_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'wrist_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'wrist_loop',
+ ],
+ },
+ {
+ 'target_name': 'wrist_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'wrist_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'wrist_loop',
+ 'wrist_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'wrist',
+ 'type': 'executable',
+ 'sources': [
+ 'wrist_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'wrist_lib',
+ 'wrist_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
new file mode 100644
index 0000000..7f0bd2e
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.h
@@ -0,0 +1,66 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+#include "frc971/control_loops/wrist/unaugmented_wrist_motor_plant.h"
+
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class WristTest_NoWindupPositive_Test;
+class WristTest_NoWindupNegative_Test;
+};
+
+class WristMotor
+ : public aos::control_loops::ControlLoop<control_loops::WristLoop> {
+ public:
+ explicit WristMotor(
+ control_loops::WristLoop *my_wrist = &control_loops::wrist);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the wrist is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
+ protected:
+ virtual void RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::WristLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status *status);
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::WristTest_NoWindupPositive_Test;
+ friend class testing::WristTest_NoWindupNegative_Test;
+
+ // Fetches and locally caches the latest set of constants.
+ bool FetchConstants(ZeroedJoint<1>::ConfigurationData *config_data);
+
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(WristMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
new file mode 100644
index 0000000..4903a28
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -0,0 +1,347 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/wrist/wrist.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the wrist and sends out queue messages containing the
+// position.
+class WristMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // wrist, which will be treated as 0 by the encoder.
+ WristMotorSimulation(double initial_position)
+ : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawWristPlant())),
+ my_wrist_loop_(".frc971.control_loops.wrist",
+ 0x1a7b7094, ".frc971.control_loops.wrist.goal",
+ ".frc971.control_loops.wrist.position",
+ ".frc971.control_loops.wrist.output",
+ ".frc971.control_loops.wrist.status") {
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ initial_position_ = initial_position;
+ wrist_plant_->X(0, 0) = initial_position_;
+ wrist_plant_->X(1, 0) = 0.0;
+ wrist_plant_->Y = wrist_plant_->C() * wrist_plant_->X;
+ last_position_ = wrist_plant_->Y(0, 0);
+ calibration_value_ = 0.0;
+ last_voltage_ = 0.0;
+ }
+
+ // Returns the absolute angle of the wrist.
+ double GetAbsolutePosition() const {
+ return wrist_plant_->Y(0, 0);
+ }
+
+ // Returns the adjusted angle of the wrist.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double angle = GetPosition();
+
+ double wrist_hall_effect_start_angle;
+ ASSERT_TRUE(constants::wrist_hall_effect_start_angle(
+ &wrist_hall_effect_start_angle));
+ double wrist_hall_effect_stop_angle;
+ ASSERT_TRUE(constants::wrist_hall_effect_stop_angle(
+ &wrist_hall_effect_stop_angle));
+
+ ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
+ my_wrist_loop_.position.MakeMessage();
+ position->pos = angle;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position >= wrist_hall_effect_start_angle &&
+ abs_position <= wrist_hall_effect_stop_angle) {
+ position->hall_effect = true;
+ } else {
+ position->hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ if ((last_position_ < wrist_hall_effect_start_angle ||
+ last_position_ > wrist_hall_effect_stop_angle) &&
+ position->hall_effect) {
+ calibration_value_ =
+ wrist_hall_effect_start_angle - initial_position_;
+ }
+
+ position->calibration = calibration_value_;
+ position.Send();
+ }
+
+ // Simulates the wrist moving for one timestep.
+ void Simulate() {
+ last_position_ = wrist_plant_->Y(0, 0);
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ wrist_plant_->U << last_voltage_;
+ wrist_plant_->Update();
+
+ // Assert that we are in the right physical range.
+ double wrist_upper_physical_limit;
+ ASSERT_TRUE(constants::wrist_upper_physical_limit(
+ &wrist_upper_physical_limit));
+ double wrist_lower_physical_limit;
+ ASSERT_TRUE(constants::wrist_lower_physical_limit(
+ &wrist_lower_physical_limit));
+
+ EXPECT_GE(wrist_upper_physical_limit,
+ wrist_plant_->Y(0, 0));
+ EXPECT_LE(wrist_lower_physical_limit,
+ wrist_plant_->Y(0, 0));
+ last_voltage_ = my_wrist_loop_.output->voltage;
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
+ private:
+ WristLoop my_wrist_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_;
+ double last_voltage_;
+};
+
+class WristTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ WristLoop my_wrist_loop_;
+
+ // Create a loop and simulation plant.
+ WristMotor wrist_motor_;
+ WristMotorSimulation wrist_motor_plant_;
+
+ WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
+ 0x1a7b7094, ".frc971.control_loops.wrist.goal",
+ ".frc971.control_loops.wrist.position",
+ ".frc971.control_loops.wrist.output",
+ ".frc971.control_loops.wrist.status"),
+ wrist_motor_(&my_wrist_loop_),
+ wrist_motor_plant_(0.5) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_wrist_loop_.goal.FetchLatest();
+ my_wrist_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_wrist_loop_.goal->goal,
+ wrist_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~WristTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(WristTest, ZerosCorrectly) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist zeros correctly starting on the hall effect sensor.
+TEST_F(WristTest, ZerosStartingOn) {
+ wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
+
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_F(WristTest, HandleMissingPosition) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ wrist_motor_plant_.SendPositionMessage();
+ }
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(WristTest, RezeroWithMissingPos) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ // After 3 seconds, simulate the encoder going missing.
+ // This should trigger a re-zero. To make sure it works, change the goal as
+ // well.
+ if (i < 300 || i > 400) {
+ wrist_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
+ }
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ }
+ if (i == 410) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(WristTest, DisableGoesUninitialized) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ // After 0.5 seconds, disable the robot.
+ if (i > 50 && i < 200) {
+ SendDSPacket(false);
+ if (i > 100) {
+ // Give the loop a couple cycled to get the message and then verify that
+ // it is in the correct state.
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
+ // When disabled, we should be applying 0 power.
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ EXPECT_EQ(0, my_wrist_loop_.output->voltage);
+ }
+ } else {
+ SendDSPacket(true);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(WristTest, NoWindupNegative) {
+ int capped_count = 0;
+ double saved_zeroing_position = 0;
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ } else if (i == 51) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position,
+ wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ if (!wrist_motor_.is_ready()) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(WristTest, NoWindupPositive) {
+ int capped_count = 0;
+ double saved_zeroing_position = 0;
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ } else {
+ if (i == 51) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ }
+ if (!wrist_motor_.is_ready()) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ EXPECT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist_main.cc b/frc971/control_loops/wrist/wrist_main.cc
new file mode 100644
index 0000000..8585180
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/wrist/wrist.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::WristMotor wrist;
+ wrist.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/wrist/wrist_motor.q b/frc971/control_loops/wrist/wrist_motor.q
new file mode 100644
index 0000000..3dbeb53
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_motor.q
@@ -0,0 +1,21 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group WristLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Position {
+ double pos;
+ bool hall_effect;
+ // The exact pos when hall_effect last changed
+ double calibration;
+ };
+
+ queue aos.control_loops.Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group WristLoop wrist;
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.cc b/frc971/control_loops/wrist/wrist_motor_plant.cc
new file mode 100644
index 0000000..49a7d6d
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeWristController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 2.56581823335, 182.185686601, 1214.37916748;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 79.7788757639, 3.78830897822, 1.04581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.h b/frc971/control_loops/wrist/wrist_motor_plant.h
new file mode 100644
index 0000000..a40ffe5
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeWristController();
+
+StateFeedbackPlant<3, 1, 1> MakeWristPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeWristLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
new file mode 100644
index 0000000..582d48b
--- /dev/null
+++ b/frc971/control_loops/zeroed_joint.h
@@ -0,0 +1,413 @@
+#ifndef FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
+#define FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class WristTest_NoWindupPositive_Test;
+class WristTest_NoWindupNegative_Test;
+};
+
+// Note: Everything in this file assumes that there is a 1 cycle delay between
+// power being requested and it showing up at the motor. It assumes that
+// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
+// that isn't true.
+
+template<int kNumZeroSensors>
+class ZeroedJoint;
+
+// This class implements the CapU function correctly given all the extra
+// information that we know about from the wrist motor.
+template<int kNumZeroSensors>
+class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop,
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint)
+ : StateFeedbackLoop<3, 1, 1>(loop),
+ zeroed_joint_(zeroed_joint),
+ voltage_(0.0),
+ last_voltage_(0.0) {
+ }
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+
+ // Returns the accumulated voltage.
+ double voltage() const { return voltage_; }
+
+ // Returns the uncapped voltage.
+ double uncapped_voltage() const { return uncapped_voltage_; }
+
+ // Zeros the accumulator.
+ void ZeroPower() { voltage_ = 0.0; }
+ private:
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
+
+ // The accumulated voltage to apply to the motor.
+ double voltage_;
+ double last_voltage_;
+ double uncapped_voltage_;
+};
+
+template<int kNumZeroSensors>
+void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
+ const double old_voltage = voltage_;
+ voltage_ += U(0, 0);
+
+ uncapped_voltage_ = voltage_;
+
+ // Do all our computations with the voltage, and then compute what the delta
+ // is to make that happen.
+ if (zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY) {
+ if (Y(0, 0) >= zeroed_joint_->config_data_.upper_limit) {
+ voltage_ = std::min(0.0, voltage_);
+ }
+ if (Y(0, 0) <= zeroed_joint_->config_data_.lower_limit) {
+ voltage_ = std::max(0.0, voltage_);
+ }
+ }
+
+ const bool is_ready =
+ zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY;
+ double limit = is_ready ?
+ 12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
+
+ voltage_ = std::min(limit, voltage_);
+ voltage_ = std::max(-limit, voltage_);
+ U(0, 0) = voltage_ - old_voltage;
+
+ // Make sure that reality and the observer can't get too far off. There is a
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // against last cycle's voltage.
+ if (X_hat(2, 0) > last_voltage_ + 2.0) {
+ X_hat(2, 0) = last_voltage_ + 2.0;
+ } else if (X_hat(2, 0) < last_voltage_ -2.0) {
+ X_hat(2, 0) = last_voltage_ - 2.0;
+ }
+
+ last_voltage_ = voltage_;
+}
+
+
+// Class to zero and control a joint with any number of zeroing sensors with a
+// state feedback controller.
+template<int kNumZeroSensors>
+class ZeroedJoint {
+ public:
+ // Sturcture to hold the hardware configuration information.
+ struct ConfigurationData {
+ // Angle at the lower hardware limit.
+ double lower_limit;
+ // Angle at the upper hardware limit.
+ double upper_limit;
+ // Speed (and direction) to move while zeroing.
+ double zeroing_speed;
+ // Speed (and direction) to move while moving off the sensor.
+ double zeroing_off_speed;
+ // Maximum voltage to apply when zeroing.
+ double max_zeroing_voltage;
+ // Angles where we see a positive edge from the hall effect sensors.
+ double hall_effect_start_angle[kNumZeroSensors];
+ };
+
+ // Current position data for the encoder and hall effect information.
+ struct PositionData {
+ // Current encoder position.
+ double position;
+ // Array of hall effect values.
+ bool hall_effects[kNumZeroSensors];
+ // Array of the last positive edge position for the sensors.
+ double hall_effect_positions[kNumZeroSensors];
+ };
+
+ ZeroedJoint(StateFeedbackLoop<3, 1, 1> loop)
+ : loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
+ state_(UNINITIALIZED),
+ error_count_(0),
+ zero_offset_(0.0),
+ capped_goal_(false) {
+ }
+
+ // Copies the provided configuration data locally.
+ void set_config_data(const ConfigurationData &config_data) {
+ config_data_ = config_data;
+ }
+
+ // Clips the goal to be inside the limits and returns the clipped goal.
+ // Requires the constants to have already been fetched.
+ double ClipGoal(double goal) const {
+ return ::std::min(config_data_.upper_limit,
+ std::max(config_data_.lower_limit, goal));
+ }
+
+ // Updates the loop and state machine.
+ // position is null if the position data is stale, output_enabled is true if
+ // the output will actually go to the motors, and goal_angle and goal_velocity
+ // are the goal position and velocities.
+ double Update(const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled,
+ double goal_angle, double goal_velocity);
+
+ // True if the code is zeroing.
+ bool is_zeroing() const { return state_ == ZEROING; }
+
+ // True if the code is moving off the hall effect.
+ bool is_moving_off() const { return state_ == MOVING_OFF; }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return state_ == UNINITIALIZED; }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return state_ == READY; }
+
+ // Returns the uncapped voltage.
+ double U_uncapped() const { return loop_->U_uncapped(0, 0); }
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return capped_goal_; }
+
+ // Timestamp
+ static const double dt;
+
+ double absolute_position() const { return loop_->X_hat(0, 0); }
+
+ private:
+ friend class ZeroedStateFeedbackLoop<kNumZeroSensors>;
+ // Friend the wrist test cases so that they can simulate windeup.
+ friend class testing::WristTest_NoWindupPositive_Test;
+ friend class testing::WristTest_NoWindupNegative_Test;
+
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<ZeroedStateFeedbackLoop<kNumZeroSensors>> loop_;
+
+ ConfigurationData config_data_;
+
+ // Returns the index of the first active sensor, or -1 if none are active.
+ int ActiveSensorIndex(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
+ if (!position) {
+ return -1;
+ }
+ int active_index = -1;
+ for (int i = 0; i < kNumZeroSensors; ++i) {
+ if (position->hall_effects[i]) {
+ if (active_index != -1) {
+ LOG(ERROR, "More than one hall effect sensor is active\n");
+ } else {
+ active_index = i;
+ }
+ }
+ }
+ return active_index;
+ }
+ // Returns true if any of the sensors are active.
+ bool AnySensorsActive(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
+ return ActiveSensorIndex(position) != -1;
+ }
+
+ // Enum to store the state of the internal zeroing state machine.
+ enum State {
+ UNINITIALIZED,
+ MOVING_OFF,
+ ZEROING,
+ READY,
+ ESTOP
+ };
+
+ // Internal state for zeroing.
+ State state_;
+
+ // Missed position packet count.
+ int error_count_;
+ // Offset from the raw encoder value to the absolute angle.
+ double zero_offset_;
+ // Position that gets incremented when zeroing the wrist to slowly move it to
+ // the hall effect sensor.
+ double zeroing_position_;
+ // Last position at which the hall effect sensor was off.
+ double last_off_position_;
+
+ // True if the zeroing goal was capped during this cycle.
+ bool capped_goal_;
+
+ // Returns true if number is between first and second inclusive.
+ bool is_between(double first, double second, double number) {
+ if ((number >= first || number >= second) &&
+ (number <= first || number <= second)) {
+ return true;
+ }
+ return false;
+ }
+
+ DISALLOW_COPY_AND_ASSIGN(ZeroedJoint);
+};
+
+template <int kNumZeroSensors>
+/*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
+
+// Updates the zeroed joint controller and state machine.
+template <int kNumZeroSensors>
+double ZeroedJoint<kNumZeroSensors>::Update(
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled,
+ double goal_angle, double goal_velocity) {
+ // Uninitialize the bot if too many cycles pass without an encoder.
+ if (position == NULL) {
+ LOG(WARNING, "no new pos given\n");
+ error_count_++;
+ } else {
+ error_count_ = 0;
+ }
+ if (error_count_ >= 4) {
+ LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
+ state_ = UNINITIALIZED;
+ }
+
+ // Compute the absolute position of the wrist.
+ double absolute_position;
+ if (position) {
+ absolute_position = position->position;
+ if (state_ == READY) {
+ absolute_position -= zero_offset_;
+ }
+ loop_->Y << absolute_position;
+ if (!AnySensorsActive(position)) {
+ last_off_position_ = position->position;
+ }
+ } else {
+ // Dead recon for now.
+ absolute_position = loop_->X_hat(0, 0);
+ }
+
+ switch (state_) {
+ case UNINITIALIZED:
+ LOG(DEBUG, "UNINITIALIZED\n");
+ if (position) {
+ // Reset the zeroing goal.
+ zeroing_position_ = absolute_position;
+ // Clear the observer state.
+ loop_->X_hat << absolute_position, 0.0, 0.0;
+ loop_->ZeroPower();
+ // Set the goal to here to make it so it doesn't move when disabled.
+ loop_->R = loop_->X_hat;
+ // Only progress if we are enabled.
+ if (::aos::robot_state->enabled) {
+ if (AnySensorsActive(position)) {
+ state_ = MOVING_OFF;
+ } else {
+ state_ = ZEROING;
+ }
+ }
+ }
+ break;
+ case MOVING_OFF:
+ LOG(DEBUG, "MOVING_OFF\n");
+ {
+ // Move off the hall effect sensor.
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && !AnySensorsActive(position)) {
+ // We are now off the sensor. Time to zero now.
+ state_ = ZEROING;
+ } else {
+ // Slowly creep off the sensor.
+ zeroing_position_ -= config_data_.zeroing_off_speed * dt;
+ loop_->R << zeroing_position_, -config_data_.zeroing_off_speed, 0.0;
+ break;
+ }
+ }
+ case ZEROING:
+ LOG(DEBUG, "ZEROING\n");
+ {
+ int active_sensor_index = ActiveSensorIndex(position);
+ if (!::aos::robot_state->enabled) {
+ // Start over if disabled.
+ state_ = UNINITIALIZED;
+ } else if (position && active_sensor_index != -1) {
+ state_ = READY;
+ // Verify that the calibration number is between the last off position
+ // and the current on position. If this is not true, move off and try
+ // again.
+ const double calibration =
+ position->hall_effect_positions[active_sensor_index];
+ if (!is_between(last_off_position_, position->position,
+ calibration)) {
+ LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
+ LOG(ERROR,
+ "Last off position was %f, current is %f, calibration is %f\n",
+ last_off_position_, position->position,
+ position->hall_effect_positions[active_sensor_index]);
+ state_ = MOVING_OFF;
+ } else {
+ // Save the zero, and then offset the observer to deal with the
+ // phantom step change.
+ const double old_zero_offset = zero_offset_;
+ zero_offset_ =
+ position->hall_effect_positions[active_sensor_index] -
+ config_data_.hall_effect_start_angle[active_sensor_index];
+ loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
+ loop_->Y(0, 0) += old_zero_offset - zero_offset_;
+ }
+ } else {
+ // Slowly creep towards the sensor.
+ zeroing_position_ += config_data_.zeroing_speed * dt;
+ loop_->R << zeroing_position_, config_data_.zeroing_speed, 0.0;
+ }
+ break;
+ }
+
+ case READY:
+ LOG(DEBUG, "READY\n");
+ {
+ const double limited_goal = ClipGoal(goal_angle);
+ loop_->R << limited_goal, goal_velocity, 0.0;
+ break;
+ }
+
+ case ESTOP:
+ LOG(DEBUG, "ESTOP\n");
+ LOG(WARNING, "have already given up\n");
+ return 0.0;
+ }
+
+ // Update the observer.
+ loop_->Update(position != NULL, !output_enabled);
+
+ capped_goal_ = false;
+ // Verify that the zeroing goal hasn't run away.
+ switch (state_) {
+ case UNINITIALIZED:
+ case READY:
+ case ESTOP:
+ // Not zeroing. No worries.
+ break;
+ case MOVING_OFF:
+ case ZEROING:
+ // Check if we have cliped and adjust the goal.
+ if (loop_->uncapped_voltage() > config_data_.max_zeroing_voltage) {
+ double dx = (loop_->uncapped_voltage() -
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ } else if(loop_->uncapped_voltage() < -config_data_.max_zeroing_voltage) {
+ double dx = (loop_->uncapped_voltage() +
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ }
+ break;
+ }
+ return loop_->voltage();
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
diff --git a/frc971/input/GyroReader.cc b/frc971/input/GyroReader.cc
index a6e1d00..9fbee44 100644
--- a/frc971/input/GyroReader.cc
+++ b/frc971/input/GyroReader.cc
@@ -3,7 +3,7 @@
#include <sys/stat.h>
#include <fcntl.h>
-#include "aos/aos_core.h"
+#include "aos/common/logging/logging.h"
#include "frc971/queues/GyroAngle.q.h"
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 43f591a..c62365b 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -26,6 +26,7 @@
'actions',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
{
@@ -40,6 +41,13 @@
'<(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',
@@ -50,6 +58,7 @@
'dependencies': [
'<(AOS)/build/aos.gyp:libaos',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
],
},
{
@@ -59,8 +68,9 @@
'GyroReader.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
],
},
{
@@ -74,6 +84,8 @@
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'actions',
+# TODO(brians) this shouldn't need to be here
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
},
],
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 6cb28d1..780eadf 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -10,6 +10,7 @@
'<(AOS)/build/aos.gyp:libaos',
'<(AOS)/atom_code/output/output.gyp:http_server',
'../frc971.gyp:common',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
'copies': [
{
@@ -30,6 +31,7 @@
'../frc971.gyp:common',
'<(AOS)/atom_code/output/output.gyp:motor_output',
'<(AOS)/atom_code/messages/messages.gyp:messages',
+ '<(AOS)/atom_code/atom_code.gyp:init',
],
}, {
'sources': ['CRIOMotorWriter.cc'],