Merge "Automatically write logs if file is specified"
diff --git a/NO_BUILD_AMD64 b/NO_BUILD_AMD64
index 28732b6..3edc6fe 100644
--- a/NO_BUILD_AMD64
+++ b/NO_BUILD_AMD64
@@ -18,3 +18,7 @@
-//y2016_bot3/wpilib/...
-//y2016_bot3:download
-//y2016_bot3:download_stripped
+-//y2016_bot3/...
+-//y2016_bot4/wpilib/...
+-//y2016_bot4:download
+-//y2016_bot4:download_stripped
diff --git a/NO_BUILD_ROBORIO b/NO_BUILD_ROBORIO
index 839238a..acd53e3 100644
--- a/NO_BUILD_ROBORIO
+++ b/NO_BUILD_ROBORIO
@@ -8,4 +8,6 @@
-//y2015_bot3/control_loops/python/...
-//y2016/control_loops/python/...
-//y2016_bot3/control_loops/python/...
+-//y2016_bot4/control_loops/python/...
-//third_party/protobuf/...
+-//y2016_bot3/...
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index 3a11256..c3400fe 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -1,3 +1,5 @@
+package(default_visibility = ["//visibility:public"])
+
cc_library(
name = 'epoll_events',
srcs = ['epoll_events.cc'],
@@ -10,13 +12,30 @@
)
cc_library(
+ name = 'intrusive_free_list',
+ hdrs = ['intrusive_free_list.h'],
+)
+
+cc_library(
+ name = 'tcp_server',
+ srcs = ['tcp_server.cc'],
+ hdrs = ['tcp_server.h'],
+ deps = [':epoll_events', ':intrusive_free_list'],
+)
+
+cc_library(
+ name = 'tcp_client',
+ srcs = ['tcp_client.cc'],
+ hdrs = ['tcp_client.h'],
+ deps = [':epoll_events'],
+)
+
+cc_library(
name = 'udp',
- visibility = ['//visibility:public'],
srcs = ['udp.cc'],
hdrs = ['udp.h'],
deps = [
'//aos/common:macros',
- '//aos/common/logging',
'//aos/common:scoped_fd',
],
)
@@ -26,6 +45,6 @@
srcs = ['udp_test.cc'],
deps = [
':udp',
- '//aos/testing:googletest'
+ '//aos/testing:googletest',
],
)
diff --git a/aos/vision/events/epoll_events.cc b/aos/vision/events/epoll_events.cc
index aaecdbf..f191259 100644
--- a/aos/vision/events/epoll_events.cc
+++ b/aos/vision/events/epoll_events.cc
@@ -1,87 +1,72 @@
#include "aos/vision/events/epoll_events.h"
-#include <string.h>
-#include <sys/types.h>
-#include <sys/socket.h>
#include <fcntl.h>
+#include <string.h>
#include <sys/epoll.h>
-
+#include <sys/socket.h>
+#include <sys/types.h>
#include <vector>
-#include "aos/common/scoped_fd.h"
#include "aos/common/logging/logging.h"
namespace aos {
namespace events {
-class EpollLoop::Impl {
- public:
- Impl() : epoll_fd_(PCHECK(epoll_create1(0))) {}
+EpollLoop::EpollLoop() : epoll_fd_(PCHECK(epoll_create1(0))) {}
- void Add(EpollEvent *event) {
- struct epoll_event temp_event;
- temp_event.data.ptr = static_cast<void *>(event);
- temp_event.events = EPOLLIN;
- PCHECK(epoll_ctl(epoll_fd(), EPOLL_CTL_ADD, event->fd(), &temp_event));
- }
+void EpollLoop::Add(EpollEvent *event) {
+ event->loop_ = this;
+ struct epoll_event temp_event;
+ temp_event.data.ptr = static_cast<void *>(event);
+ temp_event.events = EPOLLIN;
+ PCHECK(epoll_ctl(epoll_fd(), EPOLL_CTL_ADD, event->fd(), &temp_event));
+}
- void Run() {
- while (true) {
- const int timeout = CalculateTimeout();
- static constexpr size_t kNumberOfEvents = 64;
- epoll_event events[kNumberOfEvents];
- const int number_events = PCHECK(
- epoll_wait(epoll_fd(), events, kNumberOfEvents, timeout));
+void EpollLoop::Delete(EpollEvent *event) {
+ PCHECK(epoll_ctl(epoll_fd(), EPOLL_CTL_DEL, event->fd(), NULL));
+}
- for (int i = 0; i < number_events; i++) {
- EpollEvent *event =
- static_cast<EpollEvent *>(events[i].data.ptr);
- if ((events[i].events & ~(EPOLLIN | EPOLLPRI)) != 0) {
- LOG(FATAL, "unexpected epoll events set in %x on %d\n",
- events[i].events, event->fd());
- }
- event->ReadEvent();
+void EpollLoop::Run() {
+ while (true) {
+ const int timeout = CalculateTimeout();
+ static constexpr size_t kNumberOfEvents = 64;
+ epoll_event events[kNumberOfEvents];
+ const int number_events =
+ PCHECK(epoll_wait(epoll_fd(), events, kNumberOfEvents, timeout));
+
+ for (int i = 0; i < number_events; i++) {
+ EpollEvent *event = static_cast<EpollEvent *>(events[i].data.ptr);
+ if ((events[i].events & ~(EPOLLIN | EPOLLPRI)) != 0) {
+ LOG(FATAL, "unexpected epoll events set in %x on %d\n",
+ events[i].events, event->fd());
}
+ event->ReadEvent();
+ }
- for (EpollWatcher *watcher : watchers_) {
- watcher->Wake();
- }
+ for (EpollWatcher *watcher : watchers_) {
+ watcher->Wake();
}
}
+}
- void AddWait(EpollWait *wait) { waits_.push_back(wait); }
- void AddWatcher(EpollWatcher *watcher) { watchers_.push_back(watcher); }
-
- int epoll_fd() { return epoll_fd_.get(); }
-
- private:
- // Calculates the new timeout value to pass to epoll_wait.
- int CalculateTimeout() {
- const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
- int r = -1;
- for (EpollWait *c : waits_) {
- const int new_timeout = c->Recalculate(monotonic_now);
- if (new_timeout >= 0) {
- if (r < 0 || new_timeout < r) {
- r = new_timeout;
- }
- }
- }
- return r;
- }
-
- private:
- ::aos::ScopedFD epoll_fd_;
- ::std::vector<EpollWait *> waits_;
- ::std::vector<EpollWatcher *> watchers_;
-};
-
-EpollLoop::EpollLoop() : impl_(new Impl()) {}
-void EpollLoop::Run() { impl_->Run(); }
-void EpollLoop::Add(EpollEvent *event) { impl_->Add(event); }
-void EpollLoop::AddWait(EpollWait *wait) { impl_->AddWait(wait); }
+void EpollLoop::AddWait(EpollWait *wait) { waits_.push_back(wait); }
void EpollLoop::AddWatcher(EpollWatcher *watcher) {
- impl_->AddWatcher(watcher);
+ watchers_.push_back(watcher);
+}
+
+// Calculates the new timeout value to pass to epoll_wait.
+int EpollLoop::CalculateTimeout() {
+ const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ int r = -1;
+ for (EpollWait *c : waits_) {
+ const int new_timeout = c->Recalculate(monotonic_now);
+ if (new_timeout >= 0) {
+ if (r < 0 || new_timeout < r) {
+ r = new_timeout;
+ }
+ }
+ }
+ return r;
}
} // namespace events
diff --git a/aos/vision/events/epoll_events.h b/aos/vision/events/epoll_events.h
index 1cb6842..d7574f9 100644
--- a/aos/vision/events/epoll_events.h
+++ b/aos/vision/events/epoll_events.h
@@ -1,11 +1,12 @@
#ifndef AOS_VISION_EVENTS_EPOLL_EVENTS_H_
#define AOS_VISION_EVENTS_EPOLL_EVENTS_H_
-#include <stdint.h>
#include <limits.h>
-
+#include <stdint.h>
#include <memory>
+#include <vector>
+#include "aos/common/scoped_fd.h"
#include "aos/common/time.h"
namespace aos {
@@ -66,8 +67,12 @@
// loop degrades into a busy loop.
virtual void ReadEvent() = 0;
+ EpollLoop *loop() { return loop_; }
+
private:
const int fd_;
+ friend class EpollLoop;
+ EpollLoop *loop_ = nullptr;
};
// Provides a way for code to be notified every time after events are handled by
@@ -82,11 +87,6 @@
};
// A file descriptor based event loop implemented with epoll.
-//
-// There is currently no way to remove events because that's hard
-// (have to deal with events that come in for a file descriptor after it's
-// removed, which means not closing the fd or destroying the object until the
-// epoll mechanism confirms its removal) and we don't have a use for it.
class EpollLoop {
public:
EpollLoop();
@@ -97,13 +97,28 @@
void Add(EpollEvent *event);
void AddWatcher(EpollWatcher *watcher);
+ // Delete event. Note that there are caveats here as this is
+ // not idiot proof.
+ // ie:
+ // - Do not call from other threads.
+ // - Do not free while the object could still receive events.
+ // - This is safe only because the events are set as edge.
+ // TODO(parker): The thread-safety of this should be investigated and
+ // improved as well as adding support for non-edge events if this is to
+ // be used more generally.
+ void Delete(EpollEvent *event);
+
// Loops forever, handling events.
void Run();
private:
- class Impl;
+ int epoll_fd() { return epoll_fd_.get(); }
- ::std::unique_ptr<Impl> impl_;
+ int CalculateTimeout();
+
+ ::aos::ScopedFD epoll_fd_;
+ ::std::vector<EpollWait *> waits_;
+ ::std::vector<EpollWatcher *> watchers_;
};
} // namespace events
diff --git a/aos/vision/events/intrusive_free_list.h b/aos/vision/events/intrusive_free_list.h
new file mode 100644
index 0000000..0373741
--- /dev/null
+++ b/aos/vision/events/intrusive_free_list.h
@@ -0,0 +1,81 @@
+#ifndef _AOS_VISION_EVENTS_INTRUSIVE_FREE_LIST_H_
+#define _AOS_VISION_EVENTS_INTRUSIVE_FREE_LIST_H_
+
+namespace aos {
+namespace events {
+
+// Hey! Maybe you want a doubly linked list that frees things for you!
+// This allows the entry to delete itself, removing it from the list, or
+// when the list gets destructed all the entries get destructed.
+//
+// class MyType : public intrusive_free_list<MyType>::element {
+// public:
+// MyType(int i, intrusive_free_list<MyType>* list)
+// : element(list, this), i_(i) {}
+// ~MyType() { printf("o%d\n", i_); }
+// private:
+// int i_;
+// };
+//
+// void test_fn() {
+// intrusive_free_list<MyType> free_list;
+// auto o0 = new MyType(0, &free_list);
+// auto o1 = new MyType(1, &free_list);
+// auto o2 = new MyType(2, &free_list);
+// auto o3 = new MyType(2, &free_list);
+//
+// delete o2;
+// delete o1;
+// }
+//
+// // This will print:
+// o2
+// o1
+// o3
+// o0
+//
+// Note that anything that was not manually freed (o0, o3) is
+// freed by the linked list destructor at the end. This ensures everything
+// is always destructed even entities not manually destructed.
+template <class T>
+class intrusive_free_list {
+ public:
+ class element {
+ public:
+ element(intrusive_free_list<T> *list, T *t) : list_(list), prev_(nullptr) {
+ next_ = list_->begin_;
+ if (next_) next_->prev_ = t;
+ list_->begin_ = t;
+ }
+ ~element() {
+ if (next_) next_->prev_ = prev_;
+ if (prev_) {
+ prev_->next_ = next_;
+ } else {
+ list_->begin_ = next_;
+ }
+ }
+ T *next() { return next_; }
+
+ private:
+ friend class intrusive_free_list<T>;
+ // Consider using static casts and a header element to save this pointer.
+ intrusive_free_list<T> *list_;
+ T *next_;
+ T *prev_;
+ };
+ intrusive_free_list() : begin_(nullptr) {}
+ ~intrusive_free_list() {
+ while (begin_) delete begin_;
+ }
+ T *begin() { return begin_; }
+
+ private:
+ friend class element;
+ T *begin_;
+};
+
+} // namespace events
+} // namespace aos
+
+#endif // _AOS_VISION_EVENTS_INTRUSIVE_FREE_LIST_H_
diff --git a/aos/vision/events/tcp_client.cc b/aos/vision/events/tcp_client.cc
new file mode 100644
index 0000000..41485f9
--- /dev/null
+++ b/aos/vision/events/tcp_client.cc
@@ -0,0 +1,67 @@
+#include "aos/vision/events/tcp_client.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <unistd.h>
+
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace events {
+
+namespace {
+int MakeSocketNonBlocking(int sfd) {
+ int flags;
+
+ PCHECK(flags = fcntl(sfd, F_GETFL, 0));
+
+ flags |= O_NONBLOCK;
+ PCHECK(fcntl(sfd, F_SETFL, flags));
+
+ return 0;
+}
+
+int OpenClient(const char *hostname, int portno) {
+ int sockfd;
+ struct sockaddr_in serveraddr;
+ struct hostent *server;
+ /* socket: create the socket */
+ PCHECK(sockfd = socket(AF_INET, SOCK_STREAM, 0));
+
+ /* gethostbyname: get the server's DNS entry */
+ server = gethostbyname(hostname);
+ if (server == NULL) {
+ fprintf(stderr, "ERROR, no such host as %s\n", hostname);
+ exit(-1);
+ }
+
+ /* build the server's Internet address */
+ bzero((char *)&serveraddr, sizeof(serveraddr));
+ serveraddr.sin_family = AF_INET;
+ bcopy((char *)server->h_addr, (char *)&serveraddr.sin_addr.s_addr,
+ server->h_length);
+ serveraddr.sin_port = htons(portno);
+
+ /* connect: create a connection with the server */
+ PCHECK(connect(sockfd, (const struct sockaddr *)&serveraddr,
+ sizeof(serveraddr)));
+ PCHECK(MakeSocketNonBlocking(sockfd));
+
+ return sockfd;
+}
+} // namespace
+
+TcpClient::TcpClient(const char *hostname, int portno)
+ : EpollEvent(OpenClient(hostname, portno)) {}
+
+} // namespace events
+} // namespace aos
diff --git a/aos/vision/events/tcp_client.h b/aos/vision/events/tcp_client.h
new file mode 100644
index 0000000..e7b80a6
--- /dev/null
+++ b/aos/vision/events/tcp_client.h
@@ -0,0 +1,22 @@
+#ifndef _AOS_VISION_DEBUG_TCP_SERVER_H_
+#define _AOS_VISION_DEBUG_TCP_SERVER_H_
+
+#include "aos/vision/events/epoll_events.h"
+
+#include <memory>
+
+namespace aos {
+namespace events {
+
+// Handles the client connection logic to hostname:portno
+class TcpClient : public EpollEvent {
+ public:
+ TcpClient(const char *hostname, int portno);
+
+ // Implement ReadEvent from EpollEvent to use this class.
+};
+
+} // namespace events
+} // namespace aos
+
+#endif // _AOS_VISION_DEBUG_TCP_SERVER_H_
diff --git a/aos/vision/events/tcp_server.cc b/aos/vision/events/tcp_server.cc
new file mode 100644
index 0000000..06f3a41
--- /dev/null
+++ b/aos/vision/events/tcp_server.cc
@@ -0,0 +1,110 @@
+#include "aos/vision/events/tcp_server.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/epoll.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace events {
+
+namespace {
+
+int MakeSocketNonBlocking(int sfd) {
+ int flags;
+
+ PCHECK(flags = fcntl(sfd, F_GETFL, 0));
+
+ flags |= O_NONBLOCK;
+ PCHECK(fcntl(sfd, F_SETFL, flags));
+
+ return 0;
+}
+} // namespace
+
+// This is all copied from somewhere.
+int TCPServerBase::SocketBindListenOnPort(int portno) {
+ int parentfd; /* parent socket */
+ struct sockaddr_in serveraddr; /* server's addr */
+ int optval; /* flag value for setsockopt */
+
+ /*
+ * socket: create the parent socket
+ */
+ PCHECK(parentfd = socket(AF_INET, SOCK_STREAM, 0));
+
+ /* setsockopt: Handy debugging trick that lets
+ * us rerun the server immediately after we kill it;
+ * otherwise we have to wait about 20 secs.
+ * Eliminates "ERROR on binding: Address already in use" error.
+ */
+ optval = 1;
+ PCHECK(setsockopt(parentfd, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval,
+ sizeof(int)));
+
+ /*
+ * build the server's Internet address
+ */
+ bzero((char *)&serveraddr, sizeof(serveraddr));
+
+ /* this is an Internet address */
+ serveraddr.sin_family = AF_INET;
+
+ /* Listen on 0.0.0.0 */
+ serveraddr.sin_addr.s_addr = htonl(INADDR_ANY);
+
+ /* this is the port we will listen on */
+ serveraddr.sin_port = htons((uint16_t)portno);
+
+ /*
+ * bind: associate the parent socket with a port
+ */
+ PCHECK(bind(parentfd, (struct sockaddr *)&serveraddr, sizeof(serveraddr)));
+
+ PCHECK(listen(parentfd, SOMAXCONN));
+
+ LOG(INFO, "connected to port: %d on fd: %d\n", portno, parentfd);
+ return parentfd;
+}
+
+TCPServerBase::~TCPServerBase() { close(fd()); }
+
+void TCPServerBase::ReadEvent() {
+ /* We have a notification on the listening socket, which
+ means one or more incoming connections. */
+ struct sockaddr in_addr;
+ socklen_t in_len;
+ int infd;
+
+ in_len = sizeof in_addr;
+ infd = accept(fd(), &in_addr, &in_len);
+ if (infd == -1) {
+ if ((errno == EAGAIN) || (errno == EWOULDBLOCK)) {
+ /* We have processed all incoming
+ connections. */
+ return;
+ } else {
+ PLOG(WARNING, "accept");
+ return;
+ }
+ }
+
+ /* Make the incoming socket non-blocking and add it to the
+ list of fds to monitor. */
+ PCHECK(MakeSocketNonBlocking(infd));
+
+ loop()->Add(Construct(infd));
+}
+
+} // namespace events
+} // namespace aos
diff --git a/aos/vision/events/tcp_server.h b/aos/vision/events/tcp_server.h
new file mode 100644
index 0000000..3116f55
--- /dev/null
+++ b/aos/vision/events/tcp_server.h
@@ -0,0 +1,77 @@
+#ifndef _AOS_VISION_EVENTS_TCP_SERVER_H_
+#define _AOS_VISION_EVENTS_TCP_SERVER_H_
+
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/events/intrusive_free_list.h"
+
+#include <memory>
+#include <vector>
+
+namespace aos {
+namespace events {
+
+// Non-templatized base class of TCP server.
+// TCPServer implements Construct which specializes the client connection
+// based on the specific use-case.
+template <class T>
+class TCPServer;
+class SocketConnection;
+class TCPServerBase : public EpollEvent {
+ public:
+ TCPServerBase(int fd) : EpollEvent(fd) {}
+ ~TCPServerBase();
+
+ protected:
+ // Listens on port portno. File descriptor to
+ // accept on is returned.
+ static int SocketBindListenOnPort(int portno);
+
+ private:
+ virtual SocketConnection *Construct(int child_fd) = 0;
+ void ReadEvent() override;
+ friend class SocketConnection;
+ template <class T>
+ friend class TCPServer;
+ intrusive_free_list<SocketConnection> free_list;
+};
+
+// Base class for client connections. Clients are responsible for
+// deleting themselves once the connection is broken. This will remove
+// the entry from the free list.
+class SocketConnection : public EpollEvent,
+ public intrusive_free_list<SocketConnection>::element {
+ public:
+ SocketConnection(TCPServerBase *server, int fd)
+ : EpollEvent(fd), element(&server->free_list, this) {}
+};
+
+// T should be a subclass of SocketConnection.
+template <class T>
+class TCPServer : public TCPServerBase {
+ public:
+ TCPServer(int port) : TCPServerBase(SocketBindListenOnPort(port)) {}
+ SocketConnection *Construct(int child_fd) override {
+ return new T(this, child_fd);
+ }
+
+ static std::unique_ptr<TCPServer<T>> Make(int port) {
+ return std::unique_ptr<TCPServer<T>>(new TCPServer<T>(port));
+ }
+
+ // Call blk on each entry of the free-list. This is used to send a message
+ // to all clients.
+ template <typename EachBlock>
+ void Broadcast(const EachBlock &blk) {
+ auto a = free_list.begin();
+ while (a) {
+ auto client = static_cast<T *>(a);
+ blk(client);
+ a = a->next();
+ }
+ }
+};
+
+} // namespace events
+} // namespace aos
+
+#endif // _AOS_VISION_EVENTS_TCP_SERVER_H_
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
new file mode 100644
index 0000000..662addc
--- /dev/null
+++ b/aos/vision/image/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_library(
+ name = 'image_types',
+ hdrs = ['image_types.h'],
+ deps = [
+ '//aos/common/logging:logging',
+ ],
+)
+
+cc_library(
+ name = 'reader',
+ srcs = ['reader.cc'],
+ hdrs = ['V4L2.h', 'reader.h'],
+ deps = [
+ '//aos/common:time',
+ '//aos/common/logging:logging',
+ ':image_types',
+ ],
+)
+
+cc_library(
+ name = 'jpeg_routines',
+ srcs = ['jpeg_routines.cc'],
+ hdrs = ['jpeg_routines.h'],
+ deps = [
+ '//third_party/libjpeg',
+ '//aos/common/logging:logging',
+ ':image_types'
+ ],
+)
+
+cc_library(name = 'image_stream',
+ hdrs = ['image_stream.h'],
+ deps = [
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/image:reader'
+ ]
+)
diff --git a/aos/vision/image/V4L2.h b/aos/vision/image/V4L2.h
new file mode 100644
index 0000000..58e5161
--- /dev/null
+++ b/aos/vision/image/V4L2.h
@@ -0,0 +1,24 @@
+#ifndef AOS_LINUX_CODE_CAMREA_V4L2_H_
+#define AOS_LINUX_CODE_CAMREA_V4L2_H_
+
+// This file handles including everything needed to use V4L2 and has some
+// utility functions.
+
+#include <sys/ioctl.h>
+
+#include <asm/types.h> /* for videodev2.h */
+#include <linux/videodev2.h>
+
+namespace camera {
+
+static inline int xioctl(int fd, int request, void *arg) {
+ int r;
+ do {
+ r = ioctl(fd, request, reinterpret_cast<uintptr_t>(arg));
+ } while (r == -1 && errno == EINTR);
+ return r;
+}
+
+} // namespace camera
+
+#endif
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
new file mode 100644
index 0000000..ca2d8be
--- /dev/null
+++ b/aos/vision/image/image_stream.h
@@ -0,0 +1,50 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+#define _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/reader.h"
+
+#include <memory>
+
+namespace aos {
+namespace vision {
+
+class ImageStreamEvent : public ::aos::events::EpollEvent {
+ public:
+ static std::unique_ptr<::camera::Reader> GetCamera(
+ const std::string &fname, ImageStreamEvent *obj,
+ camera::CameraParams params) {
+ using namespace std::placeholders;
+ std::unique_ptr<::camera::Reader> camread(new ::camera::Reader(
+ fname,
+ std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params);
+ camread->StartAsync();
+ return camread;
+ }
+
+ explicit ImageStreamEvent(std::unique_ptr<::camera::Reader> reader)
+ : ::aos::events::EpollEvent(reader->fd()), reader_(reader) {}
+
+ explicit ImageStreamEvent(const std::string &fname,
+ camera::CameraParams params)
+ : ImageStreamEvent(GetCamera(fname, this, params)) {}
+
+ void ProcessHelper(DataRef data, uint64_t timestamp) {
+ if (data.size() < 300) {
+ LOG(INFO, "got bad img: %d of size(%lu)\n", (int)timestamp, data.size());
+ return;
+ }
+ ProcessImage(data, timestamp);
+ }
+ virtual void ProcessImage(DataRef data, uint64_t timestamp) = 0;
+
+ void ReadEvent(Context /*ctx*/) override { reader_->HandleFrame(); }
+
+ private:
+ std::unique_ptr<::camera::Reader> reader_;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_DEBUG_IMAGE_STREAM_H_
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
new file mode 100644
index 0000000..2c9ba56
--- /dev/null
+++ b/aos/vision/image/image_types.h
@@ -0,0 +1,104 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_TYPES_H_
+#define _AOS_VISION_IMAGE_IMAGE_TYPES_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <memory>
+#include <sstream>
+
+#include <experimental/string_view>
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace vision {
+
+// This will go into c++17. No sense writing my own version.
+using DataRef = std::experimental::string_view;
+
+// Represents the dimensions of an image.
+struct ImageFormat {
+ ImageFormat() : w(0), h(0) {}
+ ImageFormat(int nw, int nh) : w(nw), h(nh) {}
+ std::string ToString() {
+ std::ostringstream s;
+ s << "ImageFormat {" << w << ", " << h << "}";
+ return s.str();
+ }
+ int ImgSize() const { return w * h; }
+ bool Equals(const ImageFormat &other) const {
+ return w == other.w && h == other.h;
+ }
+
+ int w;
+ int h;
+};
+
+// Alias for RGB triple. Should be align 1 size 3.
+struct PixelRef {
+ uint8_t r;
+ uint8_t g;
+ uint8_t b;
+};
+
+// Just to be extra safe.
+static_assert(sizeof(PixelRef) == 3, "Problem with cows in fields!");
+static_assert(alignof(PixelRef) == 1, "Problem with cows in fields!");
+
+// ptr version of a ValueArray. Allows operations on buffers owned by other
+// entities. Prefer this for const-ref versions.
+//
+// Templatized because there was a grayscale version, and cairo buffers are
+// only RGBA.
+template <typename ImageType>
+class Array2dPtr {
+ public:
+ Array2dPtr() : Array2dPtr({0, 0}, nullptr) {}
+ Array2dPtr(ImageFormat fmt, ImageType *data) : fmt_(fmt), data_(data) {}
+ ImageType &get_px(int x, int y) const {
+#ifndef NDEBUG
+ if (x < 0 || x >= fmt_.w || y < 0 || y >= fmt_.h) {
+ LOG(FATAL, "%d, %d out of range [%dx %d]\n", x, y, fmt_.w, fmt_.h);
+ }
+#endif // NBOUNDSCHECK
+ return data_[(x + y * fmt_.w)];
+ }
+ void CopyFrom(const Array2dPtr &other) const {
+ memcpy(data_, other.data_, sizeof(ImageType) * fmt_.ImgSize());
+ }
+
+ const ImageFormat &fmt() const { return fmt_; }
+ ImageType *data() const { return data_; }
+
+ private:
+ ImageFormat fmt_;
+ ImageType *data_;
+};
+
+// unique_ptr version of above.
+template <typename ImageType>
+class ValueArray2d {
+ public:
+ ValueArray2d() : fmt_({0, 0}) {}
+ explicit ValueArray2d(ImageFormat fmt) : fmt_(fmt) {
+ data_.reset(new ImageType[fmt.ImgSize()]);
+ }
+
+ Array2dPtr<ImageType> get() {
+ return Array2dPtr<ImageType>{fmt_, data_.get()};
+ }
+
+ const ImageFormat &fmt() const { return fmt_; }
+ ImageType *data() { return data_.get(); }
+
+ private:
+ ImageFormat fmt_;
+ std::unique_ptr<ImageType[]> data_;
+};
+
+using ImagePtr = Array2dPtr<PixelRef>;
+using ImageValue = ValueArray2d<PixelRef>;
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_IMAGE_TYPES_H_
diff --git a/aos/vision/image/jpeg_routines.cc b/aos/vision/image/jpeg_routines.cc
new file mode 100644
index 0000000..e0c93dc
--- /dev/null
+++ b/aos/vision/image/jpeg_routines.cc
@@ -0,0 +1,249 @@
+#include "aos/vision/image/jpeg_routines.h"
+
+#include <errno.h>
+#include <setjmp.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <unistd.h>
+#include <cstring>
+
+#include "aos/common/logging/logging.h"
+#include "third_party/libjpeg/jpeglib.h"
+
+namespace aos {
+namespace vision {
+
+namespace {
+
+void decompress_add_huff_table(j_decompress_ptr cinfo, JHUFF_TBL **htblptr,
+ const UINT8 *bits, const UINT8 *val);
+
+void standard_huff_tables(j_decompress_ptr cinfo);
+
+// Error handling form libjpeg
+struct JpegErrorManager {
+ /// "public" fields
+ struct jpeg_error_mgr pub;
+ // for return to caller
+ jmp_buf setjmp_buffer;
+};
+
+char JpegLastErrorMsg[JMSG_LENGTH_MAX];
+
+// TODO(parker): Error handling needs to be investigated bettter.
+void JpegErrorExit(j_common_ptr cinfo) {
+ JpegErrorManager myerr;
+ // cinfo->err actually points to a JpegErrorManager struct
+ ::std::memcpy(&myerr, cinfo->err, sizeof(myerr));
+ // JpegErrorManager* myerr = (JpegErrorManager*) cinfo->err;
+ // note : *(cinfo->err) is now equivalent to myerr->pub
+
+ // output_message is a method to print an error message
+ //(* (cinfo->err->output_message) ) (cinfo);
+
+ // Create the message
+ (*(cinfo->err->format_message))(cinfo, JpegLastErrorMsg);
+
+ // Jump to the setjmp point
+ longjmp(myerr.setjmp_buffer, 1);
+}
+
+// This is also adapted from libjpeg to be used on decompression tables rather
+// than compression tables as it was originally intended.
+void decompress_add_huff_table(j_decompress_ptr cinfo, JHUFF_TBL **htblptr,
+ const UINT8 *bits, const UINT8 *val) {
+ if (*htblptr == NULL) *htblptr = jpeg_alloc_huff_table((j_common_ptr)cinfo);
+
+ // Copy the number-of-symbols-of-each-code-length counts.
+ memcpy((*htblptr)->bits, bits, sizeof((*htblptr)->bits));
+
+ // Validate the counts. We do this here mainly so we can copy the right
+ // number of symbols from the val[] array, without risking marching off
+ // the end of memory. jchuff.c will do a more thorough test later.
+ int nsymbols = 0;
+ for (int len = 1; len <= 16; len++) nsymbols += bits[len];
+ if (nsymbols < 1 || nsymbols > 256) {
+ LOG(FATAL, "%s:%d: Error, bad huffman table", __FILE__, __LINE__);
+ }
+
+ memcpy((*htblptr)->huffval, val, nsymbols * sizeof(uint8_t));
+}
+
+// standard_huff_tables is taken from libjpeg compression stuff
+// and is here used to set up the same tables in the decompression structure.
+// Set up the standard Huffman tables (cf. JPEG standard section K.3)
+// IMPORTANT: these are only valid for 8-bit data precision!
+void standard_huff_tables(j_decompress_ptr cinfo) {
+ /* 0-base on first 0, */
+ static const UINT8 bits_dc_luminance[17] = {0, 0, 1, 5, 1, 1, 1, 1, 1,
+ 1, 0, 0, 0, 0, 0, 0, 0};
+ static const UINT8 val_dc_luminance[] = {0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11};
+
+ /* 0-base on first 0 */
+ static const UINT8 bits_dc_chrominance[17] = {0, 0, 3, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 0, 0, 0, 0, 0};
+ static const UINT8 val_dc_chrominance[] = {0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11};
+
+ /* 0-base on first 0 */
+ static const UINT8 bits_ac_luminance[17] = {0, 0, 2, 1, 3, 3, 2, 4, 3,
+ 5, 5, 4, 4, 0, 0, 1, 0x7d};
+ static const UINT8 val_ac_luminance[] = {
+ 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06,
+ 0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
+ 0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72,
+ 0x82, 0x09, 0x0a, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
+ 0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45,
+ 0x46, 0x47, 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
+ 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75,
+ 0x76, 0x77, 0x78, 0x79, 0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
+ 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3,
+ 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
+ 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9,
+ 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
+ 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf1, 0xf2, 0xf3, 0xf4,
+ 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa};
+
+ /* 0-base on first 0 */
+ static const UINT8 bits_ac_chrominance[17] = {0, 0, 2, 1, 2, 4, 4, 3, 4,
+ 7, 5, 4, 4, 0, 1, 2, 0x77};
+ static const UINT8 val_ac_chrominance[] = {
+ 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06, 0x12, 0x41,
+ 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
+ 0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1,
+ 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26,
+ 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44,
+ 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
+ 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74,
+ 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+ 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a,
+ 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
+ 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7,
+ 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
+ 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf2, 0xf3, 0xf4,
+ 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa};
+
+ decompress_add_huff_table(cinfo, &cinfo->dc_huff_tbl_ptrs[0],
+ bits_dc_luminance, val_dc_luminance);
+ decompress_add_huff_table(cinfo, &cinfo->ac_huff_tbl_ptrs[0],
+ bits_ac_luminance, val_ac_luminance);
+ decompress_add_huff_table(cinfo, &cinfo->dc_huff_tbl_ptrs[1],
+ bits_dc_chrominance, val_dc_chrominance);
+ decompress_add_huff_table(cinfo, &cinfo->ac_huff_tbl_ptrs[1],
+ bits_ac_chrominance, val_ac_chrominance);
+}
+
+void local_emit_message(jpeg_common_struct * /*cinfo*/, int /*msg_level*/) {
+ return;
+}
+
+} // namespace
+
+ImageFormat GetFmt(DataRef data) {
+ ImageFormat fmt;
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+
+ cinfo.err = jpeg_std_error(&jerr);
+ jerr.emit_message = local_emit_message;
+
+ cinfo.out_color_space = JCS_RGB;
+ jpeg_create_decompress(&cinfo);
+
+ jpeg_mem_src(&cinfo, reinterpret_cast<unsigned char *>(
+ const_cast<char *>(data.data())),
+ data.size());
+
+ jpeg_read_header(&cinfo, TRUE);
+ fmt.w = cinfo.image_width;
+ fmt.h = cinfo.image_height;
+ jpeg_destroy_decompress(&cinfo);
+ return fmt;
+}
+
+// Returns true if successful false if an error was encountered.
+bool ProcessJpeg(DataRef data, PixelRef *out) {
+ /*
+ TODO(parker): sort of error handling.
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+
+ cinfo.err = jpeg_std_error( &jerr );
+ jerr.emit_message = local_emit_message;
+
+ cinfo.out_color_space = JCS_RGB;
+ jpeg_create_decompress( &cinfo );
+ */
+
+ static bool lost_camera_connect = false;
+ struct jpeg_decompress_struct cinfo;
+
+ // We set up the normal JPEG error routines, then override error_exit.
+ JpegErrorManager jerr;
+ cinfo.err = jpeg_std_error(&jerr.pub);
+ jerr.pub.emit_message = local_emit_message;
+ jerr.pub.error_exit = JpegErrorExit;
+ // Establish the setjmp return context for my_error_exit to use.
+ if (setjmp(jerr.setjmp_buffer)) {
+ // If we get here, the JPEG code has signaled an error.
+ if (!lost_camera_connect) {
+ printf(
+ "Lost camera connection in process_jpeg.\nLooking for reconnect "
+ "...\n");
+ fflush(stdout);
+ lost_camera_connect = true;
+ }
+ jpeg_destroy_decompress(&cinfo);
+ return false;
+ }
+
+ cinfo.out_color_space = JCS_RGB;
+ jpeg_create_decompress(&cinfo);
+
+ jpeg_mem_src(&cinfo, reinterpret_cast<unsigned char *>(
+ const_cast<char *>(data.data())),
+ data.size());
+
+ jpeg_read_header(&cinfo, TRUE);
+ standard_huff_tables(&cinfo);
+
+ /*printf( "JPEG File Information: \n" );
+ printf( "Image width and height: %d pixels and %d pixels.\n",
+ cinfo.image_width, cinfo.image_height );
+ printf( "Color components per pixel: %d.\n", cinfo.num_components );
+ printf( "Color space: %d.\n", cinfo.jpeg_color_space );
+ printf("JpegDecompressed\n");*/
+
+ jpeg_start_decompress(&cinfo);
+
+ int offset = 0;
+ int step = cinfo.num_components * cinfo.image_width;
+ unsigned char *buffers[cinfo.image_height];
+ for (size_t i = 0; i < cinfo.image_height; ++i) {
+ buffers[i] = reinterpret_cast<unsigned char *>(&out[offset]);
+ offset += step;
+ }
+
+ while (cinfo.output_scanline < cinfo.image_height) {
+ jpeg_read_scanlines(&cinfo, &buffers[cinfo.output_scanline],
+ cinfo.image_height - cinfo.output_scanline);
+ }
+
+ jpeg_finish_decompress(&cinfo);
+ jpeg_destroy_decompress(&cinfo);
+
+ if (lost_camera_connect) {
+ printf("Camera connection restablished.\n");
+ fflush(stdout);
+ lost_camera_connect = false;
+ }
+
+ return true;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/image/jpeg_routines.h b/aos/vision/image/jpeg_routines.h
new file mode 100644
index 0000000..7c8fd67
--- /dev/null
+++ b/aos/vision/image/jpeg_routines.h
@@ -0,0 +1,36 @@
+#ifndef _AOS_VISION_IMAGE_JPEGROUTINES_H_
+#define _AOS_VISION_IMAGE_JPEGROUTINES_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+// Returns true if successful false if an error was encountered.
+// Will decompress data into out. Out must be of the right size
+// as determined below.
+bool ProcessJpeg(DataRef data, PixelRef *out);
+
+// Gets the format for the particular jpeg.
+ImageFormat GetFmt(DataRef data);
+
+// Decodes jpeg from data. Will resize if necessary.
+// (Should not be necessary in most normal cases).
+//
+// Consider this the canonical way to decode jpegs if no other
+// choice is given.
+inline bool DecodeJpeg(DataRef data, ImageValue *value) {
+ auto fmt = GetFmt(data);
+ if (!value->fmt().Equals(fmt)) {
+ *value = ImageValue(fmt);
+ }
+ return ProcessJpeg(data, value->data());
+}
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_JPEGROUTINES_H_
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
new file mode 100644
index 0000000..95729da
--- /dev/null
+++ b/aos/vision/image/reader.cc
@@ -0,0 +1,273 @@
+#include "aos/common/time.h"
+
+#include "aos/common/logging/logging.h"
+#include "aos/vision/image/reader.h"
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+namespace camera {
+
+struct Reader::Buffer {
+ void *start;
+ size_t length; // for munmap
+};
+
+Reader::Reader(const std::string &dev_name, ProcessCb process,
+ CameraParams params)
+ : dev_name_(dev_name), process_(std::move(process)), params_(params) {
+ struct stat st;
+ if (stat(dev_name.c_str(), &st) == -1) {
+ PLOG(FATAL, "Cannot identify '%s'", dev_name.c_str());
+ }
+ if (!S_ISCHR(st.st_mode)) {
+ PLOG(FATAL, "%s is no device\n", dev_name.c_str());
+ }
+
+ fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
+ if (fd_ == -1) {
+ PLOG(FATAL, "Cannot open '%s'", dev_name.c_str());
+ }
+
+ Init();
+}
+
+void Reader::QueueBuffer(v4l2_buffer *buf) {
+ if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
+ PLOG(WARNING,
+ "ioctl VIDIOC_QBUF(%d, %p)."
+ " losing buf #%" PRIu32 "\n",
+ fd_, &buf, buf->index);
+ } else {
+ // LOG(DEBUG, "put buf #%" PRIu32 " into driver's queue\n", buf->index);
+ ++queued_;
+ }
+}
+
+void Reader::HandleFrame() {
+ v4l2_buffer buf;
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
+ if (errno != EAGAIN) {
+ PLOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p)", fd_, &buf);
+ }
+ return;
+ }
+ --queued_;
+
+ // Get a timestamp now as proxy for when the image was taken
+ // TODO(ben): the image should come with a timestamp, parker
+ // will know how to get it.
+ auto time = aos::monotonic_clock::now();
+
+ process_(aos::vision::DataRef(
+ reinterpret_cast<const char *>(buffers_[buf.index].start),
+ buf.bytesused),
+ time);
+ QueueBuffer(&buf);
+}
+
+void Reader::MMapBuffers() {
+ buffers_ = new Buffer[kNumBuffers];
+ v4l2_buffer buf;
+ for (unsigned int n = 0; n < kNumBuffers; ++n) {
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n;
+ if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p)", fd_, &buf);
+ }
+ buffers_[n].length = buf.length;
+ buffers_[n].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
+ MAP_SHARED, fd_, buf.m.offset);
+ if (buffers_[n].start == MAP_FAILED) {
+ PLOG(FATAL,
+ "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)",
+ (size_t)buf.length, fd_, static_cast<intmax_t>(buf.m.offset));
+ }
+ }
+}
+
+void Reader::InitMMap() {
+ v4l2_requestbuffers req;
+ CLEAR(req);
+ req.count = kNumBuffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s does not support memory mapping\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p)\n", fd_, &req);
+ }
+ }
+ queued_ = kNumBuffers;
+ if (req.count < kNumBuffers) {
+ LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
+ }
+}
+
+// Sets one of the camera's user-control values.
+// Prints the old and new values.
+// Just prints a message if the camera doesn't support this control or value.
+bool Reader::SetCameraControl(uint32_t id, const char *name, int value) {
+ struct v4l2_control getArg = {id, 0U};
+ int r = xioctl(fd_, VIDIOC_G_CTRL, &getArg);
+ if (r == 0) {
+ if (getArg.value == value) {
+ LOG(DEBUG, "Camera control %s was already %d\n", name, getArg.value);
+ return true;
+ }
+ } else if (errno == EINVAL) {
+ LOG(DEBUG, "Camera control %s is invalid\n", name);
+ errno = 0;
+ return false;
+ }
+
+ struct v4l2_control setArg = {id, value};
+ r = xioctl(fd_, VIDIOC_S_CTRL, &setArg);
+ if (r == 0) {
+ LOG(DEBUG, "Set camera control %s from %d to %d\n", name, getArg.value,
+ value);
+ return true;
+ }
+
+ LOG(DEBUG, "Couldn't set camera control %s to %d", name, value);
+ errno = 0;
+ return false;
+}
+
+void Reader::Init() {
+ v4l2_capability cap;
+ if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s is no V4L2 device\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p)", fd_, &cap);
+ }
+ }
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ LOG(FATAL, "%s is no video capture device\n", dev_name_.c_str());
+ }
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ LOG(FATAL, "%s does not support streaming i/o\n", dev_name_.c_str());
+ }
+
+ /* Select video input, video standard and tune here. */
+
+ v4l2_cropcap cropcap;
+ CLEAR(cropcap);
+ cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_CROPCAP, &cropcap) == 0) {
+ v4l2_crop crop;
+ crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ crop.c = cropcap.defrect; /* reset to default */
+
+ if (xioctl(fd_, VIDIOC_S_CROP, &crop) == -1) {
+ switch (errno) {
+ case EINVAL:
+ /* Cropping not supported. */
+ break;
+ default:
+ /* Errors ignored. */
+ PLOG(WARNING, "xioctl VIDIOC_S_CROP");
+ break;
+ }
+ }
+ } else {
+ PLOG(WARNING, "xioctl VIDIOC_CROPCAP");
+ }
+
+ v4l2_format fmt;
+ CLEAR(fmt);
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = params_.width;
+ fmt.fmt.pix.height = params_.height;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+ fmt.fmt.pix.field = V4L2_FIELD_ANY;
+ // fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ // fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
+ errno, strerror(errno));
+ }
+ /* Note VIDIOC_S_FMT may change width and height. */
+
+ /* Buggy driver paranoia. */
+ unsigned int min = fmt.fmt.pix.width * 2;
+ if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
+ min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+ if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
+
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
+ V4L2_EXPOSURE_MANUAL)) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
+ "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure)) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_BRIGHTNESS, "V4L2_CID_BRIGHTNESS",
+ params_.brightness)) {
+ LOG(FATAL, "Failed to set up camera\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_GAIN, "V4L2_CID_GAIN", params_.gain)) {
+ LOG(FATAL, "Failed to set up camera\n");
+ }
+
+ // #if 0
+ // set framerate
+ struct v4l2_streamparm *setfps;
+ setfps = (struct v4l2_streamparm *)calloc(1, sizeof(struct v4l2_streamparm));
+ memset(setfps, 0, sizeof(struct v4l2_streamparm));
+ setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ setfps->parm.capture.timeperframe.numerator = 1;
+ setfps->parm.capture.timeperframe.denominator = params_.fps;
+ if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
+ }
+ LOG(INFO, "framerate ended up at %d/%d\n",
+ setfps->parm.capture.timeperframe.numerator,
+ setfps->parm.capture.timeperframe.denominator);
+ // #endif
+
+ InitMMap();
+ LOG(INFO, "Bat Vision Successfully Initialized.\n");
+}
+
+aos::vision::ImageFormat Reader::get_format() {
+ struct v4l2_format fmt;
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
+ PLOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p)\n", fd_, &fmt);
+ }
+
+ return aos::vision::ImageFormat{(int)fmt.fmt.pix.width,
+ (int)fmt.fmt.pix.height};
+}
+
+void Reader::Start() {
+ LOG(DEBUG, "queueing buffers for the first time\n");
+ v4l2_buffer buf;
+ for (unsigned int i = 0; i < kNumBuffers; ++i) {
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ QueueBuffer(&buf);
+ }
+ LOG(DEBUG, "done with first queue\n");
+
+ v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p)\n", fd_, &type);
+ }
+}
+
+} // namespace camera
diff --git a/aos/vision/image/reader.h b/aos/vision/image/reader.h
new file mode 100644
index 0000000..eabb80a
--- /dev/null
+++ b/aos/vision/image/reader.h
@@ -0,0 +1,78 @@
+#ifndef AOS_VISION_IMAGE_READER_H_
+#define AOS_VISION_IMAGE_READER_H_
+#include <errno.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <inttypes.h>
+#include <functional>
+#include <string>
+
+#include "aos/common/time.h"
+#include "aos/vision/image/V4L2.h"
+#include "aos/vision/image/image_types.h"
+
+namespace camera {
+
+struct CameraParams {
+ int32_t width;
+ int32_t height;
+ int32_t exposure;
+ int32_t brightness;
+ int32_t gain;
+ int32_t fps;
+};
+
+class Reader {
+ public:
+ using ProcessCb = std::function<void(
+ aos::vision::DataRef data, aos::monotonic_clock::time_point timestamp)>;
+ Reader(const std::string &dev_name, ProcessCb process, CameraParams params);
+
+ aos::vision::ImageFormat get_format();
+
+ void HandleFrame();
+ void StartAsync() {
+ Start();
+ MMapBuffers();
+ }
+ int fd() { return fd_; }
+
+ private:
+ void QueueBuffer(v4l2_buffer *buf);
+ void InitMMap();
+ bool SetCameraControl(uint32_t id, const char *name, int value);
+ void Init();
+ void Start();
+ void MMapBuffers();
+ // File descriptor of the camera
+ int fd_;
+ // Name of the camera device.
+ std::string dev_name_;
+
+ ProcessCb process_;
+
+ // The number of buffers currently queued in v4l2.
+ uint32_t queued_;
+ struct Buffer;
+ // TODO(parker): This should be a smart pointer, but it cannot
+ // because the buffers are not ummapped.
+ Buffer *buffers_;
+
+ static const unsigned int kNumBuffers = 10;
+
+ // set only at initialize
+ CameraParams params_;
+};
+
+} // namespace camera
+
+#endif // AOS_VISION_IMAGE_READER_H_
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index b5f4060..2075ca7 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -125,3 +125,25 @@
':state_feedback_loop',
],
)
+
+cc_library(
+ name = 'runge_kutta',
+ hdrs = [
+ 'runge_kutta.h',
+ ],
+ deps = [
+ '//third_party/eigen',
+ ],
+)
+
+cc_test(
+ name = 'runge_kutta_test',
+ srcs = [
+ 'runge_kutta_test.cc',
+ ],
+ deps = [
+ ':runge_kutta',
+ '//aos/testing:googletest',
+ '//third_party/eigen',
+ ],
+)
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
new file mode 100644
index 0000000..fe88da0
--- /dev/null
+++ b/frc971/control_loops/runge_kutta.h
@@ -0,0 +1,25 @@
+#ifndef FRC971_CONTROL_LOOPS_RUNGE_KUTTA_H_
+#define FRC971_CONTROL_LOOPS_RUNGE_KUTTA_H_
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace control_loops {
+
+// Implements Runge Kutta integration (4th order). fn is the function to
+// integrate. It must take 1 argument of type T. The integration starts at an
+// initial value X, and integrates for dt.
+template <typename F, typename T>
+T RungeKutta(const F &fn, T X, double dt) {
+ const double half_dt = dt * 0.5;
+ auto k1 = fn(X);
+ auto k2 = fn(X + half_dt * k1);
+ auto k3 = fn(X + half_dt * k2);
+ auto k4 = fn(X + dt * k3);
+ return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_RUNGE_KUTTA_H_
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
new file mode 100644
index 0000000..680715a
--- /dev/null
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -0,0 +1,26 @@
+#include "frc971/control_loops/runge_kutta.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Tests that integrating dx/dt = e^x works.
+TEST(RungeKuttaTest, Exponential) {
+ ::Eigen::Matrix<double, 1, 1> y0;
+ y0(0, 0) = 0.0;
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKutta(
+ [](::Eigen::Matrix<double, 1, 1> x) {
+ ::Eigen::Matrix<double, 1, 1> y;
+ y(0, 0) = ::std::exp(x(0, 0));
+ return y;
+ },
+ y0, 0.1);
+ EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 23040e5..6f97bc7 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -2,6 +2,8 @@
#include "gtest/gtest.h"
+namespace frc971 {
+namespace control_loops {
namespace testing {
// Tests that everything compiles and nothing crashes even if
@@ -48,3 +50,5 @@
}
} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 9acb6fc..f410c19 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -47,8 +47,7 @@
bool zeroed = false;
double zero_offset = 0;
- ::aos::time::PhasedLoop phased_loop(
- ::aos::time::Time::FromRate(kReadingRate));
+ ::aos::time::PhasedLoop phased_loop(::aos::time::FromRate(kReadingRate));
// How many timesteps the next reading represents.
int number_readings = 0;
diff --git a/tools/cpp/arm-frc-linux-gnueabi/libs/libc.so b/tools/cpp/arm-frc-linux-gnueabi/libs/libc.so
new file mode 100644
index 0000000..714bcfe
--- /dev/null
+++ b/tools/cpp/arm-frc-linux-gnueabi/libs/libc.so
@@ -0,0 +1,5 @@
+/* GNU ld script
+ Use the shared library, but some functions are only in
+ the static library, so try that secondarily. */
+OUTPUT_FORMAT(elf32-littlearm)
+GROUP ( libc.so.6 libc_nonshared.a AS_NEEDED ( ld-linux.so.3 ) )
diff --git a/tools/cpp/arm-frc-linux-gnueabi/libs/libpthread.so b/tools/cpp/arm-frc-linux-gnueabi/libs/libpthread.so
new file mode 100644
index 0000000..71f034f
--- /dev/null
+++ b/tools/cpp/arm-frc-linux-gnueabi/libs/libpthread.so
@@ -0,0 +1,5 @@
+/* GNU ld script
+ Use the shared library, but some functions are only in
+ the static library, so try that secondarily. */
+OUTPUT_FORMAT(elf32-littlearm)
+GROUP ( libpthread.so.0 libpthread_nonshared.a )
diff --git a/vm/README.md b/vm/README.md
new file mode 100644
index 0000000..03e8965
--- /dev/null
+++ b/vm/README.md
@@ -0,0 +1,64 @@
+Requirements
+--------------------------------------------------------------------------------
+1. Install Vagrant <https://www.vagrantup.com/downloads.html>
+
+1. Install VirtualBox <https://www.virtualbox.org/wiki/Downloads>
+
+1. Add `vagrant` and `VBoxManage` to your PATH.
+ - This is most likely already done by the installation binaries.
+ It's added to the system path.
+ - To test this, type these commands in a terminal:
+
+ ~$ vagrant --version
+ Vagrant 1.8.1
+ ~$ VBoxManage --version
+ 5.0.14r105127
+
+ - You may need to log out and back in for the path modifications to take
+ effect.
+
+1. On my Jessie installation I had to apply the following patch before I could
+ successfully shut down and reboot the VM.
+
+ --- /opt/vagrant/embedded/gems/gems/vagrant-1.7.4/plugins/guests/debian8/cap/halt.rb 2015-07-17 13:15:13.000000000 -0700
+ +++ new_halt.rb 2015-11-18 20:11:29.003055639 -0800
+ @@ -4,7 +4,7 @@
+ class Halt
+ def self.halt(machine)
+ begin
+ - machine.communicate.sudo("shutdown -h -H")
+ + machine.communicate.sudo("systemctl poweroff")
+ rescue IOError
+ # Do nothing, because it probably means the machine shut down
+ # and SSH connection was lost.
+
+Usage
+--------------------------------------------------------------------------------
+1. Check this folder out on your computer somewhere.
+
+ svn co https://robotics.mvla.net/svn/frc971/2016/trunk/src/vagrant_dev_vm
+
+1. Go into the directory and build the VM.
+
+ vagrant up
+
+1. Some errors during the `vagrant up` process can be addressed by
+ re-provisioning the vagrant box. This is useful if, for example, an
+ `apt-get` invocation timed out and caused the provisioning process to abort.
+
+ vagrant provision
+
+1. Once build, reboot the VM so it starts the GUI properly.
+
+ vagrant reload
+
+1. You can then log in and open a terminal. The username and password are both
+ `user`.
+
+1. Download the code and build it.
+
+ git clone https://USERNAME@robotics.mvla.net/gerrit/971-Robot-Code
+ cd 971-Robot-Code
+ bazel build //y2016/... -- $(cat NO_BUILD_AMD64)
+
+ where USERNAME is the same username you use to log into SVN.
diff --git a/vm/Vagrantfile b/vm/Vagrantfile
new file mode 100644
index 0000000..c802a5a
--- /dev/null
+++ b/vm/Vagrantfile
@@ -0,0 +1,57 @@
+# Vagrantfile API/syntax version. Don't touch unless you know what you're
+# doing!
+VAGRANTFILE_API_VERSION = "2"
+
+# Install the necessary plugins.
+required_plugins = %w( vagrant-persistent-storage )
+required_plugins.each do |plugin|
+ unless Vagrant.has_plugin? plugin || ARGV[0] == 'plugin' then
+ exec "vagrant plugin install #{plugin};vagrant #{ARGV.join(" ")}"
+ end
+end
+
+Vagrant.configure(VAGRANTFILE_API_VERSION) do |config|
+ # All Vagrant configuration is done here. The most common configuration
+ # options are documented and commented below. For a complete reference,
+ # please see the online documentation at vagrantup.com.
+
+ # Every Vagrant virtual environment requires a box to build off of.
+ config.vm.box = "debian/jessie64"
+
+ config.vm.provider "virtualbox" do |vb|
+ # Don't boot with headless mode
+ vb.gui = true
+ vb.name = "FRC971-Development-2016"
+
+ # There are two shortcuts for modifying number of CPUs and amount of
+ # memory. Modify them to your liking.
+ vb.cpus = 2
+ vb.memory = 1024 * 2
+ end
+
+ # Use rsync to sync the /vagrant folder.
+ # NOTE: If you change these settings they will not take effect until you
+ # reboot the VM -- i.e. run a "vagrant reload".
+ config.vm.synced_folder ".", "/vagrant", type: "rsync",
+ rsync__exclude: [".git/", ".svn/", "workspace.vdi"], rsync__verbose: true
+
+ # Set up apt and install packages necessary for building the code.
+ config.vm.provision :shell, inline: "/vagrant/setup_apt.sh"
+ config.vm.provision :shell, inline: "/vagrant/setup_extra_storage.sh"
+ config.vm.provision :shell, inline: "/vagrant/setup_code_building.sh"
+ config.vm.provision :shell, inline: "/vagrant/setup_scouting.sh"
+ config.vm.provision :shell, inline: "/vagrant/setup_desktop.sh"
+ config.vm.provision :shell, inline: "/vagrant/setup_misc_packages.sh"
+ config.vm.provision :shell, inline: "/vagrant/setup_vbox_guest_additions.sh"
+
+ # Add a second disk so we have plenty of space to compile the code.
+ config.persistent_storage.enabled = true
+ config.persistent_storage.location = "workspace.vdi"
+ config.persistent_storage.size = 40000 # MiB
+ config.persistent_storage.use_lvm = false
+ config.persistent_storage.filesystem = 'ext4'
+ config.persistent_storage.mountpoint = '/home/user'
+
+ # Forward the scouting app's port.
+ config.vm.network :forwarded_port, guest: 5000, host: 5000, auto_correct: true
+end
diff --git a/vm/setup_apt.sh b/vm/setup_apt.sh
new file mode 100755
index 0000000..d31a372
--- /dev/null
+++ b/vm/setup_apt.sh
@@ -0,0 +1,15 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+export DEBIAN_FRONTEND=noninteractive
+
+# Set up contrib and non-free so we can install some more interesting programs.
+cat > /etc/apt/sources.list.d/contrib.list <<EOT
+deb http://ftp.us.debian.org/debian/ jessie contrib non-free
+deb-src http://ftp.us.debian.org/debian/ jessie contrib non-free
+EOT
+
+# Get a list of the latest packages.
+apt-get update
diff --git a/vm/setup_code_building.sh b/vm/setup_code_building.sh
new file mode 100755
index 0000000..a27b60e
--- /dev/null
+++ b/vm/setup_code_building.sh
@@ -0,0 +1,56 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+export DEBIAN_FRONTEND=noninteractive
+
+readonly PKGS=(
+ bazel
+ clang-3.6
+ clang-format-3.5
+ gfortran
+ git
+ libblas-dev
+ liblapack-dev
+ libpython3-dev
+ libpython-dev
+ python3
+ python3-matplotlib
+ python3-numpy
+ python3-scipy
+ python-matplotlib
+ python-scipy
+ resolvconf
+ ruby
+)
+
+# Set up the backports repo.
+cat > /etc/apt/sources.list.d/backports.list <<EOT
+deb http://http.debian.net/debian jessie-backports main
+EOT
+
+# Set up the LLVM repo.
+cat > /etc/apt/sources.list.d/llvm-3.6.list <<EOT
+deb http://llvm.org/apt/jessie/ llvm-toolchain-jessie-3.6 main
+deb-src http://llvm.org/apt/jessie/ llvm-toolchain-jessie-3.6 main
+EOT
+
+# Set up the 971-managed bazel repo.
+cat > /etc/apt/sources.list.d/bazel-971.list <<EOT
+deb http://robotics.mvla.net/files/frc971/packages jessie main
+EOT
+
+# Enable user namespace for sandboxing.
+cat > /etc/sysctl.d/99-enable-user-namespaces.conf <<EOT
+kernel.unprivileged_userns_clone = 1
+EOT
+
+# Accept the LLVM GPG key so we can install their packages.
+wget -O - http://llvm.org/apt/llvm-snapshot.gpg.key | apt-key add -
+
+# Install all the packages that we need/want.
+apt-get update
+for pkg in "${PKGS[@]}"; do
+ apt-get install -y -f --force-yes "$pkg"
+done
diff --git a/vm/setup_desktop.sh b/vm/setup_desktop.sh
new file mode 100755
index 0000000..d713856
--- /dev/null
+++ b/vm/setup_desktop.sh
@@ -0,0 +1,19 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+export DEBIAN_FRONTEND=noninteractive
+
+readonly PKGS=(
+ iceweasel
+ lightdm
+ mousepad
+ xfce4
+ xfce4-terminal
+)
+
+# Install all the packages that we need/want.
+for pkg in "${PKGS[@]}"; do
+ apt-get install -y -f "$pkg"
+done
diff --git a/vm/setup_extra_storage.sh b/vm/setup_extra_storage.sh
new file mode 100755
index 0000000..5fcf4cb
--- /dev/null
+++ b/vm/setup_extra_storage.sh
@@ -0,0 +1,19 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+readonly EXTRA_USER=user
+readonly EXTRA_STORAGE=/home/"${EXTRA_USER}"
+
+if ! grep -q "$EXTRA_STORAGE" /etc/passwd; then
+ PASSWORD="$(echo "$EXTRA_USER" | mkpasswd -s)"
+ useradd \
+ --home="$EXTRA_STORAGE" -M \
+ --password="$PASSWORD" \
+ --shell=/bin/bash \
+ "$EXTRA_USER"
+ chown "$EXTRA_USER:$EXTRA_USER" "$EXTRA_STORAGE"
+fi
+
+usermod -a -G sudo "$EXTRA_USER"
diff --git a/vm/setup_misc_packages.sh b/vm/setup_misc_packages.sh
new file mode 100755
index 0000000..691b2be
--- /dev/null
+++ b/vm/setup_misc_packages.sh
@@ -0,0 +1,17 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+export DEBIAN_FRONTEND=noninteractive
+
+readonly PKGS=(
+ colordiff
+ tmux
+ vim
+)
+
+# Install all the packages that we want.
+for pkg in "${PKGS[@]}"; do
+ apt-get install -y -f "$pkg"
+done
diff --git a/vm/setup_scouting.sh b/vm/setup_scouting.sh
new file mode 100755
index 0000000..3360ff5
--- /dev/null
+++ b/vm/setup_scouting.sh
@@ -0,0 +1,17 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+export DEBIAN_FRONTEND=noninteractive
+
+readonly PKGS=(
+ python3
+ python3-flask
+)
+
+# Install all the packages that we need/want.
+apt-get update
+for pkg in "${PKGS[@]}"; do
+ apt-get install -y -f --force-yes "$pkg"
+done
diff --git a/vm/setup_vbox_guest_additions.sh b/vm/setup_vbox_guest_additions.sh
new file mode 100755
index 0000000..c9f4b09
--- /dev/null
+++ b/vm/setup_vbox_guest_additions.sh
@@ -0,0 +1,15 @@
+#!/usr/bin/env bash
+
+set -e
+set -u
+
+export DEBIAN_FRONTEND=noninteractive
+
+# Install the kernel sources before the guest additions to guarantee that
+# we can compile the kernel module.
+apt-get install -q -y linux-headers-amd64
+
+# Now we can install the guest additions.
+apt-get install -q -y \
+ virtualbox-guest-dkms \
+ virtualbox-guest-x11
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
index 84e73a5..3802cb3 100644
--- a/y2014/control_loops/python/BUILD
+++ b/y2014/control_loops/python/BUILD
@@ -62,3 +62,15 @@
'//frc971/control_loops/python:controls',
]
)
+
+py_binary(
+ name = 'extended_lqr',
+ srcs = [
+ 'extended_lqr.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index dae2dd2..e4c7122 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -4,7 +4,6 @@
from frc971.control_loops.python import controls
import numpy
import sys
-import argparse
from matplotlib import pylab
import gflags
diff --git a/y2014/control_loops/python/extended_lqr.py b/y2014/control_loops/python/extended_lqr.py
new file mode 100755
index 0000000..095a43a
--- /dev/null
+++ b/y2014/control_loops/python/extended_lqr.py
@@ -0,0 +1,437 @@
+#!/usr/bin/python
+
+# This is an initial, hacky implementation of the extended LQR paper. It's just a proof of concept,
+# so don't trust it too much.
+
+import numpy
+import scipy.optimize
+from matplotlib import pylab
+import sys
+
+from frc971.control_loops.python import controls
+
+l = 100
+width = 0.2
+dt = 0.05
+num_states = 3
+num_inputs = 2
+x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
+
+def dynamics(X, U):
+ """Calculates the dynamics for a 2 wheeled robot.
+
+ Args:
+ X, numpy.matrix(3, 1), The state. [x, y, theta]
+ U, numpy.matrix(2, 1), The input. [left velocity, right velocity]
+
+ Returns:
+ numpy.matrix(3, 1), The derivative of the dynamics.
+ """
+ #return numpy.matrix([[X[1, 0]],
+ # [X[2, 0]],
+ # [U[0, 0]]])
+ return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
+ [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
+ [(U[1, 0] - U[0, 0]) / width]])
+
+def RungeKutta(f, x, dt):
+ """4th order RungeKutta integration of F starting at X."""
+ a = f(x)
+ b = f(x + dt / 2.0 * a)
+ c = f(x + dt / 2.0 * b)
+ d = f(x + dt * c)
+ return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
+def discrete_dynamics(X, U):
+ return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+
+def inverse_discrete_dynamics(X, U):
+ return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+
+def numerical_jacobian_x(fn, X, U, epsilon=1e-4):
+ """Numerically estimates the jacobian around X, U in X.
+
+ Args:
+ fn: A function of X, U.
+ X: numpy.matrix(num_states, 1), The state vector to take the jacobian
+ around.
+ U: numpy.matrix(num_inputs, 1), The input vector to take the jacobian
+ around.
+
+ Returns:
+ numpy.matrix(num_states, num_states), The jacobian of fn with X as the
+ variable.
+ """
+ num_states = X.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
+ # It's more expensive, but +- epsilon will be more reliable
+ for i in range(0, num_states):
+ dX_plus = X.copy()
+ dX_plus[i] += epsilon
+ dX_minus = X.copy()
+ dX_minus[i] -= epsilon
+ answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
+ return answer
+
+def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
+ """Numerically estimates the jacobian around X, U in U.
+
+ Args:
+ fn: A function of X, U.
+ X: numpy.matrix(num_states, 1), The state vector to take the jacobian
+ around.
+ U: numpy.matrix(num_inputs, 1), The input vector to take the jacobian
+ around.
+
+ Returns:
+ numpy.matrix(num_states, num_inputs), The jacobian of fn with U as the
+ variable.
+ """
+ num_states = X.shape[0]
+ num_inputs = U.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
+ for i in range(0, num_inputs):
+ dU_plus = U.copy()
+ dU_plus[i] += epsilon
+ dU_minus = U.copy()
+ dU_minus[i] -= epsilon
+ answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
+ return answer
+
+def numerical_jacobian_x_x(fn, X, U):
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+
+def numerical_jacobian_x_u(fn, X, U):
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+
+def numerical_jacobian_u_x(fn, X, U):
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+
+def numerical_jacobian_u_u(fn, X, U):
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+
+# Simple implementation for a quadratic cost function.
+class CostFunction:
+ def __init__(self):
+ self.num_states = num_states
+ self.num_inputs = num_inputs
+ self.dt = dt
+ self.Q = numpy.matrix([[0.1, 0, 0],
+ [0, 0.6, 0],
+ [0, 0, 0.1]]) / dt / dt
+ self.R = numpy.matrix([[0.40, 0],
+ [0, 0.40]]) / dt / dt
+
+ def estimate_Q_final(self, X_hat):
+ """Returns the quadraticized final Q around X_hat.
+
+ This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
+
+ Args:
+ X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
+
+ Result:
+ numpy.matrix(self.num_states, self.num_states)
+ """
+ zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+ return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+
+ def estimate_partial_cost_partial_x_final(self, X_hat):
+ """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+
+ Args:
+ X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
+
+ Result:
+ numpy.matrix(self.num_states, 1)
+ """
+ return numerical_jacobian_x(self.final_cost, X_hat, numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+
+ def estimate_q_final(self, X_hat):
+ """Returns q evaluated at X_hat for the final cost function."""
+ return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
+
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
+
+ Args:
+ X: numpy.matrix(self.num_states, 1)
+ U: numpy.matrix(self.num_inputs, 1), ignored
+
+ Returns:
+ numpy.matrix(1, 1), The quadratic cost of being at X
+ """
+ return X.T * self.Q * X * 1000
+
+ def cost(self, X, U):
+ """Computes the incremental cost given a position and U.
+
+ Args:
+ X: numpy.matrix(self.num_states, 1)
+ U: numpy.matrix(self.num_inputs, 1)
+
+ Returns:
+ numpy.matrix(1, 1), The quadratic cost of evaluating U.
+ """
+ return U.T * self.R * U + X.T * self.Q * X
+
+cost_fn_obj = CostFunction()
+
+S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
+
+L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
+L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
+
+S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
+
+
+last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+
+for a in range(15):
+ x_hat = x_hat_initial
+ u_t = L_t[0] * x_hat + l_t[0]
+ S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
+ s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+ s_scalar_bar_t[0] = numpy.matrix([[0]])
+
+ last_x_hat_t[0] = x_hat_initial
+
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
+
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial, u_t).T - Q_t * x_hat_initial - P_t.T * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial, u_t).T - P_t * x_hat_initial - R_t * u_t
+
+ q_scalar_t = cost_fn_obj.cost(x_hat_initial, u_t) - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T * (P_t * x_hat_initial + R_t * u_t)) - x_hat_initial.T * q_t - u_t.T * r_t
+
+ start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
+ start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
+ x_hat_next = discrete_dynamics(x_hat_initial, u_t)
+ start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+
+ B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
+ B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
+ B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
+
+ B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
+ B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
+ B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
+
+ L_bar_t[1] = B_svd_inv
+ l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
+
+ S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
+
+ TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
+ Totals_1 = start_B_t.T * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
+ Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
+
+ optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
+ optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
+
+ S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(S_bar_t[1])
+ S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
+ S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
+ for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
+ if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
+ S_bar_1_eigh_eigenvalues_stiff[i] = max(S_bar_1_eigh_eigenvalues_stiff) * 1.0
+
+ #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
+ #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
+
+ #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
+ #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
+
+ S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
+
+ print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+ print 'Min x_hat', optimal_x_1
+ s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
+ s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T * (S_bar_stiff + S_t[1]) * optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
+
+ print 'optimal_u_1', optimal_u_1
+ print 'TotalS_1', TotalS_1
+ print 'Totals_1', Totals_1
+ print 'Totals_scalar_1', Totals_scalar_1
+ print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
+ print 'overall cost 0', 0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial) + x_hat_initial.T * s_t[0] + s_scalar_t[0]
+
+ print 't forward 0'
+ print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
+ print 'x_hat[%2d]: %s' % (0, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
+ print 'u[%2d]: %s' % (0, u_t.T)
+ print ('L[ 0]: %s' % (L_t[0],)).replace('\n', '\n ')
+ print ('l[ 0]: %s' % (l_t[0],)).replace('\n', '\n ')
+
+ print ('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
+ print ('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
+ print ('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
+
+ # TODO(austin): optimal_x_1 is x_hat
+ x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
+ print 'new xhat', x_hat
+
+ S_bar_t[1] = S_bar_stiff
+
+ last_x_hat_t[1] = x_hat
+
+ for t in range(1, l):
+ print 't forward', t
+ u_t = L_t[t] * x_hat + l_t[t]
+
+ x_hat_next = discrete_dynamics(x_hat, u_t)
+ A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next, u_t)
+ B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next, u_t)
+ c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
+ print ('L[%2d]: %s' % (t, L_t[t],)).replace('\n', '\n ')
+ print ('l[%2d]: %s' % (t, l_t[t],)).replace('\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
+
+ print ('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace('\n', '\n ')
+ print ('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace('\n', '\n ')
+ print ('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace('\n', '\n ')
+
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
+
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat, u_t).T - Q_t * x_hat - P_t.T * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat, u_t).T - P_t * x_hat - R_t * u_t
+
+ q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
+
+ C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
+ D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
+ E_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
+ d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
+ e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
+
+ L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+ l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+
+ S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
+ s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
+ s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
+
+ x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]), (s_t[t + 1] + s_bar_t[t + 1]))
+
+ S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
+ s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
+ x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
+
+ for t in reversed(range(l)):
+ print 't backward', t
+ # TODO(austin): I don't think we can use L_t like this here.
+ # I think we are off by 1 somewhere...
+ u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
+
+ x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
+ print ('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace('\n', '\n ')
+ print ('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace('\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
+ # Now compute the linearized A, B, and C
+ # Start by doing it numerically, and then optimize.
+ A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
+ B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
+ c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+
+ print ('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
+ print ('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
+ print ('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
+
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
+ P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
+
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev, u_t).T - Q_t * x_hat_prev - P_T_t * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev, u_t).T - P_t * x_hat_prev - R_t * u_t
+
+ q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T * (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
+
+ C_t = P_t + B_t.T * S_t[t + 1] * A_t
+ D_t = Q_t + A_t.T * S_t[t + 1] * A_t
+ E_t = R_t + B_t.T * S_t[t + 1] * B_t
+ d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
+ e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
+ L_t[t] = -numpy.linalg.inv(E_t) * C_t
+ l_t[t] = -numpy.linalg.inv(E_t) * e_t
+ s_t[t] = d_t + C_t.T * l_t[t]
+ S_t[t] = D_t + C_t.T * L_t[t]
+ s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[t + 1] + s_scalar_t[t + 1]
+
+ x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]), (s_t[t] + s_bar_t[t]))
+ if t == 0:
+ last_x_hat_t[t] = x_hat_initial
+ else:
+ last_x_hat_t[t] = x_hat
+
+ x_hat_t = [x_hat_initial]
+
+ pylab.figure('states %d' % a)
+ pylab.ion()
+ for dim in range(num_states):
+ pylab.plot(numpy.arange(len(last_x_hat_t)),
+ [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t], marker='o', label='Xhat[%d]'%dim)
+ pylab.legend()
+ pylab.draw()
+
+ pylab.figure('xy %d' % a)
+ pylab.ion()
+ pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
+ [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t], marker='o', label='trajectory')
+ pylab.legend()
+ pylab.draw()
+
+final_u_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
+cost_to_go = []
+cost_to_come = []
+cost = []
+for t in range(l):
+ cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
+ cost_to_come.append((0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
+ cost.append(cost_to_go[-1] + cost_to_come[-1])
+ final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
+
+for t in range(l):
+ A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
+ B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
+ c_t = discrete_dynamics(last_x_hat_t[t], final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
+ print("Infeasability at", t, "is", ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) - last_x_hat_t[t + 1]).T)
+
+pylab.figure('u')
+samples = numpy.arange(len(final_u_t))
+for i in range(num_inputs):
+ pylab.plot(samples, [u[i, 0] for u in final_u_t], label='u[%d]' % i, marker='o')
+ pylab.legend()
+
+pylab.figure('cost')
+cost_samples = numpy.arange(len(cost))
+pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
+pylab.plot(cost_samples, cost_to_come, label='cost to come', marker='o')
+pylab.plot(cost_samples, cost, label='cost', marker='o')
+pylab.legend()
+
+pylab.ioff()
+pylab.show()
+
+sys.exit(1)
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index 029fcbb..548464d 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -4,7 +4,6 @@
from frc971.control_loops.python import controls
import numpy
import sys
-import argparse
from matplotlib import pylab
import gflags
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index eb6432d..7d4dea5 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -23,16 +23,22 @@
return default_value;
}
}
+
+enum ArmIndices { kShoulderIndex = 0, kWristIndex = 1 };
+
} // namespace
// Intake
Intake::Intake()
- : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>(
- ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop())),
+ : ProfiledSubsystem(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(::y2016::control_loops::superstructure::
+ MakeIntegralIntakeLoop()))),
estimator_(constants::GetValues().intake.zeroing),
profile_(::aos::controls::kLoopFrequency) {
Y_.setZero();
- unprofiled_goal_.setZero();
offset_.setZero();
AdjustProfile(0.0, 0.0);
}
@@ -66,9 +72,9 @@
}
}
- if (!zeroed_ && estimator_.zeroed()) {
+ if (!zeroed(0) && estimator_.zeroed()) {
UpdateIntakeOffset(estimator_.offset());
- zeroed_ = true;
+ set_zeroed(0, true);
}
Y_ << position.encoder;
@@ -148,10 +154,8 @@
UseUnlessZero(max_angular_acceleration, 10.0));
}
-void Intake::Reset() {
+void Intake::DoReset() {
estimator_.Reset();
- initialized_ = false;
- zeroed_ = false;
}
EstimatorState Intake::IntakeEstimatorState() {
@@ -162,15 +166,14 @@
}
Arm::Arm()
- : loop_(new ArmControlLoop(
- ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+ : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+ ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
shoulder_profile_(::aos::controls::kLoopFrequency),
wrist_profile_(::aos::controls::kLoopFrequency),
shoulder_estimator_(constants::GetValues().shoulder.zeroing),
wrist_estimator_(constants::GetValues().wrist.zeroing) {
Y_.setZero();
offset_.setZero();
- unprofiled_goal_.setZero();
AdjustProfile(0.0, 0.0, 0.0, 0.0);
}
@@ -238,13 +241,13 @@
}
}
- if (!shoulder_zeroed_ && shoulder_estimator_.zeroed()) {
+ if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
UpdateShoulderOffset(shoulder_estimator_.offset());
- shoulder_zeroed_ = true;
+ set_zeroed(kShoulderIndex, true);
}
- if (!wrist_zeroed_ && wrist_estimator_.zeroed()) {
+ if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
UpdateWristOffset(wrist_estimator_.offset());
- wrist_zeroed_ = true;
+ set_zeroed(kWristIndex, true);
}
{
@@ -373,12 +376,9 @@
loop_->set_max_voltage(1, wrist_max_voltage);
}
-void Arm::Reset() {
+void Arm::DoReset() {
shoulder_estimator_.Reset();
wrist_estimator_.Reset();
- initialized_ = false;
- shoulder_zeroed_ = false;
- wrist_zeroed_ = false;
}
EstimatorState Arm::ShoulderEstimatorState() {
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 2934011..adb40e0 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -19,93 +19,6 @@
class SuperstructureTest_DisabledGoalTest_Test;
} // namespace testing
-class Intake {
- public:
- Intake();
- // Returns whether the estimators have been initialized and zeroed.
- bool initialized() const { return initialized_; }
- bool zeroed() const { return zeroed_; }
- // Returns whether an error has occured
- bool error() const { return estimator_.error(); }
-
- // Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position);
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
- // Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(double voltage);
-
- // Forces the current goal to the provided goal, bypassing the profiler.
- void ForceGoal(double goal);
- // Sets the unprofiled goal. The profiler will generate a profile to go to
- // this goal.
- void set_unprofiled_goal(double unprofiled_goal);
- // Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration);
-
- // Returns true if we have exceeded any hard limits.
- bool CheckHardLimits();
- // Resets the internal state.
- void Reset();
-
- // Returns the current internal estimator state for logging.
- ::frc971::EstimatorState IntakeEstimatorState();
-
- // Returns the requested intake voltage.
- double intake_voltage() const { return loop_->U(0, 0); }
-
- // Returns the current position.
- double angle() const { return Y_(0, 0); }
-
- // Returns the controller error.
- const StateFeedbackLoop<3, 1, 1> &controller() const { return *loop_; }
-
- // Returns the filtered goal.
- const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
- double goal(int row, int col) const { return loop_->R(row, col); }
-
- // Returns the unprofiled goal.
- const Eigen::Matrix<double, 3, 1> &unprofiled_goal() const {
- return unprofiled_goal_;
- }
- double unprofiled_goal(int row, int col) const {
- return unprofiled_goal_(row, col);
- }
-
- // Returns the current state estimate.
- const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
- // For testing:
- // Triggers an estimator error.
- void TriggerEstimatorError() { estimator_.TriggerError(); }
-
- private:
- // Limits the provided goal to the soft limits. Prints "name" when it fails
- // to aid debugging.
- void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
-
- void UpdateIntakeOffset(double offset);
-
- ::std::unique_ptr<
- ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>> loop_;
-
- ::frc971::zeroing::ZeroingEstimator estimator_;
- aos::util::TrapezoidProfile profile_;
-
- // Current measurement.
- Eigen::Matrix<double, 1, 1> Y_;
- // Current offset. Y_ = offset_ + raw_sensor;
- Eigen::Matrix<double, 1, 1> offset_;
-
- // The goal that the profile tries to reach.
- Eigen::Matrix<double, 3, 1> unprofiled_goal_;
-
- bool initialized_ = false;
- bool zeroed_ = false;
-};
-
class ArmControlLoop
: public ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2> {
public:
@@ -183,14 +96,145 @@
}
};
-class Arm {
+template <int number_of_states, int number_of_axes>
+class ProfiledSubsystem {
public:
- Arm();
+ ProfiledSubsystem(
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>>
+ loop)
+ : loop_(::std::move(loop)) {
+ zeroed_.fill(false);
+ unprofiled_goal_.setZero();
+ }
+
+ void Reset() {
+ zeroed_.fill(false);
+ DoReset();
+ }
+
+ // Returns the controller.
+ const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes>
+ &controller() const {
+ return *loop_;
+ }
+
+ int controller_index() const { return loop_->controller_index(); }
+
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
- bool zeroed() const { return shoulder_zeroed_ && wrist_zeroed_; }
- bool shoulder_zeroed() const { return shoulder_zeroed_; }
- bool wrist_zeroed() const { return wrist_zeroed_; }
+
+ bool zeroed() const {
+ for (int i = 0; i < number_of_axes; ++i) {
+ if (!zeroed_[i]) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool zeroed(int index) const { return zeroed_[index]; };
+
+ // Returns the filtered goal.
+ const Eigen::Matrix<double, number_of_states, 1> &goal() const {
+ return loop_->R();
+ }
+ double goal(int row, int col) const { return loop_->R(row, col); }
+
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
+ // Returns the current state estimate.
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return loop_->X_hat();
+ }
+ double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+
+ protected:
+ void set_zeroed(int index, bool val) { zeroed_[index] = val; }
+
+ // TODO(austin): It's a bold assumption to assume that we will have the same
+ // number of sensors as axes. So far, that's been fine.
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>>
+ loop_;
+
+ // The goal that the profile tries to reach.
+ Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
+
+ bool initialized_ = false;
+
+ private:
+ ::std::array<bool, number_of_axes> zeroed_;
+
+ virtual void DoReset() = 0;
+};
+
+class Intake : public ProfiledSubsystem<3, 1> {
+ public:
+ Intake();
+ // Returns whether an error has occured
+ bool error() const { return estimator_.error(); }
+
+ // Updates our estimator with the latest position.
+ void Correct(::frc971::PotAndIndexPosition position);
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+ // Sets the maximum voltage that will be commanded by the loop.
+ void set_max_voltage(double voltage);
+
+ // Forces the current goal to the provided goal, bypassing the profiler.
+ void ForceGoal(double goal);
+ // Sets the unprofiled goal. The profiler will generate a profile to go to
+ // this goal.
+ void set_unprofiled_goal(double unprofiled_goal);
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration);
+
+ // Returns true if we have exceeded any hard limits.
+ bool CheckHardLimits();
+
+ // Returns the current internal estimator state for logging.
+ ::frc971::EstimatorState IntakeEstimatorState();
+
+ // Returns the requested intake voltage.
+ double intake_voltage() const { return loop_->U(0, 0); }
+
+ // Returns the current position.
+ double angle() const { return Y_(0, 0); }
+
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimator_.TriggerError(); }
+
+ private:
+ // Limits the provided goal to the soft limits. Prints "name" when it fails
+ // to aid debugging.
+ void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
+
+ // Resets the internal state.
+ void DoReset() override;
+
+ void UpdateIntakeOffset(double offset);
+
+ ::frc971::zeroing::ZeroingEstimator estimator_;
+ aos::util::TrapezoidProfile profile_;
+
+ // Current measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // Current offset. Y_ = offset_ + raw_sensor;
+ Eigen::Matrix<double, 1, 1> offset_;
+};
+
+class Arm : public ProfiledSubsystem<6, 2> {
+ public:
+ Arm();
// Returns whether an error has occured
bool error() const {
return shoulder_estimator_.error() || wrist_estimator_.error();
@@ -207,8 +251,6 @@
void set_unprofiled_goal(double unprofiled_goal_shoulder,
double unprofiled_goal_wrist);
- int controller_index() const { return loop_->controller_index(); }
-
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
@@ -226,8 +268,6 @@
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // Resets the internal state.
- void Reset();
// Returns the current internal estimator state for logging.
::frc971::EstimatorState ShoulderEstimatorState();
@@ -241,30 +281,14 @@
double shoulder_angle() const { return Y_(0, 0); }
double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
- // Returns the controller error.
- const StateFeedbackLoop<6, 2, 2> &controller() const { return *loop_; }
-
- // Returns the unprofiled goal.
- const Eigen::Matrix<double, 6, 1> &unprofiled_goal() const {
- return unprofiled_goal_;
- }
- double unprofiled_goal(int row, int col) const {
- return unprofiled_goal_(row, col);
- }
-
- // Returns the filtered goal.
- const Eigen::Matrix<double, 6, 1> &goal() const { return loop_->R(); }
- double goal(int row, int col) const { return loop_->R(row, col); }
-
- // Returns the current state estimate.
- const Eigen::Matrix<double, 6, 1> &X_hat() const { return loop_->X_hat(); }
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
// For testing:
// Triggers an estimator error.
void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
private:
+ // Resets the internal state.
+ void DoReset() override;
+
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
@@ -274,8 +298,6 @@
void UpdateShoulderOffset(double offset);
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
- ::std::unique_ptr<
- ArmControlLoop> loop_;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
@@ -284,13 +306,6 @@
Eigen::Matrix<double, 2, 1> Y_;
// Current offset. Y_ = offset_ + raw_sensor;
Eigen::Matrix<double, 2, 1> offset_;
-
- // The goal that the profile tries to reach.
- Eigen::Matrix<double, 6, 1> unprofiled_goal_;
-
- bool initialized_ = false;
- bool shoulder_zeroed_ = false;
- bool wrist_zeroed_ = false;
};
} // namespace superstructure
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index b3d86af..b50ccca 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -9,6 +9,11 @@
#include "y2016_bot3/queues/ball_detector.q.h"
namespace y2016_bot3 {
+namespace constants {
+constexpr double IntakeZero::pot_offset;
+constexpr ::frc971::constants::ZeroingConstants IntakeZero::zeroing;
+} // namespace constants
+
namespace control_loops {
namespace intake {
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index 18653b3..fdde74b 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -38,10 +38,10 @@
2.77};
struct IntakeZero {
- double pot_offset = 5.462409 + 0.333162;
- ::frc971::constants::ZeroingConstants zeroing{kZeroingSampleSize,
- kIntakeEncoderIndexDifference,
- 0.148604 - 0.291240, 0.3};
+ static constexpr double pot_offset = 5.462409 + 0.333162;
+ static constexpr ::frc971::constants::ZeroingConstants zeroing{
+ kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.148604 - 0.291240,
+ 0.3};
};
} // namespace constants
namespace control_loops {
diff --git a/y2016_bot3/control_loops/intake/intake_lib_test.cc b/y2016_bot3/control_loops/intake/intake_lib_test.cc
index fa82802..48b6946 100644
--- a/y2016_bot3/control_loops/intake/intake_lib_test.cc
+++ b/y2016_bot3/control_loops/intake/intake_lib_test.cc
@@ -2,18 +2,17 @@
#include <unistd.h>
+#include <chrono>
#include <memory>
#include "gtest/gtest.h"
#include "aos/common/queue.h"
#include "aos/common/controls/control_loop_test.h"
#include "aos/common/commonmath.h"
-#include "aos/common/time.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "y2016_bot3/control_loops/intake/intake.q.h"
#include "y2016_bot3/control_loops/intake/intake_plant.h"
-using ::aos::time::Time;
using ::frc971::control_loops::PositionSensorSimulator;
namespace y2016_bot3 {
@@ -21,6 +20,9 @@
namespace intake {
namespace testing {
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
class IntakePlant : public StateFeedbackPlant<2, 1, 1> {
public:
explicit IntakePlant(StateFeedbackPlant<2, 1, 1> &&other)
@@ -63,7 +65,9 @@
intake_plant_->mutable_X(0, 0) = start_pos;
intake_plant_->mutable_X(1, 0) = 0.0;
- pot_encoder_intake_.Initialize(start_pos, kNoiseScalar);
+ pot_encoder_intake_.Initialize(
+ start_pos, kNoiseScalar,
+ constants::IntakeZero::zeroing.measured_index_position);
}
// Sends a queue message with the position.
@@ -162,13 +166,15 @@
}
// Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const Time &run_for, bool enabled = true) {
- const auto start_time = Time::Now();
- while (Time::Now() < start_time + run_for) {
- const auto loop_start_time = Time::Now();
+ void RunForTime(monotonic_clock::duration run_for, bool enabled = true) {
+ const auto start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < start_time + run_for) {
+ const auto loop_start_time = monotonic_clock::now();
double begin_intake_velocity = intake_plant_.intake_angular_velocity();
RunIteration(enabled);
- const double loop_time = (Time::Now() - loop_start_time).ToSeconds();
+ const double loop_time =
+ chrono::duration_cast<chrono::duration<double>>(
+ (monotonic_clock::now() - loop_start_time)).count();
const double intake_acceleration =
(intake_plant_.intake_angular_velocity() - begin_intake_velocity) /
loop_time;
@@ -188,8 +194,6 @@
}
void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
-
-
// 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.
@@ -215,7 +219,7 @@
.Send());
// TODO(phil): Send a goal of some sort.
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -229,7 +233,7 @@
.Send());
// Give it a lot of time to get there.
- RunForTime(Time::InSeconds(8));
+ RunForTime(chrono::seconds(8));
VerifyNearGoal();
}
@@ -243,14 +247,13 @@
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
intake_queue_.status.FetchLatest();
EXPECT_NEAR(y2016_bot3::constants::kIntakeRange.upper,
intake_queue_.status->intake.angle, 0.001);
-
// Set some ridiculous goals to test lower limits.
ASSERT_TRUE(intake_queue_.goal.MakeWithBuilder()
.angle_intake(-M_PI * 10)
@@ -258,7 +261,7 @@
.max_angular_acceleration_intake(20)
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
intake_queue_.status.FetchLatest();
@@ -274,14 +277,14 @@
.max_angular_acceleration_intake(20)
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(IntakeTest, ZeroNoGoal) {
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
EXPECT_EQ(Intake::RUNNING, intake_.state());
}
@@ -294,7 +297,7 @@
.angle_intake(y2016_bot3::constants::kIntakeRange.upper)
.Send());
- RunForTime(Time::InSeconds(15));
+ RunForTime(chrono::seconds(15));
VerifyNearGoal();
}
@@ -306,7 +309,7 @@
.angle_intake(y2016_bot3::constants::kIntakeRange.lower)
.Send());
- RunForTime(Time::InSeconds(15));
+ RunForTime(chrono::seconds(15));
VerifyNearGoal();
}
@@ -318,14 +321,14 @@
ASSERT_TRUE(intake_queue_.goal.MakeWithBuilder()
.angle_intake(y2016_bot3::constants::kIntakeRange.lower + 0.3)
.Send());
- RunForTime(Time::InSeconds(15));
+ RunForTime(chrono::seconds(15));
EXPECT_EQ(Intake::RUNNING, intake_.state());
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(Time::InMS(100));
+ RunForTime(chrono::milliseconds(100));
EXPECT_NE(Intake::RUNNING, intake_.state());
- RunForTime(Time::InMS(10000));
+ RunForTime(chrono::milliseconds(10000));
EXPECT_EQ(Intake::RUNNING, intake_.state());
VerifyNearGoal();
}
@@ -337,11 +340,11 @@
.angle_intake(y2016_bot3::constants::kIntakeRange.lower + 0.03)
.Send());
- RunForTime(Time::InMS(100), false);
+ RunForTime(chrono::milliseconds(100), false);
EXPECT_EQ(0.0, intake_.intake_.goal(0, 0));
// Now make sure they move correctly
- RunForTime(Time::InMS(4000), true);
+ RunForTime(chrono::milliseconds(4000), true);
EXPECT_NE(0.0, intake_.intake_.goal(0, 0));
}
@@ -357,8 +360,8 @@
.Send());
// Expected states to cycle through and check in order.
- Intake::State kExpectedStateOrder[] = {
- Intake::DISABLED_INITIALIZED, Intake::ZERO_LOWER_INTAKE};
+ Intake::State kExpectedStateOrder[] = {Intake::DISABLED_INITIALIZED,
+ Intake::ZERO_LOWER_INTAKE};
// Cycle through until intake_ is initialized in intake.cc
while (intake_.state() < Intake::DISABLED_INITIALIZED) {
@@ -390,7 +393,7 @@
EXPECT_EQ(Intake::DISABLED_INITIALIZED, intake_.state());
}
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
EXPECT_EQ(Intake::RUNNING, intake_.state());
}
@@ -406,8 +409,8 @@
.Send());
// Expected states to cycle through and check in order.
- Intake::State kExpectedStateOrder[] = {
- Intake::DISABLED_INITIALIZED, Intake::ZERO_LIFT_INTAKE};
+ Intake::State kExpectedStateOrder[] = {Intake::DISABLED_INITIALIZED,
+ Intake::ZERO_LIFT_INTAKE};
// Cycle through until intake_ is initialized in intake.cc
while (intake_.state() < Intake::DISABLED_INITIALIZED) {
@@ -439,7 +442,7 @@
EXPECT_EQ(Intake::DISABLED_INITIALIZED, intake_.state());
}
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
EXPECT_EQ(Intake::RUNNING, intake_.state());
}
@@ -450,7 +453,7 @@
intake_plant_.set_power_error(1.0);
intake_queue_.goal.MakeWithBuilder().angle_intake(0.0).Send();
- RunForTime(Time::InSeconds(8));
+ RunForTime(chrono::seconds(8));
VerifyNearGoal();
}
@@ -464,15 +467,15 @@
intake_queue_.goal.MakeWithBuilder().angle_intake(0.0).Send();
// Run disabled for 2 seconds
- RunForTime(Time::InSeconds(2), false);
+ RunForTime(chrono::seconds(2), false);
EXPECT_EQ(Intake::DISABLED_INITIALIZED, intake_.state());
intake_plant_.set_power_error(1.0);
- RunForTime(Time::InSeconds(1), false);
+ RunForTime(chrono::seconds(1), false);
EXPECT_EQ(Intake::SLOW_RUNNING, intake_.state());
- RunForTime(Time::InSeconds(2), true);
+ RunForTime(chrono::seconds(2), true);
VerifyNearGoal();
}
@@ -495,7 +498,7 @@
.max_angular_acceleration_intake(20)
.Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
EXPECT_EQ(Intake::RUNNING, intake_.state());
VerifyNearGoal();
@@ -507,7 +510,7 @@
.Send());
set_peak_intake_acceleration(1.20);
- RunForTime(Time::InSeconds(4));
+ RunForTime(chrono::seconds(4));
VerifyNearGoal();
}
@@ -521,7 +524,7 @@
.max_angular_acceleration_intake(20)
.Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
EXPECT_EQ(Intake::RUNNING, intake_.state());
VerifyNearGoal();
@@ -533,7 +536,7 @@
.Send());
set_peak_intake_velocity(4.65);
- RunForTime(Time::InSeconds(4));
+ RunForTime(chrono::seconds(4));
VerifyNearGoal();
}
diff --git a/y2016_bot3/control_loops/python/drivetrain.py b/y2016_bot3/control_loops/python/drivetrain.py
index 7f470d9..b58a32f 100755
--- a/y2016_bot3/control_loops/python/drivetrain.py
+++ b/y2016_bot3/control_loops/python/drivetrain.py
@@ -4,7 +4,6 @@
from frc971.control_loops.python import controls
import numpy
import sys
-import argparse
from matplotlib import pylab
import gflags
diff --git a/y2016_bot4/BUILD b/y2016_bot4/BUILD
new file mode 100644
index 0000000..c003d4a
--- /dev/null
+++ b/y2016_bot4/BUILD
@@ -0,0 +1,47 @@
+load('/aos/downloader/downloader', 'aos_downloader')
+
+cc_binary(
+ name = 'joystick_reader',
+ srcs = [
+ 'joystick_reader.cc',
+ ],
+ deps = [
+ '//aos/common/actions:action_lib',
+ '//aos/common/logging',
+ '//aos/common/util:log_interval',
+ '//aos/common:time',
+ '//aos/input:joystick_input',
+ '//aos/linux_code:init',
+ '//frc971/autonomous:auto_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/queues:gyro',
+ '//y2016_bot4/control_loops/drivetrain:drivetrain_base',
+ ],
+)
+
+aos_downloader(
+ name = 'download',
+ start_srcs = [
+ ':joystick_reader',
+ '//aos:prime_start_binaries',
+ '//y2016_bot4/control_loops/drivetrain:drivetrain',
+ '//y2016_bot4/wpilib:wpilib_interface',
+ ],
+ srcs = [
+ '//aos:prime_binaries',
+ ],
+)
+
+aos_downloader(
+ name = 'download_stripped',
+ start_srcs = [
+ ':joystick_reader.stripped',
+ '//aos:prime_start_binaries_stripped',
+ '//y2016_bot4/control_loops/drivetrain:drivetrain.stripped',
+ '//y2016_bot4/wpilib:wpilib_interface.stripped',
+ ],
+ srcs = [
+ '//aos:prime_binaries_stripped',
+ ],
+ default_target = 'roboRIO-6971-frc.local',
+)
diff --git a/y2016_bot4/control_loops/drivetrain/BUILD b/y2016_bot4/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..c046d5d
--- /dev/null
+++ b/y2016_bot4/control_loops/drivetrain/BUILD
@@ -0,0 +1,77 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2016_bot4/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2016_bot4/control_loops/python:drivetrain',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+)
+
+genrule(
+ name = 'genrule_polydrivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2016_bot4/control_loops/python:polydrivetrain) $(OUTS)',
+ tools = [
+ '//y2016_bot4/control_loops/python:polydrivetrain',
+ ],
+ outs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.h',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain_plants',
+ srcs = [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_base',
+ srcs = [
+ 'drivetrain_base.cc',
+ ],
+ hdrs = [
+ 'drivetrain_base.h',
+ ],
+ deps = [
+ ':polydrivetrain_plants',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
+ '//frc971:shifter_hall_effect',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain_main.cc',
+ ],
+ deps = [
+ ':drivetrain_base',
+ '//aos/linux_code:init',
+ '//frc971/control_loops/drivetrain:drivetrain_lib',
+ ],
+)
diff --git a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..967e8df
--- /dev/null
+++ b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,48 @@
+#include "y2016_bot4/control_loops/drivetrain/drivetrain_base.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2016_bot4/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016_bot4/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2016_bot4/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace y2016_bot4 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig &GetDrivetrainConfig() {
+ static DrivetrainConfig kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+
+ ::y2016_bot4::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2016_bot4::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016_bot4::control_loops::drivetrain::MakeKFDrivetrainLoop,
+
+ drivetrain::kDt,
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
+
+ drivetrain::kHighGearRatio,
+ drivetrain::kHighGearRatio,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true,
+ 0.0,
+ 0.4,
+ 1.0};
+
+ return kDrivetrainConfig;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2016_bot4
diff --git a/y2016_bot4/control_loops/drivetrain/drivetrain_base.h b/y2016_bot4/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..8557c0d
--- /dev/null
+++ b/y2016_bot4/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,21 @@
+#ifndef Y2016_BOT4_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2016_BOT4_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2016_bot4 {
+namespace constants {
+// The ratio from the encoder shaft to the drivetrain wheels.
+static constexpr double kDrivetrainEncoderRatio = 1.0;
+
+} // namespace constants
+namespace control_loops {
+namespace drivetrain {
+const ::frc971::control_loops::drivetrain::DrivetrainConfig &
+GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2016_bot4
+
+#endif // Y2016_BOT4_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2016_bot4/control_loops/drivetrain/drivetrain_main.cc b/y2016_bot4/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..8d6bcff
--- /dev/null
+++ b/y2016_bot4/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,15 @@
+#include "aos/linux_code/init.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2016_bot4/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main() {
+ ::aos::Init();
+ DrivetrainLoop drivetrain(
+ ::y2016_bot4::control_loops::drivetrain::GetDrivetrainConfig());
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2016_bot4/control_loops/python/BUILD b/y2016_bot4/control_loops/python/BUILD
new file mode 100644
index 0000000..50f6038
--- /dev/null
+++ b/y2016_bot4/control_loops/python/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//y2016_bot4:__subpackages__'])
+
+py_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_binary(
+ name = 'polydrivetrain',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_library(
+ name = 'polydrivetrain_lib',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
diff --git a/y2016_bot4/control_loops/python/drivetrain.py b/y2016_bot4/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..fbe9b2a
--- /dev/null
+++ b/y2016_bot4/control_loops/python/drivetrain.py
@@ -0,0 +1,356 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+ super(Drivetrain, self).__init__(name)
+ # Number of motors per side
+ self.num_motors = 2
+ # Stall Torque in N m
+ self.stall_torque = 2.42 * self.num_motors * 0.60
+ # Stall Current in Amps
+ self.stall_current = 133.0 * self.num_motors
+ # Free Speed in RPM. Used number from last year.
+ self.free_speed = 5500.0
+ # Free Current in Amps
+ self.free_current = 4.7 * self.num_motors
+ # Moment of inertia of the drivetrain in kg m^2
+ self.J = 2.0
+ # Mass of the robot, in kg.
+ self.m = 24
+ # Radius of the robot, in meters (requires tuning by hand)
+ self.rb = 0.59055 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = 0.08255 / 2.0
+ # Resistance of the motor, divided by the number of motors.
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratios
+ self.G_low = 12.0 / 54.0
+ self.G_high = 12.0 / 54.0
+ if left_low:
+ self.Gl = self.G_low
+ else:
+ self.Gl = self.G_high
+ if right_low:
+ self.Gr = self.G_low
+ else:
+ self.Gr = self.G_high
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
+ self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+ self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[positionl], [velocityl], [positionr], velocityr]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tcl, 0, self.msn * self.tcr],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mpl, self.msn * self.mpr],
+ [0, 0],
+ [self.msn * self.mpl, self.msp * self.mpr]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ if left_low or right_low:
+ q_pos = 0.12
+ q_vel = 1.0
+ else:
+ q_pos = 0.14
+ q_vel = 0.95
+
+ # Tune the LQR controller
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
+
+ self.hlp = 0.3
+ self.llp = 0.4
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+class KFDrivetrain(Drivetrain):
+ def __init__(self, name="KFDrivetrain", left_low=True, right_low=True):
+ super(KFDrivetrain, self).__init__(name, left_low, right_low)
+
+ self.unaugmented_A_continuous = self.A_continuous
+ self.unaugmented_B_continuous = self.B_continuous
+
+ # The practical voltage applied to the wheels is
+ # V_left = U_left + left_voltage_error
+ #
+ # The states are
+ # [left position, left velocity, right position, right velocity,
+ # left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
+ self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+ self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+ self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
+ self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
+ self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+ self.A_continuous[0,6] = 1
+ self.A_continuous[2,6] = -1
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [0, -0.5 / self.rb, 0, 0.5 / self.rb, 0, 0, 0]])
+
+ self.D = numpy.matrix([[0, 0],
+ [0, 0],
+ [0, 0]])
+
+ q_pos = 0.05
+ q_vel = 1.00
+ q_voltage = 10.0
+ q_encoder_uncertainty = 2.00
+
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
+
+ r_pos = 0.0001
+ r_gyro = 0.000001
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
+ [0.0, (r_pos ** 2.0), 0.0],
+ [0.0, 0.0, (r_gyro ** 2.0)]])
+
+ # Solving for kf gains.
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.L = self.A * self.KalmanGain
+
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
+ self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+ self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+ self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+ self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+ self.InitializeState()
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), simulated_left)
+ pylab.plot(range(100), simulated_right)
+ pylab.suptitle('Acceleration Test')
+ pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ close_loop_left = []
+ close_loop_right = []
+ left_power = []
+ right_power = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, label='right power')
+ pylab.suptitle('Linear Move')
+ pylab.legend()
+ pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Angular Move')
+ pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Pivot')
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ drivetrain_low_low = Drivetrain(
+ name="DrivetrainLowLow", left_low=True, right_low=True)
+ drivetrain_low_high = Drivetrain(
+ name="DrivetrainLowHigh", left_low=True, right_low=False)
+ drivetrain_high_low = Drivetrain(
+ name="DrivetrainHighLow", left_low=False, right_low=True)
+ drivetrain_high_high = Drivetrain(
+ name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+ kf_drivetrain_low_low = KFDrivetrain(
+ name="KFDrivetrainLowLow", left_low=True, right_low=True)
+ kf_drivetrain_low_high = KFDrivetrain(
+ name="KFDrivetrainLowHigh", left_low=True, right_low=False)
+ kf_drivetrain_high_low = KFDrivetrain(
+ name="KFDrivetrainHighLow", left_low=False, right_low=True)
+ kf_drivetrain_high_high = KFDrivetrain(
+ name="KFDrivetrainHighHigh", left_low=False, right_low=False)
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2016_bot4', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces = namespaces)
+ dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ drivetrain_low_low.dt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
+ drivetrain_low_low.stall_torque))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
+ drivetrain_low_low.stall_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
+ drivetrain_low_low.free_speed))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
+ drivetrain_low_low.free_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
+ drivetrain_low_low.J))
+ dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
+ drivetrain_low_low.m))
+ dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
+ drivetrain_low_low.rb))
+ dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
+ drivetrain_low_low.r))
+ dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
+ drivetrain_low_low.resistance))
+ dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
+ drivetrain_low_low.Kv))
+ dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
+ drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+ drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
+ kf_drivetrain_high_low, kf_drivetrain_high_high],
+ namespaces = namespaces)
+ kf_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2016_bot4/control_loops/python/polydrivetrain.py b/y2016_bot4/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..6c70946
--- /dev/null
+++ b/y2016_bot4/control_loops/python/polydrivetrain.py
@@ -0,0 +1,501 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from frc971.control_loops.python import polytope
+from y2016_bot4.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python.cim import CIM
+from matplotlib import pylab
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+def CoerceGoal(region, K, w, R):
+ """Intersects a line with a region, and finds the closest point to R.
+
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
+
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
+
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
+def DoCoerceGoal(region, K, w, R):
+ if region.IsInside(R):
+ return (R, True)
+
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
+
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
+
+ t_poly = polytope.HPolytope(
+ region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
+
+ vertices = t_poly.Vertices()
+
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, False)
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+ right_low=right_low)
+ self.dt = 0.005
+ self.A_continuous = numpy.matrix(
+ [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
+ [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
+
+ self.B_continuous = numpy.matrix(
+ [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
+ [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
+ self.C = numpy.matrix(numpy.eye(2))
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+ self.PlaceControllerPoles([0.85, 0.85])
+ self.PlaceObserverPoles([0.02, 0.02])
+
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.resistance = self._drivetrain.resistance
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
+
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
+
+ def __init__(self):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+ # X is [lvel, rvel]
+ self.X = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]]),
+ numpy.matrix([[12],
+ [12],
+ [12],
+ [12]]))
+
+ self.U_max = numpy.matrix(
+ [[12.0],
+ [12.0]])
+ self.U_min = numpy.matrix(
+ [[-12.0000000000],
+ [-12.0000000000]])
+
+ self.dt = 0.005
+
+ self.R = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_ideal = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
+
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = CIM()
+ self.right_cim = CIM()
+
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ else:
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
+
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
+
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
+
+ return gear, shifter_position
+
+ def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
+
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
+
+ if not self.IsInGear(current_gear):
+ glog.debug('%s Not in gear.', gear_name)
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ glog.debug('%s Shifting up.', gear_name)
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
+
+ def FilterVelocity(self, throttle):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equal.
+
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
+
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
+
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
+
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
+ / (self.ttrust * min_K_sum + min_FF_sum))
+
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
+
+ self.left_gear = self.right_gear = True
+ if True:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
+
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
+
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
+
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+ equality_k = numpy.matrix(
+ [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+ equality_w = 0.0
+
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
+
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+ else:
+ glog.debug('Not all in gear')
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
+ self.left_cim.U_min, self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
+ self.right_cim.U_min, self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d',
+ self.left_gear, self.left_shifter_position,
+ self.right_gear, self.right_shifter_position)
+
+
+def main(argv):
+ argv = FLAGS(argv)
+
+ vdrivetrain = VelocityDrivetrain()
+
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2016_bot4', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+
+ cim_writer.Write(argv[3], argv[4])
+ return
+
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
+
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Left is high')
+ else:
+ glog.debug('Left is low')
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Right is high')
+ else:
+ glog.debug('Right is low')
+
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
+ else:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
+
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
+ return 0
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2016_bot4/control_loops/python/polydrivetrain_test.py b/y2016_bot4/control_loops/python/polydrivetrain_test.py
new file mode 100755
index 0000000..434cdca
--- /dev/null
+++ b/y2016_bot4/control_loops/python/polydrivetrain_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/python
+
+import polydrivetrain
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+class TestVelocityDrivetrain(unittest.TestCase):
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]])
+ K = numpy.matrix([[x1_max],
+ [-x1_min],
+ [x2_max],
+ [-x2_min]])
+ return polytope.HPolytope(H, K)
+
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
+ numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
+
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
+
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/y2016_bot4/joystick_reader.cc b/y2016_bot4/joystick_reader.cc
new file mode 100644
index 0000000..e7252f3
--- /dev/null
+++ b/y2016_bot4/joystick_reader.cc
@@ -0,0 +1,112 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
+#include "aos/common/actions/actions.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+#include "frc971/queues/gyro.q.h"
+#include "y2016_bot4/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+
+namespace y2016_bot4 {
+namespace input {
+namespace joysticks {
+
+//#define XBOX
+
+#ifdef XBOX
+// Xbox
+const JoystickAxis kSteeringWheel(1, 5), kDriveThrottle(1, 2);
+const ButtonLocation kQuickTurn(1, 5);
+#else
+// Steering wheel
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kQuickTurn(1, 5);
+#endif
+
+const ButtonLocation kTurn1(1, 7);
+const ButtonLocation kTurn2(1, 11);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {}
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override {
+ const bool auto_running = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+
+ if (!auto_running) {
+ HandleDrivetrain(data);
+ } else {
+ drivetrain_queue.goal.MakeWithBuilder()
+ .steering(0.0)
+ .throttle(0.0)
+ .quickturn(false)
+ .control_loop_driving(false)
+ .left_goal(0.0)
+ .right_goal(0.0)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send();
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ drivetrain_queue.status.FetchLatest();
+
+ if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ if (drivetrain_queue.status.get()) {
+ left_goal_ = drivetrain_queue.status->estimated_left_position;
+ right_goal_ = drivetrain_queue.status->estimated_right_position;
+ }
+ }
+ if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ is_control_loop_driving = true;
+ }
+ if (!drivetrain_queue.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
+ .right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+ }
+
+ private:
+ double left_goal_ = 0.0;
+ double right_goal_ = 0.0;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2016_bot4
+
+int main() {
+ ::aos::Init(-1);
+ ::y2016_bot4::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/y2016_bot4/wpilib/BUILD b/y2016_bot4/wpilib/BUILD
new file mode 100644
index 0000000..8ecfd89
--- /dev/null
+++ b/y2016_bot4/wpilib/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_binary(
+ name = 'wpilib_interface',
+ srcs = [
+ 'wpilib_interface.cc',
+ ],
+ deps = [
+ '//aos/common:stl_mutex',
+ '//aos/common/logging',
+ '//aos/common:math',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:log_interval',
+ '//aos/common:time',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
+ '//aos/common/util:phased_loop',
+ '//aos/common/util:wrapping_counter',
+ '//aos/linux_code:init',
+ '//third_party:wpilib',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops:queues',
+ '//frc971/wpilib:joystick_sender',
+ '//frc971/wpilib:loop_output_handler',
+ '//frc971/wpilib:buffered_pcm',
+ '//frc971/wpilib:gyro_sender',
+ '//frc971/wpilib:dma_edge_counting',
+ '//frc971/wpilib:interrupt_edge_counting',
+ '//frc971/wpilib:wpilib_robot_base',
+ '//frc971/wpilib:encoder_and_potentiometer',
+ '//frc971/wpilib:logging_queue',
+ '//frc971/wpilib:wpilib_interface',
+ '//frc971/wpilib:pdp_fetcher',
+ '//frc971/wpilib:ADIS16448',
+ '//frc971/wpilib:dma',
+ '//y2016_bot4/control_loops/drivetrain:polydrivetrain_plants',
+ '//y2016_bot4/control_loops/drivetrain:drivetrain_base',
+ ],
+)
diff --git a/y2016_bot4/wpilib/wpilib_interface.cc b/y2016_bot4/wpilib/wpilib_interface.cc
new file mode 100644
index 0000000..70039f5
--- /dev/null
+++ b/y2016_bot4/wpilib/wpilib_interface.cc
@@ -0,0 +1,257 @@
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <array>
+#include <functional>
+#include <mutex>
+#include <thread>
+
+#include "AnalogInput.h"
+#include "Compressor.h"
+#include "DigitalGlitchFilter.h"
+#include "DriverStation.h"
+#include "Encoder.h"
+#include "Relay.h"
+#include "Talon.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#undef ERROR
+
+#include "aos/common/commonmath.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/stl_mutex.h"
+#include "aos/common/time.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/util/wrapping_counter.h"
+#include "aos/linux_code/init.h"
+
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016_bot4/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+#include "frc971/wpilib/gyro_sender.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/wpilib_interface.h"
+
+#include "y2016_bot4/control_loops/drivetrain/drivetrain_base.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::frc971::control_loops::drivetrain_queue;
+
+namespace y2016_bot4 {
+namespace wpilib {
+
+// TODO(brian): Replace this with ::std::make_unique once all our toolchains
+// have support.
+template <class T, class... U>
+std::unique_ptr<T> make_unique(U &&... u) {
+ return std::unique_ptr<T>(new T(std::forward<U>(u)...));
+}
+
+double drivetrain_translate(int32_t in) {
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ ::y2016_bot4::constants::kDrivetrainEncoderRatio *
+ control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
+}
+
+double drivetrain_velocity_translate(double in) {
+ return (1.0 / in) / 256.0 /*cpr*/ *
+ ::y2016_bot4::constants::kDrivetrainEncoderRatio *
+ control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
+}
+
+constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
+ 5600.0 /* CIM free speed RPM */ * 12.0 / 54.0 *
+ 60.0 /* seconds per minute */ * 256.0 /* CPR */ * 4 /* edges per cycle */;
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader {
+ public:
+ SensorReader() {
+ // Set it to filter out anything shorter than 1/4 of the minimum pulse width
+ // we should ever see.
+ drivetrain_encoder_filter_.SetPeriodNanoSeconds(
+ static_cast<int>(1 / 8.0 /* built-in tolerance */ /
+ kMaxDrivetrainEncoderPulsesPerSecond * 1e9 +
+ 0.5));
+ }
+
+ // Drivetrain setters.
+ void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
+ drivetrain_encoder_filter_.Add(encoder.get());
+ drivetrain_left_encoder_ = ::std::move(encoder);
+ }
+
+ void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
+ drivetrain_encoder_filter_.Add(encoder.get());
+ drivetrain_right_encoder_ = ::std::move(encoder);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("SensorReader");
+
+ my_pid_ = getpid();
+ ds_ = &DriverStation::GetInstance();
+
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(0));
+
+ ::aos::SetCurrentThreadRealtimePriority(40);
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ }
+ }
+ RunIteration();
+ }
+ }
+
+ void RunIteration() {
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+
+ {
+ auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ drivetrain_message->right_encoder =
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+ drivetrain_message->left_encoder =
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+ drivetrain_message->left_speed =
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ drivetrain_message->right_speed =
+ drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+
+ drivetrain_message.Send();
+ }
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ int32_t my_pid_;
+ DriverStation *ds_;
+
+ ::std::unique_ptr<Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+
+ ::std::atomic<bool> run_{true};
+ DigitalGlitchFilter drivetrain_encoder_filter_, intake_encoder_filter_;
+};
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_drivetrain_left_talon(::std::unique_ptr<Talon> t0,
+ ::std::unique_ptr<Talon> t1) {
+ drivetrain_left_talon_0_ = ::std::move(t0);
+ drivetrain_left_talon_1_ = ::std::move(t1);
+ }
+
+ void set_drivetrain_right_talon(::std::unique_ptr<Talon> t0,
+ ::std::unique_ptr<Talon> t1) {
+ drivetrain_right_talon_0_ = ::std::move(t0);
+ drivetrain_right_talon_1_ = ::std::move(t1);
+ }
+
+ private:
+ virtual void Read() override {
+ ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::frc971::control_loops::drivetrain_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ drivetrain_left_talon_0_->Set(-queue->left_voltage / 12.0);
+ drivetrain_left_talon_1_->Set(-queue->left_voltage / 12.0);
+ drivetrain_right_talon_0_->Set(queue->right_voltage / 12.0);
+ drivetrain_right_talon_1_->Set(queue->right_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "drivetrain output too old\n");
+ drivetrain_left_talon_0_->Disable();
+ drivetrain_right_talon_0_->Disable();
+ drivetrain_left_talon_1_->Disable();
+ drivetrain_right_talon_1_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> drivetrain_left_talon_0_, drivetrain_right_talon_0_,
+ drivetrain_right_talon_1_, drivetrain_left_talon_1_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<Encoder> make_encoder(int index) {
+ return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
+ Encoder::k4X);
+ }
+
+ void Run() override {
+ ::aos::InitNRT();
+ ::aos::SetCurrentThreadName("StartCompetition");
+
+ ::frc971::wpilib::JoystickSender joystick_sender;
+ ::std::thread joystick_thread(::std::ref(joystick_sender));
+
+ ::frc971::wpilib::PDPFetcher pdp_fetcher;
+ ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
+ SensorReader reader;
+
+ reader.set_drivetrain_left_encoder(make_encoder(1));
+ reader.set_drivetrain_right_encoder(make_encoder(0));
+ ::std::thread reader_thread(::std::ref(reader));
+
+ ::frc971::wpilib::GyroSender gyro_sender;
+ ::std::thread gyro_thread(::std::ref(gyro_sender));
+
+ DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_drivetrain_left_talon(
+ ::std::unique_ptr<Talon>(new Talon(0)),
+ ::std::unique_ptr<Talon>(new Talon(3)));
+ drivetrain_writer.set_drivetrain_right_talon(
+ ::std::unique_ptr<Talon>(new Talon(1)),
+ ::std::unique_ptr<Talon>(new Talon(2)));
+ ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+
+ // Wait forever. Not much else to do...
+ while (true) {
+ const int r = select(0, nullptr, nullptr, nullptr, nullptr);
+ if (r != 0) {
+ PLOG(WARNING, "infinite select failed");
+ } else {
+ PLOG(WARNING, "infinite select succeeded??\n");
+ }
+ }
+
+ LOG(ERROR, "Exiting WPILibRobot\n");
+
+ joystick_sender.Quit();
+ joystick_thread.join();
+ pdp_fetcher.Quit();
+ pdp_fetcher_thread.join();
+ reader.Quit();
+ reader_thread.join();
+ gyro_sender.Quit();
+ gyro_thread.join();
+
+ drivetrain_writer.Quit();
+ drivetrain_writer_thread.join();
+
+ ::aos::Cleanup();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2016_bot4
+
+AOS_ROBOT_CLASS(::y2016_bot4::wpilib::WPILibRobot);