Merge branch 'sensor-data-timing' into hardware-interface-integration
Conflicts:
aos/common/time.h
diff --git a/aos/atom_code/core/core.cc b/aos/atom_code/core/core.cc
index 4179a0a..2921e46 100644
--- a/aos/atom_code/core/core.cc
+++ b/aos/atom_code/core/core.cc
@@ -1,15 +1,26 @@
-// This is the core scheduling system
-//
-// Purposes: All shared memory gets allocated here.
-//
-
#include <sys/select.h>
#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/types.h>
+
+#include <string>
#include "aos/atom_code/init.h"
-int main() {
+// Initializes shared memory. This is the only file that will create the shared
+// memory file if it doesn't already exist (and set everything up).
+
+int main(int argc, char **argv) {
aos::InitCreate();
+
+ if (argc > 1) {
+ if (system((std::string("touch '") + argv[1] + "'").c_str()) != 0) {
+ fprintf(stderr, "`touch '%s'` failed\n", argv[1]);
+ exit(EXIT_FAILURE);
+ }
+ }
+
select(0, NULL, NULL, NULL, NULL); // wait forever
aos::Cleanup();
}
diff --git a/aos/atom_code/input/JoystickInput.cpp b/aos/atom_code/input/JoystickInput.cpp
index 2dd31da..2c08f0f 100644
--- a/aos/atom_code/input/JoystickInput.cpp
+++ b/aos/atom_code/input/JoystickInput.cpp
@@ -18,8 +18,7 @@
buttons[0] |= (control_data_.enabled << (ENABLED - 9)) |
(control_data_.autonomous << (AUTONOMOUS - 9)) |
- (control_data_.fmsAttached << (FMS_ATTACHED - 9)) |
- (control_data_.test << (TEST_MODE - 9));
+ (control_data_.fmsAttached << (FMS_ATTACHED - 9));
for (int j = 0; j < 4; ++j) {
for (int k = 1; k <= 12; ++k) {
@@ -37,8 +36,6 @@
if (NegEdge(0, AUTONOMOUS)) LOG(INFO, "NegEdge(AUTONOMOUS)\n");
if (PosEdge(0, FMS_ATTACHED)) LOG(INFO, "PosEdge(FMS_ATTACHED)\n");
if (NegEdge(0, FMS_ATTACHED)) LOG(INFO, "NegEdge(FMS_ATTACHED)\n");
- if (PosEdge(0, TEST_MODE)) LOG(INFO, "PosEdge(TEST_MODE)\n");
- if (NegEdge(0, TEST_MODE)) LOG(INFO, "NegEdge(TEST_MODE)\n");
}
void JoystickInput::Run() {
@@ -53,7 +50,6 @@
if (!robot_state.MakeWithBuilder()
.enabled(Pressed(0, ENABLED))
.autonomous(Pressed(0, AUTONOMOUS))
- .test_mode(Pressed(0, TEST_MODE))
.team_id(ntohs(control_data_.teamID))
.Send()) {
LOG(WARNING, "sending robot_state failed\n");
diff --git a/aos/atom_code/input/JoystickInput.h b/aos/atom_code/input/JoystickInput.h
index ddb0388..de5de04 100644
--- a/aos/atom_code/input/JoystickInput.h
+++ b/aos/atom_code/input/JoystickInput.h
@@ -23,7 +23,6 @@
static const int ENABLED = 13;
static const int AUTONOMOUS = 14;
static const int FMS_ATTACHED = 15;
- static const int TEST_MODE = 16;
bool Pressed(int stick, int button) {
return buttons[stick] & MASK(button);
}
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.cc b/aos/atom_code/starter/starter.cc
new file mode 100644
index 0000000..8f8a65d
--- /dev/null
+++ b/aos/atom_code/starter/starter.cc
@@ -0,0 +1,784 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <sys/inotify.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <assert.h>
+#include <signal.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/wait.h>
+#include <inttypes.h>
+
+#include <map>
+#include <functional>
+#include <deque>
+#include <fstream>
+#include <queue>
+#include <list>
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <event2/event.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging_impl.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/unique_malloc_ptr.h"
+#include "aos/common/time.h"
+#include "aos/common/once.h"
+
+// This is the main piece of code that starts all of the rest of the code and
+// restarts it when the binaries are modified.
+//
+// Throughout, the code is not terribly concerned with thread safety because
+// there is only 1 thread. It does some setup and then lets inotify run things
+// when appropriate.
+//
+// NOTE: This program should never exit nicely. It catches all nice attempts to
+// exit, forwards them to all of the children that it has started, waits for
+// them to exit nicely, and then SIGKILLs anybody left (which will always
+// include itself).
+
+using ::std::unique_ptr;
+
+namespace aos {
+namespace starter {
+
+// TODO(brians): split out the c++ libevent wrapper stuff into its own file(s)
+class EventBaseDeleter {
+ public:
+ void operator()(event_base *base) {
+ if (base == NULL) return;
+ event_base_free(base);
+ }
+};
+typedef unique_ptr<event_base, EventBaseDeleter> EventBaseUniquePtr;
+EventBaseUniquePtr libevent_base;
+
+class EventDeleter {
+ public:
+ void operator()(event *evt) {
+ if (evt == NULL) return;
+ if (event_del(evt) != 0) {
+ LOG(WARNING, "event_del(%p) failed\n", evt);
+ }
+ }
+};
+typedef unique_ptr<event, EventDeleter> EventUniquePtr;
+
+// Watches a file path for modifications. Once created, keeps watching until
+// destroyed or RemoveWatch() is called.
+// TODO(brians): split this out into its own file + tests
+class FileWatch {
+ public:
+ // Will call callback(value) when filename is modified.
+ // If value is NULL, then a pointer to this object will be passed instead.
+ //
+ // Watching for file creations is slightly different. To do that, pass true
+ // as create, the directory where the file will be created for filename, and
+ // the name of the file (without directory name) for check_filename.
+ FileWatch(std::string filename,
+ std::function<void(void *)> callback,
+ void *value,
+ bool create = false,
+ std::string check_filename = "")
+ : filename_(filename),
+ callback_(callback),
+ value_(value),
+ create_(create),
+ check_filename_(check_filename),
+ watch_(-1) {
+ init_once.Get();
+
+ CreateWatch();
+ }
+ // Cleans up everything.
+ ~FileWatch() {
+ if (watch_ != -1) {
+ RemoveWatch();
+ }
+ }
+
+ // After calling this method, this object won't really be doing much of
+ // anything besides possibly running its callback or something.
+ void RemoveWatch() {
+ assert(watch_ != -1);
+
+ if (inotify_rm_watch(notify_fd, watch_) == -1) {
+ LOG(WARNING, "inotify_rm_watch(%d, %d) failed with %d: %s\n",
+ notify_fd, watch_, errno, strerror(errno));
+ }
+
+ RemoveWatchFromMap();
+ }
+
+ private:
+ // Performs the static initialization. Called by init_once from the
+ // constructor.
+ static void *Init() {
+ notify_fd = inotify_init1(IN_CLOEXEC);
+ EventUniquePtr notify_event(event_new(libevent_base.get(), notify_fd,
+ EV_READ | EV_PERSIST,
+ FileWatch::INotifyReadable, NULL));
+ event_add(notify_event.release(), NULL);
+ return NULL;
+ }
+
+ void RemoveWatchFromMap() {
+ if (watchers[watch_] != this) {
+ LOG(WARNING, "watcher for %s (%p) didn't find itself in the map\n",
+ filename_.c_str(), this);
+ } else {
+ watchers.erase(watch_);
+ }
+ LOG(DEBUG, "removed watch ID %d\n", watch_);
+ watch_ = -1;
+ }
+
+ void CreateWatch() {
+ assert(watch_ == -1);
+ watch_ = inotify_add_watch(notify_fd, filename_.c_str(),
+ create_ ? IN_CREATE : (IN_ATTRIB |
+ IN_MODIFY |
+ IN_DELETE_SELF |
+ IN_MOVE_SELF));
+ if (watch_ == -1) {
+ LOG(FATAL, "inotify_add_watch(%d, %s,"
+ " %s ? IN_CREATE : (IN_ATTRIB | IN_MODIFY)) failed with %d: %s\n",
+ notify_fd, filename_.c_str(), create_ ? "true" : "false",
+ errno, strerror(errno));
+ }
+ watchers[watch_] = this;
+ LOG(DEBUG, "watch for %s is %d\n", filename_.c_str(), watch_);
+ }
+
+ // This gets set up as the callback for EV_READ on the inotify file
+ // descriptor. It calls FileNotified on the appropriate instance.
+ static void INotifyReadable(int /*fd*/, short /*events*/, void *) {
+ unsigned int to_read;
+ // Use FIONREAD to figure out how many bytes there are to read.
+ if (ioctl(notify_fd, FIONREAD, &to_read) < 0) {
+ LOG(FATAL, "FIONREAD(%d, %p) failed with %d: %s\n",
+ notify_fd, &to_read, errno, strerror(errno));
+ }
+ inotify_event *notifyevt = static_cast<inotify_event *>(malloc(to_read));
+ const char *end = reinterpret_cast<char *>(notifyevt) + to_read;
+ aos::unique_c_ptr<inotify_event> freer(notifyevt);
+
+ ssize_t ret = read(notify_fd, notifyevt, to_read);
+ if (ret < 0) {
+ LOG(FATAL, "read(%d, %p, %u) failed with %d: %s\n",
+ notify_fd, notifyevt, to_read, errno, strerror(errno));
+ }
+ if (static_cast<size_t>(ret) != to_read) {
+ LOG(ERROR, "read(%d, %p, %u) returned %zd instead of %u\n",
+ notify_fd, notifyevt, to_read, ret, to_read);
+ return;
+ }
+
+ // Keep looping through until we get to the end because inotify does return
+ // multiple events at once.
+ while (reinterpret_cast<char *>(notifyevt) < end) {
+ if (watchers.count(notifyevt->wd) != 1) {
+ LOG(WARNING, "couldn't find whose watch ID %d is\n", notifyevt->wd);
+ } else {
+ LOG(DEBUG, "mask=%"PRIu32"\n", notifyevt->mask);
+ // If it was something that means the file got deleted.
+ if (notifyevt->mask & (IN_MOVE_SELF | IN_DELETE_SELF | IN_IGNORED)) {
+ watchers[notifyevt->wd]->WatchDeleted();
+ } else {
+ watchers[notifyevt->wd]->FileNotified((notifyevt->len > 0) ?
+ notifyevt->name : NULL);
+ }
+ }
+
+ notifyevt = reinterpret_cast<inotify_event *>(
+ reinterpret_cast<char *>(notifyevt) +
+ sizeof(*notifyevt) + notifyevt->len);
+ }
+ }
+
+ // INotifyReadable calls this method whenever the watch for our file gets
+ // removed somehow.
+ void WatchDeleted() {
+ LOG(DEBUG, "watch for %s deleted\n", filename_.c_str());
+ RemoveWatchFromMap();
+ CreateWatch();
+ }
+
+ // INotifyReadable calls this method whenever the watch for our file triggers.
+ void FileNotified(const char *filename) {
+ assert(watch_ != -1);
+ LOG(DEBUG, "got a notification for %s\n", filename_.c_str());
+
+ if (!check_filename_.empty()) {
+ if (filename == NULL) {
+ return;
+ }
+ if (std::string(filename) != check_filename_) {
+ return;
+ }
+ }
+
+ callback_((value_ == NULL) ? this : value_);
+ }
+
+ // To make sure that Init gets called exactly once.
+ static ::aos::Once<void> init_once;
+
+ const std::string filename_;
+ const std::function<void(void *)> callback_;
+ void *const value_;
+ const bool create_;
+ std::string check_filename_;
+
+ // The watch descriptor or -1 if we don't have one any more.
+ int watch_;
+
+ // Map from watch IDs to instances of this class.
+ // <https://patchwork.kernel.org/patch/73192/> ("inotify: do not reuse watch
+ // descriptors") says they won't get reused, but that shouldn't be counted on
+ // because we might have a modified/different version/whatever kernel.
+ static std::map<int, FileWatch *> watchers;
+ // The inotify(7) file descriptor.
+ static int notify_fd;
+
+ DISALLOW_COPY_AND_ASSIGN(FileWatch);
+};
+::aos::Once<void> FileWatch::init_once(FileWatch::Init);
+std::map<int, FileWatch *> FileWatch::watchers;
+int FileWatch::notify_fd;
+
+// Runs the given command and returns its first line of output (not including
+// the \n). LOG(FATAL)s if the command has an exit status other than 0 or does
+// not print out an entire line.
+std::string RunCommand(std::string command) {
+ // popen(3) might fail and not set it.
+ errno = 0;
+ FILE *pipe = popen(command.c_str(), "r");
+ if (pipe == NULL) {
+ LOG(FATAL, "popen(\"%s\", \"r\") failed with %d: %s\n",
+ command.c_str(), errno, strerror(errno));
+ }
+
+ // result_size is how many bytes result is currently allocated to.
+ size_t result_size = 128, read = 0;
+ unique_c_ptr<char> result(static_cast<char *>(malloc(result_size)));
+ while (true) {
+ // If we filled up the buffer, then realloc(3) it bigger.
+ if (read == result_size) {
+ result_size *= 2;
+ void *new_result = realloc(result.get(), result_size);
+ if (new_result == NULL) {
+ LOG(FATAL, "realloc(%p, %zd) failed because of %d: %s\n",
+ result.get(), result_size, errno, strerror(errno));
+ } else {
+ result.release();
+ result = unique_c_ptr<char>(static_cast<char *>(new_result));
+ }
+ }
+
+ size_t ret = fread(result.get() + read, 1, result_size - read, pipe);
+ // If the read didn't fill up the whole buffer, check to see if it was
+ // because of an error.
+ if (ret < result_size - read) {
+ if (ferror(pipe)) {
+ LOG(FATAL, "couldn't finish reading output of \"%s\"\n",
+ command.c_str());
+ }
+ }
+ read += ret;
+ if (read > 0 && result.get()[read - 1] == '\n') {
+ break;
+ }
+
+ if (feof(pipe)) {
+ LOG(FATAL, "`%s` failed. didn't print a whole line\n", command.c_str());
+ }
+ }
+
+ // Get rid of the first \n and anything after it.
+ *strchrnul(result.get(), '\n') = '\0';
+
+ int child_status = pclose(pipe);
+ if (child_status == -1) {
+ LOG(FATAL, "pclose(%p) failed with %d: %s\n", pipe,
+ errno, strerror(errno));
+ }
+
+ if (child_status != 0) {
+ LOG(FATAL, "`%s` failed. return %d\n", command.c_str(), child_status);
+ }
+
+ return std::string(result.get());
+}
+
+// Will call callback(arg) after time.
+void Timeout(time::Time time, void (*callback)(int, short, void *), void *arg) {
+ EventUniquePtr timeout(evtimer_new(libevent_base.get(), callback, arg));
+ struct timeval time_timeval = time.ToTimeval();
+ evtimer_add(timeout.release(), &time_timeval);
+}
+
+// Represents a child process. It will take care of restarting itself etc.
+class Child {
+ public:
+ // command is the (space-separated) command to run and its arguments.
+ Child(const std::string &command) : pid_(-1),
+ restart_timeout_(
+ evtimer_new(libevent_base.get(), StaticDoRestart, this)),
+ stat_at_start_valid_(false) {
+ const char *start, *end;
+ start = command.c_str();
+ while (true) {
+ end = strchrnul(start, ' ');
+ args_.push_back(std::string(start, end - start));
+ start = end + 1;
+ if (*end == '\0') {
+ break;
+ }
+ }
+
+ original_binary_ = RunCommand("which " + args_[0]);
+ binary_ = original_binary_ + ".stm";
+
+ watcher_ = unique_ptr<FileWatch>(
+ new FileWatch(original_binary_, StaticFileModified, this));
+
+ Start();
+ }
+
+ pid_t pid() { return pid_; }
+
+ // This gets called whenever the actual process dies and should (probably) be
+ // restarted.
+ void ProcessDied() {
+ pid_ = -1;
+ restarts_.push(time::Time::Now());
+ if (restarts_.size() > kMaxRestartsNumber) {
+ time::Time oldest = restarts_.front();
+ restarts_.pop();
+ if ((time::Time::Now() - oldest) <= kMaxRestartsTime) {
+ LOG(WARNING, "process %s getting restarted too often\n", name());
+ Timeout(kResumeWait, StaticStart, this);
+ return;
+ }
+ }
+ Start();
+ }
+
+ // Returns a name for logging purposes.
+ const char *name() {
+ return args_[0].c_str();
+ }
+
+ private:
+ struct CheckDiedStatus {
+ Child *self;
+ pid_t old_pid;
+ };
+
+ // How long to wait for a child to die nicely.
+ static const time::Time kProcessDieTime;
+
+ // How long to wait after the file is modified to restart it.
+ // This is important because some programs like modifying the binaries by
+ // writing them in little bits, which results in attempting to start partial
+ // binaries without this.
+ static const time::Time kRestartWaitTime;
+
+ // Only kMaxRestartsNumber restarts will be allowed in kMaxRestartsTime.
+ static const time::Time kMaxRestartsTime;
+ static const size_t kMaxRestartsNumber = 4;
+ // How long to wait if it gets restarted too many times.
+ static const time::Time kResumeWait;
+
+ static void StaticFileModified(void *self) {
+ static_cast<Child *>(self)->FileModified();
+ }
+
+ void FileModified() {
+ LOG(DEBUG, "file for %s modified\n", name());
+ struct timeval restart_time_timeval = kRestartWaitTime.ToTimeval();
+ // This will reset the timeout again if it hasn't run yet.
+ if (evtimer_add(restart_timeout_.get(), &restart_time_timeval) != 0) {
+ LOG(FATAL, "evtimer_add(%p, %p) failed\n",
+ restart_timeout_.get(), &restart_time_timeval);
+ }
+ }
+
+ static void StaticDoRestart(int, short, void *self) {
+ static_cast<Child *>(self)->DoRestart();
+ }
+
+ // Called after somebody else has finished modifying the file.
+ void DoRestart() {
+ if (stat_at_start_valid_) {
+ struct stat current_stat;
+ if (stat(original_binary_.c_str(), ¤t_stat) == -1) {
+ LOG(FATAL, "stat(%s, %p) failed with %d: %s\n",
+ original_binary_.c_str(), ¤t_stat, errno, strerror(errno));
+ }
+ if (current_stat.st_mtime == stat_at_start_.st_mtime) {
+ LOG(DEBUG, "ignoring trigger for %s because mtime didn't change\n",
+ name());
+ return;
+ }
+ }
+
+ if (pid_ != -1) {
+ LOG(DEBUG, "sending SIGTERM to child %d to restart it\n", pid_);
+ if (kill(pid_, SIGTERM) == -1) {
+ LOG(WARNING, "kill(%d, SIGTERM) failed with %d: %s\n",
+ pid_, errno, strerror(errno));
+ }
+ CheckDiedStatus *status = new CheckDiedStatus();
+ status->self = this;
+ status->old_pid = pid_;
+ Timeout(kProcessDieTime, StaticCheckDied, status);
+ } else {
+ LOG(WARNING, "%s restart attempted but not running\n", name());
+ }
+ }
+
+ static void StaticCheckDied(int, short, void *status_in) {
+ CheckDiedStatus *status = static_cast<CheckDiedStatus *>(status_in);
+ status->self->CheckDied(status->old_pid);
+ delete status;
+ }
+
+ // Checks to see if the child using the PID old_pid is still running.
+ void CheckDied(pid_t old_pid) {
+ if (pid_ == old_pid) {
+ LOG(WARNING, "child %d refused to die\n", old_pid);
+ if (kill(old_pid, SIGKILL) == -1) {
+ LOG(WARNING, "kill(%d, SIGKILL) failed with %d: %s\n",
+ old_pid, errno, strerror(errno));
+ }
+ }
+ }
+
+ static void StaticStart(int, short, void *self) {
+ static_cast<Child *>(self)->Start();
+ }
+
+ // Actually starts the child.
+ void Start() {
+ if (pid_ != -1) {
+ LOG(WARNING, "calling Start() but already have child %d running\n",
+ pid_);
+ if (kill(pid_, SIGKILL) == -1) {
+ LOG(WARNING, "kill(%d, SIGKILL) failed with %d: %s\n",
+ pid_, errno, strerror(errno));
+ return;
+ }
+ pid_ = -1;
+ }
+
+ // Remove the name that we run from (ie from a previous execution) and then
+ // hard link the real filename to it.
+ if (unlink(binary_.c_str()) != 0 && errno != ENOENT) {
+ LOG(FATAL, "removing %s failed because of %d: %s\n",
+ binary_.c_str(), errno, strerror(errno));
+ }
+ if (link(original_binary_.c_str(), binary_.c_str()) != 0) {
+ LOG(FATAL, "link('%s', '%s') failed because of %d: %s\n",
+ original_binary_.c_str(), binary_.c_str(), errno, strerror(errno));
+ }
+
+ if (stat(original_binary_.c_str(), &stat_at_start_) == -1) {
+ LOG(FATAL, "stat(%s, %p) failed with %d: %s\n",
+ original_binary_.c_str(), &stat_at_start_, errno, strerror(errno));
+ }
+ stat_at_start_valid_ = true;
+
+ if ((pid_ = fork()) == 0) {
+ ssize_t args_size = args_.size();
+ const char **argv = new const char *[args_size + 1];
+ for (int i = 0; i < args_size; ++i) {
+ argv[i] = args_[i].c_str();
+ }
+ argv[args_size] = NULL;
+ // The const_cast is safe because no code that might care if it gets
+ // modified can run afterwards.
+ execv(binary_.c_str(), const_cast<char **>(argv));
+ LOG(FATAL, "execv(%s, %p) failed with %d: %s\n",
+ binary_.c_str(), argv, errno, strerror(errno));
+ _exit(EXIT_FAILURE);
+ }
+ if (pid_ == -1) {
+ LOG(FATAL, "forking to run \"%s\" failed with %d: %s\n",
+ binary_.c_str(), errno, strerror(errno));
+ }
+ LOG(DEBUG, "started \"%s\" successfully\n", binary_.c_str());
+ }
+
+ // A history of the times that this process has been restarted.
+ std::queue<time::Time, std::list<time::Time>> restarts_;
+
+ // The currently running child's PID or NULL.
+ pid_t pid_;
+
+ // All of the arguments (including the name of the binary).
+ std::deque<std::string> args_;
+
+ // The name of the real binary that we were told to run.
+ std::string original_binary_;
+ // The name of the file that we're actually running.
+ std::string binary_;
+
+ // Watches original_binary_.
+ unique_ptr<FileWatch> watcher_;
+
+ // An event that restarts after kRestartWaitTime.
+ EventUniquePtr restart_timeout_;
+
+ // Captured from the original file when we most recently started a new child
+ // process. Used to see if it actually changes or not.
+ struct stat stat_at_start_;
+ bool stat_at_start_valid_;
+
+ DISALLOW_COPY_AND_ASSIGN(Child);
+};
+const time::Time Child::kProcessDieTime = time::Time::InSeconds(0.5);
+const time::Time Child::kMaxRestartsTime = time::Time::InSeconds(4);
+const time::Time Child::kResumeWait = time::Time::InSeconds(2);
+const time::Time Child::kRestartWaitTime = time::Time::InSeconds(1.5);
+
+// This is where all of the Child instances except core live.
+std::vector<unique_ptr<Child>> children;
+// A global place to hold on to which child is core.
+unique_ptr<Child> core;
+
+// Kills off the entire process group (including ourself).
+void KillChildren(bool try_nice) {
+ if (try_nice) {
+ static const int kNiceStopSignal = SIGTERM;
+ static const time::Time kNiceWaitTime = time::Time::InSeconds(1);
+
+ // Make sure that we don't just nicely stop ourself...
+ sigset_t mask;
+ sigemptyset(&mask);
+ sigaddset(&mask, kNiceStopSignal);
+ sigprocmask(SIG_BLOCK, &mask, NULL);
+
+ kill(-getpid(), kNiceStopSignal);
+
+ fflush(NULL);
+ time::SleepFor(kNiceWaitTime);
+ }
+
+ // Send SIGKILL to our whole process group, which will forcibly terminate any
+ // of them that are still running (us for sure, maybe more too).
+ kill(-getpid(), SIGKILL);
+}
+
+void ExitHandler() {
+ KillChildren(true);
+}
+
+void KillChildrenSignalHandler(int signum) {
+ // If we get SIGSEGV or some other random signal who knows what's happening
+ // and we should just kill everybody immediately.
+ // This is a list of all of the signals that mean some form of "nicely stop".
+ KillChildren(signum == SIGHUP || signum == SIGINT || signum == SIGQUIT ||
+ signum == SIGABRT || signum == SIGPIPE || signum == SIGTERM ||
+ signum == SIGXCPU);
+}
+
+// Returns the currently running child with PID pid or an empty unique_ptr.
+const unique_ptr<Child> &FindChild(pid_t pid) {
+ for (auto it = children.begin(); it != children.end(); ++it) {
+ if (pid == (*it)->pid()) {
+ return *it;
+ }
+ }
+
+ if (pid == core->pid()) {
+ return core;
+ }
+
+ static const unique_ptr<Child> kNothing;
+ return kNothing;
+}
+
+// Gets set up as a libevent handler for SIGCHLD.
+// Handles calling Child::ProcessDied() on the appropriate one.
+void SigCHLDReceived(int /*fd*/, short /*events*/, void *) {
+ // In a while loop in case we miss any SIGCHLDs.
+ while (true) {
+ siginfo_t infop;
+ infop.si_pid = 0;
+ if (waitid(P_ALL, 0, &infop, WEXITED | WSTOPPED | WNOHANG) != 0) {
+ LOG(WARNING, "waitid failed with %d: %s", errno, strerror(errno));
+ continue;
+ }
+ // If there are no more child process deaths to process.
+ if (infop.si_pid == 0) {
+ return;
+ }
+
+ pid_t pid = infop.si_pid;
+ int status = infop.si_status;
+ const unique_ptr<Child> &child = FindChild(pid);
+ if (child) {
+ switch (infop.si_code) {
+ case CLD_EXITED:
+ LOG(WARNING, "child %d (%s) exited with status %d\n",
+ pid, child->name(), status);
+ break;
+ case CLD_DUMPED:
+ LOG(INFO, "child %d actually dumped core. "
+ "falling through to killed by signal case\n", pid);
+ case CLD_KILLED:
+ // If somebody (possibly us) sent it SIGTERM that means that they just
+ // want it to stop, so it stopping isn't a WARNING.
+ LOG((status == SIGTERM) ? DEBUG : WARNING,
+ "child %d (%s) was killed by signal %d (%s)\n",
+ pid, child->name(), status,
+ strsignal(status));
+ break;
+ case CLD_STOPPED:
+ LOG(WARNING, "child %d (%s) was stopped by signal %d "
+ "(giving it a SIGCONT(%d))\n",
+ pid, child->name(), status, SIGCONT);
+ kill(pid, SIGCONT);
+ continue;
+ default:
+ LOG(WARNING, "something happened to child %d (%s) (killing it)\n",
+ pid, child->name());
+ kill(pid, SIGKILL);
+ continue;
+ }
+ } else {
+ LOG(WARNING, "couldn't find a Child for pid %d\n", pid);
+ return;
+ }
+
+ if (child == core) {
+ LOG(FATAL, "core died\n");
+ }
+ child->ProcessDied();
+ }
+}
+
+// This is used for communicating the name of the file to read processes to
+// start from main to Run.
+const char *child_list_file;
+
+void Run(void *watch);
+void Main() {
+ logging::Init();
+ // TODO(brians): tell logging that using the root logger from here until we
+ // bring up shm is ok
+
+ if (setpgid(0 /*self*/, 0 /*make PGID the same as PID*/) != 0) {
+ LOG(FATAL, "setpgid(0, 0) failed with %d: %s\n", errno, strerror(errno));
+ }
+
+ // Make sure that we kill all children when we exit.
+ atexit(ExitHandler);
+ // Do it on some signals too (ones that we otherwise tend to receive and then
+ // leave all of our children going).
+ signal(SIGHUP, KillChildrenSignalHandler);
+ signal(SIGINT, KillChildrenSignalHandler);
+ signal(SIGQUIT, KillChildrenSignalHandler);
+ signal(SIGILL, KillChildrenSignalHandler);
+ signal(SIGABRT, KillChildrenSignalHandler);
+ signal(SIGFPE, KillChildrenSignalHandler);
+ signal(SIGSEGV, KillChildrenSignalHandler);
+ signal(SIGPIPE, KillChildrenSignalHandler);
+ signal(SIGTERM, KillChildrenSignalHandler);
+ signal(SIGBUS, KillChildrenSignalHandler);
+ signal(SIGXCPU, KillChildrenSignalHandler);
+
+ libevent_base = EventBaseUniquePtr(event_base_new());
+
+ std::string core_touch_file = "/tmp/starter.";
+ core_touch_file += std::to_string(static_cast<intmax_t>(getpid()));
+ core_touch_file += ".core_touch_file";
+ if (system(("touch '" + core_touch_file + "'").c_str()) != 0) {
+ LOG(FATAL, "running `touch '%s'` failed\n", core_touch_file.c_str());
+ }
+ FileWatch core_touch_file_watch(core_touch_file, Run, NULL);
+ core = unique_ptr<Child>(
+ new Child("core " + core_touch_file));
+
+ FILE *pid_file = fopen("/tmp/starter.pid", "w");
+ if (pid_file == NULL) {
+ LOG(FATAL, "fopen(\"/tmp/starter.pid\", \"w\") failed with %d: %s\n",
+ errno, strerror(errno));
+ } else {
+ if (fprintf(pid_file, "%d", core->pid()) == -1) {
+ LOG(WARNING, "fprintf(%p, \"%%d\", %d) failed with %d: %s\n",
+ pid_file, core->pid(), errno, strerror(errno));
+ }
+ fclose(pid_file);
+ }
+
+ LOG(INFO, "waiting for %s to appear\n", core_touch_file.c_str());
+
+ event_base_dispatch(libevent_base.get());
+ LOG(FATAL, "event_base_dispatch(%p) returned\n", libevent_base.get());
+}
+
+// This is the callback for when core creates the file indicating that it has
+// started.
+void Run(void *watch) {
+ // Make it so it doesn't keep on seeing random changes in /tmp.
+ static_cast<FileWatch *>(watch)->RemoveWatch();
+
+ // It's safe now because core is up.
+ aos::InitNRT();
+
+ std::ifstream list_file(child_list_file);
+
+ while (true) {
+ std::string child_name;
+ getline(list_file, child_name);
+ if ((list_file.rdstate() & std::ios_base::eofbit) != 0) {
+ break;
+ }
+ if (list_file.rdstate() != 0) {
+ LOG(FATAL, "reading input file %s failed\n", child_list_file);
+ }
+ children.push_back(unique_ptr<Child>(new Child(child_name)));
+ }
+
+ EventUniquePtr sigchld(event_new(libevent_base.get(), SIGCHLD,
+ EV_SIGNAL | EV_PERSIST,
+ SigCHLDReceived, NULL));
+ event_add(sigchld.release(), NULL);
+}
+
+const char *kArgsHelp = "[OPTION]... START_LIST\n"
+ "Start all of the robot code binaries in START_LIST.\n"
+ "\n"
+ "START_LIST is the file to read binaries (looked up on PATH) to run.\n"
+ " --help display this help and exit\n";
+void PrintHelp() {
+ fprintf(stderr, "Usage: %s %s", program_invocation_name, kArgsHelp);
+}
+
+} // namespace starter
+} // namespace aos
+
+int main(int argc, char *argv[]) {
+ if (argc != 2) {
+ aos::starter::PrintHelp();
+ exit(EXIT_FAILURE);
+ }
+ if (strcmp(argv[1], "--help") == 0) {
+ aos::starter::PrintHelp();
+ exit(EXIT_SUCCESS);
+ }
+
+ aos::starter::child_list_file = argv[1];
+
+ aos::starter::Main();
+}
diff --git a/aos/atom_code/starter/starter.cpp b/aos/atom_code/starter/starter.cpp
deleted file mode 100644
index e47e213..0000000
--- a/aos/atom_code/starter/starter.cpp
+++ /dev/null
@@ -1,346 +0,0 @@
-#include "aos/atom_code/starter/starter.h"
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/signalfd.h>
-#include <sys/types.h>
-#include <fcntl.h>
-#include <sys/inotify.h>
-#include <sys/stat.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(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(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(ERROR, "`which %s` was too long. not starting '%s'\n", cmd, cmd);
- return 0;
- }
- orig_exe[ret] = '\0';
- if (pclose(which) == -1) {
- 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(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(ERROR, "`which %s.stm` was too long. not starting %s\n", cmd, cmd);
- return 0;
- }
- orig_exe[ret] = '\0';
- if (pclose(which) == -1) {
- LOG(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
- }
- }
- if (strlen(orig_exe) == 0) {
- 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(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
- }
- strncpy(exe, orig_exe, CMDLEN);
-
- strcat(exe, ".stm");
- struct stat st;
- errno = 0;
- if (stat(orig_exe, &st) != 0 && errno != ENOENT) {
- 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(WARNING, "binary '%s' doesn't exist. not starting it\n", orig_exe);
- return 0;
- }
- struct stat st2;
- // if we can confirm it's already 0 size
- bool orig_zero = stat(orig_exe, &st2) == 0 && st2.st_size == 0;
- if (!orig_zero) {
- // if it failed and it wasn't because it was missing
- if (unlink(exe) != 0 && (errno != EROFS && errno != ENOENT)) {
- 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(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(INFO, "exe ('%s') already existed\n", exe);
- }
-
- pid_t child;
- if ((child = fork()) == 0) {
- execlp(exe, orig_exe, static_cast<char *>(NULL));
- 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(WARNING, "fork on '%s' failed with %d: %s",
- cmd, errno, strerror(errno));
- if (times < 100) {
- return start(cmd, times + 1);
- } else {
- LOG(ERROR, "tried to start '%s' too many times. giving up\n", cmd);
- return 0;
- }
- } else {
- children[child] = cmd;
- files[child] = orig_exe;
- int ret = inotify_add_watch(notifyfd, orig_exe, IN_ATTRIB | IN_MODIFY);
- if (ret < 0) {
- 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;
- }
- return child;
- }
-}
-
-static bool exited = false;
-void exit_handler() {
- if(exited) {
- return;
- } else {
- exited = true;
- }
- fputs("starter: killing all children for exit\n", stdout);
- for (auto it = children.begin(); it != children.end(); ++it) {
- printf("starter: killing child %d ('%s') for exit\n", it->first, it->second);
- kill(it->first, SIGKILL);
- }
- if (sigfd != 0) {
- close(sigfd);
- }
- if (notifyfd != 0) {
- close(notifyfd);
- }
-}
-void niceexit(int status) {
- printf("starter: niceexit(%d) EXIT_SUCCESS=%d EXIT_FAILURE=%d\n",
- status, EXIT_SUCCESS, EXIT_FAILURE);
- exit_handler();
- exit(status);
-}
-
-int main(int argc, char *argv[]) {
- if (argc < 2) {
- fputs("starter: error: need an argument specifying what file to use\n", stderr);
- niceexit(EXIT_FAILURE);
- } else if(argc > 2) {
- fputs("starter: warning: too many arguments\n", stderr);
- }
-
- atexit(exit_handler);
-
- aos::logging::Init();
-
- notifyfd = inotify_init1(IN_NONBLOCK);
-
- pid_t core = start("core", 0);
- if (core == 0) {
- fputs("starter: error: core didn't exist\n", stderr);
- niceexit(EXIT_FAILURE);
- }
- fprintf(stderr, "starter: info: core's pid is %jd\n", static_cast<intmax_t>(core));
- FILE *pid_file = fopen("/tmp/starter.pid", "w");
- if (pid_file == NULL) {
- perror("fopen(/tmp/starter.pid)");
- } else {
- if (fprintf(pid_file, "%d", core) == -1) {
- fprintf(stderr, "starter: error: fprintf(pid_file, core(=%d)) failed "
- "with %d: %s",
- core, errno, strerror(errno));
- }
- fclose(pid_file);
- }
- sleep(1);
- if (kill(core, 0) != 0) {
- fprintf(stderr, "starter: couldn't kill(%jd(=core), 0) because of %d: %s\n",
- static_cast<intmax_t>(core), errno, strerror(errno));
- niceexit(EXIT_FAILURE);
- }
- fputs("starter: before init\n", stdout);
- aos::InitNRT();
- fputs("starter: after init\n", stdout);
-
- FILE *list = fopen(argv[1], "re");
- char line[CMDLEN + 1];
- char *line_copy;
- uint8_t too_long = 0;
- while (fgets(line, sizeof(line), list) != NULL) {
- if (line[strlen(line) - 1] != '\n') {
- LOG(WARNING, "command segment '%s' is too long. "
- "increase the size of the line char[] above " __FILE__ ": %d\n",
- line, __LINE__);
- too_long = 1;
- continue;
- }
- if (too_long) {
- too_long = 0;
- LOG(WARNING, "\tgot last chunk of too long line: '%s'\n", line);
- continue; // don't try running the last little chunk
- }
- line[strlen(line) - 1] = '\0'; // get rid of the \n
- line_copy = new char[strlen(line) + 1];
- memcpy(line_copy, line, strlen(line) + 1);
- fprintf(stderr, "starter: info: going to start \"%s\"\n", line_copy);
- start(line_copy, 0);
- }
- fclose(list);
- LOG(INFO, "started everything\n");
-
- sigset_t mask;
- sigemptyset (&mask);
- sigaddset (&mask, SIGCHLD);
- sigprocmask (SIG_BLOCK, &mask, NULL);
- sigfd = signalfd (-1, &mask, O_NONBLOCK);
-
- fd_set readfds;
- FD_ZERO(&readfds);
- siginfo_t infop;
- signalfd_siginfo fdsi;
- inotify_event notifyevt;
- int ret;
- while (1) {
- FD_SET(sigfd, &readfds);
- FD_SET(notifyfd, &readfds);
- timeval timeout;
- timeout.tv_sec = restarts.empty() ? 2 : 0;
- timeout.tv_usec = 100000;
- ret = select (FD_SETSIZE, &readfds, NULL, NULL, &timeout);
-
- if (ret == 0) { // timeout
- auto it = restarts.begin();
- // WARNING because the message about it dying will be
- for (; it != restarts.end(); it++) {
- LOG(WARNING, "restarting process %d ('%s') by giving it a SIGKILL(%d)\n",
- *it, children[*it], SIGKILL);
- kill(*it, SIGKILL);
- }
- restarts.clear();
- }
-
- if (FD_ISSET(notifyfd, &readfds)) {
- if ((ret = read(notifyfd, ¬ifyevt, sizeof(notifyevt))) ==
- sizeof(notifyevt)) {
- if (watches.count(notifyevt.wd)) {
- struct stat st;
- if (!children.count(watches[notifyevt.wd]) ||
- stat(files[watches[notifyevt.wd]], &st) == 0) {
- if (mtimes[notifyevt.wd] == st.st_mtime) {
- LOG(DEBUG, "ignoring trigger of watch id %d (file '%s')"
- " because mtime didn't change\n",
- notifyevt.wd, files[watches[notifyevt.wd]]);
- } else if (children.count(watches[notifyevt.wd])) {
- LOG(DEBUG, "adding process %d to the restart list\n",
- watches[notifyevt.wd]);
- restarts.insert(watches[notifyevt.wd]);
- } else {
- LOG(DEBUG, "children doesn't have entry for PID %d\n",
- watches[notifyevt.wd]);
- }
- } else {
- LOG(ERROR, "stat('%s') failed with %d: %s\n",
- files[watches[notifyevt.wd]], errno, strerror(errno));
- }
- } else {
- LOG(WARNING, "no PID for watch id %d\n", notifyevt.wd);
- }
- } else {
- if (ret == -1) {
- LOG(WARNING, "read(notifyfd) failed with %d: %s", errno, strerror(errno));
- } else {
- LOG(WARNING, "couldn't get a whole inotify_event(%d) (only got %d)\n",
- sizeof(notifyevt), ret);
- }
- }
- }
-
- if (FD_ISSET(sigfd, &readfds)) {
- while(read (sigfd, &fdsi, sizeof fdsi) > 0);
- }
- while (1) {
- infop.si_pid = 0;
- if (waitid(P_ALL, 0, &infop, WEXITED | WSTOPPED | WNOHANG) == 0) {
- if (infop.si_pid == 0) {
- goto after_loop; // no more child process changes pending
- }
- switch (infop.si_code) {
- case CLD_EXITED:
- LOG(WARNING, "child %d (%s) exited with status %d\n",
- infop.si_pid, children[infop.si_pid], infop.si_status);
- break;
- case CLD_DUMPED:
- LOG(INFO, "child %d actually dumped core. "
- "falling through to killed by signal case\n", infop.si_pid);
- case CLD_KILLED:
- LOG(WARNING, "child %d (%s) was killed by signal %d (%s)\n",
- infop.si_pid, children[infop.si_pid], infop.si_status,
- strsignal(infop.si_status));
- break;
- case CLD_STOPPED:
- LOG(WARNING, "child %d (%s) was stopped by signal %d "
- "(giving it a SIGCONT(%d))\n",
- infop.si_pid, children[infop.si_pid], infop.si_status, SIGCONT);
- kill(infop.si_pid, SIGCONT);
- continue;
- default:
- LOG(WARNING, "something happened to child %d (%s) (killing it)\n",
- infop.si_pid, children[infop.si_pid]);
- kill(infop.si_pid, SIGKILL);
- continue;
- }
- if (infop.si_pid == core) {
- fprintf(stderr, "starter: si_code=%d CLD_EXITED=%d CLD_DUMPED=%d "
- "CLD_KILLED=%d CLD_STOPPED=%d si_status=%d (sig '%s')\n",
- infop.si_code, CLD_EXITED, CLD_DUMPED, CLD_KILLED,
- CLD_STOPPED, infop.si_status, strsignal(infop.si_status));
- // core has died. logging is down too
- fputs("starter: error: core died. exiting\n", stderr);
- niceexit(EXIT_FAILURE);
- }
-
- start(children[infop.si_pid], 0);
- LOG(DEBUG, "erasing %d from children\n", infop.si_pid);
- children.erase(infop.si_pid);
- } else {
- LOG(WARNING, "waitid failed with %d: %s", errno, strerror(errno));
- }
- }
-after_loop: ;
- }
-}
diff --git a/aos/atom_code/starter/starter.gyp b/aos/atom_code/starter/starter.gyp
index cff6d07..91b5870 100644
--- a/aos/atom_code/starter/starter.gyp
+++ b/aos/atom_code/starter/starter.gyp
@@ -4,10 +4,14 @@
'target_name': 'starter_exe',
'type': 'executable',
'sources': [
- 'starter.cpp',
+ 'starter.cc',
],
'dependencies': [
'<(AOS)/atom_code/atom_code.gyp:init',
+ '<(EXTERNALS):libevent',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:once',
+ '<(AOS)/common/common.gyp:time',
],
'copies': [
{
diff --git a/aos/atom_code/starter/starter.h b/aos/atom_code/starter/starter.h
deleted file mode 100644
index 16bbe01..0000000
--- a/aos/atom_code/starter/starter.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef __AOS_STARTER_H_
-#define __AOS_STARTER_H_
-
-#include <map>
-#include <sys/types.h>
-#include <signal.h>
-#include <stdint.h>
-#include <string>
-#include <errno.h>
-#include <string.h>
-#include <sys/wait.h>
-#include <set>
-
-using namespace std;
-
-map<pid_t, const char *> children;
-map<pid_t, const char *> files; // the names of the actual files
-map<int, pid_t> watches;
-set<pid_t> restarts;
-map<int, time_t> mtimes;
-
-int sigfd = 0;
-int notifyfd = 0;
-
-const size_t CMDLEN = 5000;
-
-#endif
-
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index a7289ea..018a2f5 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 0799c05..bb76ffa 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -53,9 +53,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/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/libstdc++/README b/aos/common/libstdc++/README
new file mode 100644
index 0000000..df8269e
--- /dev/null
+++ b/aos/common/libstdc++/README
@@ -0,0 +1,3 @@
+This directory contains replacement include files for ones that the libstdc++ we're using on the cRIO either don't implement completely or doesn't have at all.
+The ones in aos/crio/libstdc++ are copied from the libstdc++ that came with the compiler and tweaked so that they work.
+When used for compiles not for vxworks, they just #include the usual standard library header.
diff --git a/aos/common/libstdc++/memory b/aos/common/libstdc++/memory
new file mode 100644
index 0000000..31ab6e9
--- /dev/null
+++ b/aos/common/libstdc++/memory
@@ -0,0 +1,4 @@
+#include <memory>
+#ifdef __VXWORKS__
+#include "aos/crio/libstdc++/unique_ptr.h"
+#endif
diff --git a/aos/common/libstdc++/utility b/aos/common/libstdc++/utility
new file mode 100644
index 0000000..50b8aa3
--- /dev/null
+++ b/aos/common/libstdc++/utility
@@ -0,0 +1,4 @@
+#include <utility>
+#ifdef __VXWORKS__
+#include "aos/crio/libstdc++/move.h"
+#endif
diff --git a/aos/common/messages/RobotState.q b/aos/common/messages/RobotState.q
index 28ab287..32baa05 100644
--- a/aos/common/messages/RobotState.q
+++ b/aos/common/messages/RobotState.q
@@ -3,7 +3,6 @@
message RobotState {
bool enabled;
bool autonomous;
- bool test_mode;
uint16_t team_id;
};
diff --git a/aos/common/queue.h b/aos/common/queue.h
index e93c655..612429a 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/zero_switch_value.h b/aos/common/zero_switch_value.h
new file mode 100644
index 0000000..d51a104
--- /dev/null
+++ b/aos/common/zero_switch_value.h
@@ -0,0 +1,33 @@
+#ifndef AOS_COMMON_ZERO_SWITCH_VALUE_H_
+#define AOS_COMMON_ZERO_SWITCH_VALUE_H_
+
+#include "aos/common/type_traits.h"
+#include "aos/common/byteorder.h"
+
+namespace aos {
+
+// Contains all of the information from a zeroing sensor of some kind.
+// It is all contained here because it all has to be retrieved at the same time
+// or else there are race conditions with the sensor triggering and retrieving
+// the encoder values that would make writing code to deal with the information
+// hard (ie if the trigger sensor is read, then it changes->triggers the
+// interrupt which reads the encoder value).
+struct ZeroSwitchValue {
+ // What the curent encoder value is.
+ int32_t current_encoder;
+ // What the value of the encoder was when the interrupt triggered.
+ int32_t edge_encoder;
+ // What the current value of the sensor is.
+ bool current_value;
+
+ void NetworkToHost() {
+ current_encoder = ntoh(current_encoder);
+ edge_encoder = ntoh(edge_encoder);
+ }
+};
+static_assert(shm_ok<ZeroSwitchValue>::value,
+ "it's getting sent over the wire");
+
+} // namespace aos
+
+#endif // AOS_COMMON_ZERO_SWITCH_VALUE_H_
diff --git a/aos/crio/hardware/README b/aos/crio/hardware/README
new file mode 100644
index 0000000..a312613
--- /dev/null
+++ b/aos/crio/hardware/README
@@ -0,0 +1,12 @@
+This directory contains replacements/wrappers for WPILib classes.
+
+Be careful using the classes that have the same names as WPILib ones! Many of
+ them bring in the declarations for the WPILib ones which means you have to
+ qualify the names of these ones (and should do the same for WPILib ones too).
+The wrappers only have the functionality that is missing from whatever WPILib
+ class they are wrapping (and can return pointer to). Use that pointer to do
+ everything else. They also have the more useful constructors that forward to
+ the WPILib ones. Some of them also have smart pointers named *_holder_. These
+ are only there to hold onto objects that WPILib needs held onto separately.
+ Many of them are sometimes left as NULL depending on how the wrapper is
+ created. They really should not be used for anything.
diff --git a/aos/crio/hardware/counter.cc b/aos/crio/hardware/counter.cc
new file mode 100644
index 0000000..4665679
--- /dev/null
+++ b/aos/crio/hardware/counter.cc
@@ -0,0 +1,73 @@
+#include "aos/crio/hardware/counter.h"
+
+#include "aos/crio/hardware/digital_source.h"
+#include "aos/common/logging/logging.h"
+
+using ::std::unique_ptr;
+
+namespace aos {
+namespace crio {
+namespace hardware {
+
+::DigitalSource *Counter::a() {
+ return a_wrapper_->source();
+}
+::DigitalSource *Counter::b() {
+ return b_wrapper_->source();
+}
+
+int Counter::GetDenominator() {
+ switch (type_) {
+ case ::CounterBase::EncodingType::k1X:
+ return 1;
+ case ::CounterBase::EncodingType::k2X:
+ return 2;
+ case ::CounterBase::EncodingType::k4X:
+ return 4;
+ }
+ BadEncodingType(type_);
+}
+
+void Counter::BadEncodingType(::CounterBase::EncodingType type) {
+ if (type == ::CounterBase::EncodingType::k1X ||
+ type == ::CounterBase::EncodingType::k2X ||
+ type == ::CounterBase::EncodingType::k4X) {
+ LOG(FATAL, "somebody didn't handle all of the possible encoding types!\n");
+ }
+ LOG(FATAL, "bad ::CounterBase::EncodingType %d\n", static_cast<int>(type));
+}
+
+unique_ptr<Counter> Counter::Create(unique_ptr<DigitalSource> a,
+ unique_ptr<DigitalSource> b,
+ ::CounterBase::EncodingType type) {
+ switch (type) {
+ case ::CounterBase::EncodingType::k4X:
+ return unique_ptr<Counter>(new EncoderCounter(::std::move(a),
+ ::std::move(b)));
+ case ::CounterBase::EncodingType::k2X:
+ case ::CounterBase::EncodingType::k1X:
+ return unique_ptr<Counter>(new CounterCounter(::std::move(a),
+ ::std::move(b),
+ type));
+ }
+ BadEncodingType(type);
+}
+
+EncoderCounter::EncoderCounter(unique_ptr<DigitalSource> a_wrapper,
+ unique_ptr<DigitalSource> b_wrapper)
+ : Counter(::std::move(a_wrapper), ::std::move(b_wrapper),
+ ::CounterBase::EncodingType::k4X),
+ encoder_(new ::Encoder(a(), b())) {}
+
+CounterCounter::CounterCounter(unique_ptr<DigitalSource> a_wrapper,
+ unique_ptr<DigitalSource> b_wrapper,
+ ::CounterBase::EncodingType type)
+ : Counter(::std::move(a_wrapper), ::std::move(b_wrapper), type),
+ counter_(new ::Counter(type, a(), b(), false /*inverted*/)) {
+ assert(type == ::CounterBase::EncodingType::k1X ||
+ type == ::CounterBase::EncodingType::k2X);
+}
+
+} // namespace hardware
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/hardware/counter.h b/aos/crio/hardware/counter.h
new file mode 100644
index 0000000..610f65a
--- /dev/null
+++ b/aos/crio/hardware/counter.h
@@ -0,0 +1,103 @@
+#ifndef AOS_CRIO_HARDWARE_ENCODER_H_
+#define AOS_CRIO_HARDWARE_ENCODER_H_
+
+#include "aos/common/libstdc++/memory"
+
+#include "WPILib/Counter.h"
+#include "WPILib/CounterBase.h"
+#include "WPILib/Encoder.h"
+
+#include "aos/common/macros.h"
+
+namespace aos {
+namespace crio {
+namespace hardware {
+
+class DigitalSource;
+
+// Wrapper for WPILib's CounterBase that implements Get() consistently.
+// WPILib doesn't really differentiate between (and actually confuses) ticks and
+// cycles. There is 1 tick for each input edge that a Counter sees and 1 cycle
+// every time the inputs repeat their pattern.
+// The constructors for both of them are such a mess that these classes
+// intentionally do not support using them. Instead, just call the constructors
+// for these classes and let them handle using the correct constructor.
+class Counter {
+ public:
+ virtual ~Counter() {}
+
+ // Returns the number of ticks (NOT divided by the number of ticks per
+ // cycle like WPILib _sometimes_ is!).
+ virtual int32_t Get() = 0;
+ // This object maintains ownership.
+ virtual ::CounterBase *counter_base() const = 0;
+ ::DigitalSource *a();
+ ::DigitalSource *b();
+
+ // Returns the denominator to convert from ticks to cycles.
+ int GetDenominator();
+
+ // Will create an instance of a subclass as appropriate for type.
+ // This should be used (except for special circumstances) for constructing all
+ // instances because it makes it much easier to change the encoding type.
+ static ::std::unique_ptr<Counter> Create(::std::unique_ptr<DigitalSource> a,
+ ::std::unique_ptr<DigitalSource> b,
+ ::CounterBase::EncodingType type);
+
+ protected:
+ Counter(::std::unique_ptr<DigitalSource> a_wrapper,
+ ::std::unique_ptr<DigitalSource> b_wrapper,
+ ::CounterBase::EncodingType type)
+ : a_wrapper_(::std::move(a_wrapper)),
+ b_wrapper_(::std::move(b_wrapper)),
+ type_(type) {}
+
+ // What to do at the end of functions that handle all encoding types to make
+ // GCC happy. Will LOG(FATAL) a message.
+ static void BadEncodingType(::CounterBase::EncodingType type)
+ __attribute__((noreturn));
+
+ private:
+ const ::std::unique_ptr<DigitalSource> a_wrapper_;
+ const ::std::unique_ptr<DigitalSource> b_wrapper_;
+
+ // Because WPILib doesn't actually keep track of it...
+ const ::CounterBase::EncodingType type_;
+
+ DISALLOW_COPY_AND_ASSIGN(Counter);
+};
+
+// Only allows creating one with k4X decoding because otherwise ::Encoder just
+// creates an internal ::Counter, which is really stupid.
+class EncoderCounter : public Counter {
+ public:
+ EncoderCounter(::std::unique_ptr<DigitalSource> a_wrapper,
+ ::std::unique_ptr<DigitalSource> b_wrapper);
+
+ virtual int32_t Get() { return encoder_->GetRaw(); }
+ virtual ::CounterBase *counter_base() const { return encoder_.get(); }
+ const ::std::unique_ptr< ::Encoder> &encoder() { return encoder_; }
+
+ private:
+ const ::std::unique_ptr< ::Encoder> encoder_;
+};
+
+class CounterCounter : public Counter {
+ public:
+ CounterCounter(::std::unique_ptr<DigitalSource> a_wrapper,
+ ::std::unique_ptr<DigitalSource> b_wrapper,
+ ::CounterBase::EncodingType type);
+
+ virtual int32_t Get() { return counter_->Get(); }
+ virtual ::CounterBase *counter_base() const { return counter_.get(); }
+ const ::std::unique_ptr< ::Counter> &counter() { return counter_; }
+
+ private:
+ const ::std::unique_ptr< ::Counter> counter_;
+};
+
+} // namespace hardware
+} // namespace crio
+} // namespace aos
+
+#endif // AOS_CRIO_HARDWARE_ENCODER_H_
diff --git a/aos/crio/hardware/digital_source.cc b/aos/crio/hardware/digital_source.cc
new file mode 100644
index 0000000..8849a3b
--- /dev/null
+++ b/aos/crio/hardware/digital_source.cc
@@ -0,0 +1,20 @@
+#include "aos/crio/hardware/digital_source.h"
+
+using ::std::unique_ptr;
+
+namespace aos {
+namespace crio {
+namespace hardware {
+
+AnalogTriggerOutput::AnalogTriggerOutput(unique_ptr< ::AnalogTrigger> trigger,
+ ::AnalogTriggerOutput::Type type,
+ float lowerVoltage,
+ float upperVoltage)
+ : trigger_holder_(::std::move(trigger)),
+ output_(trigger_holder_->CreateOutput(type)) {
+ trigger_holder_->SetLimitsVoltage(lowerVoltage, upperVoltage);
+}
+
+} // namespace hardware
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/hardware/digital_source.h b/aos/crio/hardware/digital_source.h
new file mode 100644
index 0000000..37b2c6f
--- /dev/null
+++ b/aos/crio/hardware/digital_source.h
@@ -0,0 +1,84 @@
+#ifndef AOS_CRIO_HARDWARE_DIGITAL_SOURCE_H_
+#define AOS_CRIO_HARDWARE_DIGITAL_SOURCE_H_
+
+#include "aos/common/libstdc++/memory"
+
+#include "WPILib/DigitalSource.h"
+#include "WPILib/DigitalInput.h"
+#include "WPILib/AnalogTrigger.h"
+#include "WPILib/AnalogTriggerOutput.h"
+
+#include "aos/common/macros.h"
+
+namespace aos {
+namespace crio {
+namespace hardware {
+
+// Wrapper for WPILib's class of the same name. Provides an actual Get()
+// function and makes creating analog ones easier.
+class DigitalSource {
+ public:
+ virtual ~DigitalSource() {}
+
+ virtual bool Get() = 0;
+ // This object maintains ownership.
+ virtual ::DigitalSource *source() const = 0;
+
+ protected:
+ DigitalSource() {}
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(DigitalSource);
+};
+
+class AnalogTriggerOutput : public DigitalSource {
+ public:
+ // Defaults for the voltages for AnalogTriggers. They work well for digital
+ // sensors connected to analog inputs.
+ static const float kDefaultLowerVoltage = 1.35;
+ static const float kDefaultUpperVoltage = 4;
+
+ // Will set up the voltages on trigger.
+ // Takes ownership of trigger to make sure it stays around so that the output
+ // it creates won't blow up (because it holds on to and uses it).
+ AnalogTriggerOutput(::std::unique_ptr< ::AnalogTrigger> trigger,
+ ::AnalogTriggerOutput::Type type,
+ float lowerVoltage = kDefaultLowerVoltage,
+ float upperVoltage = kDefaultUpperVoltage);
+ explicit AnalogTriggerOutput(::std::unique_ptr< ::AnalogTriggerOutput> output)
+ : output_(::std::move(output)) {}
+
+ virtual bool Get() { return output_->Get(); }
+ virtual ::DigitalSource *source() const { return output_.get(); }
+
+ private:
+ const ::std::unique_ptr< ::AnalogTrigger> trigger_holder_;
+
+ const ::std::unique_ptr< ::AnalogTriggerOutput> output_;
+};
+
+class DigitalInput : public DigitalSource {
+ public:
+ explicit DigitalInput(uint32_t channel)
+ : input_(::std::unique_ptr< ::DigitalInput>(
+ new ::DigitalInput(channel))) {
+ }
+ DigitalInput(uint8_t module, uint32_t channel)
+ : input_(::std::unique_ptr< ::DigitalInput>(
+ new ::DigitalInput(module, channel))) {
+ }
+ explicit DigitalInput(::std::unique_ptr< ::DigitalInput> input)
+ : input_(::std::move(input)) {}
+
+ virtual bool Get() { return input_->Get(); }
+ virtual ::DigitalSource *source() const { return input_.get(); }
+
+ private:
+ const ::std::unique_ptr< ::DigitalInput> input_;
+};
+
+} // namespace hardware
+} // namespace crio
+} // namespace aos
+
+#endif // AOS_CRIO_HARDWARE_DIGITAL_SOURCE_H_
diff --git a/aos/crio/hardware/hardware.gyp b/aos/crio/hardware/hardware.gyp
new file mode 100644
index 0000000..b63a7c3
--- /dev/null
+++ b/aos/crio/hardware/hardware.gyp
@@ -0,0 +1,32 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'digital_source',
+ 'type': 'static_library',
+ 'sources': [
+ 'digital_source.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ ],
+ },
+ {
+ 'target_name': 'counter',
+ 'type': 'static_library',
+ 'sources': [
+ 'counter.cc',
+ ],
+ 'dependencies': [
+ 'digital_source',
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ ],
+ },
+ ],
+}
diff --git a/aos/crio/libstdc++/move.h b/aos/crio/libstdc++/move.h
new file mode 100644
index 0000000..425fcb6
--- /dev/null
+++ b/aos/crio/libstdc++/move.h
@@ -0,0 +1,117 @@
+// Move, forward and identity for C++0x + swap -*- C++ -*-
+
+// Copyright (C) 2007, 2008, 2009 Free Software Foundation, Inc.
+//
+// This file is part of the GNU ISO C++ Library. This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 3, or (at your option)
+// any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+// Under Section 7 of GPL version 3, you are granted additional
+// permissions described in the GCC Runtime Library Exception, version
+// 3.1, as published by the Free Software Foundation.
+
+// You should have received a copy of the GNU General Public License and
+// a copy of the GCC Runtime Library Exception along with this program;
+// see the files COPYING3 and COPYING.RUNTIME respectively. If not, see
+// <http://www.gnu.org/licenses/>.
+
+/** @file move.h
+ * This is an internal header file, included by other library headers.
+ * You should not attempt to use it directly.
+ */
+
+#ifndef _MOVE_H
+#define _MOVE_H 1
+
+#include "aos/crio/type_traits/type_traits"
+
+namespace std {
+
+ /// identity
+ template<typename _Tp>
+ struct identity
+ {
+ typedef _Tp type;
+ };
+
+ /// forward (as per N2835)
+ /// Forward lvalues as rvalues.
+ template<typename _Tp>
+ inline typename enable_if<!is_lvalue_reference<_Tp>::value, _Tp&&>::type
+ forward(typename std::identity<_Tp>::type& __t)
+ { return static_cast<_Tp&&>(__t); }
+
+ /// Forward rvalues as rvalues.
+ template<typename _Tp>
+ inline typename enable_if<!is_lvalue_reference<_Tp>::value, _Tp&&>::type
+ forward(typename std::identity<_Tp>::type&& __t)
+ { return static_cast<_Tp&&>(__t); }
+
+ // Forward lvalues as lvalues.
+ template<typename _Tp>
+ inline typename enable_if<is_lvalue_reference<_Tp>::value, _Tp>::type
+ forward(typename std::identity<_Tp>::type __t)
+ { return __t; }
+
+ // Prevent forwarding rvalues as const lvalues.
+ template<typename _Tp>
+ inline typename enable_if<is_lvalue_reference<_Tp>::value, _Tp>::type
+ forward(typename std::remove_reference<_Tp>::type&& __t) = delete;
+
+ /**
+ * @brief Move a value.
+ * @ingroup mutating_algorithms
+ * @param __t A thing of arbitrary type.
+ * @return Same, moved.
+ */
+ template<typename _Tp>
+ inline typename std::remove_reference<_Tp>::type&&
+ move(_Tp&& __t)
+ { return static_cast<typename std::remove_reference<_Tp>::type&&>(__t); }
+
+ /// declval, from type_traits.
+
+#define _GLIBCXX_MOVE(_Tp) std::move(_Tp)
+#define _GLIBCXX_FORWARD(_Tp, __val) std::forward<_Tp>(__val)
+
+#if 0
+ /**
+ * @brief Swaps two values.
+ * @ingroup mutating_algorithms
+ * @param __a A thing of arbitrary type.
+ * @param __b Another thing of arbitrary type.
+ * @return Nothing.
+ */
+ template<typename _Tp>
+ inline void
+ swap(_Tp& __a, _Tp& __b)
+ {
+ // concept requirements
+ __glibcxx_function_requires(_SGIAssignableConcept<_Tp>)
+
+ _Tp __tmp = _GLIBCXX_MOVE(__a);
+ __a = _GLIBCXX_MOVE(__b);
+ __b = _GLIBCXX_MOVE(__tmp);
+ }
+
+ // _GLIBCXX_RESOLVE_LIB_DEFECTS
+ // DR 809. std::swap should be overloaded for array types.
+ template<typename _Tp, size_t _Nm>
+ inline void
+ swap(_Tp (&__a)[_Nm], _Tp (&__b)[_Nm])
+ {
+ for (size_t __n = 0; __n < _Nm; ++__n)
+ swap(__a[__n], __b[__n]);
+ }
+#endif
+
+} // namespace std
+
+#endif /* _MOVE_H */
diff --git a/aos/crio/libstdc++/unique_ptr.h b/aos/crio/libstdc++/unique_ptr.h
new file mode 100644
index 0000000..f6bda76d
--- /dev/null
+++ b/aos/crio/libstdc++/unique_ptr.h
@@ -0,0 +1,419 @@
+// unique_ptr implementation -*- C++ -*-
+
+// Copyright (C) 2008, 2009, 2010 Free Software Foundation, Inc.
+//
+// This file is part of the GNU ISO C++ Library. This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 3, or (at your option)
+// any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+// Under Section 7 of GPL version 3, you are granted additional
+// permissions described in the GCC Runtime Library Exception, version
+// 3.1, as published by the Free Software Foundation.
+
+// You should have received a copy of the GNU General Public License and
+// a copy of the GCC Runtime Library Exception along with this program;
+// see the files COPYING3 and COPYING.RUNTIME respectively. If not, see
+// <http://www.gnu.org/licenses/>.
+
+/** @file unique_ptr.h
+ * This is an internal header file, included by other library headers.
+ * You should not attempt to use it directly.
+ */
+
+#ifndef _UNIQUE_PTR_H
+#define _UNIQUE_PTR_H 1
+
+#include "aos/crio/type_traits/type_traits"
+#include "aos/common/libstdc++/utility"
+#include <assert.h>
+
+namespace std {
+
+ /**
+ * @addtogroup pointer_abstractions
+ * @{
+ */
+
+ /// Primary template, default_delete.
+ template<typename _Tp>
+ struct default_delete
+ {
+ default_delete() { }
+
+ template<typename _Up>
+ default_delete(const default_delete<_Up>&) { }
+
+ void
+ operator()(_Tp* __ptr) const
+ {
+ static_assert(sizeof(_Tp)>0,
+ "can't delete pointer to incomplete type");
+ delete __ptr;
+ }
+ };
+
+ // _GLIBCXX_RESOLVE_LIB_DEFECTS
+ // DR 740 - omit specialization for array objects with a compile time length
+ /// Specialization, default_delete.
+ template<typename _Tp>
+ struct default_delete<_Tp[]>
+ {
+ void
+ operator()(_Tp* __ptr) const
+ {
+ static_assert(sizeof(_Tp)>0,
+ "can't delete pointer to incomplete type");
+ delete [] __ptr;
+ }
+ };
+
+ /// 20.7.12.2 unique_ptr for single objects.
+ template <typename _Tp, typename _Tp_Deleter = default_delete<_Tp> >
+ class unique_ptr
+ {
+ typedef _Tp* unique_ptr::* __unspecified_pointer_type;
+
+ public:
+ typedef _Tp* pointer;
+ typedef _Tp element_type;
+ typedef _Tp_Deleter deleter_type;
+
+ // Constructors.
+ unique_ptr()
+ : _t(pointer()), _deleter(deleter_type())
+ { static_assert(!std::is_pointer<deleter_type>::value,
+ "constructed with null function pointer deleter"); }
+
+ explicit
+ unique_ptr(pointer __p)
+ : _t(__p), _deleter(deleter_type())
+ { static_assert(!std::is_pointer<deleter_type>::value,
+ "constructed with null function pointer deleter"); }
+
+ unique_ptr(pointer __p,
+ typename std::conditional<std::is_reference<deleter_type>::value,
+ deleter_type, const deleter_type&>::type __d)
+ : _t(__p), _deleter(__d) { }
+
+ unique_ptr(pointer __p,
+ typename std::remove_reference<deleter_type>::type&& __d)
+ : _t(std::move(__p)), _deleter(std::move(__d))
+ { static_assert(!std::is_reference<deleter_type>::value,
+ "rvalue deleter bound to reference"); }
+
+ // Move constructors.
+ unique_ptr(unique_ptr&& __u)
+ : _t(__u.release()), _deleter(std::forward<deleter_type>(__u.get_deleter())) { }
+
+ template<typename _Up, typename _Up_Deleter>
+ unique_ptr(unique_ptr<_Up, _Up_Deleter>&& __u)
+ : _t(__u.release()), _deleter(std::forward<deleter_type>(__u.get_deleter()))
+ { }
+
+ // Destructor.
+ ~unique_ptr() { reset(); }
+
+ // Assignment.
+ unique_ptr&
+ operator=(unique_ptr&& __u)
+ {
+ reset(__u.release());
+ get_deleter() = std::move(__u.get_deleter());
+ return *this;
+ }
+
+ template<typename _Up, typename _Up_Deleter>
+ unique_ptr&
+ operator=(unique_ptr<_Up, _Up_Deleter>&& __u)
+ {
+ reset(__u.release());
+ get_deleter() = std::move(__u.get_deleter());
+ return *this;
+ }
+
+ unique_ptr&
+ operator=(__unspecified_pointer_type)
+ {
+ reset();
+ return *this;
+ }
+
+ // Observers.
+ typename std::add_lvalue_reference<element_type>::type
+ operator*() const
+ {
+ assert(get() != pointer());
+ return *get();
+ }
+
+ pointer
+ operator->() const
+ {
+ assert(get() != pointer());
+ return get();
+ }
+
+ pointer
+ get() const
+ { return _t; }
+
+ deleter_type&
+ get_deleter()
+ { return _deleter; }
+
+ const deleter_type&
+ get_deleter() const
+ { return _deleter; }
+
+ explicit operator bool() const
+ { return get() == pointer() ? false : true; }
+
+ // Modifiers.
+ pointer
+ release()
+ {
+ pointer __p = get();
+ _t = pointer();
+ return __p;
+ }
+
+ void
+ reset(pointer __p = pointer())
+ {
+ using std::swap;
+ swap(_t, __p);
+ if (__p != pointer())
+ get_deleter()(__p);
+ }
+
+ void
+ swap(unique_ptr& __u)
+ {
+ using std::swap;
+ swap(_t, __u._t);
+ swap(_deleter, __u._deleter);
+ }
+
+ // Disable copy from lvalue.
+ unique_ptr(const unique_ptr&) = delete;
+ unique_ptr& operator=(const unique_ptr&) = delete;
+
+ private:
+ _Tp *_t;
+ _Tp_Deleter _deleter;
+ };
+
+ /// 20.7.12.3 unique_ptr for array objects with a runtime length
+ // [unique.ptr.runtime]
+ // _GLIBCXX_RESOLVE_LIB_DEFECTS
+ // DR 740 - omit specialization for array objects with a compile time length
+ template<typename _Tp, typename _Tp_Deleter>
+ class unique_ptr<_Tp[], _Tp_Deleter>
+ {
+ typedef _Tp* unique_ptr::* __unspecified_pointer_type;
+
+ public:
+ typedef _Tp* pointer;
+ typedef _Tp element_type;
+ typedef _Tp_Deleter deleter_type;
+
+ // Constructors.
+ unique_ptr()
+ : _t(pointer()), _deleter(deleter_type())
+ { static_assert(!std::is_pointer<deleter_type>::value,
+ "constructed with null function pointer deleter"); }
+
+ explicit
+ unique_ptr(pointer __p)
+ : _t(__p), _deleter(deleter_type())
+ { static_assert(!std::is_pointer<deleter_type>::value,
+ "constructed with null function pointer deleter"); }
+
+ unique_ptr(pointer __p,
+ typename std::conditional<std::is_reference<deleter_type>::value,
+ deleter_type, const deleter_type&>::type __d)
+ : _t(__p), _deleter(__d) { }
+
+ unique_ptr(pointer __p,
+ typename std::remove_reference<deleter_type>::type && __d)
+ : _t(std::move(__p)), _deleter(std::move(__d))
+ { static_assert(!std::is_reference<deleter_type>::value,
+ "rvalue deleter bound to reference"); }
+
+ // Move constructors.
+ unique_ptr(unique_ptr&& __u)
+ : _t(__u.release()), _deleter(std::forward<deleter_type>(__u.get_deleter())) { }
+
+ template<typename _Up, typename _Up_Deleter>
+ unique_ptr(unique_ptr<_Up, _Up_Deleter>&& __u)
+ : _t(__u.release()), _deleter(std::forward<deleter_type>(__u.get_deleter()))
+ { }
+
+ // Destructor.
+ ~unique_ptr() { reset(); }
+
+ // Assignment.
+ unique_ptr&
+ operator=(unique_ptr&& __u)
+ {
+ reset(__u.release());
+ get_deleter() = std::move(__u.get_deleter());
+ return *this;
+ }
+
+ template<typename _Up, typename _Up_Deleter>
+ unique_ptr&
+ operator=(unique_ptr<_Up, _Up_Deleter>&& __u)
+ {
+ reset(__u.release());
+ get_deleter() = std::move(__u.get_deleter());
+ return *this;
+ }
+
+ unique_ptr&
+ operator=(__unspecified_pointer_type)
+ {
+ reset();
+ return *this;
+ }
+
+ // Observers.
+ typename std::add_lvalue_reference<element_type>::type
+ operator[](size_t __i) const
+ {
+ assert(get() != pointer());
+ return get()[__i];
+ }
+
+ pointer
+ get() const
+ { return _t; }
+
+ deleter_type&
+ get_deleter()
+ { return _deleter; }
+
+ const deleter_type&
+ get_deleter() const
+ { return _deleter; }
+
+ explicit operator bool() const
+ { return get() == pointer() ? false : true; }
+
+ // Modifiers.
+ pointer
+ release()
+ {
+ pointer __p = get();
+ _t = pointer();
+ return __p;
+ }
+
+ void
+ reset(pointer __p = pointer())
+ {
+ using std::swap;
+ swap(_t, __p);
+ if (__p != pointer())
+ get_deleter()(__p);
+ }
+
+ // DR 821.
+ template<typename _Up>
+ void reset(_Up) = delete;
+
+ void
+ swap(unique_ptr& __u)
+ {
+ swap(_t, __u._t);
+ swap(_deleter, __u._deleter);
+ }
+
+ // Disable copy from lvalue.
+ unique_ptr(const unique_ptr&) = delete;
+ unique_ptr& operator=(const unique_ptr&) = delete;
+
+ // Disable construction from convertible pointer types.
+ // (N2315 - 20.6.5.3.1)
+ template<typename _Up>
+ unique_ptr(_Up*, typename
+ std::conditional<std::is_reference<deleter_type>::value,
+ deleter_type, const deleter_type&>::type,
+ typename std::enable_if<std::is_convertible<_Up*,
+ pointer>::value>::type* = 0) = delete;
+
+ template<typename _Up>
+ unique_ptr(_Up*, typename std::remove_reference<deleter_type>::type&&,
+ typename std::enable_if<std::is_convertible<_Up*,
+ pointer>::value>::type* = 0) = delete;
+
+ template<typename _Up>
+ explicit
+ unique_ptr(_Up*, typename std::enable_if<std::is_convertible<_Up*,
+ pointer>::value>::type* = 0) = delete;
+
+ private:
+ _Tp *_t;
+ _Tp_Deleter _deleter;
+ };
+
+ template<typename _Tp, typename _Tp_Deleter>
+ inline void
+ swap(unique_ptr<_Tp, _Tp_Deleter>& __x,
+ unique_ptr<_Tp, _Tp_Deleter>& __y)
+ { __x.swap(__y); }
+
+ template<typename _Tp, typename _Tp_Deleter,
+ typename _Up, typename _Up_Deleter>
+ inline bool
+ operator==(const unique_ptr<_Tp, _Tp_Deleter>& __x,
+ const unique_ptr<_Up, _Up_Deleter>& __y)
+ { return __x.get() == __y.get(); }
+
+ template<typename _Tp, typename _Tp_Deleter,
+ typename _Up, typename _Up_Deleter>
+ inline bool
+ operator!=(const unique_ptr<_Tp, _Tp_Deleter>& __x,
+ const unique_ptr<_Up, _Up_Deleter>& __y)
+ { return !(__x.get() == __y.get()); }
+
+ template<typename _Tp, typename _Tp_Deleter,
+ typename _Up, typename _Up_Deleter>
+ inline bool
+ operator<(const unique_ptr<_Tp, _Tp_Deleter>& __x,
+ const unique_ptr<_Up, _Up_Deleter>& __y)
+ { return __x.get() < __y.get(); }
+
+ template<typename _Tp, typename _Tp_Deleter,
+ typename _Up, typename _Up_Deleter>
+ inline bool
+ operator<=(const unique_ptr<_Tp, _Tp_Deleter>& __x,
+ const unique_ptr<_Up, _Up_Deleter>& __y)
+ { return !(__y.get() < __x.get()); }
+
+ template<typename _Tp, typename _Tp_Deleter,
+ typename _Up, typename _Up_Deleter>
+ inline bool
+ operator>(const unique_ptr<_Tp, _Tp_Deleter>& __x,
+ const unique_ptr<_Up, _Up_Deleter>& __y)
+ { return __y.get() < __x.get(); }
+
+ template<typename _Tp, typename _Tp_Deleter,
+ typename _Up, typename _Up_Deleter>
+ inline bool
+ operator>=(const unique_ptr<_Tp, _Tp_Deleter>& __x,
+ const unique_ptr<_Up, _Up_Deleter>& __y)
+ { return !(__x.get() < __y.get()); }
+
+ // @} group pointer_abstractions
+
+} // namespace std
+
+#endif /* _UNIQUE_PTR_H */
diff --git a/aos/externals/WPILib/WPILib.a b/aos/externals/WPILib/WPILib.a
index 57584f3..27cc22f 100644
--- a/aos/externals/WPILib/WPILib.a
+++ b/aos/externals/WPILib/WPILib.a
Binary files differ
diff --git a/aos/externals/WPILib/WPILib/CANJaguar.cpp b/aos/externals/WPILib/WPILib/CANJaguar.cpp
index b0cc642..cf03370 100644
--- a/aos/externals/WPILib/WPILib/CANJaguar.cpp
+++ b/aos/externals/WPILib/WPILib/CANJaguar.cpp
@@ -43,16 +43,16 @@
if (StatusIsFatal())
return;
// 3330 was the first shipping RDK firmware version for the Jaguar
- if (fwVer >= 3330 || fwVer < 92)
+ if (fwVer >= 3330 || fwVer < 101)
{
char buf[256];
if (fwVer < 3330)
{
- snprintf(buf, 256, "Jag #%d firmware (%d) is too old (must be at least version 92 of the FIRST approved firmware)", m_deviceNumber, fwVer);
+ snprintf(buf, 256, "Jag #%d firmware (%d) is too old (must be at least version 101 of the FIRST approved firmware)", m_deviceNumber, fwVer);
}
else
{
- snprintf(buf, 256, "Jag #%d firmware (%d) is not FIRST approved (must be at least version 92 of the FIRST approved firmware)", m_deviceNumber, fwVer);
+ snprintf(buf, 256, "Jag #%d firmware (%d) is not FIRST approved (must be at least version 101 of the FIRST approved firmware)", m_deviceNumber, fwVer);
}
wpi_setWPIErrorWithContext(JaguarVersionError, buf);
return;
diff --git a/aos/externals/WPILib/WPILib/Preferences.cpp b/aos/externals/WPILib/WPILib/Preferences.cpp
index c18df0b..e8c0109 100644
--- a/aos/externals/WPILib/WPILib/Preferences.cpp
+++ b/aos/externals/WPILib/WPILib/Preferences.cpp
@@ -44,9 +44,6 @@
m_readTask.Start((UINT32)this);
semTake(m_fileOpStarted, WAIT_FOREVER);
- NetworkTable::GetTable(kTableName)->PutBoolean(kSaveField, false);
- NetworkTable::GetTable(kTableName)->AddTableListener(this);
-
nUsageReporting::report(nUsageReporting::kResourceType_Preferences, 0);
}
@@ -430,7 +427,7 @@
{
value = fgetc(file);
} while (value == ' ' || value == '\t');
-
+
if (value == '\n' || value == ';')
{
if (value == '\n')
@@ -501,7 +498,7 @@
{
m_keys.push_back(name);
m_values.insert(std::pair<std::string, std::string>(name, value));
- //NetworkTable::GetTable(kTableName)->PutString(name, value);
+ NetworkTable::GetTable(kTableName)->PutString(name, value);
if (!comment.empty())
{
@@ -525,6 +522,9 @@
if (!comment.empty())
m_endComment = comment;
+
+ NetworkTable::GetTable(kTableName)->PutBoolean(kSaveField, false);
+ NetworkTable::GetTable(kTableName)->AddTableListener(this);
}
/**
diff --git a/aos/externals/WPILib/WPILib/networktables/NetworkTable.cpp b/aos/externals/WPILib/WPILib/networktables/NetworkTable.cpp
index 36ba63d..78dd4a4 100644
--- a/aos/externals/WPILib/WPILib/networktables/NetworkTable.cpp
+++ b/aos/externals/WPILib/WPILib/networktables/NetworkTable.cpp
@@ -53,7 +53,7 @@
void NetworkTable::SetClientMode(){
CheckInit();
- mode = &NetworkTableMode::Server;
+ mode = &NetworkTableMode::Client;
}
void NetworkTable::SetServerMode(){
diff --git a/aos/externals/WPILib/WPILib/networktables2/AbstractNetworkTableEntryStore.cpp b/aos/externals/WPILib/WPILib/networktables2/AbstractNetworkTableEntryStore.cpp
index 86088ac..c502432 100644
--- a/aos/externals/WPILib/WPILib/networktables2/AbstractNetworkTableEntryStore.cpp
+++ b/aos/externals/WPILib/WPILib/networktables2/AbstractNetworkTableEntryStore.cpp
@@ -9,7 +9,8 @@
#include "networktables2/TableKeyExistsWithDifferentTypeException.h"
#include <map>
#include <vector>
-
+#include <iostream>
+#include <stdio.h>
AbstractNetworkTableEntryStore::AbstractNetworkTableEntryStore(TableListenerManager& lstnManager):
listenerManager(lstnManager){
@@ -32,14 +33,23 @@
NetworkTableEntry* AbstractNetworkTableEntryStore::GetEntry(std::string& name){
{
Synchronized sync(LOCK);
- return namedEntries[name];//TODO check for not existing
+ std::map<std::string, NetworkTableEntry*>::iterator value_itr = namedEntries.find(name);
+ if(value_itr != namedEntries.end()) {
+ return value_itr->second;
+ }
+ return NULL;
}
}
NetworkTableEntry* AbstractNetworkTableEntryStore::GetEntry(EntryId entryId){
{
Synchronized sync(LOCK);
- return idEntries[entryId];//TODO check for not existing
+
+ std::map<EntryId, NetworkTableEntry*>::iterator value_itr = idEntries.find(entryId);
+ if(value_itr != idEntries.end()) {
+ return value_itr->second;
+ }
+ return NULL;
}
}
@@ -112,8 +122,8 @@
Synchronized sync(LOCK);
std::map<std::string, NetworkTableEntry*>::iterator index = namedEntries.find(name);
NetworkTableEntry* tableEntry;
- if(index == namedEntries.end() || namedEntries[name]==NULL)//if the name does not exist in the current entries
- {//TODO why doesn't find correctly detect that the entry does not exist
+ if(index == namedEntries.end())//if the name does not exist in the current entries
+ {
tableEntry = new NetworkTableEntry(name, type, value);
if(addEntry(tableEntry))
{
@@ -123,7 +133,7 @@
}
else
{
- tableEntry = namedEntries[name];
+ tableEntry = index->second;
if(tableEntry->GetType()->id != type->id){
throw TableKeyExistsWithDifferentTypeException(name, tableEntry->GetType());
}
@@ -155,7 +165,7 @@
NetworkTableEntry* tableEntry;
if(addEntry(entry)){
if(itr != namedEntries.end()){
- tableEntry = namedEntries[entry->name];
+ tableEntry = itr->second;
}
else{
tableEntry = entry;
@@ -190,8 +200,8 @@
std::map<std::string, NetworkTableEntry*>::iterator itr;
for(itr = namedEntries.begin(); itr != namedEntries.end(); itr++)
{
- NetworkTableEntry* entry = namedEntries[(*itr).first];//this may seem odd, but its so we get the address of the list element, rather than the copy stored in the itr
- listener->ValueChanged(table, (*itr).first, entry->GetValue(), true);
+ NetworkTableEntry* entry = itr->second;
+ listener->ValueChanged(table, itr->first, entry->GetValue(), true);
}
}
}
diff --git a/aos/externals/WPILib/WPILib/networktables2/stream/SocketServerStreamProvider.cpp b/aos/externals/WPILib/WPILib/networktables2/stream/SocketServerStreamProvider.cpp
index 8450cda..b4bbc00 100644
--- a/aos/externals/WPILib/WPILib/networktables2/stream/SocketServerStreamProvider.cpp
+++ b/aos/externals/WPILib/WPILib/networktables2/stream/SocketServerStreamProvider.cpp
@@ -111,8 +111,8 @@
if (connectedSocket == ERROR)
return NULL;
- int on = 1;
- setsockopt(connectedSocket, IPPROTO_TCP, TCP_NODELAY, (char *)&on, sizeof(on));
+ //int on = 1;
+ //setsockopt(connectedSocket, IPPROTO_TCP, TCP_NODELAY, (char *)&on, sizeof(on));
return new FDIOStream(connectedSocket);
}
diff --git a/aos/externals/WPILib/WPILib/networktables2/stream/SocketStreamFactory.cpp b/aos/externals/WPILib/WPILib/networktables2/stream/SocketStreamFactory.cpp
index c3ac85c..1033998 100644
--- a/aos/externals/WPILib/WPILib/networktables2/stream/SocketStreamFactory.cpp
+++ b/aos/externals/WPILib/WPILib/networktables2/stream/SocketStreamFactory.cpp
@@ -59,8 +59,8 @@
return NULL;
}//TODO close fd if an error occured
- int on = 1;
- setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&on, sizeof(on));
+ //int on = 1;
+ //setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&on, sizeof(on));
return new FDIOStream(sockfd);
#endif
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 25b358c..7c430c2 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:sensor_receiver',
'../input/input.gyp:GyroReader',
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 574f73d..71c1c3c 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -9,3 +9,4 @@
CameraReader
CameraServer
CameraHTTPStreamer
+angle_adjust
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index f122ee3..43c4a53 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -1,42 +1,147 @@
#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"
#include "aos/common/logging/logging.h"
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// Note: So far, none of the Angle Adjust numbers have been measured.
+// Do not rely on them for real life.
+
namespace frc971 {
namespace constants {
namespace {
-const double kCompHorizontal = -1.77635 + 0.180;
-const double kPracticeHorizontal = -1.77635 + -0.073631;
+const double kCompWristHallEffectStartAngle = 72 * M_PI / 180.0;
+const double kPracticeWristHallEffectStartAngle = 72 * M_PI / 180.0;
+
+const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
+const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
+
+const double kPracticeWristUpperPhysicalLimit = 95 * M_PI / 180.0;
+const double kCompWristUpperPhysicalLimit = 95 * M_PI / 180.0;
+
+const double kPracticeWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+const double kCompWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
+
+const double kPracticeWristUpperLimit = 93 * M_PI / 180.0;
+const double kCompWristUpperLimit = 93 * M_PI / 180.0;
+
+const double kPracticeWristLowerLimit = -36 * M_PI / 180.0;
+const double kCompWristLowerLimit = -36 * M_PI / 180.0;
+
+const double kWristZeroingSpeed = 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.894481;
+const double kCompAngleAdjustUpperPhysicalLimit = 0.894481;
+
+const double kPracticeAngleAdjustLowerPhysicalLimit = 0.283616;
+const double kCompAngleAdjustLowerPhysicalLimit = 0.283616;
+
+const double kPracticeAngleAdjustUpperLimit = 0.85;
+const double kCompAngleAdjustUpperLimit = 0.85;
+
+const double kPracticeAngleAdjustLowerLimit = 0.32;
+const double kCompAngleAdjustLowerLimit = 0.32;
+
+const double kAngleAdjustZeroingSpeed = -0.2;
+
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;
+
+ // 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;
+
// 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,
+ kCompAngleAdjustHallEffectStartAngle,
+ kCompAngleAdjustHallEffectStopAngle,
+ kCompAngleAdjustUpperLimit,
+ kCompAngleAdjustLowerLimit,
+ kCompAngleAdjustUpperPhysicalLimit,
+ kCompAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kCompCameraCenter};
break;
case kPracticeTeamNumber:
- values = new Values{kPracticeHorizontal, kPracticeCameraCenter};
+ values = new Values{kPracticeWristHallEffectStartAngle,
+ kPracticeWristHallEffectStopAngle,
+ kPracticeWristUpperLimit,
+ kPracticeWristLowerLimit,
+ kPracticeWristUpperPhysicalLimit,
+ kPracticeWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kPracticeAngleAdjustHallEffectStartAngle,
+ kPracticeAngleAdjustHallEffectStopAngle,
+ kPracticeAngleAdjustUpperLimit,
+ kPracticeAngleAdjustLowerLimit,
+ kPracticeAngleAdjustUpperPhysicalLimit,
+ kPracticeAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kPracticeCameraCenter};
break;
default:
LOG(ERROR, "unknown team #%"PRIu16"\n",
@@ -49,12 +154,102 @@
} // 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 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 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..98baca4 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -13,9 +13,30 @@
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 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);
+
// 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..8482a4b
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -0,0 +1,105 @@
+#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_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\n",
+ position->angle,
+ position->bottom_hall_effect ? "true" : "false",
+ position->middle_hall_effect ? "true" : "false");
+ }
+
+ 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..cb3ba1b
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust.gyp
@@ -0,0 +1,84 @@
+{
+ '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',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(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',
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(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)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'angle_adjust_loop',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'angle_adjust',
+ 'type': 'executable',
+ 'sources': [
+ 'angle_adjust_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '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..6d484d5
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_csv.cc
@@ -0,0 +1,52 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.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..7dc86c7
--- /dev/null
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -0,0 +1,311 @@
+#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/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>(MakeAngleAdjustPlant())),
+ 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);
+ 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 << my_angle_adjust_loop_.output->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));
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
+
+ private:
+ AngleAdjustLoop my_angle_adjust_loop_;
+ double initial_position_;
+ double last_position_;
+ 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..c23a64b
--- /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<2, 1, 1> MakeAngleAdjustPlantCoefficients() {
+ 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> MakeAngleAdjustController() {
+ 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, MakeAngleAdjustPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeAngleAdjustPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeAngleAdjustController());
+ return StateFeedbackLoop<2, 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..8db821f
--- /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<2, 1, 1> MakeAngleAdjustPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeAngleAdjustController();
+
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant();
+
+StateFeedbackLoop<2, 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/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 8bdc7a0..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)'],
@@ -33,8 +47,8 @@
'<(AOS)/common/common.gyp:controls',
'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..c757cd2
--- /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.1);
+
+// 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.03);
+
+ // 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 %d 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..bee58bc
--- /dev/null
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -0,0 +1,102 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class AngleAdjust(control_loop.ControlLoop):
+ def __init__(self):
+ super(AngleAdjust, self).__init__("AngleAdjust")
+ # 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.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.45, .8])
+
+ 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.
+ 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 = AngleAdjust()
+ close_loop_x = []
+ R = numpy.matrix([[1.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) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ 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..4ff2623
--- /dev/null
+++ b/frc971/control_loops/python/control_loop.py
@@ -0,0 +1,292 @@
+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, C):
+ """Calculates the discrete time values for A and B as well as initializing
+ X and Y to the correct sizes.
+
+ 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
+ C: C
+ """
+ self.A, self.B = controls.c2d(
+ A_continuous, B_continuous, dt)
+ self.X = numpy.zeros((self.A.shape[0], 1))
+ self.Y = 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..f26b99a
--- /dev/null
+++ b/frc971/control_loops/python/wrist.py
@@ -0,0 +1,95 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Wrist(control_loop.ControlLoop):
+ def __init__(self):
+ super(Wrist, self).__init__("Wrist")
+ # 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.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.86, .46])
+
+ 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]])
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ wrist = Wrist()
+ 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 = Wrist()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
+ wrist.UpdateObserver(U)
+ wrist.Update(U)
+ close_loop_x.append(wrist.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 .h file name and .cc file name"
+ else:
+ 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..575dd32
--- /dev/null
+++ b/frc971/control_loops/update_angle_adjust.sh
@@ -0,0 +1,5 @@
+#!/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
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..ea4c353
--- /dev/null
+++ b/frc971/control_loops/update_wrist.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the wrist controller.
+
+./python/wrist.py wrist/wrist_motor_plant.h wrist/wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
new file mode 100644
index 0000000..13471fa
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -0,0 +1,96 @@
+#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_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 zero=%f currently %f hall: %s\n",
+ //position->pos, zero_offset_, absolute_position,
+ //position->hall_effect ? "true" : "false");
+ }
+
+ 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..022dcfd
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -0,0 +1,69 @@
+{
+ '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',
+ ],
+ '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..a53f603
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist.h
@@ -0,0 +1,65 @@
+#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/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..e3e6039
--- /dev/null
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -0,0 +1,344 @@
+#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>(MakeWristPlant())),
+ 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;
+ }
+
+ // 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 << my_wrist_loop_.output->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));
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
+ private:
+ WristLoop my_wrist_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_;
+};
+
+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 == 430) {
+ 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..408fd85
--- /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<2, 1, 1> MakeWristPlantCoefficients() {
+ 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> MakeWristController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 119.668313646, 7.2297495639;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeWristPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeWristPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeWristLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeWristController());
+ return StateFeedbackLoop<2, 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..169e923
--- /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<2, 1, 1> MakeWristPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeWristController();
+
+StateFeedbackPlant<2, 1, 1> MakeWristPlant();
+
+StateFeedbackLoop<2, 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..4c54f27
--- /dev/null
+++ b/frc971/control_loops/zeroed_joint.h
@@ -0,0 +1,368 @@
+#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;
+};
+
+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<2, 1, 1> {
+ public:
+ ZeroedStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop,
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ zeroed_joint_(zeroed_joint) {
+ }
+
+ // Caps U, but this time respects the state of the wrist as well.
+ virtual void CapU();
+ private:
+ ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
+};
+
+template<int kNumZeroSensors>
+void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
+ if (zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY) {
+ if (Y(0, 0) >= zeroed_joint_->config_data_.upper_limit) {
+ U(0, 0) = std::min(0.0, U(0, 0));
+ }
+ if (Y(0, 0) <= zeroed_joint_->config_data_.lower_limit) {
+ U(0, 0) = std::max(0.0, U(0, 0));
+ }
+ }
+
+ const bool is_ready =
+ zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY;
+ double limit = is_ready ?
+ 12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
+
+ U(0, 0) = std::min(limit, U(0, 0));
+ U(0, 0) = std::max(-limit, U(0, 0));
+}
+
+
+// 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;
+ // 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<2, 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;
+
+ 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;
+ // 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_speed * dt;
+ loop_->R << zeroing_position_, -config_data_.zeroing_speed;
+ 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;
+ }
+ break;
+ }
+
+ case READY:
+ LOG(DEBUG, "READY\n");
+ {
+ const double limited_goal = ClipGoal(goal_angle);
+ loop_->R << limited_goal, goal_velocity;
+ 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_->U_uncapped(0, 0) > config_data_.max_zeroing_voltage) {
+ double dx = (loop_->U_uncapped(0, 0) -
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ } else if(loop_->U_uncapped(0, 0) < -config_data_.max_zeroing_voltage) {
+ double dx = (loop_->U_uncapped(0, 0) +
+ config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ zeroing_position_ -= dx;
+ capped_goal_ = true;
+ }
+ break;
+ }
+ return loop_->U(0, 0);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 7ff9e3f..27d6ae9 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -60,11 +60,17 @@
'<(EXTERNALS):WPILib',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
'<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/crio/hardware/hardware.gyp:counter',
+ '<(AOS)/crio/hardware/hardware.gyp:digital_source',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_lib',
],
'export_dependent_settings': [
'<(EXTERNALS):WPILib',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
'<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/crio/hardware/hardware.gyp:counter',
+ '<(AOS)/crio/hardware/hardware.gyp:digital_source',
],
},
{
diff --git a/frc971/input/sensor_packer.cc b/frc971/input/sensor_packer.cc
index b164e45..865b25b 100644
--- a/frc971/input/sensor_packer.cc
+++ b/frc971/input/sensor_packer.cc
@@ -3,6 +3,9 @@
#include <arpa/inet.h>
#include "aos/common/inttypes.h"
+#include "aos/common/time.h"
+
+#include "frc971/control_loops/index/index.h"
using ::aos::MutexLocker;
@@ -15,6 +18,63 @@
printf("frc971::SensorPacker started\n");
}
+void SensorPacker::ReadEncoder(EncoderReadData *data) {
+ int32_t value = data->counter->Get();
+ {
+ ::aos::MutexLocker locker(data->sync);
+ *data->output = value;
+ }
+}
+
+void SensorPacker::TopDiscEdgeReader() {
+ while (true) {
+ top_disc_posedge_output_->source()->
+ WaitForInterrupt(kInterruptTimeout);
+ int32_t position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ top_disc_posedge_position_ = position;
+ ++top_disc_posedge_count_;
+ }
+ top_disc_negedge_output_->source()->
+ WaitForInterrupt(kInterruptTimeout);
+ position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ top_disc_negedge_position_ = position;
+ ++top_disc_negedge_count_;
+ }
+ }
+}
+
+void SensorPacker::BottomDiscEdgeReader() {
+ static const aos::time::Time kBottomDiscNegedgeSleep =
+ aos::time::Time::InSeconds(
+ control_loops::IndexMotor::kBottomDiscIndexDelay);
+ while (true) {
+ bottom_disc_->source()->WaitForInterrupt(kInterruptTimeout);
+ if (bottom_disc_->Get()) {
+ aos::time::Time start = aos::time::Time::Now();
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ ++bottom_disc_negedge_count_;
+ }
+ aos::time::SleepUntil(start + kBottomDiscNegedgeSleep);
+ int32_t position = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ bottom_disc_negedge_wait_position_ = position;
+ ++bottom_disc_negedge_wait_count_;
+ }
+ } else {
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ ++bottom_disc_posedge_count_;
+ }
+ }
+ }
+}
+
void SensorPacker::PackInto(sensor_values *values) {
values->lencoder = htonl(-lencoder.GetRaw());
values->rencoder = -htonl(-rencoder.GetRaw());
diff --git a/frc971/input/sensor_packer.h b/frc971/input/sensor_packer.h
index 855431f..78f6bf6 100644
--- a/frc971/input/sensor_packer.h
+++ b/frc971/input/sensor_packer.h
@@ -1,13 +1,21 @@
#ifndef FRC971_INPUT_SENSOR_PACKER_H_
#define FRC971_INPUT_SENSOR_PACKER_H_
-#include "aos/common/mutex.h"
-#include "aos/crio/shared_libs/interrupt_notifier.h"
-#include "aos/common/sensors/sensor_packer.h"
+#include "aos/common/libstdc++/memory"
+
#include "WPILib/Task.h"
#include "WPILib/Encoder.h"
#include "WPILib/DigitalInput.h"
#include "WPILib/Counter.h"
+#include "WPILib/CounterBase.h"
+#include "WPILib/AnalogChannel.h"
+#include "WPILib/AnalogTrigger.h"
+
+#include "aos/common/mutex.h"
+#include "aos/crio/shared_libs/interrupt_notifier.h"
+#include "aos/common/sensors/sensor_packer.h"
+#include "aos/crio/hardware/counter.h"
+#include "aos/crio/hardware/digital_source.h"
#include "frc971/queues/sensor_values.h"
@@ -21,8 +29,76 @@
virtual void PackInto(sensor_values *values);
private:
- Encoder lencoder;
- Encoder rencoder;
+ // Used for passing ReadEncoder all of the information that it needs.
+ struct EncoderReadData {
+ EncoderReadData(::aos::crio::hardware::Counter *counter,
+ int32_t *output,
+ ::aos::Mutex *sync)
+ : counter(counter), output(output), sync(sync) {}
+
+ ::aos::crio::hardware::Counter *const counter;
+ int32_t *const output;
+ ::aos::Mutex *const sync;
+ };
+
+ // Handler function for an ::aos::crio::InterruptNotifier that reads an
+ // encoder and (synchronized on a ::aos::Mutex) writes the value into a
+ // pointer.
+ static void ReadEncoder(EncoderReadData *data);
+
+ // A whole day.
+ // Used for passing to WPILib's WaitForInterrupts.
+ static const float kInterruptTimeout = 60 * 60 * 24;
+
+ static void StaticTopDiscEdgeReader(void *self) {
+ static_cast<SensorPacker *>(self)->TopDiscEdgeReader();
+ }
+ static void StaticBottomDiscEdgeReader(void *self) {
+ static_cast<SensorPacker *>(self)->BottomDiscEdgeReader();
+ }
+
+ void TopDiscEdgeReader();
+ void BottomDiscEdgeReader();
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> drive_left_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> drive_right_counter_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> wrist_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> wrist_hall_effect_;
+ int32_t wrist_edge_position_;
+ ::aos::Mutex wrist_sync_;
+ ::aos::crio::InterruptNotifier<EncoderReadData> wrist_notifier_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> angle_adjust_counter_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> angle_adjust_middle_hall_effect_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> angle_adjust_bottom_hall_effect_;
+ int32_t angle_adjust_middle_edge_position_;
+ int32_t angle_adjust_bottom_edge_position_;
+ ::aos::Mutex angle_adjust_sync_;
+ ::aos::crio::InterruptNotifier<EncoderReadData>
+ angle_adjust_middle_notifier_;
+ ::aos::crio::InterruptNotifier<EncoderReadData>
+ angle_adjust_bottom_notifier_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::Counter> shooter_counter_;
+
+ ::std::unique_ptr< ::aos::crio::hardware::CounterCounter> index_counter_;
+ Task top_disc_edge_task_;
+ ::aos::Mutex top_disc_edge_sync_;
+ ::std::unique_ptr< ::AnalogTrigger> top_disc_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> top_disc_posedge_output_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> top_disc_negedge_output_;
+ int32_t top_disc_posedge_count_;
+ int32_t top_disc_negedge_count_;
+ int32_t top_disc_posedge_position_;
+ int32_t top_disc_negedge_position_;
+ Task bottom_disc_edge_task_;
+ ::aos::Mutex bottom_disc_edge_sync_;
+ ::std::unique_ptr< ::aos::crio::hardware::DigitalSource> bottom_disc_;
+ int32_t bottom_disc_posedge_count_;
+ int32_t bottom_disc_negedge_count_;
+ int32_t bottom_disc_negedge_wait_count_;
+ int32_t bottom_disc_negedge_wait_position_;
};
} // namespace frc971
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
index 95e30da..8695785 100644
--- a/frc971/input/sensor_unpacker.cc
+++ b/frc971/input/sensor_unpacker.cc
@@ -29,8 +29,10 @@
}
// TODO(aschuh): Convert to meters.
- const double left_encoder = drivetrain_translate(values->lencoder);
- const double right_encoder = drivetrain_translate(values->rencoder);
+ const double left_encoder = drivetrain_translate(
+ values->drive_left_encoder);
+ const double right_encoder = drivetrain_translate(
+ values->drive_right_encoder);
drivetrain.position.MakeWithBuilder()
.left_encoder(left_encoder)
.right_encoder(right_encoder)