Merge changes I04c06100,I7f88d30c,I1258a5ef
* changes:
Add a test autonomous as mode 0
Move y2017 auton actor to use common class
Factor out drivetrain functionality from y2016 auton actor
diff --git a/NO_BUILD_AMD64 b/NO_BUILD_AMD64
index 886e438..69ace36 100644
--- a/NO_BUILD_AMD64
+++ b/NO_BUILD_AMD64
@@ -25,5 +25,5 @@
-//y2016_bot4:download
-//y2016_bot4:download_stripped
-//y2017:wpilib_interface
--//y2017:download
-//y2017:download_stripped
+-//y2017:download
diff --git a/aos/common/logging/sizes.h b/aos/common/logging/sizes.h
index 6a4c7d5..0aae90b 100644
--- a/aos/common/logging/sizes.h
+++ b/aos/common/logging/sizes.h
@@ -3,7 +3,7 @@
// This file exists so C code and context.h can both get at these constants...
-#define LOG_MESSAGE_LEN 400
+#define LOG_MESSAGE_LEN 500
#define LOG_MESSAGE_NAME_LEN 100
#endif // AOS_COMMON_LOGGING_SIZES_H_
diff --git a/aos/common/util/global_factory.h b/aos/common/util/global_factory.h
index 148fc1e..aac49bb 100644
--- a/aos/common/util/global_factory.h
+++ b/aos/common/util/global_factory.h
@@ -44,7 +44,7 @@
class GlobalFactory {
public:
using FactoryFunction =
- std::function<std::unique_ptr<BaseClass>(FactoryArgs&&...)>;
+ std::function<std::unique_ptr<BaseClass>(FactoryArgs &&...)>;
// Gets the factory function by named. This will return a null factory
// std::function if the factory is not available, so one would be wise
@@ -67,13 +67,18 @@
class SubClassRegisterer {
public:
explicit SubClassRegisterer(const char *name) {
- (*GetMap())[name] = [](FactoryArgs&&... args) {
+ (*GetMap())[name] = [](FactoryArgs &&... args) {
return std::unique_ptr<BaseClass>(
new SubClass(std::forward<FactoryArgs>(args)...));
};
}
};
+ // Fetch all factory functions.
+ static const std::unordered_map<std::string, FactoryFunction> &GetAll() {
+ return *GetMap();
+ }
+
private:
// Actual map. (Protected by static from concurrent construction
// if there is nothing registered at static linkage time).
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index cf728c5..7d57f50 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -100,6 +100,7 @@
srcs = ['move_scale.cc'],
deps = [
':range_image',
+ '//aos/vision/image:image_types',
],
)
@@ -136,7 +137,7 @@
hdrs = ['stream_view.h'],
deps = [
':range_image',
- '//aos/vision/debug:debug_viewer',
+ '//aos/vision/debug:debug_window',
'//aos/vision/image:image_types',
],
)
diff --git a/aos/vision/blob/move_scale.h b/aos/vision/blob/move_scale.h
index c113bca..061da5c 100644
--- a/aos/vision/blob/move_scale.h
+++ b/aos/vision/blob/move_scale.h
@@ -1,22 +1,15 @@
#ifndef AOS_VISION_BLOB_MOVE_SCALE_H_
#define AOS_VISION_BLOB_MOVE_SCALE_H_
-#include <vector>
#include <limits>
+#include <vector>
#include "aos/vision/blob/range_image.h"
+#include "aos/vision/image/image_types.h"
namespace aos {
namespace vision {
-// Bounding box for a RangeImage.
-struct ImageBBox {
- int minx = std::numeric_limits<int>::max();
- int maxx = std::numeric_limits<int>::min();
- int miny = std::numeric_limits<int>::max();
- int maxy = std::numeric_limits<int>::min();
-};
-
// Sums img into bbox. bbox is constructed empty and grows with each call
// to GetBBox.
void GetBBox(const RangeImage &img, ImageBBox *bbox);
diff --git a/aos/vision/blob/range_image.cc b/aos/vision/blob/range_image.cc
index 5f2f29f..946859d 100644
--- a/aos/vision/blob/range_image.cc
+++ b/aos/vision/blob/range_image.cc
@@ -75,7 +75,7 @@
if (span.ed > maxx) maxx = span.ed;
}
}
- LOG(INFO, "maxx: %d minx: %d\n", maxx, minx);
+ printf("maxx: %d minx: %d\n", maxx, minx);
char buf[maxx - minx];
for (const auto &range : rimg.ranges()) {
int i = minx;
@@ -84,7 +84,7 @@
for (; i < span.ed; ++i) buf[i - minx] = '#';
}
for (; i < maxx; ++i) buf[i - minx] = ' ';
- LOG(INFO, "%.*s\n", maxx - minx, buf);
+ printf("%.*s\n", maxx - minx, buf);
}
}
diff --git a/aos/vision/blob/stream_view.h b/aos/vision/blob/stream_view.h
index 2ae56ac..4cee7b4 100644
--- a/aos/vision/blob/stream_view.h
+++ b/aos/vision/blob/stream_view.h
@@ -2,7 +2,7 @@
#define _AOS_VISION_BLOB_STREAM_VIEW_H_
#include "aos/vision/blob/range_image.h"
-#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/debug/debug_window.h"
#include "aos/vision/image/image_types.h"
#include <memory>
@@ -10,10 +10,10 @@
namespace aos {
namespace vision {
-class BlobStreamViewer : public DebugViewer {
+class BlobStreamViewer : public DebugWindow {
public:
- BlobStreamViewer() : DebugViewer(false) {}
- explicit BlobStreamViewer(bool flip) : DebugViewer(flip) {}
+ BlobStreamViewer() : DebugWindow(false) {}
+ explicit BlobStreamViewer(bool flip) : DebugWindow(flip) {}
void Submit(ImageFormat fmt, const BlobList &blob_list) {
SetFormatAndClear(fmt);
@@ -29,60 +29,55 @@
memset(image_.data(), 0, fmt.ImgSize() * sizeof(PixelRef));
}
- inline void DrawBlobList(const BlobList &blob_list, PixelRef color) {
+ template <typename PixelCallback>
+ inline void ForPxInBlobList(const BlobList &blob_list,
+ PixelCallback pixel_callback) {
ImagePtr ptr = img();
- for (const RangeImage &blob : blob_list) {
+ auto fmt = ptr.fmt();
+ for (const auto &blob : blob_list) {
for (int i = 0; i < (int)blob.ranges().size(); ++i) {
- for (const auto &range : blob.ranges()[i]) {
- for (int j = range.st; j < range.ed; ++j) {
- ptr.get_px(j, i + blob.min_y()) = color;
+ int y = blob.min_y() + i;
+ if (y >= 0 && y < fmt.h) {
+ for (const auto &range : blob.ranges()[i]) {
+ for (int j = std::max(0, range.st); j < std::min(fmt.w, range.ed);
+ ++j) {
+ pixel_callback(ptr.get_px(j, y));
+ }
}
}
}
}
}
+ inline void DrawBlobList(const BlobList &blob_list, PixelRef color) {
+ ForPxInBlobList(blob_list, [&](PixelRef &px) { px = color; });
+ }
+
inline void DrawSecondBlobList(const BlobList &blob_list, PixelRef color1,
PixelRef color2) {
- ImagePtr ptr = img();
- for (const auto &blob : blob_list) {
- for (int i = 0; i < (int)blob.ranges().size(); ++i) {
- for (const auto &range : blob.ranges()[i]) {
- for (int j = range.st; j < range.ed; ++j) {
- auto px = ptr.get_px(j, i + blob.min_y());
- if (px.r == 0 && px.g == 0 && px.b == 0) {
- ptr.get_px(j, i + blob.min_y()) = color1;
- } else {
- ptr.get_px(j, i + blob.min_y()) = color2;
- }
- }
- }
+ ForPxInBlobList(blob_list, [&](PixelRef &px) {
+ if (px.r == 0 && px.g == 0 && px.b == 0) {
+ px = color1;
+ } else {
+ px = color2;
}
- }
+ });
}
inline void DrawSecondBlobList(const BlobList &blob_list, PixelRef color1,
PixelRef color2, PixelRef prev_color) {
- ImagePtr ptr = img();
- for (const auto &blob : blob_list) {
- for (int i = 0; i < (int)blob.ranges().size(); ++i) {
- for (const auto &range : blob.ranges()[i]) {
- for (int j = range.st; j < range.ed; ++j) {
- auto px = ptr.get_px(j, i + blob.min_y());
- if (px.r == prev_color.r && px.g == prev_color.g &&
- px.b == prev_color.b) {
- ptr.get_px(j, i + blob.min_y()) = color2;
- } else {
- ptr.get_px(j, i + blob.min_y()) = color1;
- }
- }
- }
+ ForPxInBlobList(blob_list, [&](PixelRef &px) {
+ if (px.r == prev_color.r && px.g == prev_color.g &&
+ px.b == prev_color.b) {
+ px = color2;
+ } else {
+ px = color1;
}
- }
+ });
}
// Backwards compatible.
- DebugViewer *view() { return this; }
+ DebugWindow *view() { return this; }
ImagePtr img() { return image_.get(); }
diff --git a/aos/vision/blob/transpose.cc b/aos/vision/blob/transpose.cc
index 19ac965..72e1cc1 100644
--- a/aos/vision/blob/transpose.cc
+++ b/aos/vision/blob/transpose.cc
@@ -1,6 +1,7 @@
#include "aos/vision/blob/transpose.h"
#include <algorithm>
+#include <limits>
namespace aos {
namespace vision {
@@ -14,27 +15,30 @@
kPointAdd = 3,
kPointDel = 2,
};
+ int min_y = std::numeric_limits<int>::max();
+ for (const std::vector<ImageRange> &row : img) {
+ if (!row.empty()) min_y = std::min(row[0].st, min_y);
+ }
+
std::vector<std::vector<std::pair<int, EventT>>> events;
int y = img.min_y();
for (const std::vector<ImageRange> &row : img) {
for (const ImageRange &range : row) {
- if (range.ed >= static_cast<int>(events.size()))
- events.resize(range.ed + 1);
- events[range.st].emplace_back(y, kPointAdd);
- events[range.ed].emplace_back(y, kPointDel);
+ if (range.ed - min_y >= static_cast<int>(events.size())) {
+ events.resize(range.ed - min_y + 1);
+ }
+ events[range.st - min_y].emplace_back(y, kPointAdd);
+ events[range.ed - min_y].emplace_back(y, kPointDel);
}
++y;
}
- int min_y = 0;
- while (min_y < (int)events.size() && events[min_y].empty()) ++min_y;
-
std::vector<ImageRange> prev_ranges;
std::vector<ImageRange> cur_ranges;
std::vector<std::vector<ImageRange>> rows;
- for (int y = min_y; y < static_cast<int>(events.size()) - 1; ++y) {
- auto row_events = std::move(events[y]);
+ for (int dy = 0; dy < static_cast<int>(events.size()) - 1; ++dy) {
+ auto row_events = std::move(events[dy]);
for (const auto &range : prev_ranges) {
row_events.emplace_back(range.st, kRangeStart);
row_events.emplace_back(range.ed, kRangeEnd);
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
index 3b4fa7c..e9e45ad 100644
--- a/aos/vision/debug/BUILD
+++ b/aos/vision/debug/BUILD
@@ -11,12 +11,41 @@
],
)
-gtk_dependent_cc_library(name = "debug_viewer",
- srcs = ["debug_viewer.cc"],
- hdrs = ["debug_viewer.h"],
+gtk_dependent_cc_library(name = "debug_window",
+ srcs = ["debug_window.cc"],
+ hdrs = ["debug_window.h"],
deps = [
'@usr_repo//:gtk+-3.0',
"//aos/vision/image:image_types",
":overlay",
]
)
+
+gtk_dependent_cc_library(
+ name = 'debug_framework',
+ srcs = [
+ 'debug_framework.cc',
+ 'jpeg_list-source.cc',
+ 'tcp-source.cc',
+ 'blob_log-source.cc',
+ 'camera-source.cc'
+ ],
+ hdrs = ['debug_framework.h'],
+ deps = [
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/vision/blob:codec',
+ '//aos/vision/blob:range_image',
+ '//aos/vision/blob:stream_view',
+ '//aos/vision/blob:find_blob',
+ '//aos/vision/events:gtk_event',
+ '//aos/vision/events:epoll_events',
+ "//aos/vision/events:tcp_client",
+ '//aos/vision/image:jpeg_routines',
+ '//aos/vision/image:image_stream',
+ '//aos/vision/image:image_types',
+ '//aos/common/util:global_factory',
+ '@usr_repo//:gtk+-3.0',
+ ],
+ alwayslink = 1,
+)
diff --git a/aos/vision/debug/blob_log-source.cc b/aos/vision/debug/blob_log-source.cc
new file mode 100644
index 0000000..f2bfbb5
--- /dev/null
+++ b/aos/vision/debug/blob_log-source.cc
@@ -0,0 +1,184 @@
+#include "aos/vision/debug/debug_framework.h"
+
+#include <gdk/gdk.h>
+#include <gtk/gtk.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <fstream>
+#include <functional>
+#include <string>
+
+#include "aos/vision/blob/codec.h"
+
+namespace aos {
+namespace vision {
+
+namespace {
+
+long GetFileSize(const std::string &filename) {
+ struct stat stat_buf;
+ int rc = stat(filename.c_str(), &stat_buf);
+ return rc == 0 ? stat_buf.st_size : -1;
+}
+
+// Parses the blob-log file format.
+// File format goes:
+//
+// Repeated:
+//
+// frame_length.
+// timestamp.
+// fmt.w
+// fmt.h
+// Encoded blob.
+class InputFile {
+ public:
+ InputFile(const std::string &fname)
+ : ifs_(fname, std::ifstream::in), len_(GetFileSize(fname)) {
+ if (len_ <= 0) {
+ LOG(FATAL, "File (%s) not found. Size (%d)\n", fname.c_str(), (int)len_);
+ }
+ // assert(len_ > 0);
+ tmp_buf_.resize(len_, 0);
+ ifs_.read(&tmp_buf_[0], len_);
+ buf_ = &tmp_buf_[0];
+ }
+
+ bool ReadNext(BlobList *blob_list, ImageFormat *fmt, uint64_t *timestamp) {
+ if (buf_ - &tmp_buf_[0] >= len_) return false;
+ if (prev_ != nullptr) prev_frames_.emplace_back(prev_);
+ prev_ = buf_;
+ DoRead(blob_list, fmt, timestamp);
+ return true;
+ }
+
+ bool ReadPrev(BlobList *blob_list, ImageFormat *fmt, uint64_t *timestamp) {
+ if (prev_frames_.empty()) return false;
+ buf_ = prev_frames_.back();
+ prev_frames_.pop_back();
+ buf_ += sizeof(uint32_t);
+ DoRead(blob_list, fmt, timestamp);
+ prev_ = nullptr;
+ return true;
+ }
+
+ private:
+ void DoRead(BlobList *blob_list, ImageFormat *fmt, uint64_t *timestamp) {
+ buf_ += sizeof(uint32_t);
+ *timestamp = Int64Codec::Read(&buf_);
+ fmt->w = Int32Codec::Read(&buf_);
+ fmt->h = Int32Codec::Read(&buf_);
+ buf_ = ParseBlobList(blob_list, buf_);
+ }
+ std::vector<const char *> prev_frames_;
+ const char *buf_;
+ const char *prev_ = nullptr;
+ std::ifstream ifs_;
+
+ long len_;
+ std::vector<char> tmp_buf_;
+};
+
+// A single parsed frame.
+class BlobStreamFrame {
+ public:
+ BlobList blob_list;
+ ImageFormat fmt;
+ uint64_t timestamp;
+ void ReadNext(InputFile *fin) {
+ blob_list.clear();
+ if (!fin->ReadNext(&blob_list, &fmt, ×tamp)) {
+ exit(0);
+ return;
+ }
+ }
+ bool ReadPrev(InputFile *fin) {
+ blob_list.clear();
+ return fin->ReadPrev(&blob_list, &fmt, ×tamp);
+ }
+};
+
+} // namespace
+
+// class for installing a lambda as a gtk timeout.
+class TimeoutCallback {
+ public:
+ TimeoutCallback() {}
+
+ void Reset(guint32 interval, std::function<bool()> callback) {
+ Stop();
+ callback_ = callback;
+ timeout_key_ = g_timeout_add(interval, &TimeoutCallback::Callback, this);
+ }
+ void Stop() {
+ if (callback_) {
+ g_source_remove(timeout_key_);
+ }
+ callback_ = std::function<bool()>();
+ }
+
+ private:
+ static gint Callback(void *self) {
+ return reinterpret_cast<TimeoutCallback *>(self)->Callback();
+ }
+ gint Callback() {
+ auto callback = callback_;
+ if (!callback()) {
+ return FALSE;
+ }
+ return TRUE;
+ }
+ gint timeout_key_;
+ std::function<bool()> callback_;
+};
+
+class BlobLogImageSource : public ImageSource {
+ public:
+ void Init(const std::string &blob_log_filename,
+ DebugFrameworkInterface *interface) override {
+ interface_ = interface;
+ image_source_.reset(new InputFile(blob_log_filename));
+
+ // Tick 25 fps.
+ // TODO(parker): Make this FPS configurable.
+ cb_.Reset(1000 / 25, [this]() { return Tick(); });
+
+ frame_.ReadNext(image_source_.get());
+ interface_->NewBlobList(frame_.blob_list, frame_.fmt);
+ interface_->InstallKeyPress([this](uint32_t keyval) {
+ if (keyval == GDK_KEY_Left) {
+ frame_.ReadPrev(image_source_.get());
+ interface_->NewBlobList(frame_.blob_list, frame_.fmt);
+ } else if (keyval == GDK_KEY_Right) {
+ frame_.ReadNext(image_source_.get());
+ interface_->NewBlobList(frame_.blob_list, frame_.fmt);
+ } else {
+ return;
+ }
+ });
+ }
+
+ bool Tick() {
+ frame_.ReadNext(image_source_.get());
+ interface_->NewBlobList(frame_.blob_list, frame_.fmt);
+ return true;
+ }
+
+ const char *GetHelpMessage() override {
+ return &R"(
+ format_spec is the name of a file in blob list format.
+ This viewer source will stream blobs from the log.
+)"[1];
+ }
+
+ private:
+ TimeoutCallback cb_;
+ DebugFrameworkInterface *interface_ = nullptr;
+ std::unique_ptr<InputFile> image_source_;
+ BlobStreamFrame frame_;
+};
+
+REGISTER_IMAGE_SOURCE("blob_log", BlobLogImageSource);
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/camera-source.cc b/aos/vision/debug/camera-source.cc
new file mode 100644
index 0000000..ef48a11
--- /dev/null
+++ b/aos/vision/debug/camera-source.cc
@@ -0,0 +1,72 @@
+#include "aos/vision/debug/debug_framework.h"
+
+#include <gdk/gdk.h>
+#include <fstream>
+#include <string>
+
+#include "aos/vision/image/image_stream.h"
+
+namespace aos {
+namespace vision {
+
+class CameraImageSource : public ImageSource {
+ public:
+ void Init(const std::string &jpeg_list_filename,
+ DebugFrameworkInterface *interface) override {
+ // TODO: Get these params from a config file passed in through the
+ // constructor.
+ camera::CameraParams params = {.width = 640 * 2,
+ .height = 480 * 2,
+ .exposure = 10,
+ .brightness = 128,
+ .gain = 0,
+ .fps = 30};
+ image_stream_.reset(new ImageStream(jpeg_list_filename, params, interface));
+ }
+
+ const char *GetHelpMessage() override {
+ return &R"(
+ format_spec is filename of the camera device.
+ example: camera:/dev/video0
+ This viewer source will stream video from a usb camera of your choice.
+)"[1];
+ }
+
+ class ImageStream : public ImageStreamEvent {
+ public:
+ ImageStream(const std::string &fname, camera::CameraParams params,
+ DebugFrameworkInterface *interface)
+ : ImageStreamEvent(fname, params), interface_(interface) {
+ interface_->Loop()->Add(this);
+
+ interface_->InstallKeyPress([this](uint32_t keyval) {
+ // Takes a picture when you press 'a'.
+ // TODO(parker): Allow setting directory.
+ if (keyval == GDK_KEY_a) {
+ std::ofstream ofs(
+ std::string("/tmp/out_jpegs/test") + std::to_string(i_) + ".jpg",
+ std::ofstream::out);
+ ofs << prev_data_;
+ ofs.close();
+ ++i_;
+ }
+ });
+ }
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point) override {
+ prev_data_ = std::string(data);
+ interface_->NewJpeg(data);
+ }
+
+ private:
+ int i_ = 0;
+ std::string prev_data_;
+ DebugFrameworkInterface *interface_;
+ };
+
+ std::unique_ptr<ImageStream> image_stream_;
+};
+
+REGISTER_IMAGE_SOURCE("camera", CameraImageSource);
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/debug_framework.cc b/aos/vision/debug/debug_framework.cc
new file mode 100644
index 0000000..64f370c
--- /dev/null
+++ b/aos/vision/debug/debug_framework.cc
@@ -0,0 +1,127 @@
+#include "aos/vision/debug/debug_framework.h"
+
+#include <gtk/gtk.h>
+
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/jpeg_routines.h"
+
+namespace aos {
+namespace vision {
+
+bool DecodeJpeg(aos::vision::DataRef data,
+ aos::vision::BlobStreamViewer *view) {
+ auto fmt = aos::vision::GetFmt(data);
+ auto value = view->img();
+ if (!value.fmt().Equals(fmt)) {
+ view->SetFormatAndClear(fmt);
+ }
+ return aos::vision::ProcessJpeg(data, view->img().data());
+}
+
+class DebugFramework : public DebugFrameworkInterface {
+ public:
+ explicit DebugFramework(FilterHarness *filter) : filter_(filter) {
+ view_.key_press_event = [this](uint32_t keyval) {
+ for (const auto &event : key_press_events()) {
+ event(keyval);
+ }
+ };
+ filter->InstallViewer(&view_);
+ }
+
+ // This the first stage in the pipeline that takes
+ void NewJpeg(DataRef data) override {
+ DecodeJpeg(data, &view_);
+
+ auto fmt = view_.img().fmt();
+ HandleBlobs(FindBlobs(filter_->Threshold(view_.img())), fmt);
+ }
+
+ void NewBlobList(BlobList blob_list, ImageFormat fmt) override {
+ view_.SetFormatAndClear(fmt);
+
+ HandleBlobs(std::move(blob_list), fmt);
+ }
+
+ void HandleBlobs(BlobList blob_list, ImageFormat fmt) {
+ filter_->HandleBlobs(std::move(blob_list), fmt);
+ view_.Redraw();
+ }
+
+ aos::events::EpollLoop *Loop() override { return &loop_; }
+
+ private:
+ FilterHarness *filter_;
+ BlobStreamViewer view_;
+
+ aos::events::EpollLoop loop_;
+};
+
+std::unique_ptr<ImageSource> MakeImageSource(
+ const std::string &image_source_string,
+ DebugFrameworkInterface *interface) {
+ (void)interface;
+ // Each of the image_source strings is of the form format_type:format_spec
+ auto it = image_source_string.find(':');
+ if (it == std::string::npos) {
+ fprintf(stderr, "invalid ImageSource: %s.\n", image_source_string.c_str());
+ exit(-1);
+ }
+ auto image_source_type = image_source_string.substr(0, it);
+ // Get std::function<std::unique_ptr<ImageSource>()> from the registration
+ // factory.
+ const auto &factory = ImageSourceGlobalFactory::Get(image_source_type);
+ if (!factory) {
+ fprintf(stderr, "invalid ImageSource: %s.\n", image_source_string.c_str());
+ exit(-1);
+ }
+ auto result = factory();
+ // Construct the image source.
+ result->Init(image_source_string.substr(it + 1), interface);
+ return result;
+}
+
+const char *kHelpMessage = R"(
+
+image_source is parsed out and selects where to get the images
+from. Each source type has a different configuration format string listed
+below. The colon separates the source specifier and the source config
+parameter. A single command line argument help will print this message.
+)";
+
+void DebugFrameworkMain(int argc, char **argv, FilterHarness *filter) {
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+
+ gtk_init(&argc, &argv);
+
+ // Use fprintf because it is only supposed to be used interactively.
+ // This uses a registration system to pick out the individual file type
+ // registered by REGISTER_IMAGE_SOURCE.
+ // see jpeg_list-source.cc for ane sample of this.
+ if (argc < 2 || argv[1] == std::string("help")) {
+ fprintf(stderr, "Usage %s image_source:format_spec\n", argv[0]);
+ fprintf(stderr, "%s", kHelpMessage);
+ // Iterate through all registered entities in ImageSourceGlobalFactory
+ // and print out their individual help messages.
+ for (const auto &type : ImageSourceGlobalFactory::GetAll()) {
+ fprintf(stderr, " %s:\n", type.first.c_str());
+ fprintf(stderr, "%s", type.second()->GetHelpMessage());
+ }
+ exit(-1);
+ }
+
+ DebugFramework replay(filter);
+
+ std::unique_ptr<ImageSource> image_source = MakeImageSource(argv[1], &replay);
+
+ replay.Loop()->RunWithGtkMain();
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/debug_framework.h b/aos/vision/debug/debug_framework.h
new file mode 100644
index 0000000..2f0fcd1
--- /dev/null
+++ b/aos/vision/debug/debug_framework.h
@@ -0,0 +1,85 @@
+#ifndef _AOS_VISION_DEBUG_DEBUG_FRAMEWORK_H_
+#define _AOS_VISION_DEBUG_DEBUG_FRAMEWORK_H_
+
+#include "aos/common/util/global_factory.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+class BlobStreamViewer;
+
+// Implement per-filter to draw debug viewer information from the filter to
+// the debug BlobStreamViewer.
+class FilterHarness {
+ public:
+ virtual ~FilterHarness() {}
+
+ // Apply the filter-specific thresholding logic.
+ // Blob sources may not have this called at all.
+ virtual RangeImage Threshold(ImagePtr image) = 0;
+
+ // Each filter can only be used by one debug viewer. This will
+ // get called before calling any other methods.
+ virtual void InstallViewer(BlobStreamViewer * /*viewer*/) {}
+
+ // One frame worth of blobs. Returns if the frame is "interesting".
+ virtual bool HandleBlobs(BlobList imgs, ImageFormat fmt) = 0;
+};
+
+// For ImageSource implementations only. Allows registering key press events
+// and installing new blob lists and jpegs.
+class DebugFrameworkInterface {
+ public:
+ virtual ~DebugFrameworkInterface() {}
+
+ void InstallKeyPress(std::function<void(uint32_t)> key_press_event) {
+ key_press_events_.emplace_back(std::move(key_press_event));
+ }
+
+ virtual void NewJpeg(DataRef data) = 0;
+
+ virtual void NewBlobList(BlobList blob_list, ImageFormat fmt) = 0;
+
+ // Expose a EpollLoop to allow waiting for events.
+ virtual aos::events::EpollLoop *Loop() = 0;
+
+ protected:
+ const std::vector<std::function<void(uint32_t)>> &key_press_events() {
+ return key_press_events_;
+ }
+
+ private:
+ std::vector<std::function<void(uint32_t)>> key_press_events_;
+};
+
+// Implemented by each source type. Will stream frames to
+// DebugFrameworkInterface.
+class ImageSource {
+ public:
+ virtual ~ImageSource() {}
+
+ // Printed when you call: debug_viewer help.
+ virtual const char *GetHelpMessage() { return " No help string :(\n"; }
+
+ // Start streaming frames to DebugFrameworkInterface.
+ virtual void Init(const std::string &args,
+ DebugFrameworkInterface *interface) = 0;
+};
+
+// Factory for ImageSource.
+SETUP_FACTORY(ImageSource);
+
+#define REGISTER_IMAGE_SOURCE(key, SubClass) \
+ REGISTER_SUBCLASS_BY_KEY(key, ::aos::vision::ImageSource, SubClass)
+
+// Runs loop and never returns.
+// Feeds into a generic filter.
+void DebugFrameworkMain(int argc, char **argv, FilterHarness *filter);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_DEBUG_DEBUG_FRAMEWORK_H_
diff --git a/aos/vision/debug/debug_viewer.cc b/aos/vision/debug/debug_window.cc
similarity index 87%
rename from aos/vision/debug/debug_viewer.cc
rename to aos/vision/debug/debug_window.cc
index 331f733..acbaf8e 100644
--- a/aos/vision/debug/debug_viewer.cc
+++ b/aos/vision/debug/debug_window.cc
@@ -1,4 +1,4 @@
-#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/debug/debug_window.h"
#include <gdk-pixbuf/gdk-pixbuf.h>
#include <gtk/gtk.h>
@@ -26,7 +26,7 @@
g_signal_connect(widget, "draw", G_CALLBACK(fnptr), obj);
}
-struct DebugViewer::Internals {
+struct DebugWindow::Internals {
Internals(bool flip) : flip_(flip) {}
gboolean Draw(cairo_t *cr) {
@@ -73,18 +73,18 @@
bool clear_per_frame_ = true;
};
-void DebugViewer::SetOverlays(std::vector<OverlayBase *> *overlays) {
+void DebugWindow::SetOverlays(std::vector<OverlayBase *> *overlays) {
self->overlays = overlays;
}
-void DebugViewer::Redraw() {
+void DebugWindow::Redraw() {
if (!self->needs_draw) {
gtk_widget_queue_draw(self->drawing_area);
self->needs_draw = true;
}
}
-void DebugViewer::UpdateImage(ImagePtr ptr) {
+void DebugWindow::UpdateImage(ImagePtr ptr) {
if (ptr.data() != self->ptr.data()) {
int w = ptr.fmt().w;
int h = ptr.fmt().h;
@@ -101,13 +101,17 @@
window_height_ = h;
window_width_ = w;
}
+ if (!shown_yet_) {
+ gtk_widget_show_all(self->window);
+ shown_yet_ = true;
+ }
}
-void DebugViewer::MoveTo(int x, int y) {
+void DebugWindow::MoveTo(int x, int y) {
gtk_window_move(GTK_WINDOW(self->window), x, y);
}
-void DebugViewer::SetScale(double scale_factor_inp) {
+void DebugWindow::SetScale(double scale_factor_inp) {
int w = window_width_;
int h = window_height_;
@@ -124,12 +128,12 @@
gboolean debug_viewer_key_press_event(GtkWidget * /*widget*/,
GdkEventKey *event, gpointer user_data) {
auto &key_press_cb =
- reinterpret_cast<DebugViewer *>(user_data)->key_press_event;
+ reinterpret_cast<DebugWindow *>(user_data)->key_press_event;
if (key_press_cb) key_press_cb(event->keyval);
return FALSE;
}
-DebugViewer::DebugViewer(bool flip) : self(new Internals(flip)) {
+DebugWindow::DebugWindow(bool flip) : self(new Internals(flip)) {
self->scale_factor = scale_factor;
GtkWidget *window;
auto drawing_area = self->drawing_area = gtk_drawing_area_new();
@@ -137,7 +141,7 @@
window_height_ * scale_factor);
gtk_widget_add_events(drawing_area, GDK_KEY_PRESS_MASK);
- g_draw_signal_connect<DebugViewer::Internals, &DebugViewer::Internals::Draw>(
+ g_draw_signal_connect<DebugWindow::Internals, &DebugWindow::Internals::Draw>(
drawing_area, self.get());
window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
@@ -149,9 +153,8 @@
window_height_ * scale_factor);
gtk_container_add(GTK_CONTAINER(window), drawing_area);
- gtk_widget_show_all(window);
}
-DebugViewer::~DebugViewer() {}
+DebugWindow::~DebugWindow() {}
void CairoRender::Text(int x, int y, int /*text_x*/, int /*text_y*/,
const std::string &text) {
diff --git a/aos/vision/debug/debug_viewer.h b/aos/vision/debug/debug_window.h
similarity index 88%
rename from aos/vision/debug/debug_viewer.h
rename to aos/vision/debug/debug_window.h
index 3bada28..906bf3b 100644
--- a/aos/vision/debug/debug_viewer.h
+++ b/aos/vision/debug/debug_window.h
@@ -1,5 +1,5 @@
-#ifndef AOS_VISION_DEBUG_DEBUG_VIEWER_H_
-#define AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#ifndef AOS_VISION_DEBUG_DEBUG_WINDOW_H_
+#define AOS_VISION_DEBUG_DEBUG_WINDOW_H_
#include <cairo.h>
#include <functional>
@@ -39,11 +39,11 @@
};
// Simple debug view window.
-class DebugViewer {
+class DebugWindow {
public:
struct Internals;
- explicit DebugViewer(bool flip);
- ~DebugViewer();
+ explicit DebugWindow(bool flip);
+ ~DebugWindow();
// Explicit redraw queuing (Will not double-queue).
void Redraw();
@@ -66,6 +66,7 @@
std::function<void(uint32_t)> key_press_event;
private:
+ bool shown_yet_ = false;
double scale_factor = 1.0;
int window_width_ = 100;
int window_height_ = 100;
@@ -75,4 +76,4 @@
} // namespace vision
} // namespace aos
-#endif // AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#endif // AOS_VISION_DEBUG_DEBUG_WINDOW_H_
diff --git a/aos/vision/debug/jpeg_list-source.cc b/aos/vision/debug/jpeg_list-source.cc
new file mode 100644
index 0000000..2dbbe44
--- /dev/null
+++ b/aos/vision/debug/jpeg_list-source.cc
@@ -0,0 +1,108 @@
+#include "aos/vision/debug/debug_framework.h"
+
+#include <gdk/gdk.h>
+#include <fstream>
+#include <string>
+
+namespace aos {
+namespace vision {
+
+namespace {
+std::string GetFileContents(const std::string &filename) {
+ std::ifstream in(filename, std::ios::in | std::ios::binary);
+ if (in) {
+ std::string contents;
+ in.seekg(0, std::ios::end);
+ contents.resize(in.tellg());
+ in.seekg(0, std::ios::beg);
+ in.read(&contents[0], contents.size());
+ in.close();
+ return (contents);
+ }
+ fprintf(stderr, "Could not read file: %s\n", filename.c_str());
+ exit(-1);
+}
+
+std::vector<std::string> Split(DataRef inp, char delim) {
+ size_t i = 0;
+ std::vector<size_t> pos;
+ while (i < inp.size()) {
+ i = inp.find(delim, i);
+ if (i == std::string::npos) break;
+ // fprintf(stderr, "k=%d, i=%d\n", k, (int)i);
+ pos.emplace_back(i);
+ i = i + 1;
+ }
+ std::vector<std::string> res;
+ res.reserve(pos.size() + 1);
+ i = 0;
+ for (auto p : pos) {
+ res.emplace_back(inp.substr(i, p - i).to_string());
+ i = p + 1;
+ }
+ res.emplace_back(inp.substr(i).to_string());
+ return res;
+}
+} // namespace
+
+class JpegListImageSource : public ImageSource {
+ public:
+ void Init(const std::string &jpeg_list_filename,
+ DebugFrameworkInterface *interface) override {
+ interface_ = interface;
+ auto contents = GetFileContents(jpeg_list_filename);
+
+ std::string basename;
+ auto it = jpeg_list_filename.find_last_of('/');
+ if (it != std::string::npos) {
+ basename = jpeg_list_filename.substr(0, it + 1);
+ }
+
+ for (const auto &jpeg_filename : Split(contents, '\n')) {
+ [&]() {
+ if (jpeg_filename.empty()) return;
+ for (std::size_t i = 0; i < jpeg_filename.size(); ++i) {
+ if (jpeg_filename[i] == '#') return;
+ if (jpeg_filename[i] != ' ') break;
+ }
+ if (jpeg_filename[0] == '/') {
+ images_.emplace_back(GetFileContents(jpeg_filename));
+ } else {
+ images_.emplace_back(GetFileContents(basename + jpeg_filename));
+ }
+ }();
+ }
+ fprintf(stderr, "loaded %lu items\n", images_.size());
+ if (!images_.empty()) {
+ interface_->NewJpeg(images_[idx_]);
+ interface_->InstallKeyPress([this](uint32_t keyval) {
+ if (keyval == GDK_KEY_Left && idx_ > 0) {
+ --idx_;
+ } else if (keyval == GDK_KEY_Right && idx_ < images_.size()) {
+ idx_ = (idx_ + 1) % images_.size();
+ } else {
+ return;
+ }
+ interface_->NewJpeg(images_[idx_]);
+ });
+ }
+ }
+
+ const char *GetHelpMessage() override {
+ return &R"(
+ format_spec is the name of a file with each jpeg filename on a new line.
+ This viewer source will load each jpeg individually and cycle through them
+ with the arrow keys.
+)"[1];
+ }
+
+ private:
+ DebugFrameworkInterface *interface_ = nullptr;
+ std::vector<std::string> images_;
+ size_t idx_ = 0;
+};
+
+REGISTER_IMAGE_SOURCE("jpeg_list", JpegListImageSource);
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
index fbd6838..5a9804f 100644
--- a/aos/vision/debug/overlay.h
+++ b/aos/vision/debug/overlay.h
@@ -117,18 +117,39 @@
~PixelLinesOverlay() {}
// build a segment for this line
- void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+ void AddLine(Vector<2> st, Vector<2> ed) { AddLine(st, ed, color); }
// build a segment for this line
- void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ void AddLine(Vector<2> st, Vector<2> ed, PixelRef newColor) {
lines_.emplace_back(
std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
}
- void start_new_profile() { start_profile = true; }
+ void DrawCross(aos::vision::Vector<2> center, int width,
+ aos::vision::PixelRef color) {
+ using namespace aos::vision;
+ AddLine(Vector<2>(center.x() - width, center.y()),
+ Vector<2>(center.x() + width, center.y()), color);
+ AddLine(Vector<2>(center.x(), center.y() - width),
+ Vector<2>(center.x(), center.y() + width), color);
+ }
+
+ void DrawBBox(const ImageBBox &box, aos::vision::PixelRef color) {
+ using namespace aos::vision;
+ AddLine(Vector<2>(box.minx, box.miny), Vector<2>(box.maxx, box.miny),
+ color);
+ AddLine(Vector<2>(box.maxx, box.miny), Vector<2>(box.maxx, box.maxy),
+ color);
+ AddLine(Vector<2>(box.maxx, box.maxy), Vector<2>(box.minx, box.maxy),
+ color);
+ AddLine(Vector<2>(box.minx, box.maxy), Vector<2>(box.minx, box.miny),
+ color);
+ }
+
+ void StartNewProfile() { start_profile = true; }
// add a new point connected to the last point in the line
- void add_point(Vector<2> pt, PixelRef newColor) {
+ void AddPoint(Vector<2> pt, PixelRef newColor) {
if (lines_.empty() || start_profile) {
lines_.emplace_back(
std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
diff --git a/aos/vision/debug/tcp-source.cc b/aos/vision/debug/tcp-source.cc
new file mode 100644
index 0000000..37c38d5
--- /dev/null
+++ b/aos/vision/debug/tcp-source.cc
@@ -0,0 +1,138 @@
+#include "aos/vision/debug/debug_framework.h"
+
+#include <gdk/gdk.h>
+#include <gtk/gtk.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <cstdlib>
+#include <fstream>
+#include <functional>
+#include <string>
+
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/events/tcp_client.h"
+
+namespace aos {
+namespace vision {
+
+class BufferedLengthDelimReader {
+ public:
+ union data_len {
+ uint32_t len;
+ char buf[4];
+ };
+ BufferedLengthDelimReader() {
+ num_read_ = 0;
+ img_read_ = -1;
+ }
+ template <typename Lamb>
+ void up(int fd, Lamb lam) {
+ ssize_t count;
+ if (img_read_ < 0) {
+ count = read(fd, &len_.buf[num_read_], sizeof(len_.buf) - num_read_);
+ if (count < 0) return;
+ num_read_ += count;
+ if (num_read_ < 4) return;
+ num_read_ = 0;
+ img_read_ = 0;
+ data_.clear();
+ if (len_.len > 200000) {
+ printf("bad size: %d\n", len_.len);
+ exit(-1);
+ }
+ data_.resize(len_.len);
+ } else {
+ count = read(fd, &data_[img_read_], len_.len - img_read_);
+ if (count < 0) return;
+ img_read_ += count;
+ if (img_read_ < (int)len_.len) return;
+ lam(DataRef{&data_[0], len_.len});
+ img_read_ = -1;
+ }
+ }
+
+ private:
+ data_len len_;
+ int num_read_;
+ std::vector<char> data_;
+ int img_read_;
+};
+
+bool ParsePort(const std::string &port, int *portno) {
+ if (port.empty()) return false;
+ int value = 0;
+ if (port[0] == '0') return false;
+ for (char item : port) {
+ if (item < '0' || item > '9') return false;
+ value = value * 10 + (item - '0');
+ }
+ *portno = value;
+ return true;
+}
+
+class TCPImageSource : public ImageSource {
+ public:
+ class Impl : public aos::events::TcpClient {
+ public:
+ Impl(const std::string &hostname, int portno,
+ DebugFrameworkInterface *interface)
+ : aos::events::TcpClient(hostname.c_str(), portno),
+ interface_(interface) {}
+
+ void ReadEvent() override {
+ read_.up(fd(), [&](DataRef data) {
+ BlobList blobl;
+ const char *buf = data.data();
+ buf += sizeof(uint32_t);
+
+ ImageFormat fmt;
+ Int64Codec::Read(&buf);
+ fmt.w = Int32Codec::Read(&buf);
+ fmt.h = Int32Codec::Read(&buf);
+ buf = ParseBlobList(&blobl, buf);
+ interface_->NewBlobList(blobl, fmt);
+ });
+ }
+
+ BufferedLengthDelimReader read_;
+ DebugFrameworkInterface *interface_ = nullptr;
+ };
+
+ void Init(const std::string &addr_and_port,
+ DebugFrameworkInterface *interface) override {
+ auto it = addr_and_port.rfind(':');
+ if (it == std::string::npos) {
+ fprintf(stderr, "usage is: tcp:hostname:port\n");
+ exit(-1);
+ }
+ auto hostname = addr_and_port.substr(0, it);
+ auto port = addr_and_port.substr(it + 1);
+ int portno = 0;
+ if (!ParsePort(port, &portno)) {
+ fprintf(stderr, "usage is: tcp:hostname:port\n");
+ exit(-1);
+ }
+
+ impl_.reset(new Impl(hostname, portno, interface));
+
+ interface->Loop()->Add(impl_.get());
+
+ interface->InstallKeyPress([this](uint32_t /*keyval*/) {});
+ }
+
+ const char *GetHelpMessage() override {
+ return &R"(
+ format_spec is in ipaddr:port format.
+ This viewer soure connects to a target_sender binary and views the live
+ blob-stream.
+)"[1];
+ }
+
+ private:
+ std::unique_ptr<Impl> impl_;
+};
+
+REGISTER_IMAGE_SOURCE("tcp", TCPImageSource);
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index 2b5b9e7..9c55a1c 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -1,5 +1,5 @@
load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
-package(default_visibility = ["//visibility:public"])
+package(default_visibility = ['//visibility:public'])
cc_library(
name = 'epoll_events',
@@ -12,12 +12,14 @@
],
)
-cc_library(name = "socket_types",
- hdrs = ["socket_types.h"],
- deps = [
- "//aos/vision/events:tcp_server",
- "//aos/vision/image:image_types",
- ],
+cc_library(
+ name = 'socket_types',
+ hdrs = ['socket_types.h'],
+ deps = [
+ '//aos/vision/events:tcp_server',
+ '//aos/vision/image:image_types',
+ '//third_party/protobuf:protobuf',
+ ],
)
cc_library(
@@ -59,10 +61,10 @@
)
gtk_dependent_cc_library(
- name = "gtk_event",
- srcs = ["gtk_event.cc"],
+ name = 'gtk_event',
+ srcs = ['gtk_event.cc'],
deps = [
- ":epoll_events",
+ ':epoll_events',
'@usr_repo//:gtk+-3.0',
],
)
diff --git a/aos/vision/events/epoll_events.cc b/aos/vision/events/epoll_events.cc
index e4f789f..d49043c 100644
--- a/aos/vision/events/epoll_events.cc
+++ b/aos/vision/events/epoll_events.cc
@@ -36,7 +36,7 @@
for (int i = 0; i < number_events; i++) {
EpollEvent *event = static_cast<EpollEvent *>(events[i].data.ptr);
- if ((events[i].events & ~(EPOLLIN | EPOLLPRI)) != 0) {
+ if ((events[i].events & ~(EPOLLIN | EPOLLPRI | EPOLLERR)) != 0) {
LOG(FATAL, "unexpected epoll events set in %x on %d\n",
events[i].events, event->fd());
}
diff --git a/aos/vision/events/epoll_events.h b/aos/vision/events/epoll_events.h
index ee288c4..3b7d75a 100644
--- a/aos/vision/events/epoll_events.h
+++ b/aos/vision/events/epoll_events.h
@@ -44,7 +44,7 @@
// Duplicate above to allow Done to change itself.
if (time_ < monotonic_clock::epoch()) return -1;
if (time_ <= now) {
- return -1;// Recalculate(now);
+ return -1;
}
if (time_ - now > ::std::chrono::milliseconds(INT_MAX)) {
diff --git a/aos/vision/events/gtk_event.cc b/aos/vision/events/gtk_event.cc
index e8b43aa..0c518e0 100644
--- a/aos/vision/events/gtk_event.cc
+++ b/aos/vision/events/gtk_event.cc
@@ -1,9 +1,9 @@
-#include <gtk/gtk.h>
#include <gdk/gdk.h>
-#include <thread>
+#include <gtk/gtk.h>
#include <sys/epoll.h>
-#include <mutex>
#include <condition_variable>
+#include <mutex>
+#include <thread>
#include "aos/vision/events/epoll_events.h"
@@ -43,16 +43,19 @@
std::thread t([&]() {
std::unique_lock<std::mutex> lk(m);
while (true) {
- cv.wait(lk, [&all_events_handled]{return all_events_handled;});
+ cv.wait(lk, [&all_events_handled] { return all_events_handled; });
// Wait for handle_cb to be done.
- number_events = PCHECK(epoll_wait(epoll_fd(), events, kNumberOfEvents, timeout));
+ number_events =
+ PCHECK(epoll_wait(epoll_fd(), events, kNumberOfEvents, timeout));
all_events_handled = false;
// Trigger handle_cb on main_thread to avoid concurrency.
- gdk_threads_add_idle(+[](gpointer user_data) -> gboolean {
- auto& handle_cb = *reinterpret_cast<HandleCBType*>(user_data);
- handle_cb();
- return G_SOURCE_REMOVE;
- }, &handle_cb);
+ gdk_threads_add_idle(
+ +[](gpointer user_data) -> gboolean {
+ auto &handle_cb = *reinterpret_cast<HandleCBType *>(user_data);
+ handle_cb();
+ return G_SOURCE_REMOVE;
+ },
+ &handle_cb);
}
});
gtk_main();
diff --git a/aos/vision/events/socket_types.h b/aos/vision/events/socket_types.h
index 2432cb9..d377214 100644
--- a/aos/vision/events/socket_types.h
+++ b/aos/vision/events/socket_types.h
@@ -3,6 +3,8 @@
#include <poll.h>
#include <stdint.h>
+#include <sys/socket.h>
+#include <sys/types.h>
#include "aos/vision/events/tcp_server.h"
#include "aos/vision/image/image_types.h"
@@ -32,7 +34,12 @@
char buf[512];
while (true) {
count = read(fd(), &buf, sizeof buf);
- if (count <= 0) return;
+ if (count <= 0) {
+ if (errno != EAGAIN) {
+ CloseConnection();
+ return;
+ }
+ }
}
}
@@ -46,14 +53,15 @@
void Emit(vision::DataRef data) {
data_len len;
len.len = data.size();
- int res = write(fd(), len.buf, sizeof len.buf);
+ int res = send(fd(), len.buf, sizeof len.buf, MSG_NOSIGNAL);
if (res == -1) {
- printf("Emit Error on write\n");
+ CloseConnection();
+ return;
}
size_t write_count = 0;
while (write_count < data.size()) {
- int len =
- write(fd(), &data.data()[write_count], data.size() - write_count);
+ int len = send(fd(), &data.data()[write_count], data.size() - write_count,
+ MSG_NOSIGNAL);
if (len == -1) {
if (errno == EAGAIN) {
struct pollfd waiting;
@@ -61,7 +69,7 @@
waiting.events = POLLOUT;
poll(&waiting, 1, -1);
} else {
- close(fd());
+ CloseConnection();
return;
}
} else {
@@ -70,6 +78,13 @@
if (write_count != data.size()) printf("wrote: %d\n", len);
}
}
+
+ private:
+ void CloseConnection() {
+ loop()->Delete(this);
+ close(fd());
+ delete this;
+ }
};
} // namespace events
diff --git a/aos/vision/events/tcp_client.cc b/aos/vision/events/tcp_client.cc
index 41485f9..4ac45af 100644
--- a/aos/vision/events/tcp_client.cc
+++ b/aos/vision/events/tcp_client.cc
@@ -30,7 +30,7 @@
return 0;
}
-int OpenClient(const char *hostname, int portno) {
+int OpenClient(const std::string &hostname, int portno) {
int sockfd;
struct sockaddr_in serveraddr;
struct hostent *server;
@@ -38,9 +38,9 @@
PCHECK(sockfd = socket(AF_INET, SOCK_STREAM, 0));
/* gethostbyname: get the server's DNS entry */
- server = gethostbyname(hostname);
+ server = gethostbyname(hostname.c_str());
if (server == NULL) {
- fprintf(stderr, "ERROR, no such host as %s\n", hostname);
+ fprintf(stderr, "ERROR, no such host as %s\n", hostname.c_str());
exit(-1);
}
@@ -60,7 +60,7 @@
}
} // namespace
-TcpClient::TcpClient(const char *hostname, int portno)
+TcpClient::TcpClient(const std::string &hostname, int portno)
: EpollEvent(OpenClient(hostname, portno)) {}
} // namespace events
diff --git a/aos/vision/events/tcp_client.h b/aos/vision/events/tcp_client.h
index 7ce01f7..74f1418 100644
--- a/aos/vision/events/tcp_client.h
+++ b/aos/vision/events/tcp_client.h
@@ -4,6 +4,7 @@
#include "aos/vision/events/epoll_events.h"
#include <memory>
+#include <string>
namespace aos {
namespace events {
@@ -11,7 +12,7 @@
// Handles the client connection logic to hostname:portno
class TcpClient : public EpollEvent {
public:
- TcpClient(const char *hostname, int portno);
+ TcpClient(const std::string &hostname, int portno);
// Implement ReadEvent from EpollEvent to use this class.
};
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index b5367f6..4c0437b 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -22,7 +22,8 @@
}
int TXUdpSocket::Send(const char *data, int size) {
- return PCHECK(send(fd_.get(), data, size, 0));
+ // Don't fail on send. If no one is connected that is fine.
+ return send(fd_.get(), data, size, 0);
}
RXUdpSocket::RXUdpSocket(int port)
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index ac25394..5a1ad21 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -18,8 +18,8 @@
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));
+ fname, std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2),
+ params));
camread->StartAsync();
return camread;
}
@@ -33,12 +33,13 @@
void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp) {
if (data.size() < 300) {
- LOG(INFO, "got bad img of size(%zu)\n", data.size());
+ LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
return;
}
ProcessImage(data, timestamp);
}
- virtual void ProcessImage(DataRef data, aos::monotonic_clock::time_point timestamp) = 0;
+ virtual void ProcessImage(DataRef data,
+ aos::monotonic_clock::time_point timestamp) = 0;
void ReadEvent() override { reader_->HandleFrame(); }
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
index 2c9ba56..f8e5e65 100644
--- a/aos/vision/image/image_types.h
+++ b/aos/vision/image/image_types.h
@@ -12,6 +12,14 @@
namespace aos {
namespace vision {
+// Bounding box for a RangeImage.
+struct ImageBBox {
+ int minx = std::numeric_limits<int>::max();
+ int maxx = std::numeric_limits<int>::min();
+ int miny = std::numeric_limits<int>::max();
+ int maxy = std::numeric_limits<int>::min();
+};
+
// This will go into c++17. No sense writing my own version.
using DataRef = std::experimental::string_view;
@@ -19,7 +27,7 @@
struct ImageFormat {
ImageFormat() : w(0), h(0) {}
ImageFormat(int nw, int nh) : w(nw), h(nh) {}
- std::string ToString() {
+ std::string ToString() const {
std::ostringstream s;
s << "ImageFormat {" << w << ", " << h << "}";
return s.str();
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
index 95729da..03e2fc8 100644
--- a/aos/vision/image/reader.cc
+++ b/aos/vision/image/reader.cc
@@ -29,6 +29,9 @@
}
Init();
+
+ InitMMap();
+ LOG(INFO, "Bat Vision Successfully Initialized.\n");
}
void Reader::QueueBuffer(v4l2_buffer *buf) {
@@ -57,6 +60,18 @@
}
--queued_;
+ if (tick_id_ % 10 == 0) {
+ 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");
+ }
+ }
+ ++tick_id_;
// 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.
@@ -66,6 +81,7 @@
reinterpret_cast<const char *>(buffers_[buf.index].start),
buf.bytesused),
time);
+
QueueBuffer(&buf);
}
@@ -105,7 +121,7 @@
}
}
queued_ = kNumBuffers;
- if (req.count < kNumBuffers) {
+ if (req.count != kNumBuffers) {
LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
}
}
@@ -221,7 +237,6 @@
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));
@@ -235,10 +250,6 @@
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() {
diff --git a/aos/vision/image/reader.h b/aos/vision/image/reader.h
index eabb80a..28735fb 100644
--- a/aos/vision/image/reader.h
+++ b/aos/vision/image/reader.h
@@ -41,8 +41,8 @@
void HandleFrame();
void StartAsync() {
- Start();
MMapBuffers();
+ Start();
}
int fd() { return fd_; }
@@ -60,6 +60,7 @@
ProcessCb process_;
+ int tick_id_ = 0;
// The number of buffers currently queued in v4l2.
uint32_t queued_;
struct Buffer;
@@ -67,7 +68,10 @@
// because the buffers are not ummapped.
Buffer *buffers_;
- static const unsigned int kNumBuffers = 10;
+ // TODO(parker): The timestamps should be queue insertion timestamps
+ // which will remove the impact of kNumBuffers.
+ // TODO(parker): Flush the queue (or tweak the FPS) if we fall behind.
+ static const unsigned int kNumBuffers = 5;
// set only at initialize
CameraParams params_;
diff --git a/aos/vision/tools/BUILD b/aos/vision/tools/BUILD
index 56bbb5b..ac6a6ff 100644
--- a/aos/vision/tools/BUILD
+++ b/aos/vision/tools/BUILD
@@ -17,3 +17,14 @@
"//aos/vision/events:gtk_event",
],
)
+
+cc_binary(
+ name = 'camera_primer',
+ srcs = ['camera_primer.cc'],
+ deps = [
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/vision/image:image_stream',
+ '//aos/vision/events:epoll_events',
+ ],
+)
diff --git a/aos/vision/tools/camera_primer.cc b/aos/vision/tools/camera_primer.cc
new file mode 100644
index 0000000..a431ac8
--- /dev/null
+++ b/aos/vision/tools/camera_primer.cc
@@ -0,0 +1,49 @@
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/image_stream.h"
+
+class ImageStream : public aos::vision::ImageStreamEvent {
+ public:
+ ImageStream(const std::string &fname, camera::CameraParams params)
+ : ImageStreamEvent(fname, params) {}
+ void ProcessImage(aos::vision::DataRef /*data*/,
+ aos::monotonic_clock::time_point) override {
+ if (i_ > 20) {
+ exit(0);
+ }
+ ++i_;
+ }
+
+ private:
+ int i_ = 0;
+};
+
+// camera_primer drops the first 20 frames. This is to get around issues
+// where the first N frames from the camera are garbage. Thus each year
+// you should write a startup script like so:
+//
+// camera_primer
+// target_sender
+int main(int argc, char **argv) {
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+
+ camera::CameraParams params = {.width = 640 * 2,
+ .height = 480 * 2,
+ .exposure = 10,
+ .brightness = 128,
+ .gain = 0,
+ .fps = 30};
+
+ if (argc != 2) {
+ fprintf(stderr, "usage: %s path_to_camera\n", argv[0]);
+ exit(-1);
+ }
+ ImageStream stream(argv[1], params);
+
+ aos::events::EpollLoop loop;
+ loop.Add(&stream);
+ loop.Run();
+}
diff --git a/aos/vision/tools/jpeg_vision_test.cc b/aos/vision/tools/jpeg_vision_test.cc
index 37d9ffe..52dd22f 100644
--- a/aos/vision/tools/jpeg_vision_test.cc
+++ b/aos/vision/tools/jpeg_vision_test.cc
@@ -2,42 +2,64 @@
// should not placed on a robot. This is okay as it is a utility of limited use
// only.
-#include <stdio.h>
-#include <stdlib.h>
+#include <gtk/gtk.h>
#include <netdb.h>
#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
#include <string.h>
-#include <gtk/gtk.h>
-#include <vector>
-#include <memory>
#include <fstream>
+#include <memory>
+#include <vector>
-#include "aos/common/logging/logging.h"
#include "aos/common/logging/implementations.h"
-#include "aos/vision/math/vector.h"
-#include "aos/vision/image/reader.h"
-#include "aos/vision/image/jpeg_routines.h"
-#include "aos/vision/blob/threshold.h"
+#include "aos/common/logging/logging.h"
#include "aos/vision/blob/range_image.h"
#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/blob/threshold.h"
#include "aos/vision/events/epoll_events.h"
-#include "aos/vision/image/image_stream.h"
#include "aos/vision/events/tcp_server.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/image/reader.h"
+#include "aos/vision/math/vector.h"
namespace aos {
namespace vision {
+void DrawVLine(ImagePtr ptr, int x, PixelRef color = {255, 0, 0}) {
+ for (int y = 0; y < ptr.fmt().h; ++y) {
+ ptr.get_px(x, y) = color;
+ }
+}
+void DrawHLine(ImagePtr ptr, int y, PixelRef color = {255, 0, 0}) {
+ for (int x = 0; x < ptr.fmt().w; ++x) {
+ ptr.get_px(x, y) = color;
+ }
+}
+
// Connects up a camera with our processing.
class ChannelImageStream : public ImageStreamEvent {
public:
ChannelImageStream(const std::string &fname,
const camera::CameraParams ¶ms)
- : ImageStreamEvent(fname, params), view_(true) {
+ : ImageStreamEvent(fname, params), view_(false) {
// Lambda to record image data to a file on key press.
- view_.view()->key_press_event = [this](uint32_t /*keyval*/) {
- std::ofstream ofs("/tmp/test.jpg", std::ofstream::out);
- ofs << prev_data_;
- ofs.close();
+ view_.view()->key_press_event = [this](uint32_t keyval) {
+ if (keyval == 'j') {
+ std::ofstream ofs("/tmp/test.jpg", std::ofstream::out);
+ ofs << prev_data_;
+ ofs.close();
+ } else if (keyval == 'a') {
+ ++dx;
+ } else if (keyval == 'd') {
+ --dx;
+ } else if (keyval == 'w') {
+ ++dy;
+ } else if (keyval == 's') {
+ --dy;
+ }
+ fprintf(stderr, "dx: %d dy: %d\n", dx, dy);
};
}
@@ -51,7 +73,6 @@
ImagePtr img_ptr = view_.img();
prev_data_ = data.to_string();
-
// Threshold the image with the given lambda.
RangeImage rimg = DoThreshold(img_ptr, [](PixelRef &px) {
if (px.g > 88) {
@@ -65,6 +86,13 @@
return false;
});
+ DrawVLine(img_ptr, fmt.w / 4);
+ DrawVLine(img_ptr, fmt.w / 2 + dy, {0, 255, 0});
+ DrawVLine(img_ptr, fmt.w - fmt.w / 4);
+
+ DrawHLine(img_ptr, fmt.h / 4);
+ DrawHLine(img_ptr, fmt.h / 2 + dx, {0, 255, 0});
+ DrawHLine(img_ptr, fmt.h - fmt.h / 4);
view_.DrawBlobList({rimg}, {255, 255, 255});
view_.Redraw();
@@ -75,6 +103,9 @@
// responsible for handling drawing
BlobStreamViewer view_;
+
+ int dx = 0;
+ int dy = 0;
};
} // namespace aos
} // namespace vision
@@ -91,7 +122,7 @@
.exposure = 10,
.brightness = 128,
.gain = 0,
- .fps = 10};
+ .fps = 25};
aos::vision::ChannelImageStream strm1("/dev/video1", params);
diff --git a/build_tests/BUILD b/build_tests/BUILD
index be6a201..72dc6da 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -74,6 +74,14 @@
proto_cc_library(
name = 'proto_build_test_library',
src = 'proto.proto',
+ deps = [
+ ':proto_build_test_library_base',
+ ],
+)
+
+proto_cc_library(
+ name = 'proto_build_test_library_base',
+ src = 'proto_base.proto',
)
cc_test(
diff --git a/build_tests/proto.cc b/build_tests/proto.cc
index 23ff879..bb09003 100644
--- a/build_tests/proto.cc
+++ b/build_tests/proto.cc
@@ -6,6 +6,7 @@
::frc971::TestProto test_proto1, test_proto2;
test_proto1.set_s("Hi!");
test_proto1.set_i(971);
+ test_proto1.mutable_base_proto()->set_a("silly");
::std::string serialized;
ASSERT_TRUE(test_proto1.SerializeToString(&serialized));
diff --git a/build_tests/proto.proto b/build_tests/proto.proto
index 367a650..d0e6fe2 100644
--- a/build_tests/proto.proto
+++ b/build_tests/proto.proto
@@ -4,9 +4,13 @@
import "google/protobuf/empty.proto";
+import "build_tests/proto_base.proto";
+
message TestProto {
string s = 1;
int32 i = 2;
// Making sure that well-known protos work.
.google.protobuf.Empty empty = 3;
+ // Making sure we can depend on other protos.
+ BaseProto base_proto = 4;
}
diff --git a/build_tests/proto_base.proto b/build_tests/proto_base.proto
new file mode 100644
index 0000000..8fc2143
--- /dev/null
+++ b/build_tests/proto_base.proto
@@ -0,0 +1,8 @@
+syntax = "proto2";
+
+package frc971;
+
+message BaseProto {
+ optional string a = 1;
+ optional string b = 2;
+}
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 8f8b381..7272d48 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -109,9 +109,10 @@
'state_feedback_loop.h',
],
deps = [
- '//third_party/eigen',
+ '//aos/common/controls:control_loop',
'//aos/common/logging',
'//aos/common:macros',
+ '//third_party/eigen',
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6a2822b..aa6cd60 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -112,14 +112,14 @@
break;
}
- kf_.set_controller_index(ControllerIndexFromGears());
+ kf_.set_index(ControllerIndexFromGears());
{
GearLogging gear_logging;
gear_logging.left_state = static_cast<uint32_t>(left_gear_);
gear_logging.right_state = static_cast<uint32_t>(right_gear_);
gear_logging.left_loop_high = MaybeHigh(left_gear_);
gear_logging.right_loop_high = MaybeHigh(right_gear_);
- gear_logging.controller_index = kf_.controller_index();
+ gear_logging.controller_index = kf_.index();
LOG_STRUCT(DEBUG, "state", gear_logging);
}
const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
@@ -151,7 +151,7 @@
rate, angle, down_estimator_.X_hat(0, 0), down_estimator_.X_hat(1, 0));
down_U_(0, 0) = rate;
}
- down_estimator_.UpdateObserver(down_U_);
+ down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
// TODO(austin): Signal the current gear to both loops.
@@ -292,7 +292,7 @@
last_left_voltage_ = left_voltage;
last_right_voltage_ = right_voltage;
- kf_.UpdateObserver(U);
+ kf_.UpdateObserver(U, ::aos::controls::kLoopFrequency);
}
void DrivetrainLoop::Zero(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 403d32c..c66a4e3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -67,11 +67,11 @@
explicit DrivetrainPlant(StateFeedbackPlant<4, 2, 2> &&other)
: StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
- void CheckU() override {
- assert(U(0, 0) <= U_max(0, 0) + 0.00001 + left_voltage_offset_);
- assert(U(0, 0) >= U_min(0, 0) - 0.00001 + left_voltage_offset_);
- assert(U(1, 0) <= U_max(1, 0) + 0.00001 + right_voltage_offset_);
- assert(U(1, 0) >= U_min(1, 0) - 0.00001 + right_voltage_offset_);
+ void CheckU(const Eigen::Matrix<double, 2, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + left_voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + left_voltage_offset_);
+ EXPECT_LE(U(1, 0), U_max(1, 0) + 0.00001 + right_voltage_offset_);
+ EXPECT_GE(U(1, 0), U_min(1, 0) - 0.00001 + right_voltage_offset_);
}
double left_voltage_offset() const { return left_voltage_offset_; }
@@ -154,7 +154,7 @@
last_left_position_ = drivetrain_plant_->Y(0, 0);
last_right_position_ = drivetrain_plant_->Y(1, 0);
EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- drivetrain_plant_->mutable_U() = last_U_;
+ ::Eigen::Matrix<double, 2, 1> U = last_U_;
last_U_ << my_drivetrain_queue_.output->left_voltage,
my_drivetrain_queue_.output->right_voltage;
{
@@ -166,23 +166,21 @@
if (left_gear_high_) {
if (right_gear_high_) {
- drivetrain_plant_->set_plant_index(3);
+ drivetrain_plant_->set_index(3);
} else {
- drivetrain_plant_->set_plant_index(2);
+ drivetrain_plant_->set_index(2);
}
} else {
if (right_gear_high_) {
- drivetrain_plant_->set_plant_index(1);
+ drivetrain_plant_->set_index(1);
} else {
- drivetrain_plant_->set_plant_index(0);
+ drivetrain_plant_->set_index(0);
}
}
- drivetrain_plant_->mutable_U(0, 0) +=
- drivetrain_plant_->left_voltage_offset();
- drivetrain_plant_->mutable_U(1, 0) +=
- drivetrain_plant_->right_voltage_offset();
- drivetrain_plant_->Update();
+ U(0, 0) += drivetrain_plant_->left_voltage_offset();
+ U(1, 0) += drivetrain_plant_->right_voltage_offset();
+ drivetrain_plant_->Update(U);
}
void set_left_voltage_offset(double left_voltage_offset) {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 654ca2c..91c2fc9 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,16 +1,16 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/controls/polytope.h"
#include "aos/common/commonmath.h"
-#include "aos/common/logging/queue_logging.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/logging/logging.h"
#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "aos/common/messages/robot_state.q.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
@@ -22,13 +22,16 @@
U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
- /*[*/ 0, -1 /*]]*/).finished(),
+ /*[*/ 0, -1 /*]]*/)
+ .finished(),
(Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/).finished(),
+ /*[*/ 12 /*]]*/)
+ .finished(),
(Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
- /*[*/ -12, 12, 12, -12 /*]*/).finished()),
+ /*[*/ -12, 12, 12, -12 /*]*/)
+ .finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
ttrust_(1.1),
wheel_(0.0),
@@ -148,19 +151,19 @@
double PolyDrivetrain::FilterVelocity(double throttle) const {
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B().inverse() *
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A());
+ loop_->plant().coefficients(kHighGearController).A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
const double high_min_FF_sum = FF_high.col(0).sum();
const double adjusted_ff_voltage =
@@ -173,14 +176,14 @@
double PolyDrivetrain::MaxVelocity() {
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B().inverse() *
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A());
+ loop_->plant().coefficients(kHighGearController).A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -206,8 +209,8 @@
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
// Invert the plant to figure out how the velocity filter would have to
// work
@@ -241,11 +244,13 @@
// Construct a constraint on R by manipulating the constraint on U
::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
- U_Poly_.static_H() * (loop_->K() + FF),
- U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
- (loop_->K() + FF).inverse() *
- ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
- loop_->K() * loop_->X_hat()));
+ U_Poly_.static_H() * (loop_->controller().K() + FF),
+ U_Poly_.static_k() +
+ U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+ (loop_->controller().K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(
+ U_Poly_.StaticVertices(),
+ loop_->controller().K() * loop_->X_hat()));
// Limit R back inside the box.
loop_->mutable_R() =
@@ -254,7 +259,7 @@
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+ loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
@@ -262,7 +267,7 @@
if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
loop_->mutable_X_hat() =
- loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
+ loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
}
} else {
const double current_left_velocity =
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 20f1a49..570b87f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -17,7 +17,7 @@
PolyDrivetrain(const DrivetrainConfig &dt_config,
StateFeedbackLoop<7, 2, 3> *kf);
- int controller_index() const { return loop_->controller_index(); }
+ int controller_index() const { return loop_->index(); }
// Computes the speed of the motor given the hall effect position and the
// speed of the robot.
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index b7a68a9..4aeb74b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -35,9 +35,11 @@
LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
- position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
+ position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
+ kf_->controller().K(1, 0), kf_->controller().K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
+ velocity_K << kf_->controller().K(0, 1), kf_->controller().K(0, 3),
+ kf_->controller().K(1, 1), kf_->controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -144,7 +146,7 @@
goal.right_velocity_goal, 0.0, 0.0, 0.0;
use_profile_ =
- !kf_->Kff().isZero(0) &&
+ !kf_->controller().Kff().isZero(0) &&
(goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
goal.angular.max_velocity != 0.0 &&
goal.angular.max_acceleration != 0.0);
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 031795a..4d09bbc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -60,7 +60,7 @@
return *loop_;
}
- int controller_index() const { return loop_->controller_index(); }
+ int controller_index() const { return loop_->index(); }
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
@@ -259,8 +259,10 @@
status->estimator_state = this->EstimatorState(0);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power = this->controller().K(0, 0) * error(0, 0);
- status->velocity_power = this->controller().K(0, 1) * error(1, 0);
+ status->position_power =
+ this->controller().controller().K(0, 0) * error(0, 0);
+ status->velocity_power =
+ this->controller().controller().K(0, 1) * error(1, 0);
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index bb34b85..7301390 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -16,7 +16,9 @@
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
+ def __init__(self, gain_schedule_name, loops, namespaces=None,
+ write_constants=False, plant_type='StateFeedbackPlant',
+ observer_type='StateFeedbackObserver'):
"""Constructs a control loop writer.
Args:
@@ -25,6 +27,8 @@
in order.
namespaces: array[string], a list of names of namespaces to nest in
order. If None, the default will be used.
+ plant_type: string, The C++ type of the plant.
+ observer_type: string, The C++ type of the observer.
"""
self._gain_schedule_name = gain_schedule_name
self._loops = loops
@@ -40,6 +44,8 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
def AddConstant(self, constant):
"""Adds a constant to write.
@@ -62,29 +68,46 @@
self.WriteHeader(header_file)
self.WriteCC(os.path.basename(header_file), cc_file)
- def _GenericType(self, typename):
+ def _GenericType(self, typename, extra_args=None):
"""Returns a loop template using typename for the type."""
num_states = self._loops[0].A.shape[0]
num_inputs = self._loops[0].B.shape[1]
num_outputs = self._loops[0].C.shape[0]
- return '%s<%d, %d, %d>' % (
- typename, num_states, num_inputs, num_outputs)
+ if extra_args is not None:
+ extra_args = ', ' + extra_args
+ else:
+ extra_args = ''
+ return '%s<%d, %d, %d%s>' % (
+ typename, num_states, num_inputs, num_outputs, extra_args)
def _ControllerType(self):
"""Returns a template name for StateFeedbackController."""
return self._GenericType('StateFeedbackController')
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType(self._observer_type)
+
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
- return self._GenericType('StateFeedbackLoop')
+ extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
+ return self._GenericType('StateFeedbackLoop', extra_args)
def _PlantType(self):
"""Returns a template name for StateFeedbackPlant."""
- return self._GenericType('StateFeedbackPlant')
+ return self._GenericType(self._plant_type)
- def _CoeffType(self):
+ def _PlantCoeffType(self):
"""Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType('StateFeedbackPlantCoefficients')
+ return self._GenericType(self._plant_type + 'Coefficients')
+
+ def _ControllerCoeffType(self):
+ """Returns a template name for StateFeedbackControllerCoefficients."""
+ return self._GenericType('StateFeedbackControllerCoefficients')
+
+ def _ObserverCoeffType(self):
+ """Returns a template name for StateFeedbackObserverCoefficients."""
+ return self._GenericType(self._observer_type + 'Coefficients')
def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
"""Writes the header file to the file named header_file.
@@ -104,14 +127,22 @@
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlantHeader())
+ fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
fd.write('\n')
fd.write(loop.DumpControllerHeader())
fd.write('\n')
+ fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
+ fd.write('\n')
fd.write('%s Make%sPlant();\n\n' %
(self._PlantType(), self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' %
+ (self._ControllerType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sObserver();\n\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+
fd.write('%s Make%sLoop();\n\n' %
(self._LoopType(), self._gain_schedule_name))
@@ -132,33 +163,55 @@
fd.write(self._namespace_start)
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlant())
+ fd.write(loop.DumpPlant(self._PlantCoeffType()))
fd.write('\n')
for loop in self._loops:
fd.write(loop.DumpController())
fd.write('\n')
+ for loop in self._loops:
+ fd.write(loop.DumpObserver(self._ObserverCoeffType()))
+ fd.write('\n')
+
fd.write('%s Make%sPlant() {\n' %
(self._PlantType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._CoeffType(), len(self._loops)))
+ self._PlantCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._CoeffType(), self._CoeffType(),
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
loop.PlantFunction()))
fd.write(' return %s(&plants);\n' % self._PlantType())
fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write('%s Make%sController() {\n' %
+ (self._ControllerType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerType(), len(self._loops)))
+ self._ControllerCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerType(), self._ControllerType(),
+ (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._LoopType())
+ fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sObserver() {\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
+ self._ObserverCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ObserverCoeffType(), self._ObserverCoeffType(),
+ loop.ObserverFunction()))
+ fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ fd.write(' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
+ (self._LoopType(), self._gain_schedule_name,
+ self._gain_schedule_name, self._gain_schedule_name))
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -250,43 +303,45 @@
return ''.join(ans)
- def DumpPlantHeader(self):
+ def DumpPlantHeader(self, plant_coefficient_type):
"""Writes out a c++ header declaration which will create a Plant object.
Returns:
string, The header declaration for the function.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
- num_states, num_inputs, num_outputs, self._name)
+ return '%s Make%sPlantCoefficients();\n' % (
+ plant_coefficient_type, self._name)
- def DumpPlant(self):
+ def DumpPlant(self, plant_coefficient_type):
"""Writes out a c++ function which will create a PlantCoefficients object.
Returns:
string, The function which will create the object.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
- ' Make%sPlantCoefficients() {\n' % (
- num_states, num_inputs, num_outputs, self._name)]
+ ans = ['%s Make%sPlantCoefficients() {\n' % (
+ plant_coefficient_type, self._name)]
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
- ans.append(self._DumpMatrix('B', self.B))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
ans.append(self._DumpMatrix('C', self.C))
ans.append(self._DumpMatrix('D', self.D))
ans.append(self._DumpMatrix('U_max', self.U_max))
ans.append(self._DumpMatrix('U_min', self.U_min))
- ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
- num_states, num_inputs, num_outputs))
+ if plant_coefficient_type.startswith('StateFeedbackPlant'):
+ ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
+ ans.append(self._DumpMatrix('B', self.B))
+ ans.append(' return %s'
+ '(A, A_inv, B, C, D, U_max, U_min);\n' % (
+ plant_coefficient_type))
+ elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
+ ans.append(' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
+ plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+
ans.append('}\n')
return ''.join(ans)
@@ -296,7 +351,11 @@
def ControllerFunction(self):
"""Returns the name of the controller function."""
- return 'Make%sController()' % self._name
+ return 'Make%sControllerCoefficients()' % self._name
+
+ def ObserverFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sObserverCoefficients()' % self._name
def DumpControllerHeader(self):
"""Writes out a c++ header declaration which will create a Controller object.
@@ -307,7 +366,7 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())
def DumpController(self):
@@ -319,19 +378,77 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
+ ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())]
- ans.append(self._DumpMatrix('L', self.L))
ans.append(self._DumpMatrix('K', self.K))
if not hasattr(self, 'Kff'):
self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
ans.append(self._DumpMatrix('Kff', self.Kff))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
- ans.append(' return StateFeedbackController<%d, %d, %d>'
- '(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
- num_states, num_inputs, num_outputs, self._name))
+ ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d>'
+ '(K, Kff);\n' % (
+ num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
+
+ def DumpObserverHeader(self, observer_coefficient_type):
+ """Writes out a c++ header declaration which will create a Observer object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s %s;\n' % (
+ observer_coefficient_type, self.ObserverFunction())
+
+ def DumpObserver(self, observer_coefficient_type):
+ """Returns a c++ function which will create a Observer object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = ['%s %s {\n' % (
+ observer_coefficient_type, self.ObserverFunction())]
+
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ ans.append(self._DumpMatrix('L', self.L))
+ ans.append(' return %s(L);\n' % (observer_coefficient_type,))
+ elif observer_coefficient_type.startswith('HybridKalman'):
+ ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous))
+ ans.append(self._DumpMatrix('R_continuous', self.R_continuous))
+ ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state))
+ ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
+ observer_coefficient_type,))
+
+ ans.append('}\n')
+ return ''.join(ans)
+
+class HybridControlLoop(ControlLoop):
+ def __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
+
+ def Discritize(self, dt):
+ [self.A, self.B, self.Q, self.R] = \
+ controls.kalmd(self.A_continuous, self.B_continuous,
+ self.Q_continuous, self.R_continuous, dt)
+
+ def PredictHybridObserver(self, U, dt):
+ self.Discritize(dt)
+ self.X_hat = self.A * self.X_hat + self.B * U
+ self.P = (self.A * self.P * self.A.T + self.Q)
+
+ def CorrectHybridObserver(self, U):
+ Y_bar = self.Y - self.C * self.X_hat
+ C_t = self.C.T
+ S = self.C * self.P * C_t + self.R
+ self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
+ self.X_hat = self.X_hat + self.KalmanGain * Y_bar
+ self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+
+ def InitializeState(self):
+ super(HybridControlLoop, self).InitializeState()
+ if hasattr(self, 'Q_steady_state'):
+ self.P = self.Q_steady_state
+ else:
+ self.P = numpy.matrix(numpy.zeros((self.A.shape[0], self.A.shape[0])))
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 211b478..7b0317d 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -151,6 +151,49 @@
return K, P
+
+def kalmd(A_continuous, B_continuous, Q_continuous, R_continuous, dt):
+ """Converts a continuous time kalman filter to discrete time.
+
+ Args:
+ A_continuous: The A continuous matrix
+ B_continuous: the B continuous matrix
+ Q_continuous: The continuous cost matrix
+ R_continuous: The R continuous matrix
+ dt: Timestep
+
+ The math for this is from:
+ https://www.mathworks.com/help/control/ref/kalmd.html
+
+ Returns:
+ The discrete matrices of A, B, Q, and R.
+ """
+ # TODO(austin): Verify that the dimensions make sense.
+ number_of_states = A_continuous.shape[0]
+ number_of_inputs = B_continuous.shape[1]
+ M = numpy.zeros((len(A_continuous) + number_of_inputs,
+ len(A_continuous) + number_of_inputs))
+ M[0:number_of_states, 0:number_of_states] = A_continuous
+ M[0:number_of_states, number_of_states:] = B_continuous
+ M_exp = scipy.linalg.expm(M * dt)
+ A_discrete = M_exp[0:number_of_states, 0:number_of_states]
+ B_discrete = numpy.matrix(M_exp[0:number_of_states, number_of_states:])
+ Q_continuous = (Q_continuous + Q_continuous.T) / 2.0
+ R_continuous = (R_continuous + R_continuous.T) / 2.0
+ M = numpy.concatenate((-A_continuous, Q_continuous), axis=1)
+ M = numpy.concatenate(
+ (M, numpy.concatenate((numpy.matrix(
+ numpy.zeros((number_of_states, number_of_states))),
+ numpy.transpose(A_continuous)), axis = 1)), axis = 0)
+ phi = numpy.matrix(scipy.linalg.expm(M*dt))
+ phi12 = phi[0:number_of_states, number_of_states:(2*number_of_states)]
+ phi22 = phi[number_of_states:2*number_of_states, number_of_states:2*number_of_states]
+ Q_discrete = phi22.T * phi12
+ Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
+ R_discrete = R_continuous / dt
+ return (A_discrete, B_discrete, Q_discrete, R_discrete)
+
+
def TwoStateFeedForwards(B, Q):
"""Computes the feed forwards constant for a 2 state controller.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index a81f2cc..6d05444 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -3,15 +3,23 @@
#include <assert.h>
-#include <vector>
-#include <memory>
#include <iostream>
+#include <memory>
+#include <utility>
+#include <vector>
+#include <chrono>
#include "Eigen/Dense"
+#include "unsupported/Eigen/MatrixFunctions"
+#include "aos/common/controls/control_loop.h"
#include "aos/common/logging/logging.h"
#include "aos/common/macros.h"
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename PlantType, typename ObserverType>
+class StateFeedbackLoop;
+
// For everything in this file, "inputs" and "outputs" are defined from the
// perspective of the plant. This means U is an input and Y is an output
// (because you give the plant U (powers) and it gives you back a Y (sensor
@@ -20,86 +28,36 @@
// input because that's what comes back from the sensors).
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackPlantCoefficients final {
+struct StateFeedbackPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
- : A_(other.A()),
- A_continuous_(other.A_continuous()),
- B_(other.B()),
- B_continuous_(other.B_continuous()),
- C_(other.C()),
- D_(other.D()),
- U_min_(other.U_min()),
- U_max_(other.U_max()) {}
+ : A(other.A),
+ A_inv(other.A_inv),
+ B(other.B),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {}
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
- const Eigen::Matrix<double, number_of_states, number_of_states>
- &A_continuous,
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
- const Eigen::Matrix<double, number_of_states, number_of_inputs>
- &B_continuous,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
- : A_(A),
- A_continuous_(A_continuous),
- B_(B),
- B_continuous_(B_continuous),
- C_(C),
- D_(D),
- U_min_(U_min),
- U_max_(U_max) {}
+ : A(A), A_inv(A_inv), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return A_;
- }
- double A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_continuous()
- const {
- return A_continuous_;
- }
- double A_continuous(int i, int j) const { return A_continuous()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return B_;
- }
- double B(int i, int j) const { return B()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B_continuous() const {
- return B_continuous_;
- }
- double B_continuous(int i, int j) const { return B_continuous()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return C_;
- }
- double C(int i, int j) const { return C()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
- return D_;
- }
- double D(int i, int j) const { return D()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return U_min_;
- }
- double U_min(int i, int j) const { return U_min()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return U_max_;
- }
- double U_max(int i, int j) const { return U_max()(i, j); }
-
- private:
- const Eigen::Matrix<double, number_of_states, number_of_states> A_;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous_;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous_;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> C_;
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D_;
- const Eigen::Matrix<double, number_of_inputs, 1> U_min_;
- const Eigen::Matrix<double, number_of_inputs, 1> U_max_;
-
- StateFeedbackPlantCoefficients &operator=(
- StateFeedbackPlantCoefficients other) = delete;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -109,44 +67,47 @@
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *
- coefficients)
- : coefficients_(::std::move(*coefficients)), plant_index_(0) {
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ *coefficients)
+ : coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
}
StateFeedbackPlant(StateFeedbackPlant &&other)
- : plant_index_(other.plant_index_) {
+ : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
- U_.swap(other.U_);
}
virtual ~StateFeedbackPlant() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return coefficients().A();
+ return coefficients().A;
}
double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv() const {
+ return coefficients().A_inv;
+ }
+ double A_inv(int i, int j) const { return A_inv()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return coefficients().B();
+ return coefficients().B;
}
double B(int i, int j) const { return B()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return coefficients().C();
+ return coefficients().C;
}
double C(int i, int j) const { return C()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
- return coefficients().D();
+ return coefficients().D;
}
double D(int i, int j) const { return D()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return coefficients().U_min();
+ return coefficients().U_min;
}
double U_min(int i, int j) const { return U_min()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return coefficients().U_max();
+ return coefficients().U_max;
}
double U_max(int i, int j) const { return U_max()(i, j); }
@@ -154,50 +115,58 @@
double X(int i, int j) const { return X()(i, j); }
const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
double Y(int i, int j) const { return Y()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
- double U(int i, int j) const { return U()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
double &mutable_X(int i, int j) { return mutable_X()(i, j); }
Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
- Eigen::Matrix<double, number_of_inputs, 1> &mutable_U() { return U_; }
- double &mutable_U(int i, int j) { return mutable_U()(i, j); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &
- coefficients() const {
- return *coefficients_[plant_index_];
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
}
- int plant_index() const { return plant_index_; }
- void set_plant_index(int plant_index) {
- assert(plant_index >= 0);
- assert(plant_index < static_cast<int>(coefficients_.size()));
- plant_index_ = plant_index;
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ int index() const { return index_; }
+ void set_index(int index) {
+ assert(index >= 0);
+ assert(index < static_cast<int>(coefficients_.size()));
+ index_ = index;
}
void Reset() {
X_.setZero();
Y_.setZero();
- U_.setZero();
}
// Assert that U is within the hardware range.
- virtual void CheckU() {
+ virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
for (int i = 0; i < kNumInputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0) + 0.00001);
- assert(U(i, 0) >= U_min(i, 0) - 0.00001);
+ if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ LOG(FATAL, "U out of range\n");
+ }
}
}
// Computes the new X and Y given the control input.
- void Update() {
+ void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
- CheckU();
- X_ = A() * X() + B() * U();
- Y_ = C() * X() + D() * U();
+ CheckU(U);
+ X_ = Update(X(), U);
+ Y_ = C() * X() + D() * U;
+ }
+
+ Eigen::Matrix<double, number_of_states, 1> Update(
+ const Eigen::Matrix<double, number_of_states, 1> X,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U) const {
+ return A() * X + B() * U;
}
protected:
@@ -209,133 +178,614 @@
private:
Eigen::Matrix<double, number_of_states, 1> X_;
Eigen::Matrix<double, number_of_outputs, 1> Y_;
- Eigen::Matrix<double, number_of_inputs, 1> U_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> coefficients_;
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
- int plant_index_;
+ int index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
};
-// A Controller is a structure which holds a plant and the K and L matrices.
-// This is designed such that multiple controllers can share one set of state to
-// support gain scheduling easily.
+// A container for all the controller coefficients.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-struct StateFeedbackController final {
+struct StateFeedbackControllerCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
- StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> plant;
- StateFeedbackController(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ StateFeedbackControllerCoefficients(
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff,
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
- : L(L), K(K), Kff(Kff), A_inv(A_inv), plant(plant) {}
-
- // TODO(Brian): Remove this overload once they're all converted.
- StateFeedbackController(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
- : L(L),
- K(K),
- Kff(::Eigen::Matrix<double, number_of_inputs,
- number_of_states>::Zero()),
- A_inv(A_inv),
- plant(plant) {}
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
+ : K(K), Kff(Kff) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackLoop {
+struct StateFeedbackHybridPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- StateFeedbackLoop(const StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs> &controller)
- : controller_index_(0) {
- controllers_.emplace_back(
- new StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs>(controller));
+ StateFeedbackHybridPlantCoefficients(
+ const StateFeedbackHybridPlantCoefficients &other)
+ : A_continuous(other.A_continuous),
+ B_continuous(other.B_continuous),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {}
+
+ StateFeedbackHybridPlantCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &A_continuous,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ &B_continuous,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ : A_continuous(A_continuous),
+ B_continuous(B_continuous),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max) {}
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackHybridPlant {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ StateFeedbackHybridPlant(
+ ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ *coefficients)
+ : coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
}
- StateFeedbackLoop(::std::vector<::std::unique_ptr<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
- : controllers_(::std::move(*controllers)), controller_index_(0) {
- Reset();
+ StateFeedbackHybridPlant(StateFeedbackHybridPlant &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ X_.swap(other.X_);
+ Y_.swap(other.Y_);
}
- StateFeedbackLoop(StateFeedbackLoop &&other) {
- X_hat_.swap(other.X_hat_);
- R_.swap(other.R_);
- next_R_.swap(other.next_R_);
- U_.swap(other.U_);
- U_uncapped_.swap(other.U_uncapped_);
- ff_U_.swap(other.ff_U_);
- ::std::swap(controllers_, other.controllers_);
- controller_index_ = other.controller_index_;
- }
-
- virtual ~StateFeedbackLoop() {}
+ virtual ~StateFeedbackHybridPlant() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return controller().plant.A();
+ return A_;
}
double A(int i, int j) const { return A()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return controller().plant.B();
+ return B_;
}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv()
- const {
- return controller().A_inv;
- }
- double A_inv(int i, int j) const { return A_inv()(i, j); }
double B(int i, int j) const { return B()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return controller().plant.C();
+ return coefficients().C;
}
double C(int i, int j) const { return C()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
- return controller().plant.D();
+ return coefficients().D;
}
double D(int i, int j) const { return D()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return controller().plant.U_min();
+ return coefficients().U_min;
}
double U_min(int i, int j) const { return U_min()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return controller().plant.U_max();
+ return coefficients().U_max;
}
double U_max(int i, int j) const { return U_max()(i, j); }
+ const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
+ double X(int i, int j) const { return X()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
+ double Y(int i, int j) const { return Y()(i, j); }
+
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
+ double &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
+ double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+
+ const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ int index() const { return index_; }
+ void set_index(int index) {
+ assert(index >= 0);
+ assert(index < static_cast<int>(coefficients_.size()));
+ index_ = index;
+ }
+
+ void Reset() {
+ X_.setZero();
+ Y_.setZero();
+ A_.setZero();
+ B_.setZero();
+ UpdateAB(::aos::controls::kLoopFrequency);
+ }
+
+ // Assert that U is within the hardware range.
+ virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ for (int i = 0; i < kNumInputs; ++i) {
+ if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ LOG(FATAL, "U out of range\n");
+ }
+ }
+ }
+
+ // Computes the new X and Y given the control input.
+ void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ ::std::chrono::nanoseconds dt) {
+ // Powers outside of the range are more likely controller bugs than things
+ // that the plant should deal with.
+ CheckU(U);
+ X_ = Update(X(), U, dt);
+ Y_ = C() * X() + D() * U;
+ }
+
+ Eigen::Matrix<double, number_of_states, 1> Update(
+ const Eigen::Matrix<double, number_of_states, 1> X,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ ::std::chrono::nanoseconds dt) {
+ UpdateAB(dt);
+ return A() * X + B() * U;
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int kNumStates = number_of_states;
+ static const int kNumOutputs = number_of_outputs;
+ static const int kNumInputs = number_of_inputs;
+
+ private:
+ void UpdateAB(::std::chrono::nanoseconds dt) {
+ Eigen::Matrix<double, number_of_states + number_of_inputs,
+ number_of_states + number_of_inputs>
+ M_state_continuous;
+ M_state_continuous.setZero();
+ M_state_continuous.template block<number_of_states, number_of_states>(0,
+ 0) =
+ coefficients().A_continuous *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+ M_state_continuous.template block<number_of_states, number_of_inputs>(
+ 0, number_of_states) =
+ coefficients().B_continuous *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+
+ Eigen::Matrix<double, number_of_states + number_of_inputs,
+ number_of_states + number_of_inputs>
+ M_state = M_state_continuous.exp();
+ A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
+ B_ = M_state.template block<number_of_states, number_of_inputs>(
+ 0, number_of_states);
+ }
+
+ Eigen::Matrix<double, number_of_states, 1> X_;
+ Eigen::Matrix<double, number_of_outputs, 1> Y_;
+
+ Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+
+
+ ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+
+ int index_;
+
+ DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackController {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit StateFeedbackController(
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ : coefficients_(::std::move(*controllers)) {}
+
+ StateFeedbackController(StateFeedbackController &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
- return controller().K;
+ return coefficients().K;
}
double K(int i, int j) const { return K()(i, j); }
const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
- return controller().Kff;
+ return coefficients().Kff;
}
double Kff(int i, int j) const { return Kff()(i, j); }
+
+ void Reset() {}
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackObserverCoefficients final {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+
+ StateFeedbackObserverCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+ : L(L) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackObserver {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit StateFeedbackObserver(
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ : coefficients_(::std::move(*observers)) {}
+
+ StateFeedbackObserver(StateFeedbackObserver &&other)
+ : X_hat_(other.X_hat_), index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
- return controller().L;
+ return coefficients().L;
}
double L(int i, int j) const { return L()(i, j); }
const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
return X_hat_;
}
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+
+ void Reset(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> * /*loop*/) {
+ X_hat_.setZero();
+ }
+
+ void Predict(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> *loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds /*dt*/) {
+ mutable_X_hat() = loop->plant().Update(X_hat(), new_u);
+ }
+
+ void Correct(const StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> &loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ mutable_X_hat() += loop.plant().A_inv() * L() *
+ (Y - loop.plant().C() * X_hat() - loop.plant().D() * U);
+ }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ // Internal state estimate.
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct HybridKalmanCoefficients final {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> Q_continuous;
+ const Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_continuous;
+ const Eigen::Matrix<double, number_of_states, number_of_states> P_steady_state;
+
+ HybridKalmanCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &Q_continuous,
+ const Eigen::Matrix<double, number_of_outputs, number_of_outputs>
+ &R_continuous,
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &P_steady_state)
+ : Q_continuous(Q_continuous),
+ R_continuous(R_continuous),
+ P_steady_state(P_steady_state) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class HybridKalman {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit HybridKalman(
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ : coefficients_(::std::move(*observers)) {}
+
+ HybridKalman(HybridKalman &&other)
+ : X_hat_(other.X_hat_), index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ // Getters for Q
+ const Eigen::Matrix<double, number_of_states, number_of_states> &Q() const {
+ return Q_;
+ }
+ double Q(int i, int j) const { return Q()(i, j); }
+ // Getters for R
+ const Eigen::Matrix<double, number_of_outputs, number_of_outputs> &R() const {
+ return R_;
+ }
+ double R(int i, int j) const { return R()(i, j); }
+
+ // Getters for P
+ const Eigen::Matrix<double, number_of_states, number_of_states> &P() const {
+ return P_;
+ }
+ double P(int i, int j) const { return P()(i, j); }
+
+ // Getters for X_hat
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return X_hat_;
+ }
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+
+ void Reset(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop) {
+ X_hat_.setZero();
+ P_ = coefficients().P_steady_state;
+ UpdateQR(loop, ::aos::controls::kLoopFrequency);
+ }
+
+ void Predict(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt) {
+ // Trigger the predict step. This will update A() and B() in the plant.
+ mutable_X_hat() = loop->mutable_plant()->Update(X_hat(), new_u, dt);
+
+ UpdateQR(loop, dt);
+ P_ = loop->plant().A() * P_ * loop->plant().A().transpose() + Q_;
+ }
+
+ void Correct(const StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> &loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ Eigen::Matrix<double, number_of_outputs, 1> Y_bar =
+ Y - (loop.plant().C() * X_hat_ + loop.plant().D() * U);
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
+ loop.plant().C() * P_ * loop.plant().C().transpose() + R_;
+ Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
+ KalmanGain = (S.transpose().ldlt().solve(
+ (P() * loop.plant().C().transpose()).transpose()))
+ .transpose();
+ X_hat_ = X_hat_ + KalmanGain * Y_bar;
+ P_ = (loop.plant().coefficients().A_continuous.Identity() -
+ KalmanGain * loop.plant().C()) *
+ P();
+ }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const HybridKalmanCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const HybridKalmanCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ void UpdateQR(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop,
+ ::std::chrono::nanoseconds dt) {
+ // Now, compute the discrete time Q and R coefficients.
+ Eigen::Matrix<double, number_of_states, number_of_states> Qtemp =
+ (coefficients().Q_continuous +
+ coefficients().Q_continuous.transpose()) /
+ 2.0;
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> Rtemp =
+ (coefficients().R_continuous +
+ coefficients().R_continuous.transpose()) /
+ 2.0;
+
+ Eigen::Matrix<double, 2 * number_of_states, 2 * number_of_states> M_gain;
+ M_gain.setZero();
+ // Set up the matrix M = [[-A, Q], [0, A.T]]
+ M_gain.template block<number_of_states, number_of_states>(0, 0) =
+ -loop->plant().coefficients().A_continuous;
+ M_gain.template block<number_of_states, number_of_states>(
+ 0, number_of_states) = Qtemp;
+ M_gain.template block<number_of_states, number_of_states>(
+ number_of_states, number_of_states) =
+ loop->plant().coefficients().A_continuous.transpose();
+
+ Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
+ (M_gain *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count())
+ .exp();
+
+ // Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
+ // Phi22 = phi[number_of_states:2*number_of_states,
+ // number_of_states:2*number_of_states]
+ Eigen::Matrix<double, number_of_states, number_of_states> phi12 =
+ phi.block(0, number_of_states, number_of_states, number_of_states);
+ Eigen::Matrix<double, number_of_states, number_of_states> phi22 = phi.block(
+ number_of_states, number_of_states, number_of_states, number_of_states);
+
+ Q_ = phi22.transpose() * phi12;
+ Q_ = (Q_ + Q_.transpose()) / 2.0;
+ R_ = Rtemp /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+ }
+
+ // Internal state estimate.
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ // Internal covariance estimate.
+ Eigen::Matrix<double, number_of_states, number_of_states> P_;
+
+ // Discretized Q and R for the kalman filter.
+ Eigen::Matrix<double, number_of_states, number_of_states> Q_;
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_;
+
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename PlantType = StateFeedbackPlant<
+ number_of_states, number_of_inputs, number_of_outputs>,
+ typename ObserverType = StateFeedbackObserver<
+ number_of_states, number_of_inputs, number_of_outputs>>
+class StateFeedbackLoop {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit StateFeedbackLoop(
+ PlantType &&plant,
+ StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &&controller,
+ ObserverType &&observer)
+ : plant_(::std::move(plant)),
+ controller_(::std::move(controller)),
+ observer_(::std::move(observer)) {
+ Reset();
+ }
+
+ StateFeedbackLoop(StateFeedbackLoop &&other)
+ : plant_(::std::move(other.plant_)),
+ controller_(::std::move(other.controller_)),
+ observer_(::std::move(other.observer_)) {
+ R_.swap(other.R_);
+ next_R_.swap(other.next_R_);
+ U_.swap(other.U_);
+ U_uncapped_.swap(other.U_uncapped_);
+ ff_U_.swap(other.ff_U_);
+ }
+
+ virtual ~StateFeedbackLoop() {}
+
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return observer().X_hat();
+ }
double X_hat(int i, int j) const { return X_hat()(i, j); }
const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
double R(int i, int j) const { return R()(i, j); }
@@ -354,7 +804,9 @@
}
double ff_U(int i, int j) const { return ff_U()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() {
+ return observer_.mutable_X_hat();
+ }
double &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_R() { return R_; }
double &mutable_R(int i, int j) { return mutable_R()(i, j); }
@@ -371,42 +823,44 @@
return mutable_U_uncapped()(i, j);
}
- const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &
- controller() const {
- return *controllers_[controller_index_];
- }
+ const PlantType &plant() const { return plant_; }
+ PlantType *mutable_plant() { return &plant_; }
const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &
- controller(int index) const {
- return *controllers_[index];
+ number_of_outputs>
+ &controller() const {
+ return controller_;
}
+ const ObserverType &observer() const { return observer_; }
+
void Reset() {
- X_hat_.setZero();
R_.setZero();
next_R_.setZero();
U_.setZero();
U_uncapped_.setZero();
ff_U_.setZero();
+
+ plant_.Reset();
+ controller_.Reset();
+ observer_.Reset(this);
}
// If U is outside the hardware range, limit it before the plant tries to use
// it.
virtual void CapU() {
for (int i = 0; i < kNumInputs; ++i) {
- if (U(i, 0) > U_max(i, 0)) {
- U_(i, 0) = U_max(i, 0);
- } else if (U(i, 0) < U_min(i, 0)) {
- U_(i, 0) = U_min(i, 0);
+ if (U(i, 0) > plant().U_max(i, 0)) {
+ U_(i, 0) = plant().U_max(i, 0);
+ } else if (U(i, 0) < plant().U_min(i, 0)) {
+ U_(i, 0) = plant().U_min(i, 0);
}
}
}
// Corrects X_hat given the observation in Y.
void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- X_hat_ += A_inv() * L() * (Y - C() * X_hat_ - D() * U());
+ observer_.Correct(*this, U(), Y);
}
const Eigen::Matrix<double, number_of_states, 1> error() const {
@@ -415,17 +869,20 @@
// Returns the calculated controller power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+ // TODO(austin): Should this live in StateSpaceController?
ff_U_ = FeedForward();
- return K() * error() + ff_U_;
+ return controller().K() * error() + ff_U_;
}
// Calculates the feed forwards power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
- return Kff() * (next_R() - A() * R());
+ // TODO(austin): Should this live in StateSpaceController?
+ return controller().Kff() * (next_R() - plant().A() * R());
}
// stop_motors is whether or not to output all 0s.
- void Update(bool stop_motors) {
+ void Update(bool stop_motors,
+ ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
if (stop_motors) {
U_.setZero();
U_uncapped_.setZero();
@@ -435,7 +892,7 @@
CapU();
}
- UpdateObserver(U_);
+ UpdateObserver(U_, dt);
UpdateFFReference();
}
@@ -443,31 +900,32 @@
// Updates R() after any CapU operations happen on U().
void UpdateFFReference() {
ff_U_ -= U_uncapped() - U();
- if (!Kff().isZero(0)) {
- R_ = A() * R() + B() * ff_U_;
+ if (!controller().Kff().isZero(0)) {
+ R_ = plant().A() * R() + plant().B() * ff_U_;
}
}
- void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
- X_hat_ = A() * X_hat() + B() * new_u;
+ void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt) {
+ observer_.Predict(this, new_u, dt);
}
- // Sets the current controller to be index, clamped to be within range.
- void set_controller_index(int index) {
- if (index < 0) {
- controller_index_ = 0;
- } else if (index >= static_cast<int>(controllers_.size())) {
- controller_index_ = static_cast<int>(controllers_.size()) - 1;
- } else {
- controller_index_ = index;
- }
+ // Sets the current controller to be index.
+ void set_index(int index) {
+ plant_.set_index(index);
+ controller_.set_index(index);
+ observer_.set_index(index);
}
- int controller_index() const { return controller_index_; }
+ int index() const { return plant_.index(); }
protected:
- ::std::vector<::std::unique_ptr<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs>>> controllers_;
+ PlantType plant_;
+
+ StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+ controller_;
+
+ ObserverType observer_;
// These are accessible from non-templated subclasses.
static constexpr int kNumStates = number_of_states;
@@ -478,8 +936,6 @@
Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
private:
- // Internal state estimate.
- Eigen::Matrix<double, number_of_states, 1> X_hat_;
// Current goal (Used by the feed-back controller).
Eigen::Matrix<double, number_of_states, 1> R_;
// Goal to go to in the next cycle (Used by Feed-Forward controller.)
@@ -489,8 +945,6 @@
// Computed output before being capped.
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
- int controller_index_;
-
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index a08841e..072d044 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -6,6 +6,114 @@
namespace control_loops {
namespace testing {
+StateFeedbackHybridPlantCoefficients<3, 1, 1>
+MakeIntegralShooterPlantCoefficients() {
+ Eigen::Matrix<double, 1, 3> C;
+ C(0, 0) = 1.0;
+ C(0, 1) = 0.0;
+ C(0, 2) = 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D(0, 0) = 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max(0, 0) = 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min(0, 0) = -12.0;
+ Eigen::Matrix<double, 3, 3> A_continuous;
+ A_continuous(0, 0) = 0.0;
+ A_continuous(0, 1) = 1.0;
+ A_continuous(0, 2) = 0.0;
+ A_continuous(1, 0) = 0.0;
+ A_continuous(1, 1) = -8.1021414789556374;
+ A_continuous(1, 2) = 443.75;
+ A_continuous(2, 0) = 0.0;
+ A_continuous(2, 1) = 0.0;
+ A_continuous(2, 2) = 0.0;
+ Eigen::Matrix<double, 3, 1> B_continuous;
+ B_continuous(0, 0) = 0.0;
+ B_continuous(1, 0) = 443.75;
+ B_continuous(2, 0) = 0.0;
+ return StateFeedbackHybridPlantCoefficients<3, 1, 1>(
+ A_continuous, B_continuous, C, D, U_max, U_min);
+}
+
+StateFeedbackControllerCoefficients<3, 1, 1>
+MakeIntegralShooterControllerCoefficients() {
+ Eigen::Matrix<double, 1, 3> K;
+ K(0, 0) = 0.0;
+ K(0, 1) = 0.027731156542808996;
+ K(0, 2) = 1.0;
+ Eigen::Matrix<double, 1, 3> Kff;
+ Kff(0, 0) = 0.0;
+ Kff(0, 1) = 0.45989503537638587;
+ Kff(0, 2) = 0.0;
+ return StateFeedbackControllerCoefficients<3, 1, 1>(K, Kff);
+}
+
+HybridKalmanCoefficients<3, 1, 1> MakeIntegralShooterObserverCoefficients() {
+ Eigen::Matrix<double, 3, 3> Q_continuous;
+ Q_continuous(0, 0) = 0.0001;
+ Q_continuous(0, 1) = 0.0;
+ Q_continuous(0, 2) = 0.0;
+ Q_continuous(1, 0) = 0.0;
+ Q_continuous(1, 1) = 4.0;
+ Q_continuous(1, 2) = 0.0;
+ Q_continuous(2, 0) = 0.0;
+ Q_continuous(2, 1) = 0.0;
+ Q_continuous(2, 2) = 0.040000000000000008;
+ Eigen::Matrix<double, 1, 1> R_continuous;
+ R_continuous(0, 0) = 9.9999999999999995e-07;
+ Eigen::Matrix<double, 3, 3> P_steady_state;
+ P_steady_state(0, 0) = 7.1645559451160497e-05;
+ P_steady_state(0, 1) = 0.0031205034236441768;
+ P_steady_state(0, 2) = 0.00016022137220036598;
+ P_steady_state(1, 0) = 0.0031205034236441768;
+ P_steady_state(1, 1) = 0.25313549121689616;
+ P_steady_state(1, 2) = 0.015962850974712596;
+ P_steady_state(2, 0) = 0.00016022137220036598;
+ P_steady_state(2, 1) = 0.015962850974712596;
+ P_steady_state(2, 2) = 0.0019821816120708254;
+ return HybridKalmanCoefficients<3, 1, 1>(Q_continuous, R_continuous,
+ P_steady_state);
+}
+
+StateFeedbackHybridPlant<3, 1, 1> MakeIntegralShooterPlant() {
+ ::std::vector<
+ ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>>
+ plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
+ new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
+ MakeIntegralShooterPlantCoefficients()));
+ return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>>
+ controllers(1);
+ controllers[0] =
+ ::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
+ new StateFeedbackControllerCoefficients<3, 1, 1>(
+ MakeIntegralShooterControllerCoefficients()));
+ return StateFeedbackController<3, 1, 1>(&controllers);
+}
+
+HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>> observers(
+ 1);
+ observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
+ new HybridKalmanCoefficients<3, 1, 1>(
+ MakeIntegralShooterObserverCoefficients()));
+ return HybridKalman<3, 1, 1>(&observers);
+}
+
+StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+MakeIntegralShooterLoop() {
+ return StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
+ MakeIntegralShooterPlant(), MakeIntegralShooterController(),
+ MakeIntegralShooterObserver());
+}
+
// Tests that everything compiles and nothing crashes even if
// number_of_inputs!=number_of_outputs.
// There used to be lots of bugs in this area.
@@ -16,39 +124,66 @@
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
- Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
Eigen::Matrix<double, 4, 1>::Constant(1),
Eigen::Matrix<double, 4, 1>::Constant(-1));
- {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>> v;
- v.emplace_back(new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
- StateFeedbackPlant<2, 4, 7> plant(&v);
- plant.Update();
- plant.Reset();
- plant.CheckU();
- }
- {
- StateFeedbackLoop<2, 4, 7> test_loop(StateFeedbackController<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(),
- Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
- test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
- test_loop.Update(false);
- test_loop.CapU();
- }
- {
- StateFeedbackLoop<2, 4, 7> test_loop(StateFeedbackController<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(),
- Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
- test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
- test_loop.Update(false);
- test_loop.CapU();
- }
+ // Build a plant.
+ ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
+ v_plant;
+ v_plant.emplace_back(
+ new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
+ StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+ plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
+ plant.Reset();
+ plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
+
+ // Now build a controller.
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<2, 4, 7>>>
+ v_controller;
+ v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity()));
+ StateFeedbackController<2, 4, 7> controller(&v_controller);
+
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
+ v_observer;
+ v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity()));
+ StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ ::std::move(plant), ::std::move(controller), ::std::move(observer));
+ test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
+ test_loop.Update(false);
+ test_loop.CapU();
+}
+
+// Tests that the continuous to discrete calculation for the kalman filter
+// matches what was computed both in Python and in Matlab.
+TEST(StateFeedbackLoopTest, PythonMatch) {
+ auto test_loop = MakeIntegralShooterLoop();
+ test_loop.Update(false, ::std::chrono::milliseconds(5));
+
+ Eigen::Matrix<double, 3, 3> A_discrete;
+ A_discrete << 1, 0.00490008, 0.00547272, 0, 0.96029888, 2.17440921, 0, 0, 1;
+
+ Eigen::Matrix<double, 3, 1> B_discrete;
+ B_discrete << 0.00547272, 2.17440921, 0;
+
+ Eigen::Matrix<double, 3, 3> Q_discrete;
+ Q_discrete << 6.62900602e-07, 4.86205253e-05, 3.66076676e-07, 4.86205253e-05,
+ 1.95296358e-02, 2.18908995e-04, 3.66076676e-07, 2.18908995e-04,
+ 2.00000000e-04;
+
+ Eigen::Matrix<double, 1, 1> R_discrete;
+ R_discrete << 0.0002;
+
+ EXPECT_TRUE(A_discrete.isApprox(test_loop.plant().A(), 0.001));
+ EXPECT_TRUE(B_discrete.isApprox(test_loop.plant().B(), 0.001));
+ EXPECT_TRUE(Q_discrete.isApprox(test_loop.observer().Q(), 0.001));
+ EXPECT_TRUE(R_discrete.isApprox(test_loop.observer().R(), 0.001));
}
} // namespace testing
diff --git a/tools/build_rules/protobuf.bzl b/tools/build_rules/protobuf.bzl
index d5f4284..c9bb9ef 100644
--- a/tools/build_rules/protobuf.bzl
+++ b/tools/build_rules/protobuf.bzl
@@ -1,16 +1,22 @@
def _do_proto_cc_library_impl(ctx):
+ srcs = ctx.files.srcs
+ deps = []
+ deps += ctx.files.srcs
+
+ for dep in ctx.attr.deps:
+ deps += dep.proto.deps
+
message = 'Building %s and %s from %s' % (ctx.outputs.pb_h.short_path,
ctx.outputs.pb_cc.short_path,
- ctx.file.src.short_path)
+ ctx.files.srcs[0].short_path)
ctx.action(
- inputs = ctx.files.src + ctx.files._well_known_protos,
+ inputs = deps + ctx.files._well_known_protos,
executable = ctx.executable._protoc,
arguments = [
'--cpp_out=%s' % ctx.configuration.genfiles_dir.path,
'-I.',
'-Ithird_party/protobuf/src',
- ctx.file.src.path,
- ],
+ ] + [s.path for s in srcs],
mnemonic = 'ProtocCc',
progress_message = message,
outputs = [
@@ -19,8 +25,15 @@
],
)
-def _do_proto_cc_library_outputs(src):
- basename = src.name[:-len('.proto')]
+ return struct(
+ proto = struct(
+ srcs = srcs,
+ deps = deps,
+ )
+ )
+
+def _do_proto_cc_library_outputs(srcs):
+ basename = srcs[0].name[:-len('.proto')]
return {
'pb_h': '%s.pb.h' % basename,
'pb_cc': '%s.pb.cc' % basename,
@@ -29,11 +42,10 @@
_do_proto_cc_library = rule(
implementation = _do_proto_cc_library_impl,
attrs = {
- 'src': attr.label(
+ 'srcs': attr.label_list(
allow_files = FileType(['.proto']),
- mandatory = True,
- single_file = True,
),
+ 'deps': attr.label_list(providers = ["proto"]),
'_protoc': attr.label(
default = Label('//third_party/protobuf:protoc'),
executable = True,
@@ -47,7 +59,7 @@
output_to_genfiles = True,
)
-def proto_cc_library(name, src, visibility = None):
+def proto_cc_library(name, src, deps = [], visibility = None):
'''Generates a cc_library from a single .proto file. Does not support
dependencies on any .proto files except the well-known ones protobuf comes
with (which are unconditionally depended on).
@@ -58,7 +70,8 @@
_do_proto_cc_library(
name = '%s__proto_srcs' % name,
- src = src,
+ srcs = [src],
+ deps = [('%s__proto_srcs' % o_name) for o_name in deps],
visibility = ['//visibility:private'],
)
basename = src[:-len('.proto')]
@@ -66,6 +79,6 @@
name = name,
srcs = [ '%s.pb.cc' % basename ],
hdrs = [ '%s.pb.h' % basename ],
- deps = [ '//third_party/protobuf' ],
+ deps = [ '//third_party/protobuf' ] + deps,
visibility = visibility,
)
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 7ab4f3d..418b591 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -24,11 +24,11 @@
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
loop_(constants::GetValues().make_drivetrain_loop()) {
- loop_.set_controller_index(3);
+ loop_.set_index(3);
}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
+ static const auto &K = loop_.controller().K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 9320e6b..86ec084 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -106,11 +106,11 @@
// (H * position_K) * position_error <= k - H * UVel
Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 1),
- K(1, 0), K(1, 1);
+ position_K << controller().K(0, 0), controller().K(0, 1),
+ controller().K(1, 0), controller().K(1, 1);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 2), K(0, 3),
- K(1, 2), K(1, 3);
+ velocity_K << controller().K(0, 2), controller().K(0, 3),
+ controller().K(1, 2), controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(1, 0);
@@ -924,19 +924,19 @@
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::max(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -946,19 +946,19 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::min(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 4079da4..8444cea 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -214,9 +214,10 @@
const constants::Values& v = constants::GetValues();
EXPECT_TRUE(claw_queue.output.FetchLatest());
- claw_plant_->mutable_U() << claw_queue.output->bottom_claw_voltage,
+ Eigen::Matrix<double, 2, 1> U;
+ U << claw_queue.output->bottom_claw_voltage,
claw_queue.output->top_claw_voltage;
- claw_plant_->Update();
+ claw_plant_->Update(U);
// Check that the claw is within the limits.
EXPECT_GE(v.claw.upper_claw.upper_limit, claw_plant_->Y(0, 0));
@@ -541,7 +542,8 @@
claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
0.0;
Eigen::Matrix<double, 2, 1> uncapped_voltage =
- claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat());
+ claw_motor_.claw_.controller().K() *
+ (R - claw_motor_.claw_.X_hat());
// Use a factor of 1.8 because so long as it isn't actually running
// away, the CapU function will deal with getting the actual output
// down.
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index af310f0..70a6cad 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -56,13 +56,14 @@
void ZeroedStateFeedbackLoop::CapGoal() {
if (uncapped_voltage() > max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() - max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -70,13 +71,14 @@
::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() + max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -88,10 +90,11 @@
}
void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
- if (controller_index() == 0) {
- mutable_R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ if (index() == 0) {
+ mutable_R(2, 0) = (-plant().A(1, 0) / plant().A(1, 2) * R(0, 0) -
+ plant().A(1, 1) / plant().A(1, 2) * R(1, 0));
} else {
- mutable_R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ mutable_R(2, 0) = -plant().A(1, 1) / plant().A(1, 2) * R(1, 0);
}
}
@@ -104,8 +107,8 @@
mutable_X_hat(0, 0) += doffset;
// Offset the goal so we don't move.
mutable_R(0, 0) += doffset;
- if (controller_index() == 0) {
- mutable_R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ if (index() == 0) {
+ mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
LOG_STRUCT(DEBUG, "sensor edge (fake?)",
::y2014::control_loops::ShooterChangeCalibration(
@@ -257,16 +260,16 @@
// Probably not needed yet.
if (position) {
- int last_controller_index = shooter_.controller_index();
+ int last_index = shooter_.index();
if (position->plunger && position->latch) {
// Use the controller without the spring if the latch is set and the
// plunger is back
- shooter_.set_controller_index(1);
+ shooter_.set_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_index(0);
}
- if (shooter_.controller_index() != last_controller_index) {
+ if (shooter_.index() != last_index) {
shooter_.RecalculatePowerGoal();
}
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index fb991e3..f2ddbc8 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -83,8 +83,9 @@
desired_velocity);
mutable_R() << desired_position - kPositionOffset, desired_velocity,
- (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity);
+ (-plant().A(1, 0) / plant().A(1, 2) *
+ (desired_position - kPositionOffset) -
+ plant().A(1, 1) / plant().A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 9248725..df3f5fd 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -102,12 +102,12 @@
// Only disengage the spring if we are greater than 0, which is where the
// latch will take the load off the pusher.
if (GetAbsolutePosition() > 0.0) {
- shooter_plant_->set_plant_index(1);
+ shooter_plant_->set_index(1);
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
}
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
@@ -221,16 +221,14 @@
}
if (brake_piston_state_) {
- shooter_plant_->mutable_U() << 0.0;
shooter_plant_->mutable_X(1, 0) = 0.0;
- shooter_plant_->mutable_Y() = shooter_plant_->C() * shooter_plant_->X() +
- shooter_plant_->D() * shooter_plant_->U();
+ shooter_plant_->mutable_Y() = shooter_plant_->C() * shooter_plant_->X();
} else {
- shooter_plant_->mutable_U() << last_voltage_;
- //shooter_plant_->U << shooter_queue_.output->voltage;
- shooter_plant_->Update();
+ Eigen::Matrix<double, 1, 1> U;
+ U << last_voltage_;
+ shooter_plant_->Update(U);
}
- LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
+ LOG(DEBUG, "Plant index is %d\n", shooter_plant_->index());
// Handle latch hall effect
if (!latch_piston_state_ && latch_delay_count_ > 0) {
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 8772af7..91873eb 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -25,7 +25,8 @@
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = constants::GetValues().make_drivetrain_loop().K();
+ static const auto K =
+ constants::GetValues().make_drivetrain_loop().controller().K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index 25b5b9d..6a56105 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -25,7 +25,7 @@
double ClawCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
// Compute K matrix to compensate for position errors.
- double Kp = K(0, 0);
+ double Kp = controller().K(0, 0);
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
diff --git a/y2015/control_loops/claw/claw_lib_test.cc b/y2015/control_loops/claw/claw_lib_test.cc
index ed82322..72f84b8 100644
--- a/y2015/control_loops/claw/claw_lib_test.cc
+++ b/y2015/control_loops/claw/claw_lib_test.cc
@@ -74,8 +74,9 @@
EXPECT_TRUE(claw_queue_.output.FetchLatest());
// Feed voltages into physics simulation.
- claw_plant_->mutable_U() << claw_queue_.output->voltage;
- claw_plant_->Update();
+ ::Eigen::Matrix<double, 1, 1> U;
+ U << claw_queue_.output->voltage;
+ claw_plant_->Update(U);
const double wrist_angle = claw_plant_->Y(0, 0);
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index a6acae7..d34cd33 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -39,8 +39,8 @@
// Compute the K matrix used to compensate for position errors.
Eigen::Matrix<double, 2, 2> Kp;
Kp.setZero();
- Kp.col(0) = this->K().col(0);
- Kp.col(1) = this->K().col(2);
+ Kp.col(0) = this->controller().K().col(0);
+ Kp.col(1) = this->controller().K().col(2);
Eigen::Matrix<double, 2, 2> Kp_inv = Kp.inverse();
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
index 16a1f15..8f12730 100644
--- a/y2015/control_loops/fridge/fridge_lib_test.cc
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -133,23 +133,25 @@
void Simulate() {
EXPECT_TRUE(fridge_queue_.output.FetchLatest());
+ ::Eigen::Matrix<double, 2, 1> arm_U;
+ ::Eigen::Matrix<double, 2, 1> elevator_U;
// Feed voltages into physics simulation.
if (arm_power_error_ != 0.0) {
- arm_plant_->mutable_U() << ::aos::Clip(
+ arm_U << ::aos::Clip(
fridge_queue_.output->left_arm + arm_power_error_, -12, 12),
::aos::Clip(fridge_queue_.output->right_arm + arm_power_error_, -12,
12);
} else {
- arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
+ arm_U << fridge_queue_.output->left_arm,
fridge_queue_.output->right_arm;
}
- elevator_plant_->mutable_U() << fridge_queue_.output->left_elevator,
+ elevator_U << fridge_queue_.output->left_elevator,
fridge_queue_.output->right_elevator;
// Use the plant to generate the next physical state given the voltages to
// the motors.
- arm_plant_->Update();
- elevator_plant_->Update();
+ arm_plant_->Update(arm_U);
+ elevator_plant_->Update(elevator_U);
const double left_arm_angle = arm_plant_->Y(0, 0);
const double right_arm_angle = arm_plant_->Y(1, 0);
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 29a55a6..b601853 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -112,7 +112,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ 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]]])
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -178,7 +178,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 313a818..f9587a0 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -30,7 +30,9 @@
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
static const auto K =
- ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop().K();
+ ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop()
+ .controller()
+ .K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2015_bot3/control_loops/elevator/elevator.cc b/y2015_bot3/control_loops/elevator/elevator.cc
index d8b1368..eb3d870 100644
--- a/y2015_bot3/control_loops/elevator/elevator.cc
+++ b/y2015_bot3/control_loops/elevator/elevator.cc
@@ -20,7 +20,7 @@
double SimpleCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
// Compute K matrix to compensate for position errors.
- double Kp = K(0, 0);
+ double Kp = controller().K(0, 0);
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
diff --git a/y2015_bot3/control_loops/elevator/elevator_lib_test.cc b/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
index 3455f1d..ba3d5a6 100644
--- a/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
+++ b/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
@@ -39,6 +39,7 @@
".y2015_bot3.control_loops.elevator_queue.status") {
// Initialize the elevator.
InitializePosition(kElevLowerLimit);
+ U_.setZero();
}
void InitializePosition(double start_pos) {
@@ -67,9 +68,9 @@
void Simulate() {
EXPECT_TRUE(queue_.output.FetchLatest());
- plant_->mutable_U() << queue_.output->elevator;
+ U_ << queue_.output->elevator;
- plant_->Update();
+ plant_->Update(U_);
plant_->mutable_X()(1, 0) += acceleration_offset_ * 0.005;
const double height = plant_->Y(0, 0);
@@ -82,10 +83,11 @@
void MoveTo(double position) { position_sim_.MoveTo(position); }
- double GetVoltage() const { return plant_->U()(0,0); }
+ double GetVoltage() const { return U_(0, 0); }
private:
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> plant_;
+ ::Eigen::Matrix<double, 1, 1> U_;
PositionSensorSimulator position_sim_;
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index 1d2a74e..8efe374 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -112,7 +112,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ 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]]])
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -178,7 +178,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 93b3b17..5cd7ce5 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -28,9 +28,9 @@
explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
- assert(U(0, 0) <= U_max(0, 0) + 0.00001 + voltage_offset_);
- assert(U(0, 0) >= U_min(0, 0) - 0.00001 + voltage_offset_);
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
double voltage_offset() const { return voltage_offset_; }
@@ -81,15 +81,15 @@
void Simulate() {
EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- shooter_plant_left_->mutable_U(0, 0) =
- shooter_queue_.output->voltage_left +
- shooter_plant_left_->voltage_offset();
- shooter_plant_right_->mutable_U(0, 0) =
- shooter_queue_.output->voltage_right +
- shooter_plant_right_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> U_left;
+ ::Eigen::Matrix<double, 1, 1> U_right;
+ U_left(0, 0) = shooter_queue_.output->voltage_left +
+ shooter_plant_left_->voltage_offset();
+ U_right(0, 0) = shooter_queue_.output->voltage_right +
+ shooter_plant_right_->voltage_offset();
- shooter_plant_left_->Update();
- shooter_plant_right_->Update();
+ shooter_plant_left_->Update(U_left);
+ shooter_plant_right_->Update(U_right);
}
::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 3bf0966..2366fb7 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -670,16 +670,22 @@
// Calculate the loops for a cycle.
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power = intake_.controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power = intake_.controller().K(0, 1) * error(1, 0);
+ status->intake.position_power =
+ intake_.controller().controller().K(0, 0) * error(0, 0);
+ status->intake.velocity_power =
+ intake_.controller().controller().K(0, 1) * error(1, 0);
}
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power = arm_.controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power = arm_.controller().K(0, 1) * error(1, 0);
- status->wrist.position_power = arm_.controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power = arm_.controller().K(0, 3) * error(3, 0);
+ status->shoulder.position_power =
+ arm_.controller().controller().K(0, 0) * error(0, 0);
+ status->shoulder.velocity_power =
+ arm_.controller().controller().K(0, 1) * error(1, 0);
+ status->wrist.position_power =
+ arm_.controller().controller().K(0, 2) * error(2, 0);
+ status->wrist.velocity_power =
+ arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 8f3a59e..5d8c85a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,14 +28,16 @@
const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
const Eigen::Matrix<double, 2, 1> accelerating_ff =
- controller(0).Kff * (next_R() - controller(0).plant.A() * R());
+ controller().coefficients(0).Kff *
+ (next_R() - plant().coefficients(0).A * R());
const Eigen::Matrix<double, 2, 1> accelerating_controller =
- controller(0).K * error() + accelerating_ff;
+ controller().coefficients(0).K * error() + accelerating_ff;
const Eigen::Matrix<double, 2, 1> decelerating_ff =
- controller(1).Kff * (next_R() - controller(1).plant.A() * R());
+ controller().coefficients(1).Kff *
+ (next_R() - plant().coefficients(1).A * R());
const Eigen::Matrix<double, 2, 1> decelerating_controller =
- controller(1).K * error() + decelerating_ff;
+ controller().coefficients(1).K * error() + decelerating_ff;
const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
bool use_accelerating_controller = true;
@@ -48,11 +50,11 @@
use_accelerating_controller = false;
}
if (use_accelerating_controller) {
+ set_index(0);
ff_U_ = accelerating_ff;
- set_controller_index(0);
return accelerating_controller;
} else {
- set_controller_index(1);
+ set_index(1);
ff_U_ = decelerating_ff;
return decelerating_controller;
}
@@ -66,18 +68,18 @@
if (U(0, 0) > max_voltage(0)) {
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
- overage_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
if (U(0, 0) < min_voltage(0)) {
const double under_amount = U(0, 0) - min_voltage(0);
mutable_U(0, 0) = min_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
- under_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ under_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index e95267c..b3e980d 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -33,11 +33,11 @@
explicit ArmPlant(StateFeedbackPlant<4, 2, 2> &&other)
: StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
- void CheckU() override {
- assert(U(0, 0) <= U_max(0, 0) + 0.00001 + shoulder_voltage_offset_);
- assert(U(0, 0) >= U_min(0, 0) - 0.00001 + shoulder_voltage_offset_);
- assert(U(1, 0) <= U_max(1, 0) + 0.00001 + wrist_voltage_offset_);
- assert(U(1, 0) >= U_min(1, 0) - 0.00001 + wrist_voltage_offset_);
+ void CheckU(const ::Eigen::Matrix<double, 2, 1> &U) override {
+ EXPECT_LT(U(0, 0), U_max(0, 0) + 0.00001 + shoulder_voltage_offset_);
+ EXPECT_GT(U(0, 0), U_min(0, 0) - 0.00001 + shoulder_voltage_offset_);
+ EXPECT_LT(U(1, 0), U_max(1, 0) + 0.00001 + wrist_voltage_offset_);
+ EXPECT_GT(U(1, 0), U_min(1, 0) - 0.00001 + wrist_voltage_offset_);
}
double shoulder_voltage_offset() const { return shoulder_voltage_offset_; }
@@ -60,10 +60,10 @@
explicit IntakePlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
for (int i = 0; i < kNumInputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0) + 0.00001 + voltage_offset_);
- assert(U(i, 0) >= U_min(i, 0) - 0.00001 + voltage_offset_);
+ EXPECT_LE(U(i, 0), U_max(i, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(i, 0), U_min(i, 0) - 0.00001 + voltage_offset_);
}
}
@@ -158,11 +158,13 @@
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
// Feed voltages into physics simulation.
- intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
- intake_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ ::Eigen::Matrix<double, 2, 1> arm_U;
+ intake_U << superstructure_queue_.output->voltage_intake +
+ intake_plant_->voltage_offset();
- arm_plant_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
- arm_plant_->shoulder_voltage_offset(),
+ arm_U << superstructure_queue_.output->voltage_shoulder +
+ arm_plant_->shoulder_voltage_offset(),
superstructure_queue_.output->voltage_wrist +
arm_plant_->wrist_voltage_offset();
@@ -194,23 +196,23 @@
// Use the plant to generate the next physical state given the voltages to
// the motors.
- intake_plant_->Update();
+ intake_plant_->Update(intake_U);
{
const double bemf_voltage = arm_plant_->X(1, 0) / kV_shoulder;
bool is_accelerating = false;
if (bemf_voltage > 0) {
- is_accelerating = arm_plant_->U(0, 0) > bemf_voltage;
+ is_accelerating = arm_U(0, 0) > bemf_voltage;
} else {
- is_accelerating = arm_plant_->U(0, 0) < bemf_voltage;
+ is_accelerating = arm_U(0, 0) < bemf_voltage;
}
if (is_accelerating) {
- arm_plant_->set_plant_index(0);
+ arm_plant_->set_index(0);
} else {
- arm_plant_->set_plant_index(1);
+ arm_plant_->set_index(1);
}
}
- arm_plant_->Update();
+ arm_plant_->Update(arm_U);
const double angle_intake = intake_plant_->Y(0, 0);
const double angle_shoulder = arm_plant_->Y(0, 0);
diff --git a/y2016/vision/blob_filters.cc b/y2016/vision/blob_filters.cc
index e9f881d..860fa80 100644
--- a/y2016/vision/blob_filters.cc
+++ b/y2016/vision/blob_filters.cc
@@ -74,7 +74,7 @@
if (do_overlay_) {
for (FittedLine &line : lines) {
- overlay_->add_line(Vector<2>(line.st.x, line.st.y),
+ overlay_->AddLine(Vector<2>(line.st.x, line.st.y),
Vector<2>(line.ed.x, line.ed.y), {255, 0, 0});
}
}
@@ -105,9 +105,9 @@
Segment<2> bottom(MakeLine(bottomLine));
if (do_overlay_) {
- overlay_->add_line(left.A(), left.B(), {155, 0, 255});
- overlay_->add_line(right.A(), right.B(), {255, 155, 0});
- overlay_->add_line(bottom.A(), bottom.B(), {255, 0, 155});
+ overlay_->AddLine(left.A(), left.B(), {155, 0, 255});
+ overlay_->AddLine(right.A(), right.B(), {255, 155, 0});
+ overlay_->AddLine(bottom.A(), bottom.B(), {255, 0, 155});
}
res.emplace_back(left.Intersect(bottom), right.Intersect(bottom));
@@ -238,8 +238,8 @@
if (do_overlay_) {
double a = hist_lr[i];
Vector<2> mid = a * s + (1.0 - a) * e;
- overlay_->add_line(s, mid, {0, 0, 255});
- overlay_->add_line(mid, e, {255, 255, 0});
+ overlay_->AddLine(s, mid, {0, 0, 255});
+ overlay_->AddLine(mid, e, {255, 255, 0});
}
}
double check_vert_up = L22_dist(hist_size_, vert_hist_, hist_lr);
@@ -259,8 +259,8 @@
if (do_overlay_) {
double a = hist_ub[i];
Vector<2> mid = a * s + (1.0 - a) * e;
- overlay_->add_line(s, mid, {0, 0, 255});
- overlay_->add_line(mid, e, {255, 255, 0});
+ overlay_->AddLine(s, mid, {0, 0, 255});
+ overlay_->AddLine(mid, e, {255, 255, 0});
}
}
double check_horiz = L22_dist(hist_size_, horiz_hist_, hist_ub);
@@ -268,16 +268,16 @@
if (do_overlay_) {
Vector<2> A = blob->upper_left + Vector<2>(-10, 10);
Vector<2> B = blob->upper_left - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
A = blob->upper_right + Vector<2>(-10, 10);
B = blob->upper_right - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
A = blob->lower_right + Vector<2>(-10, 10);
B = blob->lower_right - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
A = blob->lower_left + Vector<2>(-10, 10);
B = blob->lower_left - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
}
// If this target is better upside down, if it is flip the blob.
// The horizontal will not be effected, so we will not change that.
@@ -292,16 +292,16 @@
if (do_overlay_) {
Vector<2> A = blob->upper_left + Vector<2>(10, 10);
Vector<2> B = blob->upper_left - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
A = blob->upper_right + Vector<2>(10, 10);
B = blob->upper_right - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
A = blob->lower_right + Vector<2>(10, 10);
B = blob->lower_right - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
A = blob->lower_left + Vector<2>(10, 10);
B = blob->lower_left - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
}
// average the two distances
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
index 706c348..2790a83 100644
--- a/y2016/vision/target_sender.cc
+++ b/y2016/vision/target_sender.cc
@@ -228,12 +228,12 @@
std::thread cam0([stereo]() {
RunCamera(0, GetCameraParams(stereo.calibration()),
stereo.calibration().right_camera_name(),
- stereo.calibration().roborio_ip_addr(), 8082);
+ stereo.calibration().roborio_ip_addr(), 8080);
});
std::thread cam1([stereo]() {
RunCamera(1, GetCameraParams(stereo.calibration()),
stereo.calibration().left_camera_name(),
- stereo.calibration().roborio_ip_addr(), 8082);
+ stereo.calibration().roborio_ip_addr(), 8080);
});
cam0.join();
cam1.join();
diff --git a/y2017/BUILD b/y2017/BUILD
index fd32ae2..a7e08ba 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -25,6 +25,25 @@
)
cc_binary(
+ name = 'joystick_reader',
+ srcs = [
+ 'joystick_reader.cc',
+ ],
+ deps = [
+ ':constants',
+ '//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',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ ],
+)
+
+cc_binary(
name = 'wpilib_interface',
srcs = [
'wpilib_interface.cc',
@@ -49,7 +68,6 @@
'//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',
@@ -66,7 +84,7 @@
aos_downloader(
name = 'download',
start_srcs = [
- #':joystick_reader',
+ ':joystick_reader',
':wpilib_interface',
'//aos:prime_start_binaries',
'//y2017/control_loops/drivetrain:drivetrain',
@@ -81,7 +99,7 @@
aos_downloader(
name = 'download_stripped',
start_srcs = [
- #':joystick_reader.stripped',
+ ':joystick_reader.stripped',
':wpilib_interface.stripped',
'//aos:prime_start_binaries_stripped',
'//y2017/control_loops/drivetrain:drivetrain.stripped',
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 7fe0693..ea99805 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -103,8 +103,21 @@
break;
case kCompTeamNumber:
- intake->pot_offset = 0.2921 + 0.00039;
- intake->zeroing.measured_absolute_position = 0.045209;
+ intake->pot_offset = 0.26712;
+ intake->zeroing.measured_absolute_position = 0.008913;
+
+ turret->pot_offset = 0;
+ turret->zeroing.measured_absolute_position = 0;
+
+ hood->zeroing.measured_index_position = 0.652898 - 0.488117;
+
+ r->down_error = 0;
+ r->vision_name = "competition";
+ break;
+
+ case kPracticeTeamNumber:
+ intake->pot_offset = 0.2921 + 0.00039 + 0.012236;
+ intake->zeroing.measured_absolute_position = 0.033408;
turret->pot_offset = -5.45 - 0.026111;
turret->zeroing.measured_absolute_position = 0.2429;
@@ -112,22 +125,10 @@
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
r->down_error = 0;
- r->vision_name = "competition";
- break;
-
- case kPracticeTeamNumber:
- intake->pot_offset = 0;
- intake->zeroing.measured_absolute_position = 0;
-
- turret->pot_offset = 0;
- turret->zeroing.measured_absolute_position = 0;
-
- hood->zeroing.measured_index_position = 0.05;
-
- r->down_error = 0;
r->vision_name = "practice";
break;
+
default:
LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
}
diff --git a/y2017/constants.h b/y2017/constants.h
index 49b7afa..e011501 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -74,9 +74,8 @@
control_loops::superstructure::intake::kOutputRatio /
constants::Values::kIntakeEncoderRatio *
kIntakeEncoderCountsPerRevolution;
- static constexpr ::frc971::constants::Range kIntakeRange{
- -0.29878633 * 0.0254, 9.23012063 * 0.0254, (-0.29878633 + 0.125) * 0.0254,
- (9.23012063 - 0.125) * 0.0254};
+ static constexpr ::frc971::constants::Range kIntakeRange{-0.01, 0.240, 0.01,
+ 0.21};
static constexpr double kHoodEncoderCountsPerRevolution = 2048 * 4;
static constexpr double kHoodEncoderRatio = 20.0 / 345.0;
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 6b1d59a..1818d62 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -76,7 +76,7 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.80])
+ self.PlaceControllerPoles([.75])
glog.debug('K: %s', repr(self.K))
glog.debug('Poles are %s',
@@ -154,7 +154,7 @@
q_pos = 0.01
q_vel = 2.0
- q_voltage = 0.4
+ q_voltage = 0.6
if voltage_error_noise is not None:
q_voltage = voltage_error_noise
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 6c0f2f3..0858e45 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -14,7 +14,14 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class VelocityShooter(control_loop.ControlLoop):
+
+def PlotDiff(list1, list2, time):
+ pylab.subplot(1, 1, 1)
+ pylab.plot(time, numpy.subtract(list1, list2), label='diff')
+ pylab.legend()
+
+
+class VelocityShooter(control_loop.HybridControlLoop):
def __init__(self, name='VelocityShooter'):
super(VelocityShooter, self).__init__(name)
# Number of motors
@@ -141,14 +148,19 @@
q_pos = 0.01
q_vel = 2.0
q_voltage = 0.2
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_voltage ** 2.0)]])
r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous, B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
@@ -173,9 +185,10 @@
self.x_hat = []
self.u = []
self.offset = []
+ self.diff = []
def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None):
+ observer_shooter=None, hybrid_obs = False):
"""Runs the shooter plant with an initial condition and goal.
Args:
@@ -211,6 +224,7 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(shooter.X[0, 0])
+ self.diff.append(shooter.X[1, 0] - observer_shooter.X_hat[1, 0])
if self.v:
last_v = self.v[-1]
@@ -221,8 +235,9 @@
self.a.append((self.v[-1] - last_v) / shooter.dt)
if observer_shooter is not None:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
+ if i != 0:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
self.offset.append(observer_shooter.X_hat[2, 0])
applied_U = U.copy()
@@ -231,7 +246,11 @@
shooter.Update(applied_U)
if observer_shooter is not None:
- observer_shooter.PredictObserver(U)
+ if hybrid_obs:
+ observer_shooter.PredictHybridObserver(U, shooter.dt)
+ else:
+ observer_shooter.PredictObserver(U)
+
self.t.append(initial_t + i * shooter.dt)
self.u.append(U[0, 0])
@@ -251,24 +270,42 @@
pylab.plot(self.t, self.a, label='a')
pylab.legend()
- pylab.show()
+ pylab.figure()
+ pylab.draw()
def main(argv):
scenario_plotter = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter = IntegralShooter()
-
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter, iterations=200)
-
if FLAGS.plot:
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
+ iterations = 200
+
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter, iterations=iterations)
+
scenario_plotter.Plot()
+ scenario_plotter_int = ScenarioPlotter()
+
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter_hybrid = IntegralShooter()
+
+ scenario_plotter_int.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid, iterations=iterations,
+ hybrid_obs = True)
+
+ scenario_plotter_int.Plot()
+ PlotDiff(scenario_plotter.x_hat, scenario_plotter_int.x_hat,
+ scenario_plotter.t)
+
+ pylab.show()
+
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
@@ -284,7 +321,9 @@
integral_shooter = IntegralShooter('IntegralShooter')
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
integral_loop_writer.Write(argv[3], argv[4])
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index c58e005..f5daa42 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -31,7 +31,7 @@
}
// The zeroing and operating voltages.
- static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kZeroingVoltage = 2.0;
static constexpr double kOperatingVoltage = 12.0;
void Iterate(const control_loops::HoodGoal *unsafe_goal,
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index 78e9528..0560ec4 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -21,7 +21,6 @@
namespace {
constexpr double kTolerance = 10.0;
-constexpr double kMinStuckVoltage = 3.0;
constexpr chrono::milliseconds kForwardTimeout{500};
constexpr chrono::milliseconds kReverseTimeout{500};
constexpr chrono::milliseconds kReverseMinTimeout{100};
@@ -61,21 +60,15 @@
double IndexerController::voltage() const { return loop_->U(0, 0); }
-double IndexerController::StuckRatio() const {
- double applied_voltage = voltage();
+double IndexerController::StuckVoltage() const {
+ const double applied_voltage = voltage() + loop_->X_hat(2, 0);
if (applied_voltage < 0) {
- applied_voltage = ::std::min(applied_voltage, -kMinStuckVoltage);
+ return +stuck_indexer_X_hat_current_(2, 0) + applied_voltage;
} else {
- applied_voltage = ::std::max(applied_voltage, kMinStuckVoltage);
+ return -stuck_indexer_X_hat_current_(2, 0) - applied_voltage;
}
- // Look at the ratio of the current controller power to the voltage_error
- // term. If our output is dominated by the voltage_error, then we are likely
- // pretty stuck and should try reversing.
- // We don't want to worry about dividing by zero, so keep the applied voltage
- // away from 0 though a min/max.
- return -stuck_indexer_X_hat_current_(2, 0) / applied_voltage;
}
-bool IndexerController::IsStuck() const { return StuckRatio() > 0.6; }
+bool IndexerController::IsStuck() const { return StuckVoltage() > 1.5; }
void IndexerController::Reset() { reset_ = true; }
@@ -123,7 +116,8 @@
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
loop_->Update(disabled);
- stuck_indexer_detector_->UpdateObserver(loop_->U());
+ stuck_indexer_detector_->UpdateObserver(loop_->U(),
+ ::aos::controls::kLoopFrequency);
}
void IndexerController::SetStatus(IndexerStatus *status) {
@@ -139,7 +133,7 @@
status->stuck = IsStuck();
- status->stuck_ratio = StuckRatio();
+ status->stuck_voltage = StuckVoltage();
}
void Indexer::Reset() { indexer_.Reset(); }
diff --git a/y2017/control_loops/superstructure/indexer/indexer.h b/y2017/control_loops/superstructure/indexer/indexer.h
index 5c1cc1b..9d71eed 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.h
+++ b/y2017/control_loops/superstructure/indexer/indexer.h
@@ -39,7 +39,7 @@
// Returns true if the indexer is stuck.
bool IsStuck() const;
- double StuckRatio() const;
+ double StuckVoltage() const;
// Executes the control loop for a cycle.
void Update(bool disabled);
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index c4d1630..8991658 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -23,7 +23,8 @@
// TODO(austin): Pseudo current limit?
ShooterController::ShooterController()
- : loop_(new StateFeedbackLoop<3, 1, 1>(
+ : loop_(new StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
Y_.setZero();
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index 41e24c0..7dc44bf 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -49,7 +49,9 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<
+ 3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 5;
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index b4e6c9b..427dadb 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -69,7 +69,7 @@
// True if the indexer is stuck.
bool stuck;
- float stuck_ratio;
+ float stuck_voltage;
// The state of the indexer state machine.
int32_t state;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 3075f03..579fb1e 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -35,7 +35,7 @@
explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -54,7 +54,7 @@
explicit IndexerPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -73,7 +73,7 @@
explicit HoodPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -92,7 +92,7 @@
explicit TurretPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -111,7 +111,7 @@
explicit IntakePlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -284,31 +284,34 @@
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
voltage_check_intake);
- hood_plant_->mutable_U() << superstructure_queue_.output->voltage_hood +
- hood_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> hood_U;
+ hood_U << superstructure_queue_.output->voltage_hood +
+ hood_plant_->voltage_offset();
- turret_plant_->mutable_U() << superstructure_queue_.output->voltage_turret +
- turret_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> turret_U;
+ turret_U << superstructure_queue_.output->voltage_turret +
+ turret_plant_->voltage_offset();
- intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
- intake_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ intake_U << superstructure_queue_.output->voltage_intake +
+ intake_plant_->voltage_offset();
- shooter_plant_->mutable_U()
- << superstructure_queue_.output->voltage_shooter +
- shooter_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> shooter_U;
+ shooter_U << superstructure_queue_.output->voltage_shooter +
+ shooter_plant_->voltage_offset();
- indexer_plant_->mutable_U()
- << superstructure_queue_.output->voltage_indexer +
- indexer_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> indexer_U;
+ indexer_U << superstructure_queue_.output->voltage_indexer +
+ indexer_plant_->voltage_offset();
- hood_plant_->Update();
- turret_plant_->Update();
- intake_plant_->Update();
- shooter_plant_->Update();
+ hood_plant_->Update(hood_U);
+ turret_plant_->Update(turret_U);
+ intake_plant_->Update(intake_U);
+ shooter_plant_->Update(shooter_U);
if (freeze_indexer_) {
indexer_plant_->mutable_X(1, 0) = 0.0;
} else {
- indexer_plant_->Update();
+ indexer_plant_->Update(indexer_U);
}
double angle_hood = hood_plant_->Y(0, 0);
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
new file mode 100644
index 0000000..179fac6
--- /dev/null
+++ b/y2017/joystick_reader.cc
@@ -0,0 +1,244 @@
+#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 "y2017/control_loops/superstructure/superstructure.q.h"
+
+#include "y2017/constants.h"
+#include "frc971/autonomous/auto.q.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::y2017::control_loops::superstructure_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 y2017 {
+namespace input {
+namespace joysticks {
+
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kQuickTurn(1, 5);
+
+const ButtonLocation kTurn1(1, 7);
+const ButtonLocation kTurn2(1, 11);
+
+const ButtonLocation kIntakeDown(3, 9);
+const ButtonLocation kIntakeIn(3, 12);
+const ButtonLocation kIntakeOut(3, 8);
+const POVLocation kHang(3, 90);
+const ButtonLocation kFire(3, 3);
+const ButtonLocation kCloseShot(3, 7);
+const ButtonLocation kMiddleShot(3, 6);
+const POVLocation kFarShot(3, 270);
+
+const ButtonLocation kVisionAlign(3, 5);
+
+const ButtonLocation kReverseIndexer(3, 4);
+const ButtonLocation kExtra1(3, 11);
+const ButtonLocation kExtra2(3, 10);
+const ButtonLocation kExtra3(3, 2);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {}
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override {
+ bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ vision_valid_ = false;
+
+ if (!auto_running_) {
+ HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process any pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+ }
+
+ 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");
+ }
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ // Default the intake to in.
+ intake_goal_ = constants::Values::kIntakeRange.lower;
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ if (data.IsPressed(kIntakeDown)) {
+ intake_goal_ = 0.23;
+ }
+
+ if (data.IsPressed(kVisionAlign)) {
+ // Align shot using vision
+ // TODO(campbell): Add vision aligning.
+ shooter_velocity_ = 100.0;
+ } else if (data.IsPressed(kCloseShot)) {
+ // Close shot
+ hood_goal_ = 0.5;
+ shooter_velocity_ = 350.0;
+ } else if (data.IsPressed(kMiddleShot)) {
+ // Medium distance shot
+ hood_goal_ = 0.4;
+ shooter_velocity_ = 350.0;
+ } else if (data.IsPressed(kFarShot)) {
+ // Far shot
+ hood_goal_ = 0.6;
+ shooter_velocity_ = 250.0;
+ } else {
+ hood_goal_ = 0.15;
+ shooter_velocity_ = 0.0;
+ }
+
+ if (data.IsPressed(kExtra1)) {
+ turret_goal_ += -0.1;
+ }
+ if (data.IsPressed(kExtra2)) {
+ turret_goal_ = 0.0;
+ }
+ if (data.IsPressed(kExtra3)) {
+ turret_goal_ += 0.1;
+ }
+
+ fire_ = data.IsPressed(kFire) && shooter_velocity_ != 0.0;
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->intake.distance = intake_goal_;
+ new_superstructure_goal->turret.angle = turret_goal_;
+ new_superstructure_goal->hood.angle = hood_goal_;
+ new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+
+ new_superstructure_goal->intake.profile_params.max_velocity = 0.50;
+ new_superstructure_goal->turret.profile_params.max_velocity = 6.0;
+ new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
+
+ new_superstructure_goal->intake.profile_params.max_acceleration = 5.0;
+ new_superstructure_goal->turret.profile_params.max_acceleration = 15.0;
+ new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
+
+ if (data.IsPressed(kHang)) {
+ new_superstructure_goal->intake.voltage_rollers = -12.0;
+ } else if (data.IsPressed(kIntakeIn)) {
+ new_superstructure_goal->intake.voltage_rollers = 12.0;
+ } else if (data.IsPressed(kIntakeOut)) {
+ new_superstructure_goal->intake.voltage_rollers = -8.0;
+ } else {
+ new_superstructure_goal->intake.voltage_rollers = 0.0;
+ }
+
+ if (data.IsPressed(kReverseIndexer)) {
+ new_superstructure_goal->indexer.voltage_rollers = -4.0;
+ new_superstructure_goal->indexer.angular_velocity = -4.0;
+ new_superstructure_goal->indexer.angular_velocity = -1.0;
+ } else if (fire_) {
+ new_superstructure_goal->indexer.voltage_rollers = 2.0;
+ new_superstructure_goal->indexer.angular_velocity = 3.0 * M_PI;
+ new_superstructure_goal->indexer.angular_velocity = 1.0;
+ } else {
+ new_superstructure_goal->indexer.voltage_rollers = 0.0;
+ new_superstructure_goal->indexer.angular_velocity = 0.0;
+ }
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+
+ private:
+ void StartAuto() { LOG(INFO, "Starting auto mode\n"); }
+
+ void StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+ }
+
+ // Current goals to send to the robot.
+ double intake_goal_ = 0.0;
+ double turret_goal_ = 0.0;
+ double hood_goal_ = 0.3;
+ double shooter_velocity_ = 0.0;
+
+ // Goals to send to the drivetrain in closed loop mode.
+ double left_goal_;
+ double right_goal_;
+
+ bool was_running_ = false;
+ bool auto_running_ = false;
+
+ bool vision_valid_ = false;
+
+ bool fire_ = false;
+
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2017
+
+int main() {
+ ::aos::Init(-1);
+ ::y2017::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
new file mode 100644
index 0000000..b70e67f
--- /dev/null
+++ b/y2017/vision/BUILD
@@ -0,0 +1,91 @@
+load('/aos/build/queues', 'queue_library')
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+load('/tools/build_rules/protobuf', 'proto_cc_library')
+
+package(default_visibility = ["//visibility:public"])
+
+queue_library(
+ name = 'vision_queue',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'vision.q',
+ ],
+)
+
+proto_cc_library(
+ name = 'vision_result',
+ src = 'vision_result.proto',
+)
+
+proto_cc_library(
+ name = 'vision_config',
+ src = 'vision_config.proto',
+)
+
+cc_binary(
+ name = 'target_sender',
+ srcs = [
+ 'target_sender.cc',
+ ],
+ deps = [
+ ':vision_result',
+ ':vision_config',
+ ':target_finder',
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/common:time',
+ '//aos/vision/image:reader',
+ '//aos/vision/image:jpeg_routines',
+ '//aos/vision/image:image_stream',
+ '//aos/vision/events:udp',
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/events:socket_types',
+ '//aos/vision/blob:range_image',
+ '//aos/vision/blob:codec',
+ '//aos/vision/blob:find_blob',
+ '//aos/vision/blob:threshold',
+ ],
+)
+
+cc_binary(
+ name = 'target_receiver',
+ srcs = [
+ 'target_receiver.cc',
+ ],
+ visibility = ['//visibility:public'],
+ deps = [
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/linux_code:init',
+ '//aos/common:time',
+ '//aos/vision/events:udp',
+ ':vision_queue',
+ ':vision_result',
+ '//aos/common:mutex',
+ ],
+)
+
+cc_library(
+ name = 'target_finder',
+ srcs = ['target_finder.cc'],
+ hdrs = ['target_finder.h'],
+ deps = [
+ '//aos/vision/blob:threshold',
+ '//aos/vision/blob:transpose',
+ '//aos/vision/debug:overlay',
+ '//aos/vision/math:vector',
+ ],
+)
+
+gtk_dependent_cc_binary(
+ name = 'debug_viewer',
+ srcs = ['debug_viewer.cc'],
+ deps = [
+ ':target_finder',
+ '//aos/vision/blob:threshold',
+ '//aos/vision/blob:transpose',
+ '//aos/vision/blob:move_scale',
+ '//aos/vision/math:vector',
+ '//aos/vision/debug:debug_framework',
+ ]
+)
diff --git a/y2017/vision/debug_viewer.cc b/y2017/vision/debug_viewer.cc
new file mode 100644
index 0000000..83f306f
--- /dev/null
+++ b/y2017/vision/debug_viewer.cc
@@ -0,0 +1,129 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+#include "y2017/vision/target_finder.h"
+
+#include "aos/vision/blob/move_scale.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/math/vector.h"
+
+using aos::vision::ImageRange;
+using aos::vision::ImageFormat;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+
+namespace y2017 {
+namespace vision {
+
+BlobList RenderTargetListShifted(const std::vector<TargetComponent> &list) {
+ BlobList out;
+ for (const auto &entity : list) {
+ out.emplace_back(entity.RenderShifted());
+ }
+ return out;
+}
+
+RangeImage TargetComponent::RenderShifted() const {
+ std::vector<std::vector<ImageRange>> out_range_list;
+ int y = 0;
+ double max_y = -b / (2 * a);
+ double parab_off = max_y * max_y * a + max_y * b;
+ RangeImage t_img = Transpose(*img);
+ for (const auto &row : t_img) {
+ int off = -(y * y * a + y * b - parab_off);
+ // int off = 0;
+ // fprintf(stderr, "off: %d %d\n", off, y);
+ std::vector<ImageRange> row_out;
+ for (const ImageRange &range : row) {
+ row_out.emplace_back(ImageRange{off + range.st, off + range.ed});
+ }
+ ++y;
+ out_range_list.emplace_back(std::move(row_out));
+ }
+ return RangeImage(t_img.min_y(), std::move(out_range_list));
+}
+
+class FilterHarnessExample : public aos::vision::FilterHarness {
+ public:
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
+ return finder_.Threshold(image);
+ }
+
+ void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
+ viewer_ = viewer;
+ viewer_->SetScale(0.5);
+ overlays_.push_back(&overlay_);
+ overlays_.push_back(finder_.GetOverlay());
+ viewer_->view()->SetOverlays(&overlays_);
+ }
+
+ bool HandleBlobs(BlobList imgs, ImageFormat /*fmt*/) override {
+ // reset for next drawing cycle
+ for (auto &overlay : overlays_) {
+ overlay->Reset();
+ }
+
+ // Remove bad blobs.
+ finder_.PreFilter(imgs);
+
+ // calculate each component/
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(imgs);
+
+ DrawComponents(target_component_list);
+
+ // Put the compenents together into targets and pick the best.
+ Target final_target;
+ bool found_target =
+ finder_.FindTargetFromComponents(target_component_list, &final_target);
+
+ // BlobList newImg = RenderTargetListShifted(target_component_list);
+ if (viewer_) {
+ viewer_->DrawBlobList(imgs, {0, 0, 255});
+ }
+
+ if (found_target) {
+ BlobList list;
+ list.emplace_back(*(final_target.comp1.img));
+ list.emplace_back(*(final_target.comp2.img));
+ viewer_->DrawBlobList(list, {0, 255, 0});
+ overlay_.DrawCross(final_target.screen_coord, 25, {255, 255, 255});
+ }
+
+ // No targets.
+ return found_target;
+ }
+
+ void DrawComponents(const std::vector<TargetComponent> comp) {
+ for (const TargetComponent &t : comp) {
+ aos::vision::ImageBBox bbox;
+ GetBBox(*(t.img), &bbox);
+ overlay_.DrawBBox(bbox, {255, 0, 0});
+
+ overlay_.StartNewProfile();
+ for (int i = 0; i < bbox.maxx - bbox.minx; i += 10) {
+ double y0 = t.a * i * i + t.b * i + t.c_0;
+ double y1 = t.a * i * i + t.b * i + t.c_1;
+ overlay_.AddPoint(aos::vision::Vector<2>(i + t.mini, y0), {255, 0, 0});
+ overlay_.AddPoint(aos::vision::Vector<2>(i + t.mini, y1), {255, 0, 0});
+ }
+ }
+ }
+
+ private:
+ // implementation of the filter pipeline.
+ TargetFinder finder_;
+ aos::vision::BlobStreamViewer *viewer_ = nullptr;
+ aos::vision::PixelLinesOverlay overlay_;
+ std::vector<aos::vision::OverlayBase *> overlays_;
+};
+
+} // namespace vision
+} // namespace y2017
+
+int main(int argc, char **argv) {
+ y2017::vision::FilterHarnessExample filter_harness;
+ aos::vision::DebugFrameworkMain(argc, argv, &filter_harness);
+}
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
new file mode 100644
index 0000000..e39fe73
--- /dev/null
+++ b/y2017/vision/target_finder.cc
@@ -0,0 +1,238 @@
+#include "y2017/vision/target_finder.h"
+
+#include <math.h>
+
+namespace y2017 {
+namespace vision {
+
+// Blobs now come in three types:
+// 0) normal blob.
+// 1) first part of a split blob.
+// 2) second part of a split blob.
+void ComputeXShiftPolynomial(int type, const RangeImage &img,
+ TargetComponent *target) {
+ target->img = &img;
+ RangeImage t_img = Transpose(img);
+ int spacing = 10;
+ int n = t_img.size() - spacing * 2;
+ target->n = n;
+ if (n <= 0) {
+ printf("Empty blob aborting (%d).\n", n);
+ return;
+ }
+ Eigen::MatrixXf A = Eigen::MatrixXf::Zero(n * 2, 4);
+ Eigen::VectorXf b = Eigen::VectorXf::Zero(n * 2);
+ int i = 0;
+ for (const auto &row : t_img) {
+ // We decided this was a split target, but this is not a split row.
+ if (i >= spacing && i - spacing < n) {
+ int j = (i - spacing) * 2;
+ // normal blob or the first part of a split.
+ if (type == 0 || type == 1) {
+ b(j) = row[0].st;
+ } else {
+ b(j) = row[1].st;
+ }
+ A(j, 0) = (i) * (i);
+ A(j, 1) = (i);
+ A(j, 2) = 1;
+ ++j;
+ // normal target or the second part of a split.
+ if (type == 0 || type == 2) {
+ b(j) = row[row.size() - 1].ed;
+ } else {
+ b(j) = row[0].ed;
+ }
+ A(j, 0) = i * i;
+ A(j, 1) = i;
+ A(j, 3) = 1;
+ }
+ ++i;
+ }
+ Eigen::VectorXf sol =
+ A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+ target->a = sol(0);
+ target->b = sol(1);
+ target->c_0 = sol(2);
+ target->c_1 = sol(3);
+
+ target->mini = t_img.min_y();
+
+ Eigen::VectorXf base = A * sol;
+ Eigen::VectorXf error_v = b - base;
+ target->fit_error = error_v.dot(error_v);
+}
+
+double TargetFinder::DetectConnectedTarget(const RangeImage &img) {
+ using namespace aos::vision;
+ RangeImage t_img = Transpose(img);
+ int total = 0;
+ int split = 0;
+ int count = t_img.mini();
+ for (const auto &row : t_img) {
+ if (row.size() == 1) {
+ total++;
+ } else if (row.size() == 2) {
+ split++;
+ }
+ count++;
+ }
+ return (double)split / total;
+}
+
+std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+ const BlobList &blobs) {
+ std::vector<TargetComponent> list;
+ TargetComponent newTarg;
+ for (std::size_t i = 0; i < blobs.size(); ++i) {
+ double split_ratio;
+ if ((split_ratio = DetectConnectedTarget(blobs[i])) > 0.50) {
+ // Split type blob, do it two parts.
+ ComputeXShiftPolynomial(1, blobs[i], &newTarg);
+ list.emplace_back(newTarg);
+ ComputeXShiftPolynomial(2, blobs[i], &newTarg);
+ list.emplace_back(newTarg);
+ } else {
+ // normal type blob.
+ ComputeXShiftPolynomial(0, blobs[i], &newTarg);
+ list.emplace_back(newTarg);
+ }
+ }
+
+ return list;
+}
+
+aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
+ return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
+ if (px.g > 88) {
+ return true;
+ uint8_t min = std::min(px.b, px.r);
+ uint8_t max = std::max(px.b, px.r);
+ if (min >= px.g || max >= px.g) return false;
+ uint8_t a = px.g - min;
+ uint8_t b = px.g - max;
+ return (a > 10 && b > 10);
+ }
+ return false;
+ });
+}
+
+void TargetFinder::PreFilter(BlobList &imgs) {
+ imgs.erase(std::remove_if(imgs.begin(), imgs.end(),
+ [](RangeImage &img) {
+ // We can drop images with a small number of
+ // pixels, but images
+ // must be over 20px or the math will have issues.
+ return (img.npixels() < 100 || img.height() < 25);
+ }),
+ imgs.end());
+}
+
+bool TargetFinder::FindTargetFromComponents(
+ std::vector<TargetComponent> component_list, Target *final_target) {
+ using namespace aos::vision;
+ if (component_list.size() < 2 || final_target == NULL) {
+ // We don't enough parts for a traget.
+ return false;
+ }
+
+ // A0 * c + A1*s = b
+ Eigen::MatrixXf A = Eigen::MatrixXf::Zero(4, 2);
+ // A0: Offset component will be constant across all equations.
+ A(0, 0) = 1;
+ A(1, 0) = 1;
+ A(2, 0) = 1;
+ A(3, 0) = 1;
+
+ // A1: s defines the scaling and defines an expexted target.
+ // So these are the center heights of the top and bottom of the two targets.
+ A(0, 1) = -1;
+ A(1, 1) = 0;
+ A(2, 1) = 2;
+ A(3, 1) = 4;
+
+ // Track which pair is the best fit.
+ double best_error = -1;
+ double best_offset = -1;
+ Eigen::VectorXf best_v;
+ // Write down the two indicies.
+ std::pair<int, int> selected;
+ // We are regressing the combined estimated center, might write that down.
+ double regressed_y_center;
+
+ Eigen::VectorXf b = Eigen::VectorXf::Zero(4);
+ for (size_t i = 0; i < component_list.size(); i++) {
+ for (size_t j = 0; j < component_list.size(); j++) {
+ if (i == j) {
+ continue;
+ } else {
+ if (component_list[i].a < 0.0 || component_list[j].a < 0.0) {
+ // one of the targets is upside down (ie curved up), this can't
+ // happen.
+ continue;
+ }
+ // b is the target offests.
+ b(0) = component_list[j].EvalMinTop();
+ b(1) = component_list[j].EvalMinBot();
+ b(2) = component_list[i].EvalMinTop();
+ b(3) = component_list[i].EvalMinBot();
+ }
+
+ Eigen::VectorXf sol =
+ A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+ Eigen::VectorXf base = A * sol;
+ Eigen::VectorXf error_v = b - base;
+ double error = error_v.dot(error_v);
+ // offset in scrren x of the two targets.
+ double offset = std::abs(component_list[i].CenterPolyOne() -
+ component_list[j].CenterPolyOne());
+ // How much do we care about offset. As far as I've seen, error is like
+ // 5-20, offset are around 10. Value selected for worst garbage can image.
+ const double offsetWeight = 2.1;
+ error += offsetWeight * offset;
+ if ((best_error < 0 || error < best_error) && !isnan(error)) {
+ best_error = error;
+ best_offset = offset;
+ best_v = error_v;
+ selected.first = i;
+ selected.second = j;
+ regressed_y_center = sol(0);
+ }
+ }
+ }
+
+ // If we missed or the error is ridiculous just give up here.
+ if (best_error < 0 || best_error > 300.0 || isnan(best_error)) {
+ fprintf(stderr, "Bogus target dude (%f).\n", best_error);
+ return false;
+ }
+
+ fprintf(stderr,
+ "Selected (%d, %d):\n\t"
+ "err(%.2f, %.2f, %.2f, %.2f)(%.2f)(%.2f).\n\t"
+ "c00(%.2f, %.2f)(%.2f)\n",
+ selected.first, selected.second, best_v(0), best_v(1), best_v(2),
+ best_v(3), best_error, best_offset,
+ component_list[selected.first].CenterPolyOne(),
+ component_list[selected.second].CenterPolyOne(),
+ component_list[selected.first].CenterPolyOne() -
+ component_list[selected.second].CenterPolyOne());
+
+ double avgOff = (component_list[selected.first].mini +
+ component_list[selected.second].mini) /
+ 2.0;
+ double avgOne = (component_list[selected.first].CenterPolyOne() +
+ component_list[selected.second].CenterPolyOne()) /
+ 2.0;
+
+ final_target->screen_coord.x(avgOne + avgOff);
+ final_target->screen_coord.y(regressed_y_center);
+ final_target->comp1 = component_list[selected.first];
+ final_target->comp2 = component_list[selected.second];
+
+ return true;
+}
+
+} // namespace vision
+} // namespace y2017
diff --git a/y2017/vision/target_finder.h b/y2017/vision/target_finder.h
new file mode 100644
index 0000000..79417d1
--- /dev/null
+++ b/y2017/vision/target_finder.h
@@ -0,0 +1,86 @@
+#ifndef _Y2017_VISION_TARGET_FINDER_H_
+#define _Y2017_VISION_TARGET_FINDER_H_
+
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/vector.h"
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+namespace y2017 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+ const RangeImage *img = nullptr;
+
+ // Polynomial constants.
+ double a;
+ double b;
+ double c_0;
+ double c_1;
+
+ double mini;
+
+ double CenterPolyOne() { return -b / (2.0 * a); }
+ double CenterPolyTwo() { return (c_0); }
+
+ double EvalMinAt(double c) {
+ double min = CenterPolyOne();
+ return a * (min * min) + b * min + c;
+ }
+
+ double EvalMinTop() { return EvalMinAt(c_1); }
+ double EvalMinBot() { return EvalMinAt(c_0); }
+
+ // Fit error is not normalized by n.
+ double fit_error;
+ int n;
+
+ RangeImage RenderShifted() const;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+ TargetComponent comp1;
+
+ TargetComponent comp2;
+ aos::vision::Vector<2> screen_coord;
+};
+
+class TargetFinder {
+ public:
+ // Turn a bloblist into components of a target.
+ std::vector<TargetComponent> FillTargetComponentList(const BlobList &blobs);
+
+ // Turn a raw image into blob range image.
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+ // filter out obvious or durranged blobs.
+ void PreFilter(BlobList &imgs);
+
+ // Piece the compenents together into a target.
+ bool FindTargetFromComponents(std::vector<TargetComponent> component_list,
+ Target *final_target);
+
+ // Get the local overlay for debug if we are doing that.
+ aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+ private:
+ // Find a loosly connected target.
+ double DetectConnectedTarget(const RangeImage &img);
+
+ // TODO(ben): move to overlay
+ void DrawCross(aos::vision::Vector<2> center, aos::vision::PixelRef color);
+
+ aos::vision::PixelLinesOverlay overlay_;
+};
+
+} // namespace vision
+} // namespace y2017
+
+#endif // _Y2017_VISION_TARGET_FINDER_H_
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
new file mode 100644
index 0000000..851d3a5
--- /dev/null
+++ b/y2017/vision/target_receiver.cc
@@ -0,0 +1,64 @@
+#include <netdb.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
+#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_result.pb.h"
+
+using aos::monotonic_clock;
+
+namespace y2017 {
+namespace vision {
+
+void ComputeDistanceAngle(const TargetResult &target, double *distance,
+ double *angle) {
+ // TODO: fix this.
+ *distance = target.y();
+ *angle = target.x();
+}
+
+} // namespace vision
+} // namespace y2017
+
+int main() {
+ using namespace y2017::vision;
+ ::aos::events::RXUdpSocket recv(8080);
+ char raw_data[65507];
+
+ while (true) {
+ // TODO(austin): Don't malloc.
+ VisionResult target;
+ int size = recv.Recv(raw_data, sizeof(raw_data));
+ monotonic_clock::time_point now = monotonic_clock::now();
+ auto target_time = now -
+ std::chrono::nanoseconds(target.send_timestamp() -
+ target.image_timestamp()) +
+ // It takes a bit to shoot a frame. Push the frame
+ // further back in time.
+ std::chrono::milliseconds(10);
+
+ if (!target.ParseFromArray(raw_data, size)) {
+ continue;
+ }
+
+ auto new_vision_status = vision_status.MakeMessage();
+ new_vision_status->image_valid = target.has_target();
+ if (new_vision_status->image_valid) {
+ new_vision_status->target_time =
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ target_time.time_since_epoch())
+ .count();
+
+ ComputeDistanceAngle(target.target(), &new_vision_status->distance,
+ &new_vision_status->angle);
+ }
+
+ LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+ if (!new_vision_status.Send()) {
+ LOG(ERROR, "Failed to send vision information\n");
+ }
+ }
+}
diff --git a/y2017/vision/target_sender.cc b/y2017/vision/target_sender.cc
new file mode 100644
index 0000000..548d01d
--- /dev/null
+++ b/y2017/vision/target_sender.cc
@@ -0,0 +1,246 @@
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/image/reader.h"
+#include "y2017/vision/target_finder.h"
+#include "y2017/vision/vision_config.pb.h"
+#include "y2017/vision/vision_result.pb.h"
+
+namespace y2017 {
+namespace vision {
+
+using aos::events::TCPServer;
+using aos::vision::DataRef;
+using aos::vision::Int32Codec;
+using aos::vision::ImageValue;
+using aos::vision::Int64Codec;
+using aos::events::TXUdpSocket;
+using aos::events::DataSocket;
+using aos::vision::ImageFormat;
+using aos::vision::ImageStreamEvent;
+
+int64_t Nanos(aos::monotonic_clock::duration time_diff) {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(time_diff)
+ .count();
+}
+
+int64_t NowNanos() {
+ return Nanos(aos::monotonic_clock::now().time_since_epoch());
+}
+
+inline bool FileExist(const std::string &name) {
+ struct stat buffer;
+ return (stat(name.c_str(), &buffer) == 0);
+}
+
+class BlobLog {
+ public:
+ explicit BlobLog(const char *prefix, const char *extension) {
+ int index = 0;
+ while (true) {
+ std::string file = prefix + std::to_string(index) + extension;
+ if (FileExist(file)) {
+ index++;
+ } else {
+ printf("Logging to file (%s)\n", file.c_str());
+ ofst_.open(file);
+ assert(ofst_.is_open());
+ break;
+ }
+ }
+ }
+
+ ~BlobLog() { ofst_.close(); }
+
+ void WriteLogEntry(DataRef data) { ofst_.write(&data[0], data.size()); }
+
+ private:
+ std::ofstream ofst_;
+};
+
+ImageFormat GetImageFormat(const CameraSettings ¶ms) {
+ return ImageFormat{params.width(), params.height()};
+}
+
+class ImageSender : public ImageStreamEvent {
+ public:
+ ImageSender(camera::CameraParams params, GameSpecific game_cfg,
+ const std::string &fname, const std::string &ipadder, int port)
+ : ImageStreamEvent(fname, params),
+ game_cfg_(game_cfg),
+ udp_serv_(ipadder, 8080),
+ tcp_serv_(port),
+ log_("./logging/blob_record_", ".dat") {}
+
+ void WriteAndSendBlob(ImageFormat fmt, int64_t timestamp,
+ aos::vision::BlobList blobl) {
+ // Write blob to file for logging.
+ int blob_size = CalculateSize(blobl);
+ int tmp_size = blob_size + sizeof(int32_t) * 3 + sizeof(uint64_t);
+ char *buf;
+ std::string blob_data;
+ blob_data.resize(tmp_size, 0);
+ {
+ buf = Int32Codec::Write(&blob_data[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ buf = Int32Codec::Write(buf, fmt.w);
+ buf = Int32Codec::Write(buf, fmt.h);
+ SerializeBlob(blobl, buf);
+ }
+ log_.WriteLogEntry(blob_data);
+
+ // Send the blob back to the debug-viewer
+ tcp_serv_.Broadcast([&](DataSocket *client) { client->Emit(blob_data); });
+ }
+
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
+ using namespace aos::vision;
+ int64_t timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
+ tp.time_since_epoch())
+ .count();
+ DecodeJpeg(data, &image_);
+ ImageFormat fmt = image_.fmt();
+
+ RangeImage rimg = finder_.Threshold(image_.get());
+ BlobList blobl = aos::vision::FindBlobs(rimg);
+
+ // Remove bad blobs before we log.
+ finder_.PreFilter(blobl);
+
+ // Write to the logi and stream to the debug-viewer.
+ WriteAndSendBlob(fmt, timestamp, blobl);
+
+ // Calculate each component.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(blobl);
+
+ // Put the compenents together into targets and pick the best.
+ y2017::vision::Target final_target;
+ bool found_target =
+ finder_.FindTargetFromComponents(target_component_list, &final_target);
+
+ std::string dat;
+ VisionResult result;
+ result.set_image_timestamp(timestamp);
+ result.set_send_timestamp(NowNanos());
+ if (found_target) {
+ result.mutable_target()->set_x(final_target.screen_coord.x());
+ result.mutable_target()->set_y(final_target.screen_coord.y());
+ }
+ // always send data
+ if (result.SerializeToString(&dat)) {
+ if (print_once_) {
+ printf("Beginning data streaming camera...\n");
+ print_once_ = false;
+ }
+
+ // Send only target over udp.
+ udp_serv_.Send(dat.data(), dat.size());
+ }
+ }
+
+ TCPServer<DataSocket> *GetTCPServ() { return &tcp_serv_; }
+
+ // Configuration related to the game.
+ GameSpecific game_cfg_;
+
+ // target selction code.
+ TargetFinder finder_;
+
+ // print when we start
+ bool print_once_ = true;
+
+ // udp socket on which to send to robot
+ TXUdpSocket udp_serv_;
+
+ // tcp socket on which to debug to laptop
+ TCPServer<DataSocket> tcp_serv_;
+
+ ImageValue image_;
+ BlobLog log_;
+ aos::monotonic_clock::time_point tstart;
+
+ private:
+};
+
+void RunCamera(CameraSettings settings, GameSpecific game_cfg,
+ const std::string &device, const std::string &ip_addr,
+ int port) {
+ printf("Creating camera (%dx%d).\n", settings.width(), settings.height());
+ camera::CameraParams params = {settings.width(),
+ settings.height(),
+ settings.exposure(),
+ settings.brightness(),
+ 0,
+ (int32_t)settings.fps()};
+ ImageSender strm(params, game_cfg, device, ip_addr, port);
+
+ aos::events::EpollLoop loop;
+ loop.Add(strm.GetTCPServ());
+ loop.Add(&strm);
+ printf("Running Camera\n");
+ loop.Run();
+}
+
+bool ReadConfiguration(const std::string &file_name, VisionConfig *cfg) {
+ using namespace google::protobuf::io;
+ using namespace google::protobuf;
+ if (cfg == nullptr) {
+ return false;
+ }
+
+ // fd will close when it goes out of scope.
+ std::ifstream fd(file_name);
+ if (!fd.is_open()) {
+ fprintf(stderr, "File (%s) not found.\n", file_name.c_str());
+ return false;
+ }
+ IstreamInputStream is(&fd);
+ if (!TextFormat::Parse(&is, cfg)) {
+ fprintf(stderr, "Unable to parse file (%s).\n", file_name.c_str());
+ return false;
+ }
+
+ return true;
+}
+
+} // namespace vision
+} // namespace y2017
+
+int main(int, char **) {
+ using namespace y2017::vision;
+
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ VisionConfig cfg;
+ if (ReadConfiguration("ConfigFile.pb.ascii", &cfg)) {
+ if (cfg.robot_configs().count("Laptop") != 1) {
+ fprintf(stderr, "Could not find config key.\n");
+ return -1;
+ }
+ const RobotConfig &rbt = cfg.robot_configs().at("Laptop");
+ RunCamera(cfg.camera_params(), cfg.game_params(), rbt.camera_device_path(),
+ rbt.roborio_ipaddr(), rbt.port());
+ }
+
+ return EXIT_SUCCESS;
+}
diff --git a/y2017/vision/vision.q b/y2017/vision/vision.q
new file mode 100644
index 0000000..b3eeee8
--- /dev/null
+++ b/y2017/vision/vision.q
@@ -0,0 +1,14 @@
+package y2017.vision;
+
+message VisionStatus {
+ bool image_valid;
+
+ // Distance to the target in meters.
+ double distance;
+ // The angle in radians of the bottom of the target.
+ double angle;
+
+ // Capture time of the angle using the clock behind monotonic_clock::now().
+ int64_t target_time;
+};
+queue VisionStatus vision_status;
diff --git a/y2017/vision/vision_config.proto b/y2017/vision/vision_config.proto
new file mode 100644
index 0000000..0bb7279
--- /dev/null
+++ b/y2017/vision/vision_config.proto
@@ -0,0 +1,59 @@
+syntax = "proto2";
+
+package y2017.vision;
+
+// Stores configuration for camera related settings and specs.
+message CameraSettings {
+ // The focal length of the camera in pixels.
+ optional double focal_length = 1 [default = 1418.6];
+
+ // Width of the image.
+ optional int32 width = 2 [default = 1280];
+
+ // Height of the image.
+ optional int32 height = 3 [default = 960];
+
+ // Exposure setting.
+ optional int32 exposure = 4 [default = 10];
+
+ // Brightness setting.
+ optional int32 brightness = 5 [default = 128];
+
+ // Hardware gain multiplier on pixel values.
+ optional double gain = 6 [default = 1.0];
+
+ // Frames per second to run camera.
+ optional double fps = 7 [default = 30.0];
+}
+
+message GameSpecific {
+ // Needs more woojy.
+ optional int32 woojy = 1;
+}
+
+// Configurations that may be robot dependent.
+message RobotConfig {
+ // Device name of the camera (ie /dev/video1).
+ optional string camera_device_path = 1;
+
+ // RoboRIO IP address.
+ optional string roborio_ipaddr = 2;
+
+ // Jetson board IP address.
+ optional string jetson_ipaddr = 3;
+
+ // Port to use (both sides).
+ optional int32 port = 4;
+}
+
+// Stores configuration information for a given set of hardware.
+message VisionConfig {
+ // Map robot name to the robot dependent configuration.
+ map<string, RobotConfig> robot_configs = 1;
+
+ // Parameters for camera bringup.
+ optional CameraSettings camera_params = 2;
+
+ // Parameters for this specific game
+ optional GameSpecific game_params = 3;
+}
diff --git a/y2017/vision/vision_result.proto b/y2017/vision/vision_result.proto
new file mode 100644
index 0000000..04366db
--- /dev/null
+++ b/y2017/vision/vision_result.proto
@@ -0,0 +1,19 @@
+syntax = "proto2";
+
+package y2017.vision;
+
+// Represents a target found by the vision processing code.
+// X is an estimate of the center of the target.
+// Y is an estimate of the top of the bottom retroreflective tape.
+message TargetResult {
+ optional double x = 1;
+ optional double y = 2;
+}
+
+// Represents the best target in the image if there is such a target
+// along with timing information.
+message VisionResult {
+ optional int64 image_timestamp = 1;
+ optional int64 send_timestamp = 2;
+ optional TargetResult target = 3;
+}
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index b8fdd62..1343640 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -42,7 +42,6 @@
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/buffered_pcm.h"
-#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/dma_edge_counting.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
@@ -288,7 +287,7 @@
dma_synchronizer_->Start();
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- ::std::chrono::milliseconds(4));
+ ::std::chrono::milliseconds(0));
::aos::SetCurrentThreadRealtimePriority(40);
while (run_) {
@@ -454,8 +453,8 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_right_victor_->SetSpeed(-queue->right_voltage / 12.0);
+ drivetrain_left_victor_->SetSpeed(-queue->left_voltage / 12.0);
+ drivetrain_right_victor_->SetSpeed(queue->right_voltage / 12.0);
}
virtual void Stop() override {
@@ -508,7 +507,7 @@
indexer_victor_->SetSpeed(queue->voltage_indexer / 12.0);
indexer_roller_victor_->SetSpeed(queue->voltage_indexer_rollers /
12.0);
- turret_victor_->SetSpeed(::aos::Clip(queue->voltage_turret,
+ turret_victor_->SetSpeed(::aos::Clip(-queue->voltage_turret,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
hood_victor_->SetSpeed(
@@ -578,10 +577,7 @@
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
- ::frc971::wpilib::GyroSender gyro_sender;
- ::std::thread gyro_thread(::std::ref(gyro_sender));
-
- auto imu_trigger = make_unique<DigitalInput>(5);
+ auto imu_trigger = make_unique<DigitalInput>(3);
::frc971::wpilib::ADIS16448 imu(SPI::Port::kOnboardCS1, imu_trigger.get());
::std::thread imu_thread(::std::ref(imu));
@@ -628,8 +624,6 @@
pdp_fetcher_thread.join();
reader.Quit();
reader_thread.join();
- gyro_sender.Quit();
- gyro_thread.join();
imu.Quit();
imu_thread.join();