Merge "Reformat python and c++ files"
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index c139195..3e24fab 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -1,5 +1,7 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "ring_buffer",
hdrs = [
@@ -41,6 +43,7 @@
hdrs = [
"sized_array.h",
],
+ compatible_with = mcu_cpus,
)
cc_test(
diff --git a/aos/containers/priority_queue.h b/aos/containers/priority_queue.h
index 0bc94e7..8fb5e5c 100644
--- a/aos/containers/priority_queue.h
+++ b/aos/containers/priority_queue.h
@@ -78,8 +78,17 @@
bool empty() const { return size_ == 0; }
bool full() const { return size_ == buffer_size; }
+ // Removes all the elements from the queue:
+ void clear() {
+ size_ = 0;
+ bottom_ = buffer_size;
+ top_ = buffer_size;
+ }
+
Data &top() { return list_[top_].data; }
+ const Data &top() const { return list_[top_].data; }
Data &get(size_t idx) { return list_[idx].data; }
+ const Data &get(size_t idx) const { return list_[idx].data; }
iterator begin() { return iterator(this, bottom_); }
iterator end() { return iterator(this, buffer_size); }
diff --git a/aos/containers/priority_queue_test.cc b/aos/containers/priority_queue_test.cc
index 2c45990..c05d827 100644
--- a/aos/containers/priority_queue_test.cc
+++ b/aos/containers/priority_queue_test.cc
@@ -52,7 +52,7 @@
ASSERT_EQ(4u, queue_.size());
ASSERT_FALSE(queue_.full());
::std::vector<int> reverse_expected{20, 15, 11};
- EXPECT_EQ(20, queue_.top());
+ EXPECT_EQ(10, *queue_.begin());
for (; it != queue_.end(); ++it) {
EXPECT_EQ(reverse_expected.back(), *it);
reverse_expected.pop_back();
@@ -60,6 +60,30 @@
ASSERT_TRUE(reverse_expected.empty());
}
+// Tests that the clear() method works properly.
+TEST_F(PriorityQueueTest, ClearQueue) {
+ queue_.PushFromBottom(10);
+ queue_.PushFromBottom(20);
+ queue_.PushFromBottom(15);
+ queue_.PushFromBottom(11);
+ ASSERT_EQ(4u, queue_.size());
+ ASSERT_FALSE(queue_.full());
+ queue_.clear();
+ ASSERT_TRUE(queue_.empty());
+ ASSERT_EQ(0u, queue_.size());
+
+ queue_.PushFromBottom(1);
+ queue_.PushFromBottom(3);
+ queue_.PushFromBottom(2);
+ ASSERT_EQ(3u, queue_.size());
+ ::std::vector<int> reverse_expected{3, 2, 1};
+ for (const int val : queue_) {
+ EXPECT_EQ(reverse_expected.back(), val);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
TEST_F(PriorityQueueTest, FullBufferInsertTop) {
for (int ii = 0; ii < 10; ++ii) {
queue_.PushFromBottom(ii);
diff --git a/aos/time/BUILD b/aos/time/BUILD
index b76cd99..6570490 100644
--- a/aos/time/BUILD
+++ b/aos/time/BUILD
@@ -1,4 +1,4 @@
-package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
cc_library(
name = "time",
@@ -8,11 +8,31 @@
hdrs = [
"time.h",
],
+ visibility = ["//visibility:public"],
deps = [
"//aos:macros",
- "//aos/mutex:mutex",
- "//aos/logging",
"//aos/ipc_lib:shared_mem",
+ "//aos/logging",
+ "//aos/mutex",
+ "//aos/type_traits",
+ ],
+)
+
+# TODO(Brian): Remove this hack once bazel chases deps through selects correctly.
+cc_library(
+ name = "time_mcu",
+ srcs = [
+ "time.cc",
+ ],
+ hdrs = [
+ "time.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:macros",
+ "//aos/type_traits",
+ "//motors/core",
],
)
@@ -24,7 +44,7 @@
deps = [
":time",
"//aos/logging",
- "//aos/util:death_test_log_implementation",
"//aos/testing:googletest",
+ "//aos/util:death_test_log_implementation",
],
)
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 0d7295c..06f4b12 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -6,6 +6,8 @@
#include <atomic>
#include <chrono>
+#ifdef __linux__
+
// We only use global_core from here, which is weak, so we don't really have a
// dependency on it.
#include "aos/ipc_lib/shared_mem.h"
@@ -13,8 +15,19 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
+#else // __linux__
+
+#include "motors/core/kinetis.h"
+
+// The systick interrupt increments this every 1ms.
+extern "C" volatile uint32_t systick_millis_count;
+
+#endif // __linux__
+
namespace chrono = ::std::chrono;
+#ifdef __linux__
+
namespace std {
namespace this_thread {
template <>
@@ -42,10 +55,13 @@
} // namespace this_thread
} // namespace std
+#endif // __linux__
namespace aos {
namespace time {
+#ifdef __linux__
+
// State required to enable and use mock time.
namespace {
// True if mock time is enabled.
@@ -98,6 +114,8 @@
chrono::duration_cast<chrono::nanoseconds>(offset).count();
}
+#endif // __linux__
+
struct timespec to_timespec(
const ::aos::monotonic_clock::duration duration) {
struct timespec time_timespec;
@@ -119,11 +137,11 @@
constexpr monotonic_clock::time_point monotonic_clock::min_time;
monotonic_clock::time_point monotonic_clock::now() noexcept {
- {
- if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
- MutexLocker time_mutex_locker(&time::time_mutex);
- return time::current_mock_time;
- }
+#ifdef __linux__
+
+ if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
+ MutexLocker time_mutex_locker(&time::time_mutex);
+ return time::current_mock_time;
}
struct timespec current_time;
@@ -139,7 +157,45 @@
return time_point(::std::chrono::seconds(current_time.tv_sec) +
::std::chrono::nanoseconds(current_time.tv_nsec)) + offset;
+
+#else // __linux__
+
+ __disable_irq();
+ const uint32_t current_counter = SYST_CVR;
+ uint32_t ms_count = systick_millis_count;
+ const uint32_t istatus = SCB_ICSR;
+ __enable_irq();
+ // If the interrupt is pending and the timer has already wrapped from 0 back
+ // up to its max, then add another ms.
+ if ((istatus & SCB_ICSR_PENDSTSET) && current_counter > 50) {
+ ++ms_count;
+ }
+
+ // It counts down, but everything we care about counts up.
+ const uint32_t counter_up = ((F_CPU / 1000) - 1) - current_counter;
+
+ // "3.2.1.2 System Tick Timer" in the TRM says "The System Tick Timer's clock
+ // source is always the core clock, FCLK".
+ using systick_duration =
+ std::chrono::duration<uint32_t, std::ratio<1, F_CPU>>;
+
+ return time_point(aos::time::round<std::chrono::nanoseconds>(
+ std::chrono::milliseconds(ms_count) + systick_duration(counter_up)));
+
+#endif // __linux__
}
+#ifdef __linux__
+realtime_clock::time_point realtime_clock::now() noexcept {
+ struct timespec current_time;
+ if (clock_gettime(CLOCK_REALTIME, ¤t_time) != 0) {
+ PLOG(FATAL, "clock_gettime(%jd, %p) failed",
+ static_cast<uintmax_t>(CLOCK_REALTIME), ¤t_time);
+ }
+
+ return time_point(::std::chrono::seconds(current_time.tv_sec) +
+ ::std::chrono::nanoseconds(current_time.tv_nsec));
+}
+#endif // __linux__
} // namespace aos
diff --git a/aos/time/time.h b/aos/time/time.h
index 9bef741..136629c 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -24,7 +24,32 @@
typedef ::std::chrono::time_point<monotonic_clock> time_point;
static monotonic_clock::time_point now() noexcept;
- static constexpr bool is_steady = true;
+ // This clock is still subject to rate adjustments based on adjtime, so it is
+ // not steady.
+ static constexpr bool is_steady = false;
+
+ // Returns the epoch (0).
+ static constexpr monotonic_clock::time_point epoch() {
+ return time_point(zero());
+ }
+
+ static constexpr monotonic_clock::duration zero() { return duration(0); }
+
+ static constexpr time_point min_time{
+ time_point(duration(::std::numeric_limits<duration::rep>::min()))};
+};
+
+class realtime_clock {
+ public:
+ typedef ::std::chrono::nanoseconds::rep rep;
+ typedef ::std::chrono::nanoseconds::period period;
+ typedef ::std::chrono::nanoseconds duration;
+ typedef ::std::chrono::time_point<monotonic_clock> time_point;
+
+#ifdef __linux__
+ static monotonic_clock::time_point now() noexcept;
+#endif // __linux__
+ static constexpr bool is_steady = false;
// Returns the epoch (0).
static constexpr monotonic_clock::time_point epoch() {
@@ -39,6 +64,8 @@
namespace time {
+#ifdef __linux__
+
// Enables returning the mock time value for Now instead of checking the system
// clock.
void EnableMockTime(monotonic_clock::time_point now);
@@ -78,6 +105,8 @@
DISALLOW_COPY_AND_ASSIGN(TimeFreezer);
};
+#endif // __linux__
+
// Converts a monotonic_clock::duration into a timespec object.
struct timespec to_timespec(::aos::monotonic_clock::duration duration);
@@ -85,9 +114,72 @@
// epoch.
struct timespec to_timespec(::aos::monotonic_clock::time_point time);
+namespace time_internal {
+
+template <class T>
+struct is_duration : std::false_type {};
+template <class Rep, class Period>
+struct is_duration<std::chrono::duration<Rep, Period>> : std::true_type {};
+
+} // namespace time_internal
+
+// Returns the greatest duration t representable in ToDuration that is less or
+// equal to d.
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/duration/floor.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Rep, class Period,
+ class = std::enable_if_t<time_internal::is_duration<To>{}>>
+constexpr To floor(const std::chrono::duration<Rep, Period> &d) {
+ To t = std::chrono::duration_cast<To>(d);
+ if (t > d) return t - To{1};
+ return t;
+}
+
+// Returns the value t representable in ToDuration that is the closest to d. If
+// there are two such values, returns the even value (that is, the value t such
+// that t % 2 == 0).
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/duration/round.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Rep, class Period,
+ class = std::enable_if_t<
+ time_internal::is_duration<To>{} &&
+ !std::chrono::treat_as_floating_point<typename To::rep>{}>>
+constexpr To round(const std::chrono::duration<Rep, Period> &d) {
+ To t0 = aos::time::floor<To>(d);
+ To t1 = t0 + To{1};
+ auto diff0 = d - t0;
+ auto diff1 = t1 - d;
+ if (diff0 == diff1) {
+ if (t0.count() & 1) return t1;
+ return t0;
+ } else if (diff0 < diff1) {
+ return t0;
+ }
+ return t1;
+}
+
+// Returns the nearest time point to tp representable in ToDuration, rounding to
+// even in halfway cases, like std::chrono::round in C++17.
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/time_point/round.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Clock, class FromDuration,
+ class = std::enable_if_t<
+ time_internal::is_duration<To>{} &&
+ !std::chrono::treat_as_floating_point<typename To::rep>{}>>
+constexpr std::chrono::time_point<Clock, To> round(
+ const std::chrono::time_point<Clock, FromDuration> &tp) {
+ return std::chrono::time_point<Clock, To>{
+ aos::time::round<To>(tp.time_since_epoch())};
+}
+
} // namespace time
} // namespace aos
+#ifdef __linux__
+
namespace std {
namespace this_thread {
// Template specialization for monotonic_clock, since we can use clock_nanosleep
@@ -98,5 +190,6 @@
} // namespace this_thread
} // namespace std
+#endif // __linux__
#endif // AOS_TIME_H_
diff --git a/aos/type_traits/BUILD b/aos/type_traits/BUILD
index 171bb8d..e5590b0 100644
--- a/aos/type_traits/BUILD
+++ b/aos/type_traits/BUILD
@@ -1,10 +1,13 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "type_traits",
hdrs = [
"type_traits.h",
],
+ compatible_with = mcu_cpus,
)
cc_test(
diff --git a/aos/type_traits/type_traits.h b/aos/type_traits/type_traits.h
index f0a2e72..437cb3e 100644
--- a/aos/type_traits/type_traits.h
+++ b/aos/type_traits/type_traits.h
@@ -1,8 +1,6 @@
#ifndef AOS_TYPE_TRAITS_
#define AOS_TYPE_TRAITS_
-#include <features.h>
-
#include <type_traits>
namespace aos {
diff --git a/aos/vision/blob/threshold.h b/aos/vision/blob/threshold.h
index 8b6051f..eef5b20 100644
--- a/aos/vision/blob/threshold.h
+++ b/aos/vision/blob/threshold.h
@@ -9,15 +9,15 @@
// ThresholdFn should be a lambda.
template <typename ThresholdFn>
-RangeImage DoThreshold(const ImagePtr &img, ThresholdFn &&fn) {
+RangeImage DoThreshold(ImageFormat fmt, ThresholdFn &&fn) {
std::vector<std::vector<ImageRange>> ranges;
- ranges.reserve(img.fmt().h);
- for (int y = 0; y < img.fmt().h; ++y) {
+ ranges.reserve(fmt.h);
+ for (int y = 0; y < fmt.h; ++y) {
bool p_score = false;
int pstart = -1;
std::vector<ImageRange> rngs;
- for (int x = 0; x < img.fmt().w; ++x) {
- if (fn(img.get_px(x, y)) != p_score) {
+ for (int x = 0; x < fmt.w; ++x) {
+ if (fn(x, y) != p_score) {
if (p_score) {
rngs.emplace_back(ImageRange(pstart, x));
} else {
@@ -27,13 +27,29 @@
}
}
if (p_score) {
- rngs.emplace_back(ImageRange(pstart, img.fmt().w));
+ rngs.emplace_back(ImageRange(pstart, fmt.w));
}
ranges.push_back(rngs);
}
return RangeImage(0, std::move(ranges));
}
+// ThresholdFn should be a lambda.
+template <typename ThresholdFn>
+RangeImage DoThreshold(const ImagePtr &img, ThresholdFn &&fn) {
+ return DoThreshold(img.fmt(),
+ [&](int x, int y) { return fn(img.get_px(x, y)); });
+}
+
+// YUYV image types:
+inline RangeImage DoThresholdYUYV(ImageFormat fmt, const char *data,
+ uint8_t value) {
+ return DoThreshold(fmt, [&](int x, int y) {
+ uint8_t v = data[y * fmt.w * 2 + x * 2];
+ return v > value;
+ });
+}
+
} // namespace vision
} // namespace aos
diff --git a/aos/vision/debug/debug_framework.cc b/aos/vision/debug/debug_framework.cc
index 46f5400..1d94217 100644
--- a/aos/vision/debug/debug_framework.cc
+++ b/aos/vision/debug/debug_framework.cc
@@ -58,7 +58,7 @@
InstallKeyPress(key_press);
}
if (GetScreenHeight() < 1024) {
- view_.SetScale(0.75);
+ view_.SetScale(1.0);
}
}
@@ -70,6 +70,17 @@
return HandleBlobs(FindBlobs(filter_->Threshold(view_.img())), fmt);
}
+ bool NewImage(ImageFormat fmt,
+ const std::function<bool(ImagePtr data)> &process) override {
+ auto value = view_.img();
+ if (!value.fmt().Equals(fmt)) {
+ view_.SetFormatAndClear(fmt);
+ }
+ process(view_.img());
+
+ return HandleBlobs(FindBlobs(filter_->Threshold(view_.img())), fmt);
+ }
+
bool NewBlobList(BlobList blob_list, ImageFormat fmt) override {
view_.SetFormatAndClear(fmt);
diff --git a/aos/vision/debug/debug_framework.h b/aos/vision/debug/debug_framework.h
index d5b345f..a46812f 100644
--- a/aos/vision/debug/debug_framework.h
+++ b/aos/vision/debug/debug_framework.h
@@ -55,6 +55,9 @@
// if the frame is "interesting" ie has a target.
virtual bool NewJpeg(DataRef data) = 0;
+ virtual bool NewImage(ImageFormat fmt,
+ const std::function<bool(ImagePtr data)> &process) = 0;
+
virtual bool NewBlobList(BlobList blob_list, ImageFormat fmt) = 0;
virtual bool JustCheckForTarget(BlobList imgs, ImageFormat fmt) = 0;
diff --git a/aos/vision/debug/jpeg_list-source.cc b/aos/vision/debug/jpeg_list-source.cc
index 2dbbe44..027acc9 100644
--- a/aos/vision/debug/jpeg_list-source.cc
+++ b/aos/vision/debug/jpeg_list-source.cc
@@ -1,5 +1,7 @@
#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/image/image_dataset.h"
+
#include <gdk/gdk.h>
#include <fstream>
#include <string>
@@ -7,74 +9,15 @@
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));
- }
- }();
- }
+ images_ = LoadDataset(jpeg_list_filename);
fprintf(stderr, "loaded %lu items\n", images_.size());
if (!images_.empty()) {
- interface_->NewJpeg(images_[idx_]);
+ SetCurrentFrame();
interface_->InstallKeyPress([this](uint32_t keyval) {
if (keyval == GDK_KEY_Left && idx_ > 0) {
--idx_;
@@ -83,11 +26,30 @@
} else {
return;
}
- interface_->NewJpeg(images_[idx_]);
+ SetCurrentFrame();
});
}
}
+ void SetCurrentFrame() {
+ const auto &frame = images_[idx_];
+ if (frame.is_jpeg) {
+ interface_->NewJpeg(frame.data);
+ } else {
+ const auto &data = frame.data;
+ interface_->NewImage({640, 480},
+ [&](ImagePtr img_data) {
+ for (int y = 0; y < 480; ++y) {
+ for (int x = 0; x < 640; ++x) {
+ uint8_t v = data[y * 640 * 2 + x * 2 + 0];
+ img_data.get_px(x, y) = PixelRef{v, v, v};
+ }
+ }
+ return false;
+ });
+ }
+ }
+
const char *GetHelpMessage() override {
return &R"(
format_spec is the name of a file with each jpeg filename on a new line.
@@ -98,7 +60,7 @@
private:
DebugFrameworkInterface *interface_ = nullptr;
- std::vector<std::string> images_;
+ std::vector<DatasetFrame> images_;
size_t idx_ = 0;
};
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
index 0122c94..8eb8a23 100644
--- a/aos/vision/debug/overlay.h
+++ b/aos/vision/debug/overlay.h
@@ -121,8 +121,12 @@
// build a segment for this line
void AddLine(Vector<2> st, Vector<2> ed, PixelRef newColor) {
- lines_.emplace_back(
- std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
+ AddLine(Segment<2>(st, ed), newColor);
+ }
+
+ // draw a segment.
+ void AddLine(Segment<2> seg, PixelRef newColor) {
+ lines_.emplace_back(std::pair<Segment<2>, PixelRef>(seg, newColor));
}
void DrawCross(aos::vision::Vector<2> center, int width,
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
index 9bc3c19..be702d7 100644
--- a/aos/vision/image/BUILD
+++ b/aos/vision/image/BUILD
@@ -42,6 +42,15 @@
)
cc_library(
+ name = "image_dataset",
+ srcs = ["image_dataset.cc"],
+ hdrs = ["image_dataset.h"],
+ deps = [
+ ":image_types",
+ ],
+)
+
+cc_library(
name = "image_stream",
hdrs = ["image_stream.h"],
deps = [
diff --git a/aos/vision/image/image_dataset.cc b/aos/vision/image/image_dataset.cc
new file mode 100644
index 0000000..cd3ec46
--- /dev/null
+++ b/aos/vision/image/image_dataset.cc
@@ -0,0 +1,84 @@
+#include "aos/vision/image/image_dataset.h"
+
+#include <fstream>
+
+#include "aos/vision/image/image_types.h"
+
+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
+
+std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename) {
+ std::vector<DatasetFrame> images;
+ 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;
+ }
+ bool is_jpeg = true;
+ size_t l = jpeg_filename.size();
+ if (l > 4 && jpeg_filename[l - 1] == 'v') {
+ is_jpeg = false;
+ }
+ if (jpeg_filename[0] == '/') {
+ images.emplace_back(
+ DatasetFrame{is_jpeg, GetFileContents(jpeg_filename)});
+ } else {
+ images.emplace_back(
+ DatasetFrame{is_jpeg, GetFileContents(basename + jpeg_filename)});
+ }
+ }();
+ }
+
+ return images;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/image/image_dataset.h b/aos/vision/image/image_dataset.h
new file mode 100644
index 0000000..ce8c841
--- /dev/null
+++ b/aos/vision/image/image_dataset.h
@@ -0,0 +1,21 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_DATASET_H_
+#define _AOS_VISION_IMAGE_IMAGE_DATASET_H_
+
+#include <string>
+#include <vector>
+
+namespace aos {
+namespace vision {
+
+struct DatasetFrame {
+ // TODO: These should be V4L formats ideally.
+ bool is_jpeg = true;
+ std::string data;
+};
+
+std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_IMAGE_DATASET_H_
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
index d4ddbf2..c50400a 100644
--- a/aos/vision/image/image_types.h
+++ b/aos/vision/image/image_types.h
@@ -96,6 +96,7 @@
}
const ImageFormat &fmt() const { return fmt_; }
+ const ImageType *data() const { return data_.get(); }
ImageType *data() { return data_.get(); }
private:
diff --git a/aos/vision/math/segment.h b/aos/vision/math/segment.h
index 706beaf..d6927cb 100644
--- a/aos/vision/math/segment.h
+++ b/aos/vision/math/segment.h
@@ -44,7 +44,7 @@
}
// Intersect two lines in a plane.
- Vector<2> Intersect(const Segment<2> &other) {
+ Vector<2> Intersect(const Segment<2> &other) const {
static_assert(Size == 2, "Only works for size == 2");
double x1 = A_.x();
double y1 = A_.y();
diff --git a/aos/vision/math/vector.h b/aos/vision/math/vector.h
index 3205236..642c63f 100644
--- a/aos/vision/math/vector.h
+++ b/aos/vision/math/vector.h
@@ -186,6 +186,11 @@
return tmp.MagSqr();
}
+ // Returns the distance between this and other.
+ double DistanceTo(const Vector<size> &other) const {
+ return std::sqrt(SquaredDistanceTo(other));
+ }
+
private:
// The actual data.
::Eigen::Matrix<double, 1, size> data_;
@@ -195,6 +200,17 @@
inline double PointsCrossProduct(const Vector<2> &a, const Vector<2> &b) {
return a.x() * b.y() - a.y() * b.x();
}
+
+// Rotates along the z, y, and then x axis (all radians).
+inline Vector<3> Rotate(double rx, double ry, double rz, const Vector<3> vec) {
+ ::Eigen::AngleAxis<double> ax(rx, ::Eigen::Vector3d(1.0, 0.0, 0.0));
+ ::Eigen::AngleAxis<double> ay(ry, ::Eigen::Vector3d(0.0, 1.0, 0.0));
+ ::Eigen::AngleAxis<double> az(rz, ::Eigen::Vector3d(0.0, 0.0, 1.0));
+ Vector<3> result;
+ result.SetData(ax * ay * az * vec.GetData());
+ return result;
+}
+
// scalar multiply
template <int Size>
inline Vector<Size> operator*(const double &lhs, Vector<Size> &rhs) {
diff --git a/frc971/constants.h b/frc971/constants.h
index fc3e12b..a52add2 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -96,6 +96,8 @@
double upper_hard;
double lower;
double upper;
+
+ double middle() const { return (lower_hard + upper_hard) / 2.0; }
};
} // namespace constants
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 44eb262..18ed29e 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -188,3 +188,33 @@
// Maximum acceleration for the profile.
float max_acceleration;
};
+
+// Definition of a constraint on a trajectory
+struct Constraint {
+ // Type of constraint
+ // 0: Null constraint. Ignore and all following
+ // 1: longitual acceleration
+ // 2: lateral acceleration
+ // 3: voltage
+ // 4: velocity
+ uint8_t constraint_type;
+ float value;
+ // start and end distance are only checked for velocity limits.
+ float start_distance;
+ float end_distance;
+};
+
+// Parameters for computing a trajectory using a chain of splines and
+// constraints.
+struct MultiSpline {
+ // index of the spline. Zero indicates the spline should not be computed.
+ uint32_t spline_idx;
+ // Number of splines. The spline point arrays will be expected to have
+ // 6 + 5 * (n - 1) points in them. The endpoints are shared between
+ // neighboring splines.
+ uint8_t spline_count;
+ float[36] spline_x;
+ float[36] spline_y;
+
+ Constraint[6] constraints;
+};
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index aee3a37..d95e101 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -49,6 +49,23 @@
)
cc_library(
+ name = "splinedrivetrain",
+ srcs = [
+ "splinedrivetrain.cc",
+ ],
+ hdrs = [
+ "splinedrivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":spline",
+ ":distance_spline",
+ ":trajectory",
+ ]
+)
+
+cc_library(
name = "ssdrivetrain",
srcs = [
"ssdrivetrain.cc",
@@ -170,6 +187,7 @@
":gear",
":polydrivetrain",
":ssdrivetrain",
+ ":splinedrivetrain",
"//aos/controls:control_loop",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a50b429..168f81d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -38,6 +38,7 @@
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_spline_(dt_config_),
down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -329,12 +330,15 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
+ dt_spline_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
dt_closedloop_.Update(output != NULL && controller_type == 1);
+ dt_spline_.Update(output != NULL && controller_type == 2);
+
switch (controller_type) {
case 0:
dt_openloop_.SetOutput(output);
@@ -342,6 +346,9 @@
case 1:
dt_closedloop_.SetOutput(output);
break;
+ case 2:
+ dt_spline_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a357eab..05bf711 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
namespace frc971 {
namespace control_loops {
@@ -50,6 +51,7 @@
StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ SplineDrivetrain dt_spline_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index c66b03e..f63f685 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -72,6 +72,14 @@
// The control loop will profile if these are all non-zero.
.frc971.ProfileParameters linear;
.frc971.ProfileParameters angular;
+
+ // Parameters for a spline to follow. This just contains info on a spline to
+ // compute. Each time this is sent, spline drivetrain will compute a new
+ // spline.
+ .frc971.MultiSpline spline;
+
+ // Which spline to follow.
+ uint32_t spline_handle;
};
message Position {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
new file mode 100644
index 0000000..0d6b6ae
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -0,0 +1,101 @@
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+const int kMaxSplineConstraints = 6;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config) {}
+
+// TODO(alex): put in another thread to avoid malloc in RT.
+void SplineDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ current_spline_handle_ = goal.spline_handle;
+ const ::frc971::MultiSpline &multispline = goal.spline;
+ if (multispline.spline_idx) {
+ current_spline_idx_ = multispline.spline_idx;
+ auto x = multispline.spline_x;
+ auto y = multispline.spline_y;
+ ::std::vector<Spline> splines = ::std::vector<Spline>();
+ for (int i = 0; i < multispline.spline_count; ++i) {
+ ::Eigen::Matrix<double, 2, 6> points =
+ ::Eigen::Matrix<double, 2, 6>::Zero();
+ for (int j = 0; j < 6; ++j) {
+ points(0, j) = x[i * 5 + j];
+ points(1, j) = y[i * 5 + j];
+ }
+ splines.emplace_back(Spline(points));
+ }
+
+ distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+ new DistanceSpline(::std::move(splines)));
+
+ current_trajectory_ = ::std::unique_ptr<Trajectory>(
+ new Trajectory(distance_spline_.get(), dt_config_));
+
+ for (int i = 0; i < kMaxSplineConstraints; ++i) {
+ const ::frc971::Constraint &constraint = multispline.constraints[i];
+ switch (constraint.constraint_type) {
+ case 0:
+ break;
+ case 1:
+ current_trajectory_->set_longitudal_acceleration(constraint.value);
+ break;
+ case 2:
+ current_trajectory_->set_lateral_acceleration(constraint.value);
+ break;
+ case 3:
+ current_trajectory_->set_voltage_limit(constraint.value);
+ break;
+ case 4:
+ current_trajectory_->LimitVelocity(constraint.start_distance,
+ constraint.end_distance,
+ constraint.value);
+ break;
+ }
+ }
+
+ current_trajectory_->Plan();
+ current_xva_ = current_trajectory_->FFAcceleration(0);
+ current_xva_(1) = 0.0;
+ }
+}
+
+void SplineDrivetrain::Update(bool enable) {
+ if (enable && current_trajectory_ &&
+ !current_trajectory_->is_at_end(current_state_)) {
+ next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, ¤t_state_);
+ }
+}
+
+// TODO(alex): Handle drift.
+void SplineDrivetrain::SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ if (!output) {
+ return;
+ }
+ if (!current_trajectory_) {
+ return;
+ }
+ if (current_spline_handle_ == current_spline_idx_) {
+ if (!current_trajectory_->is_at_end(current_state_)) {
+ double current_distance = current_xva_(0);
+ ::Eigen::Matrix<double, 2, 1> FFVoltage =
+ current_trajectory_->FFVoltage(current_distance);
+ output->left_voltage = FFVoltage(0);
+ output->right_voltage = FFVoltage(1);
+ current_xva_ = next_xva_;
+ }
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
new file mode 100644
index 0000000..4bb7ac9
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -0,0 +1,44 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class SplineDrivetrain {
+ public:
+ SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
+
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+
+ void Update(bool enabled);
+
+ void SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output);
+ // TODO(alex): What status do we need?
+ void PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ private:
+ const DrivetrainConfig<double> dt_config_;
+
+ uint32_t current_spline_handle_; // Current spline told to excecute.
+ uint32_t current_spline_idx_; // Current excecuting spline.
+ ::std::unique_ptr<DistanceSpline> distance_spline_;
+ ::std::unique_ptr<Trajectory> current_trajectory_;
+ ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
+ ::Eigen::Matrix<double, 2, 1> current_state_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 508a6a6..13bd6e6 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -1,6 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSE_H_
#define FRC971_CONTROL_LOOPS_POSE_H_
+#include <vector>
+
#include "Eigen/Dense"
#include "aos/util/math.h"
@@ -39,6 +41,9 @@
// The type that contains the translational (x, y, z) component of the Pose.
typedef Eigen::Matrix<Scalar, 3, 1> Pos;
+ // Provide a default constructor that crease a pose at the origin.
+ TypedPose() : TypedPose({0.0, 0.0, 0.0}, 0.0) {}
+
// Construct a Pose in the absolute frame with a particular position and yaw.
TypedPose(const Pos &abs_pos, Scalar theta) : pos_(abs_pos), theta_(theta) {}
// Construct a Pose relative to another Pose (base).
@@ -69,9 +74,12 @@
// Provide access to the position and yaw relative to the base Pose.
Pos rel_pos() const { return pos_; }
Scalar rel_theta() const { return theta_; }
+ const TypedPose<Scalar> *base() const { return base_; }
Pos *mutable_pos() { return &pos_; }
void set_theta(Scalar theta) { theta_ = theta; }
+ // Swap out the base Pose, keeping the current relative position/angle.
+ void set_base(const TypedPose<Scalar> *new_base) { base_ = new_base; }
// For 2-D calculation, provide the heading, which is distinct from the
// yaw/theta value. heading is the heading relative to the base Pose if you
@@ -137,6 +145,7 @@
template <typename Scalar = double>
class TypedLineSegment {
public:
+ TypedLineSegment() {}
TypedLineSegment(const TypedPose<Scalar> &pose1,
const TypedPose<Scalar> &pose2)
: pose1_(pose1), pose2_(pose2) {}
@@ -162,9 +171,18 @@
(::aos::math::PointsAreCCW<Scalar>(p1, p2, q1) !=
::aos::math::PointsAreCCW<Scalar>(p1, p2, q2));
}
+
+ TypedPose<Scalar> pose1() const { return pose1_; }
+ TypedPose<Scalar> pose2() const { return pose2_; }
+ TypedPose<Scalar> *mutable_pose1() { return &pose1_; }
+ TypedPose<Scalar> *mutable_pose2() { return &pose2_; }
+
+ ::std::vector<TypedPose<Scalar>> PlotPoints() const {
+ return {pose1_, pose2_};
+ }
private:
- const TypedPose<Scalar> pose1_;
- const TypedPose<Scalar> pose2_;
+ TypedPose<Scalar> pose1_;
+ TypedPose<Scalar> pose2_;
}; // class TypedLineSegment
typedef TypedLineSegment<double> LineSegment;
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
index cb994a2..4fd3f15 100644
--- a/frc971/control_loops/pose_test.cc
+++ b/frc971/control_loops/pose_test.cc
@@ -23,6 +23,8 @@
EXPECT_EQ(1.0, pose.abs_pos().x());
EXPECT_EQ(1.0, pose.abs_pos().y());
+ EXPECT_EQ(1.0, pose.abs_xy().x());
+ EXPECT_EQ(1.0, pose.abs_xy().y());
EXPECT_EQ(0.5, pose.abs_pos().z());
EXPECT_EQ(0.5, pose.rel_theta());
@@ -32,6 +34,11 @@
EXPECT_EQ(3.14, pose.rel_theta());
pose.mutable_pos()->x() = 9.71;
EXPECT_EQ(9.71, pose.rel_pos().x());
+
+ EXPECT_EQ(nullptr, pose.base());
+ Pose new_base;
+ pose.set_base(&new_base);
+ EXPECT_EQ(&new_base, pose.base());
}
// Check that Poses behave as expected when constructed relative to another
@@ -76,6 +83,22 @@
EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
}
+// Tests that basic accessors for LineSegment behave as expected.
+TEST(LineSegmentTest, BasicAccessorTest) {
+ LineSegment l;
+ EXPECT_EQ(0.0, l.pose1().rel_theta());
+ l.mutable_pose1()->set_theta(1.234);
+ EXPECT_EQ(1.234, l.pose1().rel_theta());
+ EXPECT_EQ(0.0, l.pose2().rel_theta());
+ l.mutable_pose2()->set_theta(5.678);
+ EXPECT_EQ(5.678, l.pose2().rel_theta());
+
+ const ::std::vector<Pose> plot_pts = l.PlotPoints();
+ ASSERT_EQ(2u, plot_pts.size());
+ EXPECT_EQ(l.pose1().rel_theta(), plot_pts[0].rel_theta());
+ EXPECT_EQ(l.pose2().rel_theta(), plot_pts[1].rel_theta());
+}
+
// Tests that basic checks for intersection function as expected.
TEST(LineSegmentTest, TrivialIntersectTest) {
Pose p1({0, 0, 0}, 0.0), p2({2, 0, 0}, 0.0);
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 2b5a37e..2228231 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -283,12 +283,15 @@
pylab.show()
-def PlotStep(params, R):
+def PlotStep(params, R, plant_params=None):
"""Plots a step move to the goal.
Args:
+ params: AngularSystemParams for the controller and observer
+ plant_params: AngularSystemParams for the plant. Defaults to params if
+ plant_params is None.
R: numpy.matrix(2, 1), the goal"""
- plant = AngularSystem(params, params.name)
+ plant = AngularSystem(plant_params or params, params.name)
controller = IntegralAngularSystem(params, params.name)
observer = IntegralAngularSystem(params, params.name)
@@ -307,12 +310,15 @@
kick_magnitude=0.0)
-def PlotKick(params, R):
+def PlotKick(params, R, plant_params=None):
"""Plots a step motion with a kick at 1.0 seconds.
Args:
+ params: AngularSystemParams for the controller and observer
+ plant_params: AngularSystemParams for the plant. Defaults to params if
+ plant_params is None.
R: numpy.matrix(2, 1), the goal"""
- plant = AngularSystem(params, params.name)
+ plant = AngularSystem(plant_params or params, params.name)
controller = IntegralAngularSystem(params, params.name)
observer = IntegralAngularSystem(params, params.name)
@@ -331,15 +337,22 @@
kick_magnitude=2.0)
-def PlotMotion(params, R, max_velocity=10.0, max_acceleration=70.0):
+def PlotMotion(params,
+ R,
+ max_velocity=10.0,
+ max_acceleration=70.0,
+ plant_params=None):
"""Plots a trapezoidal motion.
Args:
+ params: AngularSystemParams for the controller and observer
+ plant_params: AngularSystemParams for the plant. Defaults to params if
+ plant_params is None.
R: numpy.matrix(2, 1), the goal,
max_velocity: float, The max velocity of the profile.
max_acceleration: float, The max acceleration of the profile.
"""
- plant = AngularSystem(params, params.name)
+ plant = AngularSystem(plant_params or params, params.name)
controller = IntegralAngularSystem(params, params.name)
observer = IntegralAngularSystem(params, params.name)
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 175fffa..042bd5c 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -289,12 +289,15 @@
pylab.show()
-def PlotStep(params, R):
+def PlotStep(params, R, plant_params=None):
"""Plots a step move to the goal.
Args:
+ params: LinearSystemParams for the controller and observer
+ plant_params: LinearSystemParams for the plant. Defaults to params if
+ plant_params is None.
R: numpy.matrix(2, 1), the goal"""
- plant = LinearSystem(params, params.name)
+ plant = LinearSystem(plant_params or params, params.name)
controller = IntegralLinearSystem(params, params.name)
observer = IntegralLinearSystem(params, params.name)
@@ -313,12 +316,15 @@
kick_magnitude=0.0)
-def PlotKick(params, R):
+def PlotKick(params, R, plant_params=None):
"""Plots a step motion with a kick at 1.0 seconds.
Args:
+ params: LinearSystemParams for the controller and observer
+ plant_params: LinearSystemParams for the plant. Defaults to params if
+ plant_params is None.
R: numpy.matrix(2, 1), the goal"""
- plant = LinearSystem(params, params.name)
+ plant = LinearSystem(plant_params or params, params.name)
controller = IntegralLinearSystem(params, params.name)
observer = IntegralLinearSystem(params, params.name)
@@ -337,15 +343,22 @@
kick_magnitude=2.0)
-def PlotMotion(params, R, max_velocity=0.3, max_acceleration=10.0):
+def PlotMotion(params,
+ R,
+ max_velocity=0.3,
+ max_acceleration=10.0,
+ plant_params=None):
"""Plots a trapezoidal motion.
Args:
+ params: LinearSystemParams for the controller and observer
+ plant_params: LinearSystemParams for the plant. Defaults to params if
+ plant_params is None.
R: numpy.matrix(2, 1), the goal,
max_velocity: float, The max velocity of the profile.
max_acceleration: float, The max acceleration of the profile.
"""
- plant = LinearSystem(params, params.name)
+ plant = LinearSystem(plant_params or params, params.name)
controller = IntegralLinearSystem(params, params.name)
observer = IntegralLinearSystem(params, params.name)
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 9748a41..26b2cb2 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -1,6 +1,7 @@
#!/usr/bin/python3
from __future__ import print_function
import os
+import sys
import copy
import basic_window
from color import Color, palette
@@ -13,7 +14,7 @@
from gi.repository import Gdk, Gtk, GLib
import cairo
import enum
-import csv # For writing to csv files
+import json # For writing to json files
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
@@ -124,6 +125,9 @@
self.x = 0
self.y = 0
+ module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+ self.path_to_export = os.path.join(module_path,
+ 'points_for_pathedit.json')
# update list of control points
self.point_selected = False
# self.adding_spline = False
@@ -335,40 +339,6 @@
cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
- elif self.mode == Mode.kExporting:
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "VIEWING", 1, 1, 1, 1)
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- # for item in self.selected_points:
- # print(str(item))
- for i, point in enumerate(self.selected_points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
- elif self.mode == Mode.kImporting:
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "VIEWING", 1, 1, 1, 1)
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- for item in self.selected_points:
- print(str(item))
- for i, point in enumerate(self.selected_points):
- print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
elif self.mode == Mode.kConstraint:
print("Drawn")
set_color(cr, palette["BLACK"])
@@ -397,26 +367,22 @@
print("Found q key and exiting.")
quit_main_loop()
if keyval == Gdk.KEY_e:
- self.mode = Mode.kExporting
- # Will export to csv file
- with open('points_for_pathedit.csv', mode='w') as points_file:
- writer = csv.writer(
- points_file,
- delimiter=',',
- quotechar='"',
- quoting=csv.QUOTE_MINIMAL)
- for item in self.selected_points:
- writer.writerow([str(item[0]), str(item[1])])
- print("Wrote: " + str(item[0]) + " " + str(item[1]))
+ # Will export to json file
+ self.mode = Mode.kEditing
+ print(str(sys.argv))
+ print('out to: ', self.path_to_export)
+ exportList = [l.tolist() for l in self.splines]
+ with open(self.path_to_export, mode='w') as points_file:
+ json.dump(exportList, points_file)
+ print("Wrote: " + str(self.splines))
if keyval == Gdk.KEY_i:
- self.mode = Mode.kImporting
- # import from csv file
+ # import from json file
+ self.mode = Mode.kEditing
self.selected_points = []
- with open('points_for_pathedit.csv') as points_file:
- reader = csv.reader(points_file, delimiter=',')
- for row in reader:
- self.add_point(float(row[0]), float(row[1]))
- print("Added: " + row[0] + " " + row[1])
+ self.splines = []
+ with open(self.path_to_export) as points_file:
+ self.splines = json.load(points_file)
+ print("Added: " + str(self.splines))
if keyval == Gdk.KEY_p:
self.mode = Mode.kPlacing
# F0 = A1
diff --git a/motors/core/time.h b/motors/core/time.h
index 247de2c..d03cb10 100644
--- a/motors/core/time.h
+++ b/motors/core/time.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+// This whole file is deprecated. Use //aos/time instead.
+
#ifdef __cplusplus
extern "C"
{
diff --git a/motors/peripheral/BUILD b/motors/peripheral/BUILD
index ef2e833..2c71973 100644
--- a/motors/peripheral/BUILD
+++ b/motors/peripheral/BUILD
@@ -71,6 +71,7 @@
visibility = ["//visibility:public"],
deps = [
":uart_buffer",
+ "//aos/containers:sized_array",
"//motors:util",
"//motors/core",
"//third_party/GSL",
diff --git a/motors/peripheral/uart.cc b/motors/peripheral/uart.cc
index 3f7ddaf..9cd4014 100644
--- a/motors/peripheral/uart.cc
+++ b/motors/peripheral/uart.cc
@@ -39,18 +39,24 @@
// M_UART_RIE /* Enable RX interrupt or DMA */
// Also set in C5: M_UART_TDMAS /* Do DMA for TX */ |
// M_UART_RDMAS /* Do DMA for RX */
- c2_value_ = M_UART_TE;
+ c2_value_ = 0;
module_->C2 = c2_value_;
module_->PFIFO =
M_UART_TXFE /* Enable TX FIFO */ | M_UART_RXFE /* Enable RX FIFO */;
module_->CFIFO =
M_UART_TXFLUSH /* Flush TX FIFO */ | M_UART_RXFLUSH /* Flush RX FIFO */;
+ c2_value_ = M_UART_TE | M_UART_RE;
+ module_->C2 = c2_value_;
// TODO(Brian): Adjust for DMA?
module_->TWFIFO = tx_fifo_size_ - 1;
- module_->RWFIFO = rx_fifo_size_ - 1;
+ module_->RWFIFO = 1;
}
void Uart::DoWrite(gsl::span<const char> data) {
+ // In theory, we could be more efficient about this by writing the number of
+ // bytes we know there's space for and only calling SpaceAvailable() (or
+ // otherwise reading S1) before the final one. In practice, the FIFOs are so
+ // short on this part it probably won't help anything.
for (int i = 0; i < data.size(); ++i) {
while (!SpaceAvailable()) {
}
@@ -58,38 +64,91 @@
}
}
-Uart::~Uart() { DisableTransmitInterrupt(); }
+aos::SizedArray<char, 4> Uart::DoRead() {
+ // In theory, we could be more efficient about this by reading the number of
+ // bytes we know to be accessible and only calling DataAvailable() (or
+ // otherwise reading S1) before the final one. In practice, the FIFOs are so
+ // short on this part it probably won't help anything.
+ aos::SizedArray<char, 4> result;
+ while (DataAvailable() && !result.full()) {
+ result.push_back(ReadCharacter());
+ }
+ return result;
+}
+
+Uart::~Uart() {
+ DoDisableTransmitInterrupt();
+ DoDisableReceiveInterrupt();
+}
+
+InterruptBufferedUart::~InterruptBufferedUart() {
+ uart_.DisableReceiveInterrupt(DisableInterrupts());
+}
void InterruptBufferedUart::Initialize(int baud_rate) {
uart_.Initialize(baud_rate);
+ {
+ DisableInterrupts disable_interrupts;
+ uart_.EnableReceiveInterrupt(disable_interrupts);
+ }
}
void InterruptBufferedUart::Write(gsl::span<const char> data) {
DisableInterrupts disable_interrupts;
- uart_.EnableTransmitInterrupt();
- static_assert(buffer_.size() >= 8,
- "Need enough space to not turn the interrupt off each time");
+ uart_.EnableTransmitInterrupt(disable_interrupts);
while (!data.empty()) {
- const int bytes_written = buffer_.PushSpan(data);
+ const int bytes_written = transmit_buffer_.PushSpan(data);
data = data.subspan(bytes_written);
WriteCharacters(data.empty(), disable_interrupts);
ReenableInterrupts{&disable_interrupts};
}
}
-void InterruptBufferedUart::WriteCharacters(bool disable_empty,
- const DisableInterrupts &) {
+gsl::span<char> InterruptBufferedUart::Read(gsl::span<char> buffer) {
+ size_t bytes_read = 0;
+ {
+ DisableInterrupts disable_interrupts;
+ const gsl::span<const char> read_data =
+ receive_buffer_.PopSpan(buffer.size());
+ std::copy(read_data.begin(), read_data.end(), buffer.begin());
+ bytes_read += read_data.size();
+ }
+ {
+ DisableInterrupts disable_interrupts;
+ const gsl::span<const char> read_data =
+ receive_buffer_.PopSpan(buffer.size() - bytes_read);
+ std::copy(read_data.begin(), read_data.end(),
+ buffer.subspan(bytes_read).begin());
+ bytes_read += read_data.size();
+ }
+ return buffer.subspan(0, bytes_read);
+}
+
+void InterruptBufferedUart::WriteCharacters(
+ bool disable_empty, const DisableInterrupts &disable_interrupts) {
while (true) {
- if (buffer_.empty()) {
+ if (transmit_buffer_.empty()) {
if (disable_empty) {
- uart_.DisableTransmitInterrupt();
+ uart_.DisableTransmitInterrupt(disable_interrupts);
}
return;
}
if (!uart_.SpaceAvailable()) {
return;
}
- uart_.WriteCharacter(buffer_.PopSingle());
+ uart_.WriteCharacter(transmit_buffer_.PopSingle());
+ }
+}
+
+void InterruptBufferedUart::ReadCharacters(const DisableInterrupts &) {
+ while (true) {
+ if (receive_buffer_.full()) {
+ return;
+ }
+ if (!uart_.DataAvailable()) {
+ return;
+ }
+ receive_buffer_.PushSingle(uart_.ReadCharacter());
}
}
diff --git a/motors/peripheral/uart.h b/motors/peripheral/uart.h
index ca40ab7..ffb1529 100644
--- a/motors/peripheral/uart.h
+++ b/motors/peripheral/uart.h
@@ -1,6 +1,7 @@
#ifndef MOTORS_PERIPHERAL_UART_H_
#define MOTORS_PERIPHERAL_UART_H_
+#include "aos/containers/sized_array.h"
#include "motors/core/kinetis.h"
#include "motors/peripheral/uart_buffer.h"
#include "motors/util.h"
@@ -25,22 +26,59 @@
DoWrite(data);
}
+ // Returns all the data which is currently available.
+ aos::SizedArray<char, 4> Read(const DisableInterrupts &) {
+ return DoRead();
+ }
+
bool SpaceAvailable() const { return module_->S1 & M_UART_TDRE; }
// Only call this if SpaceAvailable() has just returned true.
void WriteCharacter(char c) { module_->D = c; }
- void EnableTransmitInterrupt() {
+ bool DataAvailable() const { return module_->S1 & M_UART_RDRF; }
+ // Only call this if DataAvailable() has just returned true.
+ char ReadCharacter() { return module_->D; }
+
+ // TODO(Brian): These APIs for enabling/disabling interrupts aren't quite
+ // right. Redo them some time. Some issues:
+ // * They get called during initialization/destruction time, which means
+ // interrupts don't really need to be disabled because everything is
+ // singlethreaded.
+ // * Often, several C2 modifications are made in a single
+ // interrupts-disabled section. These could be batched to reduce
+ // peripheral writes. Sometimes, no modifications are made at all, in
+ // which case there doesn't even need to be a single write.
+
+ void EnableTransmitInterrupt(const DisableInterrupts &) {
c2_value_ |= M_UART_TIE;
module_->C2 = c2_value_;
}
- void DisableTransmitInterrupt() {
- c2_value_ &= ~M_UART_TIE;
+ void DisableTransmitInterrupt(const DisableInterrupts &) {
+ DoDisableTransmitInterrupt();
+ }
+
+ void EnableReceiveInterrupt(const DisableInterrupts &) {
+ c2_value_ |= M_UART_RIE;
module_->C2 = c2_value_;
}
+ void DisableReceiveInterrupt(const DisableInterrupts &) {
+ DoDisableReceiveInterrupt();
+ }
+
private:
+ void DoDisableTransmitInterrupt() {
+ c2_value_ &= ~M_UART_TIE;
+ module_->C2 = c2_value_;
+ }
+ void DoDisableReceiveInterrupt() {
+ c2_value_ &= ~M_UART_RIE;
+ module_->C2 = c2_value_;
+ }
+
void DoWrite(gsl::span<const char> data);
+ aos::SizedArray<char, 4> DoRead();
KINETISK_UART_t *const module_;
const int module_clock_frequency_;
@@ -51,27 +89,37 @@
};
// Interrupt-based buffered interface to a UART.
+// TODO(Brian): Move DisableInterrupts calls up to the caller of this.
class InterruptBufferedUart {
public:
InterruptBufferedUart(KINETISK_UART_t *module, int module_clock_frequency)
: uart_(module, module_clock_frequency) {}
+ ~InterruptBufferedUart();
void Initialize(int baud_rate);
+ // Queues up the given data for immediate writing. Blocks only if the queue
+ // fills up before all of data is enqueued.
void Write(gsl::span<const char> data);
+ // Reads currently available data.
+ // Returns all the data which is currently available (possibly none);
+ // buffer is where to store the result. The return value will be a subspan of
+ // this.
+ gsl::span<char> Read(gsl::span<char> buffer);
+
// Should be called as the body of the interrupt handler.
void HandleInterrupt(const DisableInterrupts &disable_interrupts) {
WriteCharacters(true, disable_interrupts);
+ ReadCharacters(disable_interrupts);
}
private:
void WriteCharacters(bool disable_empty, const DisableInterrupts &);
+ void ReadCharacters(const DisableInterrupts &);
Uart uart_;
- UartBuffer<1024> buffer_;
-
- bool interrupt_enabled_ = false;
+ UartBuffer<1024> transmit_buffer_, receive_buffer_;
};
} // namespace teensy
diff --git a/motors/peripheral/uart_buffer.h b/motors/peripheral/uart_buffer.h
index 63dd70d..879ab5c 100644
--- a/motors/peripheral/uart_buffer.h
+++ b/motors/peripheral/uart_buffer.h
@@ -15,15 +15,25 @@
// Returns the number of characters added.
__attribute__((warn_unused_result)) int PushSpan(gsl::span<const char> data);
+ // max is the maximum size the returned spans should be.
+ // The data in the result is only valid until another method is called.
+ // Note that this may not return all available data when doing so would
+ // require wrapping around, but it will always return a non-empty span if any
+ // data is available.
+ gsl::span<const char> PopSpan(int max);
+
bool empty() const { return size_ == 0; }
+ bool full() const { return size_ == kSize; }
// This may only be called when !empty().
char PopSingle();
+ // This may only be called when !full().
+ void PushSingle(char c);
static constexpr int size() { return kSize; }
private:
- // The index at which we will push the next character.
+ // The index at which we will pop the next character.
int start_ = 0;
// How many characters we currently have.
int size_ = 0;
@@ -31,7 +41,7 @@
::std::array<char, kSize> data_;
};
-template<int kSize>
+template <int kSize>
int UartBuffer<kSize>::PushSpan(gsl::span<const char> data) {
const int end_location = (start_ + size_) % kSize;
const int remaining_end = ::std::min(kSize - size_, kSize - end_location);
@@ -52,7 +62,16 @@
return on_end + on_start;
}
-template<int kSize>
+template <int kSize>
+gsl::span<const char> UartBuffer<kSize>::PopSpan(int max) {
+ const size_t result_size = std::min(max, std::min(kSize - start_, size_));
+ const auto result = gsl::span<const char>(data_).subspan(start_, result_size);
+ start_ = (start_ + result_size) % kSize;
+ size_ -= result_size;
+ return result;
+}
+
+template <int kSize>
char UartBuffer<kSize>::PopSingle() {
const char r = data_[start_];
--size_;
@@ -60,6 +79,13 @@
return r;
}
+template <int kSize>
+void UartBuffer<kSize>::PushSingle(char c) {
+ const int end_location = (start_ + size_) % kSize;
+ data_[end_location] = c;
+ ++size_;
+}
+
} // namespace teensy
} // namespace frc971
diff --git a/motors/peripheral/uart_buffer_test.cc b/motors/peripheral/uart_buffer_test.cc
index 5ab7c98..c464f8a 100644
--- a/motors/peripheral/uart_buffer_test.cc
+++ b/motors/peripheral/uart_buffer_test.cc
@@ -181,6 +181,106 @@
ASSERT_TRUE(buffer.empty());
}
+// Tests that using PopSpan with single characters works correctly.
+TEST(UartBufferTest, PopSpanSingle) {
+ UartBuffer<3> buffer;
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(1);
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(2);
+ ASSERT_FALSE(buffer.full());
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(1u, result[0]);
+ }
+
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(3);
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(4);
+ ASSERT_TRUE(buffer.full());
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(2u, result[0]);
+ }
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(3u, result[0]);
+ }
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(4u, result[0]);
+ }
+}
+
+// Tests that using PopSpan with multiple characters works correctly.
+TEST(UartBufferTest, PopSpanMultiple) {
+ UartBuffer<1024> buffer;
+ for (int i = 0; i < 10; ++i) {
+ buffer.PushSingle(i);
+ }
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+ {
+ const auto result = buffer.PopSpan(5);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i), result[i]);
+ }
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 5), result[i]);
+ }
+ }
+ ASSERT_TRUE(buffer.PopSpan(5).empty());
+ ASSERT_TRUE(buffer.PopSpan(3000).empty());
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+}
+
+// Tests that using PopSpan with multiple characters works correctly when
+// wrapping.
+TEST(UartBufferTest, PopSpanWrapMultiple) {
+ UartBuffer<10> buffer;
+ for (int i = 0; i < 10; ++i) {
+ buffer.PushSingle(i);
+ }
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+ {
+ const auto result = buffer.PopSpan(5);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i), result[i]);
+ }
+ }
+ for (int i = 0; i < 5; ++i) {
+ buffer.PushSingle(20 + i);
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 5), result[i]);
+ }
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 20), result[i]);
+ }
+ }
+}
+
} // namespace testing
} // namespace teensy
} // namespace frc971
diff --git a/third_party/Phoenix-frc-lib/BUILD b/third_party/Phoenix-frc-lib/BUILD
index d735d52..f7c27c2 100644
--- a/third_party/Phoenix-frc-lib/BUILD
+++ b/third_party/Phoenix-frc-lib/BUILD
@@ -21,7 +21,8 @@
"**/Tasking/**",
"cpp/src/RCRadio3Ch.cpp",
"cpp/src/CompileTest.cpp",
- "**/MotorControl/**",
+ "cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp",
+ "cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp",
"**/Sensors/**",
],
),
@@ -32,7 +33,6 @@
],
exclude = [
"**/Tasking/**",
- "**/MotorControl/**",
"**/Sensors/**",
"cpp/include/ctre/phoenix/RCRadio3Ch.h",
],
diff --git a/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h b/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
index 6378f6f..ca95aa9 100644
--- a/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
+++ b/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
@@ -13,7 +13,7 @@
#include "ctre/phoenix/Motion/TrajectoryPoint.h"
#include "ctre/phoenix/Motion/MotionProfileStatus.h"
/* WPILIB */
-#include "SpeedController.h"
+//#include "SpeedController.h"
/* forward proto's */
namespace ctre {
diff --git a/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/IMotorController.h b/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
index 947022e..481ec4d 100644
--- a/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
+++ b/third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
@@ -17,7 +17,7 @@
#include "ctre/phoenix/ErrorCode.h"
#include "IFollower.h"
/* WPILIB */
-#include "SpeedController.h"
+//#include "SpeedController.h"
namespace ctre {
namespace phoenix {
diff --git a/third_party/google-glog/BUILD b/third_party/google-glog/BUILD
index 37fb27b..0c41228 100644
--- a/third_party/google-glog/BUILD
+++ b/third_party/google-glog/BUILD
@@ -1,5 +1,5 @@
-licenses(['notice'])
+licenses(["notice"])
-load(':bazel/glog.bzl', 'glog_library')
+load(":bazel/glog.bzl", "glog_library")
glog_library()
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 91fbb2d..93e5d09 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -9,4 +9,4 @@
bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
-bazel build -c opt --curses=no --color=no //motors/... --cpu=cortex-m4f
+bazel build -c opt --curses=no --color=no //motors/... //y2019/jevois/... --cpu=cortex-m4f
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 2d5e556..2de1741 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -728,14 +728,14 @@
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));
- ::frc971::wpilib::BufferedPcm *pcm = new ::frc971::wpilib::BufferedPcm();
- SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_left_drivetrain_shifter(pcm->MakeSolenoid(0));
- solenoid_writer.set_right_drivetrain_shifter(pcm->MakeSolenoid(1));
- solenoid_writer.set_claw(pcm->MakeSolenoid(2));
- solenoid_writer.set_arm_brakes(pcm->MakeSolenoid(3));
- solenoid_writer.set_hook(pcm->MakeSolenoid(4));
- solenoid_writer.set_forks(pcm->MakeSolenoid(5));
+ ::frc971::wpilib::BufferedPcm pcm;
+ SolenoidWriter solenoid_writer(&pcm);
+ solenoid_writer.set_left_drivetrain_shifter(pcm.MakeSolenoid(0));
+ solenoid_writer.set_right_drivetrain_shifter(pcm.MakeSolenoid(1));
+ solenoid_writer.set_claw(pcm.MakeSolenoid(2));
+ solenoid_writer.set_arm_brakes(pcm.MakeSolenoid(3));
+ solenoid_writer.set_hook(pcm.MakeSolenoid(4));
+ solenoid_writer.set_forks(pcm.MakeSolenoid(5));
::std::thread solenoid_thread(::std::ref(solenoid_writer));
diff --git a/y2019/BUILD b/y2019/BUILD
index 576cf6c..ab4a842 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -5,6 +5,7 @@
":joystick_reader",
":wpilib_interface",
"//y2019/control_loops/drivetrain:drivetrain",
+ "//y2019/control_loops/superstructure:superstructure",
],
)
@@ -23,6 +24,7 @@
"//aos/mutex",
"//aos/network:team_number",
"//frc971:constants",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
"//y2019/control_loops/superstructure/elevator:elevator_plants",
"//y2019/control_loops/superstructure/intake:intake_plants",
@@ -55,6 +57,7 @@
"//frc971/control_loops:queues",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//frc971/wpilib:ADIS16448",
+ "//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
diff --git a/y2019/constants.cc b/y2019/constants.cc
index e91a49c..b53fb99 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -12,14 +12,16 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "aos/once.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
+#include "y2019/control_loops/superstructure/elevator/integral_elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/integral_intake_plant.h"
+#include "y2019/control_loops/superstructure/stilts/integral_stilts_plant.h"
+#include "y2019/control_loops/superstructure/wrist/integral_wrist_plant.h"
namespace y2019 {
namespace constants {
+using ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator;
+
const int Values::kZeroingSampleSize;
namespace {
@@ -31,94 +33,144 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
Values::PotAndAbsConstants *const elevator = &r->elevator;
- Values::Intake *const intake = &r->intake;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const elevator_params = &(elevator->subsystem_params);
Values::PotAndAbsConstants *const stilts = &r->stilts;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const stilts_params = &(stilts->subsystem_params);
Values::PotAndAbsConstants *const wrist = &r->wrist;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const wrist_params = &(wrist->subsystem_params);
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
+ &r->intake;
- elevator->zeroing.average_filter_size = Values::kZeroingSampleSize;
- elevator->zeroing.one_revolution_distance =
+ // Elevator constants.
+ elevator_params->zeroing_voltage = 3.0;
+ elevator_params->operating_voltage = 12.0;
+ elevator_params->zeroing_profile_params = {0.1, 1.0};
+ elevator_params->default_profile_params = {4.0, 16.0};
+ elevator_params->range = Values::kElevatorRange();
+ elevator_params->make_integral_loop =
+ &control_loops::superstructure::elevator::MakeIntegralElevatorLoop;
+ elevator_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ elevator_params->zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kElevatorEncoderRatio();
- elevator->zeroing.zeroing_threshold = 0.0005;
- elevator->zeroing.moving_buffer_size = 20;
- elevator->zeroing.allowable_encoder_error = 0.9;
+ elevator_params->zeroing_constants.zeroing_threshold = 0.005;
+ elevator_params->zeroing_constants.moving_buffer_size = 20;
+ elevator_params->zeroing_constants.allowable_encoder_error = 0.9;
- intake->zeroing.average_filter_size = Values::kZeroingSampleSize;
- intake->zeroing.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
- intake->zeroing.zeroing_threshold = 0.0005;
- intake->zeroing.moving_buffer_size = 20;
- intake->zeroing.allowable_encoder_error = 0.9;
-
- stilts->zeroing.average_filter_size = Values::kZeroingSampleSize;
- stilts->zeroing.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kStiltsEncoderRatio();
- stilts->zeroing.zeroing_threshold = 0.0005;
- stilts->zeroing.moving_buffer_size = 20;
- stilts->zeroing.allowable_encoder_error = 0.9;
-
- wrist->zeroing.average_filter_size = Values::kZeroingSampleSize;
- wrist->zeroing.one_revolution_distance =
+ // Wrist constants.
+ wrist_params->zeroing_voltage = 4.0;
+ wrist_params->operating_voltage = 12.0;
+ wrist_params->zeroing_profile_params = {0.5, 2.0};
+ wrist_params->default_profile_params = {10.0, 40.0};
+ wrist_params->range = Values::kWristRange();
+ wrist_params->make_integral_loop =
+ &control_loops::superstructure::wrist::MakeIntegralWristLoop;
+ wrist_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ wrist_params->zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kWristEncoderRatio();
- wrist->zeroing.zeroing_threshold = 0.0005;
- wrist->zeroing.moving_buffer_size = 20;
- wrist->zeroing.allowable_encoder_error = 0.9;
+ wrist_params->zeroing_constants.zeroing_threshold = 0.0005;
+ wrist_params->zeroing_constants.moving_buffer_size = 20;
+ wrist_params->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Intake constants.
+ intake->zeroing_voltage = 3.0;
+ intake->operating_voltage = 12.0;
+ intake->zeroing_profile_params = {0.5, 3.0};
+ intake->default_profile_params = {6.0, 30.0};
+ intake->range = Values::kIntakeRange();
+ intake->make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ intake->zeroing_constants.moving_buffer_size = 20;
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Stilts constants.
+ stilts_params->zeroing_voltage = 3.0;
+ stilts_params->operating_voltage = 12.0;
+ stilts_params->zeroing_profile_params = {0.1, 0.5};
+ stilts_params->default_profile_params = {0.5, 0.5};
+ stilts_params->range = Values::kStiltsRange();
+ stilts_params->make_integral_loop =
+ &control_loops::superstructure::stilts::MakeIntegralStiltsLoop;
+ stilts_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ stilts_params->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kStiltsEncoderRatio();
+ stilts_params->zeroing_constants.zeroing_threshold = 0.0005;
+ stilts_params->zeroing_constants.moving_buffer_size = 20;
+ stilts_params->zeroing_constants.allowable_encoder_error = 0.9;
switch (team) {
// A set of constants for tests.
case 1:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
- elevator->zeroing.measured_absolute_position = 0.0;
- elevator->potentiometer_offset = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.049419;
+ elevator->potentiometer_offset = -0.022320;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 1.756847;
+ intake->zeroing_constants.middle_position =
+ Values::kIntakeRange().middle();
- stilts->zeroing.measured_absolute_position = 0.0;
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
stilts->potentiometer_offset = 0.0;
- wrist->zeroing.measured_absolute_position = 0.0;
- wrist->potentiometer_offset = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.357394;
+ wrist->potentiometer_offset = -1.479097 - 2.740303;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.047838;
+ stilts->potentiometer_offset = -0.093820;
break;
case kCodingRobotTeamNumber:
- elevator->zeroing.measured_absolute_position = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.0;
elevator->potentiometer_offset = 0.0;
- intake->zeroing.measured_absolute_position = 0.0;
- intake->zeroing.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.middle_position = 0.0;
- stilts->zeroing.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
-
- wrist->zeroing.measured_absolute_position = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
+
+ stilts_params->zeroing_constants.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
break;
default:
diff --git a/y2019/constants.h b/y2019/constants.h
index 21c061c..46d28f1 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -5,9 +5,10 @@
#include <stdint.h>
#include "frc971/constants.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
@@ -27,6 +28,7 @@
struct Values {
static const int kZeroingSampleSize = 200;
+ // Drivetrain Constants
static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
@@ -61,10 +63,10 @@
static constexpr ::frc971::constants::Range kElevatorRange() {
return ::frc971::constants::Range{
- 0.0, // Bottom Hard
- 1.44, // Top Hard
- 0.025, // Bottom Soft
- 1.415 // Top Soft
+ -0.01, // Bottom Hard
+ 1.62, // Top Hard
+ 0.01, // Bottom Soft
+ 1.59 // Top Soft
};
}
@@ -82,10 +84,10 @@
static constexpr ::frc971::constants::Range kIntakeRange() {
return ::frc971::constants::Range{
- -1.15, // Back Hard
- 1.36, // Front Hard
- -1.14, // Back Soft
- 1.22 // Front Soft
+ -1.30, // Back Hard
+ 1.35, // Front Hard
+ -1.25, // Back Soft
+ 1.30 // Front Soft
};
}
@@ -108,7 +110,7 @@
static constexpr ::frc971::constants::Range kWristRange() {
return ::frc971::constants::Range{
-3.14, // Back Hard
- 2.58, // Front Hard
+ 3.14, // Front Hard
-2.97, // Back Soft
2.41 // Front Soft
};
@@ -117,6 +119,7 @@
// Stilts
static constexpr double kStiltsEncoderCountsPerRevolution() { return 4096.0; }
+ // Stilts Constants
static constexpr double kStiltsEncoderRatio() {
return (1.0 /* Gear ratio */) *
control_loops::superstructure::stilts::kRadius;
@@ -136,25 +139,27 @@
static constexpr ::frc971::constants::Range kStiltsRange() {
return ::frc971::constants::Range{
- -0.026, // Top Hard
- 0.693, // Bottom Hard
- -0.02, // Top Soft
- 0.673 // Bottom Soft
+ -0.01, // Top Hard
+ 0.72, // Bottom Hard
+ 0.00, // Top Soft
+ 0.71 // Bottom Soft
};
}
struct PotAndAbsConstants {
- ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
double potentiometer_offset;
};
+
PotAndAbsConstants elevator;
PotAndAbsConstants wrist;
- PotAndAbsConstants stilts;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ intake;
- struct Intake {
- ::frc971::constants::AbsoluteEncoderZeroingConstants zeroing;
- };
- Intake intake;
+ PotAndAbsConstants stilts;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 7135e8b..ff53b7b 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -35,6 +35,7 @@
public:
typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
TypedTarget(const Pose &pose) : pose_(pose) {}
+ TypedTarget() {}
Pose pose() const { return pose_; }
bool occluded() const { return occluded_; }
@@ -117,6 +118,8 @@
// The target that this view corresponds to.
const TypedTarget<Scalar> *target;
+ // The Pose the camera was at when viewing the target:
+ Pose camera_pose;
};
// Important parameters for dealing with camera noise calculations.
@@ -172,11 +175,12 @@
// separately for simulation.
::aos::SizedArray<TargetView, num_targets> target_views() const {
::aos::SizedArray<TargetView, num_targets> views;
+ Pose camera_abs_pose = pose_.Rebase(nullptr);
// Because there are num_targets in targets_ and because AddTargetIfVisible
// adds at most 1 view to views, we should never exceed the size of
// SizedArray.
for (const auto &target : targets_) {
- AddTargetIfVisible(target, &views);
+ AddTargetIfVisible(target, camera_abs_pose, &views);
}
return views;
}
@@ -189,12 +193,12 @@
::std::vector<::std::vector<Pose>> PlotPoints() const {
::std::vector<::std::vector<Pose>> list_of_lists;
for (const auto &view : target_views()) {
- list_of_lists.push_back({pose_, view.target.pose()});
+ list_of_lists.push_back({pose_, view.target->pose()});
}
return list_of_lists;
}
- const Pose pose() const { return pose_; }
+ const Pose &pose() const { return pose_; }
Scalar fov() const { return fov_; }
private:
@@ -202,11 +206,12 @@
// If the specified target is visible from the current camera Pose, adds it to
// the views array.
void AddTargetIfVisible(
- const TypedTarget<Scalar> &target,
+ const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
::aos::SizedArray<TargetView, num_targets> *views) const;
// The Pose of this camera.
const Pose pose_;
+
// Field of view of the camera, in radians.
const Scalar fov_;
@@ -215,6 +220,8 @@
const NoiseParameters noise_parameters_;
// A list of all the targets on the field.
+ // TODO(james): Is it worth creating some sort of cache for the targets and
+ // obstacles? e.g., passing around pointer to the targets/obstacles.
const ::std::array<TypedTarget<Scalar>, num_targets> targets_;
// Known obstacles on the field which can interfere with our view of the
// targets. An "obstacle" is a line segment which we cannot see through, as
@@ -225,7 +232,7 @@
template <int num_targets, int num_obstacles, typename Scalar>
void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
- const TypedTarget<Scalar> &target,
+ const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
::aos::SizedArray<TargetView, num_targets> *views) const {
if (target.occluded()) {
return;
@@ -233,8 +240,6 @@
// Precompute the current absolute pose of the camera, because we will reuse
// it a bunch.
- const Pose camera_abs_pose = pose_.Rebase(nullptr);
-
const Pose relative_pose = target.pose().Rebase(&camera_abs_pose);
const Scalar heading = relative_pose.heading();
const Scalar distance = relative_pose.xy_norm();
@@ -280,6 +285,7 @@
view.noise = {noise_parameters_.heading_noise, distance_noise, height_noise,
skew_noise};
view.target = ⌖
+ view.camera_pose = camera_abs_pose;
views->push_back(view);
}
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
index 9a73ccf..ce1a2a4 100644
--- a/y2019/control_loops/drivetrain/camera_test.cc
+++ b/y2019/control_loops/drivetrain/camera_test.cc
@@ -93,6 +93,14 @@
EXPECT_GT(views[0].noise.distance, 0.0);
EXPECT_GT(views[0].noise.skew, 0.0);
EXPECT_GT(views[0].noise.height, 0.0);
+
+ // Check that the PlotPoints for debugging are as expected (should be a single
+ // line from the camera to the one visible target).
+ const auto plot_pts = camera_.PlotPoints();
+ ASSERT_EQ(1u, plot_pts.size());
+ ASSERT_EQ(2u, plot_pts[0].size());
+ EXPECT_EQ(camera_.pose().abs_pos(), plot_pts[0][0].abs_pos());
+ EXPECT_EQ(views[0].target->pose().abs_pos(), plot_pts[0][1].abs_pos());
}
// Check that occluding the middle target makes it invisible.
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.cc b/y2019/control_loops/drivetrain/drivetrain_base.cc
index 0c34882..02bd805 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_base.cc
@@ -24,12 +24,11 @@
const DrivetrainConfig<double> &GetDrivetrainConfig() {
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
- ::frc971::control_loops::drivetrain::IMUType::IMU_Y,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
- drivetrain::MakeDrivetrainLoop,
- drivetrain::MakeVelocityDrivetrainLoop,
+ drivetrain::MakeDrivetrainLoop, drivetrain::MakeVelocityDrivetrainLoop,
drivetrain::MakeKFDrivetrainLoop,
drivetrain::MakeHybridVelocityDrivetrainLoop,
@@ -37,14 +36,12 @@
chrono::duration<double>(drivetrain::kDt)),
drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
- drivetrain::kHighGearRatio, drivetrain::kLowGearRatio,
- drivetrain::kJ,
- drivetrain::kMass,
- kThreeStateDriveShifter, kThreeStateDriveShifter,
+ drivetrain::kHighGearRatio, drivetrain::kLowGearRatio, drivetrain::kJ,
+ drivetrain::kMass, kThreeStateDriveShifter, kThreeStateDriveShifter,
true /* default_high_gear */, 0 /* down_offset if using constants use
constants::GetValues().down_error */,
- 0.8 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
- 1.5 /* wheel_multiplier */,
+ 0.7 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
+ 1.2 /* wheel_multiplier */,
};
return kDrivetrainConfig;
diff --git a/y2019/control_loops/python/drivetrain.py b/y2019/control_loops/python/drivetrain.py
index b0a213b..e2d6eec 100644
--- a/y2019/control_loops/python/drivetrain.py
+++ b/y2019/control_loops/python/drivetrain.py
@@ -18,10 +18,10 @@
# Encoder is on a 24 tooth gear attached to the output gear.
kDrivetrain = drivetrain.DrivetrainParams(
- J=4.0,
- mass=40.0,
+ J=1.5,
+ mass=38.5,
# TODO(austin): Measure.
- robot_radius=0.616 / 2.0,
+ robot_radius=0.45 / 2.0,
wheel_radius=4.0 * 0.0254 / 2.0,
G=9.0 / 52.0,
q_pos=0.14,
diff --git a/y2019/control_loops/python/elevator.py b/y2019/control_loops/python/elevator.py
index 41c05c6..480ed50 100755
--- a/y2019/control_loops/python/elevator.py
+++ b/y2019/control_loops/python/elevator.py
@@ -2,6 +2,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import linear_system
+import copy
import numpy
import sys
import gflags
@@ -24,19 +25,22 @@
radius=2.25 * 0.0254 / 2.0,
mass=first_stage_mass + carriage_mass,
q_pos=0.070,
- q_vel=1.2,
+ q_vel=1.35,
kalman_q_pos=0.12,
kalman_q_vel=2.00,
kalman_q_voltage=35.0,
kalman_r_position=0.05,
dt=0.00505)
+kElevatorModel = copy.copy(kElevator)
+kElevatorModel.mass = carriage_mass + first_stage_mass + 1.0
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[1.5], [0.0]])
- linear_system.PlotKick(kElevator, R)
- linear_system.PlotMotion(kElevator, R, max_velocity=5.0)
+ linear_system.PlotKick(kElevator, R, plant_params=kElevatorModel)
+ linear_system.PlotMotion(
+ kElevator, R, max_velocity=5.0, plant_params=kElevatorModel)
# Write the generated constants out to a file.
if len(argv) != 5:
diff --git a/y2019/control_loops/python/intake.py b/y2019/control_loops/python/intake.py
index 2ac139b..9f4f488 100755
--- a/y2019/control_loops/python/intake.py
+++ b/y2019/control_loops/python/intake.py
@@ -41,8 +41,8 @@
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
- angular_system.PlotMotion(kIntake, R)
angular_system.PlotKick(kIntake, R)
+ angular_system.PlotMotion(kIntake, R)
# Write the generated constants out to a file.
if len(argv) != 5:
diff --git a/y2019/control_loops/python/wrist.py b/y2019/control_loops/python/wrist.py
index 2e292a2..66b7b26 100755
--- a/y2019/control_loops/python/wrist.py
+++ b/y2019/control_loops/python/wrist.py
@@ -4,6 +4,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import angular_system
from frc971.control_loops.python import controls
+import copy
import numpy
import sys
from matplotlib import pylab
@@ -36,12 +37,15 @@
kalman_q_voltage=4.0,
kalman_r_position=0.05)
+kWristModel = copy.copy(kWrist)
+kWristModel.J = 0.1348
+
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
- angular_system.PlotMotion(kWrist, R)
- angular_system.PlotKick(kWrist, R)
+ angular_system.PlotKick(kWrist, R, plant_params=kWristModel)
+ angular_system.PlotMotion(kWrist, R, plant_params=kWristModel)
# Write the generated constants out to a file.
if len(argv) != 5:
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 453c680..f55e1ad 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -23,8 +23,30 @@
"superstructure.h",
],
deps = [
+ ":collision_avoidance",
":superstructure_queue",
"//aos/controls:control_loop",
+ "//y2019:constants",
+ ],
+)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos:queues",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2019/control_loops/superstructure/intake:intake_plants",
],
)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index 7a26b90..ed6787f 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -128,8 +128,8 @@
}
if (unsafe_goal) {
- const double wrist_goal = unsafe_goal->wrist.angle;
- const double intake_goal = unsafe_goal->intake.joint_angle;
+ const double wrist_goal = unsafe_goal->wrist.unsafe_goal;
+ const double intake_goal = unsafe_goal->intake.unsafe_goal;
// Compute if we need to move the intake.
const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 1ad9582..5a8a772 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -53,10 +53,10 @@
// TODO(sabina): set all the numbers to correctly match the robot.
// Height above which we can move the wrist freely.
- static constexpr double kElevatorClearHeight = 0.5;
+ static constexpr double kElevatorClearHeight = 0.35;
// Height above which we can move the wrist down.
- static constexpr double kElevatorClearWristDownHeight = 0.3;
+ static constexpr double kElevatorClearWristDownHeight = 0.25;
// Height the carriage needs to be above to move the intake.
static constexpr double kElevatorClearIntakeHeight = 0.4;
@@ -65,8 +65,8 @@
static constexpr double kWristMinAngle = -M_PI / 2.0;
// Angles outside of which the intake is fully clear of the wrist.
- static constexpr double kIntakeOutAngle = M_PI / 6.0;
- static constexpr double kIntakeInAngle = -M_PI / 3.0;
+ static constexpr double kIntakeOutAngle = 0.3;
+ static constexpr double kIntakeInAngle = -1.1;
// Angles within which we will crash the wrist into the elevator if the
// elevator is below kElevatorClearHeight.
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 92a1a70..17f9fdc 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -36,20 +36,20 @@
was_collided = avoidance.IsCollided(&status);
}
- safe_goal.wrist.angle =
- ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+ safe_goal.wrist.unsafe_goal =
+ ::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
avoidance.max_wrist_goal());
- safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+ safe_goal.elevator.unsafe_goal = ::std::max(unsafe_goal.elevator.unsafe_goal,
avoidance.min_elevator_goal());
- safe_goal.intake.joint_angle =
- ::aos::Clip(unsafe_goal.intake.joint_angle,
+ safe_goal.intake.unsafe_goal =
+ ::aos::Clip(unsafe_goal.intake.unsafe_goal,
avoidance.min_intake_goal(), avoidance.max_intake_goal());
- LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
- LimitedMove(&status.elevator.position, safe_goal.elevator.height);
- LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+ LimitedMove(&status.wrist.position, safe_goal.wrist.unsafe_goal);
+ LimitedMove(&status.elevator.position, safe_goal.elevator.unsafe_goal);
+ LimitedMove(&status.intake.position, safe_goal.intake.unsafe_goal);
if (IsMoving()) {
break;
}
@@ -78,9 +78,9 @@
void CheckGoals() {
// check to see if we reached the goals
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
- ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
private:
@@ -101,9 +101,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeInAngle - avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the bottom
@@ -123,9 +123,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -152,9 +152,9 @@
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
Iterate();
@@ -174,9 +174,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeInAngle - avoidance.kEpsIntake;
Iterate();
@@ -190,10 +190,10 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle =
+ unsafe_goal.wrist.unsafe_goal =
avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -213,10 +213,10 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle =
+ unsafe_goal.wrist.unsafe_goal =
avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -236,9 +236,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle = 4.0;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = 4.0;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes to be have the elevator at the half way
@@ -249,10 +249,10 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Unreasonable Wrist Goal
@@ -260,9 +260,9 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
// sets the status position messgaes to be have the elevator at the half way
@@ -274,18 +274,18 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist in Elevator
TEST_F(CollisionAvoidanceTests, FixElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.angle = 3.14;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = 3.14;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes
@@ -295,18 +295,18 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist in Intake
TEST_F(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
// sets the status position messgaes
@@ -317,17 +317,17 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist Above Elevator
TEST_F(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.angle = 0.0;
- unsafe_goal.elevator.height = 0.0;
- unsafe_goal.intake.joint_angle =
+ unsafe_goal.wrist.unsafe_goal = 0.0;
+ unsafe_goal.elevator.unsafe_goal = 0.0;
+ unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
// sets the status position messgaes
@@ -337,10 +337,10 @@
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
} // namespace testing
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index e87ef6e..5edfce3 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -2,28 +2,89 @@
#include "aos/controls/control_loops.q.h"
#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
+void suction_cups(
+ const SuperstructureQueue::Goal *unsafe_goal,
+ SuperstructureQueue::Output *output) {
+ const double on_voltage = 12.0;
+
+ if(unsafe_goal && output) {
+ if(unsafe_goal->suction.top || unsafe_goal->suction.bottom) {
+ output->pump_voltage = on_voltage;
+ }
+ }
+}
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name) {}
+ : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+ elevator_(constants::GetValues().elevator.subsystem_params),
+ wrist_(constants::GetValues().wrist.subsystem_params),
+ intake_(constants::GetValues().intake),
+ stilts_(constants::GetValues().stilts.subsystem_params) {}
-void Superstructure::RunIteration(
- const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) {
- (void)unsafe_goal;
- (void)position;
- (void)output;
- (void)status;
-
+void Superstructure::RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) {
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
+ elevator_.Reset();
+ wrist_.Reset();
+ intake_.Reset();
+ stilts_.Reset();
}
+
+ elevator_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->elevator) : nullptr,
+ &(position->elevator),
+ output != nullptr ? &(output->elevator_voltage) : nullptr,
+ &(status->elevator));
+
+ wrist_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->wrist) : nullptr,
+ &(position->wrist),
+ output != nullptr ? &(output->wrist_voltage) : nullptr,
+ &(status->wrist));
+
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake_joint),
+ output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+ &(status->intake));
+
+ stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
+ &(position->stilts),
+ output != nullptr ? &(output->stilts_voltage) : nullptr,
+ &(status->stilts));
+
+ status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
+ status->intake.zeroed && status->stilts.zeroed;
+
+ status->estopped = status->elevator.estopped || status->wrist.estopped ||
+ status->intake.estopped || status->stilts.estopped;
+
+ if (output) {
+ if (status->intake.position > kMinIntakeAngleForRollers) {
+ output->intake_roller_voltage =
+ (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
+
+ } else {
+ output->intake_roller_voltage = 0.0;
+ }
+ }
+
+ // TODO(theo) move these up when Iterate() is split
+ // update the goals
+ collision_avoidance_.UpdateGoal(status, unsafe_goal);
+
+ elevator_.set_min_position(collision_avoidance_.min_elevator_goal());
+ wrist_.set_min_position(collision_avoidance_.min_wrist_goal());
+ wrist_.set_max_position(collision_avoidance_.max_wrist_goal());
+ intake_.set_min_position(collision_avoidance_.min_intake_goal());
+ intake_.set_max_position(collision_avoidance_.max_intake_goal());
}
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 0d6765e..c3e53f2 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,6 +2,9 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/controls/control_loop.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
namespace y2019 {
@@ -16,14 +19,35 @@
const ::std::string &name =
".y2019.control_loops.superstructure.superstructure_queue");
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+ const PotAndAbsoluteEncoderSubsystem &elevator() const { return elevator_; }
+ const PotAndAbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+ const AbsoluteEncoderSubsystem &intake() const { return intake_; }
+ const PotAndAbsoluteEncoderSubsystem &stilts() const { return stilts_; }
+
protected:
- virtual void RunIteration(
- const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) override;
private:
+ PotAndAbsoluteEncoderSubsystem elevator_;
+ PotAndAbsoluteEncoderSubsystem wrist_;
+ AbsoluteEncoderSubsystem intake_;
+ PotAndAbsoluteEncoderSubsystem stilts_;
+
+ CollisionAvoidance collision_avoidance_;
+
+ static constexpr double kMinIntakeAngleForRollers = -0.7;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index cd004ac..a71f81a 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -3,25 +3,6 @@
import "aos/controls/control_loops.q";
import "frc971/control_loops/profiled_subsystem.q";
-struct ElevatorGoal {
- // Meters, 0 = lowest position - mechanical hard stop,
- // positive = upward
- double height;
-
- .frc971.ProfileParameters profile_params;
-};
-
-struct IntakeGoal {
- // Positive is rollers intaking inward.
- double roller_voltage;
-
- // 0 = linkage on the sprocket is pointing straight up,
- // positive = forward
- double joint_angle;
-
- .frc971.ProfileParameters profile_params;
-};
-
struct SuctionGoal {
// True = open solenoid (apply suction)
// Top/bottom are when wrist is forward
@@ -29,31 +10,29 @@
bool bottom;
};
-struct StiltsGoal {
- // Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the bottom
- // of the middle wheels.
- double height;
-
- .frc971.ProfileParameters profile_params;
-};
-
-struct WristGoal {
- // 0 = Straight up parallel to elevator
- // Positive rotates toward intake from 0
- double angle;
- .frc971.ProfileParameters profile_params;
-};
-
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
message Goal {
- ElevatorGoal elevator;
- IntakeGoal intake;
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal elevator;
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal intake;
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
+
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the bottom
+ // of the middle wheels.
+ .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
+
+ // Positive is rollers intaking inward.
+ double roller_voltage;
+
SuctionGoal suction;
- StiltsGoal stilts;
- WristGoal wrist;
};
message Status {
@@ -69,7 +48,7 @@
// Status of each subsystem.
.frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
.frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+ .frc971.control_loops.AbsoluteEncoderProfiledJointStatus intake;
.frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
};
@@ -106,7 +85,8 @@
// Voltage sent to rollers on intake. Positive rolls inward.
double intake_roller_voltage;
- // Voltage sent to motors to move stilts height. Positive moves robot upward.
+ // Voltage sent to motors to move stilts height. Positive moves robot
+ // upward.
double stilts_voltage;
// True opens solenoid (applies suction)
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..bd18b60
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,709 @@
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/controls/control_loop_test.h"
+#include "aos/queue.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
+#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
+#include "y2019/control_loops/superstructure/superstructure.h"
+#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+namespace {
+constexpr double kNoiseScalar = 0.01;
+} // namespace
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::CappedTestPlant;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation()
+ : elevator_plant_(
+ new CappedTestPlant(::y2019::control_loops::superstructure::
+ elevator::MakeElevatorPlant())),
+ elevator_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kElevatorEncoderRatio()),
+
+ wrist_plant_(new CappedTestPlant(
+ ::y2019::control_loops::superstructure::wrist::MakeWristPlant())),
+ wrist_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kWristEncoderRatio()),
+
+ intake_plant_(new CappedTestPlant(
+ ::y2019::control_loops::superstructure::intake::MakeIntakePlant())),
+ intake_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kIntakeEncoderRatio()),
+
+ stilts_plant_(new CappedTestPlant(
+ ::y2019::control_loops::superstructure::stilts::MakeStiltsPlant())),
+ stilts_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kStiltsEncoderRatio()),
+
+ superstructure_queue_(
+ ".y2019.control_loops.superstructure.superstructure_queue",
+ ".y2019.control_loops.superstructure.superstructure_queue.goal",
+ ".y2019.control_loops.superstructure.superstructure_queue.output",
+ ".y2019.control_loops.superstructure.superstructure_queue.status",
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position") {
+ // Start the elevator out in the middle by default.
+ InitializeElevatorPosition(constants::Values::kElevatorRange().upper);
+
+ // Start the wrist out in the middle by default.
+ InitializeWristPosition(constants::Values::kWristRange().upper);
+
+ InitializeIntakePosition(constants::Values::kIntakeRange().upper);
+
+ // Start the stilts out in the middle by default.
+ InitializeStiltsPosition(constants::Values::kStiltsRange().lower);
+ }
+
+ void InitializeElevatorPosition(double start_pos) {
+ elevator_plant_->mutable_X(0, 0) = start_pos;
+ elevator_plant_->mutable_X(1, 0) = 0.0;
+
+ elevator_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .elevator.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
+ void InitializeWristPosition(double start_pos) {
+ wrist_plant_->mutable_X(0, 0) = start_pos;
+ wrist_plant_->mutable_X(1, 0) = 0.0;
+ wrist_pot_encoder_.Initialize(start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .wrist.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
+ void InitializeIntakePosition(double start_pos) {
+ intake_plant_->mutable_X(0, 0) = start_pos;
+ intake_plant_->mutable_X(1, 0) = 0.0;
+
+ intake_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .intake.zeroing_constants.measured_absolute_position);
+ }
+
+ void InitializeStiltsPosition(double start_pos) {
+ stilts_plant_->mutable_X(0, 0) = start_pos;
+ stilts_plant_->mutable_X(1, 0) = 0.0;
+
+ stilts_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .stilts.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
+ // Sends a queue message with the position of the superstructure.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
+ superstructure_queue_.position.MakeMessage();
+
+ elevator_pot_encoder_.GetSensorValues(&position->elevator);
+ wrist_pot_encoder_.GetSensorValues(&position->wrist);
+ intake_pot_encoder_.GetSensorValues(&position->intake_joint);
+ stilts_pot_encoder_.GetSensorValues(&position->stilts);
+ position.Send();
+ }
+
+ double elevator_position() const { return elevator_plant_->X(0, 0); }
+ double elevator_velocity() const { return elevator_plant_->X(1, 0); }
+
+ double wrist_position() const { return wrist_plant_->X(0, 0); }
+ double wrist_velocity() const { return wrist_plant_->X(1, 0); }
+
+ double intake_position() const { return intake_plant_->X(0, 0); }
+ double intake_velocity() const { return intake_plant_->X(1, 0); }
+
+ double stilts_position() const { return stilts_plant_->X(0, 0); }
+ double stilts_velocity() const { return stilts_plant_->X(1, 0); }
+
+ // Sets the difference between the commanded and applied powers.
+ // This lets us test that the integrators work.
+
+ void set_elevator_voltage_offset(double voltage_offset) {
+ elevator_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_wrist_voltage_offset(double voltage_offset) {
+ wrist_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_intake_voltage_offset(double voltage_offset) {
+ intake_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_stilts_voltage_offset(double voltage_offset) {
+ stilts_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ // Simulates the superstructure for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+
+ const double voltage_check_elevator =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->elevator.state) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().elevator.subsystem_params.operating_voltage
+ : constants::GetValues().elevator.subsystem_params.zeroing_voltage;
+
+ const double voltage_check_wrist =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->wrist.state) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().wrist.subsystem_params.operating_voltage
+ : constants::GetValues().wrist.subsystem_params.zeroing_voltage;
+
+ const double voltage_check_intake =
+ (static_cast<AbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->intake.state) ==
+ AbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().intake.operating_voltage
+ : constants::GetValues().intake.zeroing_voltage;
+
+ const double voltage_check_stilts =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_queue_.status->stilts.state) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().stilts.subsystem_params.operating_voltage
+ : constants::GetValues().stilts.subsystem_params.zeroing_voltage;
+
+ EXPECT_NEAR(superstructure_queue_.output->elevator_voltage, 0.0,
+ voltage_check_elevator);
+
+ EXPECT_NEAR(superstructure_queue_.output->wrist_voltage, 0.0,
+ voltage_check_wrist);
+
+ EXPECT_NEAR(superstructure_queue_.output->intake_joint_voltage, 0.0,
+ voltage_check_intake);
+
+ EXPECT_NEAR(superstructure_queue_.output->stilts_voltage, 0.0,
+ voltage_check_stilts);
+
+ ::Eigen::Matrix<double, 1, 1> elevator_U;
+ elevator_U << superstructure_queue_.output->elevator_voltage +
+ elevator_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> wrist_U;
+ wrist_U << superstructure_queue_.output->wrist_voltage +
+ wrist_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ intake_U << superstructure_queue_.output->intake_joint_voltage +
+ intake_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> stilts_U;
+ stilts_U << superstructure_queue_.output->stilts_voltage +
+ stilts_plant_->voltage_offset();
+
+ elevator_plant_->Update(elevator_U);
+ wrist_plant_->Update(wrist_U);
+ intake_plant_->Update(intake_U);
+ stilts_plant_->Update(stilts_U);
+
+ const double position_elevator = elevator_plant_->Y(0, 0);
+ const double position_wrist = wrist_plant_->Y(0, 0);
+ const double position_intake = intake_plant_->Y(0, 0);
+ const double position_stilts = stilts_plant_->Y(0, 0);
+
+ elevator_pot_encoder_.MoveTo(position_elevator);
+ wrist_pot_encoder_.MoveTo(position_wrist);
+ intake_pot_encoder_.MoveTo(position_intake);
+ stilts_pot_encoder_.MoveTo(position_stilts);
+
+ EXPECT_GE(position_elevator,
+ constants::Values::kElevatorRange().lower_hard);
+ EXPECT_LE(position_elevator,
+ constants::Values::kElevatorRange().upper_hard);
+
+ EXPECT_GE(position_wrist, constants::Values::kWristRange().lower_hard);
+ EXPECT_LE(position_wrist, constants::Values::kWristRange().upper_hard);
+
+ EXPECT_GE(position_intake, constants::Values::kIntakeRange().lower_hard);
+ EXPECT_LE(position_intake, constants::Values::kIntakeRange().upper_hard);
+
+ EXPECT_GE(position_stilts, constants::Values::kStiltsRange().lower_hard);
+ EXPECT_LE(position_stilts, constants::Values::kStiltsRange().upper_hard);
+ }
+
+ private:
+ ::std::unique_ptr<CappedTestPlant> elevator_plant_;
+ PositionSensorSimulator elevator_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> wrist_plant_;
+ PositionSensorSimulator wrist_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> intake_plant_;
+ PositionSensorSimulator intake_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> stilts_plant_;
+ PositionSensorSimulator stilts_pot_encoder_;
+
+ SuperstructureQueue superstructure_queue_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : superstructure_queue_(
+ ".y2019.control_loops.superstructure.superstructure_queue",
+ ".y2019.control_loops.superstructure.superstructure_queue.goal",
+ ".y2019.control_loops.superstructure.superstructure_queue.output",
+ ".y2019.control_loops.superstructure.superstructure_queue.status",
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position"),
+ superstructure_(&event_loop_) {
+ set_team_id(::frc971::control_loops::testing::kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.status.FetchLatest();
+
+ EXPECT_NEAR(superstructure_queue_.goal->elevator.unsafe_goal,
+ superstructure_queue_.status->elevator.position, 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->wrist.unsafe_goal,
+ superstructure_plant_.wrist_position(), 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->intake.unsafe_goal,
+ superstructure_queue_.status->intake.position, 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->stilts.unsafe_goal,
+ superstructure_plant_.stilts_position(), 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ superstructure_plant_.SendPositionMessage();
+ superstructure_.Iterate();
+ superstructure_plant_.Simulate();
+
+ TickTime(::std::chrono::microseconds(5050));
+ }
+
+ void CheckCollisions() {
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_FALSE(
+ collision_avoidance_.IsCollided(superstructure_queue_.status.get()));
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunIteration();
+ superstructure_queue_.status.FetchLatest();
+ // 2 Seconds
+ ASSERT_LE(i, 2 * 1.0 / .00505);
+ } while (!superstructure_queue_.status.get()->zeroed);
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
+ bool check_collisions = true) {
+ const auto start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < start_time + run_for) {
+ const auto loop_start_time = monotonic_clock::now();
+ double begin_elevator_velocity =
+ superstructure_plant_.elevator_velocity();
+ double begin_wrist_velocity = superstructure_plant_.wrist_velocity();
+ double begin_intake_velocity = superstructure_plant_.intake_velocity();
+ double begin_stilts_velocity = superstructure_plant_.stilts_velocity();
+
+ RunIteration(enabled);
+ if (check_collisions) {
+ CheckCollisions();
+ }
+
+ const double loop_time =
+ chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time).count();
+
+ const double elevator_acceleration =
+ (superstructure_plant_.elevator_velocity() -
+ begin_elevator_velocity) /
+ loop_time;
+ const double wrist_acceleration =
+ (superstructure_plant_.wrist_velocity() - begin_wrist_velocity) /
+ loop_time;
+ const double intake_acceleration =
+ (superstructure_plant_.intake_velocity() - begin_intake_velocity) /
+ loop_time;
+ const double stilts_acceleration =
+ (superstructure_plant_.stilts_velocity() - begin_stilts_velocity) /
+ loop_time;
+
+ EXPECT_GE(peak_elevator_acceleration_, elevator_acceleration);
+ EXPECT_LE(-peak_elevator_acceleration_, elevator_acceleration);
+ EXPECT_GE(peak_elevator_velocity_,
+ superstructure_plant_.elevator_velocity());
+ EXPECT_LE(-peak_elevator_velocity_,
+ superstructure_plant_.elevator_velocity());
+
+ EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_GE(peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
+ EXPECT_LE(-peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
+
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_intake_velocity_, superstructure_plant_.intake_velocity());
+ EXPECT_LE(-peak_intake_velocity_,
+ superstructure_plant_.intake_velocity());
+
+ EXPECT_GE(peak_stilts_acceleration_, stilts_acceleration);
+ EXPECT_LE(-peak_stilts_acceleration_, stilts_acceleration);
+ EXPECT_GE(peak_stilts_velocity_, superstructure_plant_.stilts_velocity());
+ EXPECT_LE(-peak_stilts_velocity_,
+ superstructure_plant_.stilts_velocity());
+ }
+ }
+
+ void set_peak_elevator_acceleration(double value) {
+ peak_elevator_acceleration_ = value;
+ }
+ void set_peak_elevator_velocity(double value) {
+ peak_elevator_velocity_ = value;
+ }
+
+ void set_peak_wrist_acceleration(double value) {
+ peak_wrist_acceleration_ = value;
+ }
+ void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
+
+ void set_peak_intake_acceleration(double value) {
+ peak_intake_acceleration_ = value;
+ }
+ void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+
+ void set_peak_stilts_acceleration(double value) {
+ peak_stilts_acceleration_ = value;
+ }
+ void set_peak_stilts_velocity(double value) { peak_stilts_velocity_ = value; }
+
+ ::aos::ShmEventLoop event_loop_;
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory
+ // that is no longer valid.
+ SuperstructureQueue superstructure_queue_;
+
+ // Create a control loop and simulation.
+ Superstructure superstructure_;
+ SuperstructureSimulation superstructure_plant_;
+
+ // Creat a collision avoidance object
+ CollisionAvoidance collision_avoidance_;
+
+ private:
+ // The acceleration limits to check for while moving.
+ double peak_elevator_acceleration_ = 1e10;
+ double peak_wrist_acceleration_ = 1e10;
+ double peak_intake_acceleration_ = 1e10;
+ double peak_stilts_acceleration_ = 1e10;
+
+ // The velocity limits to check for while moving.
+ double peak_elevator_velocity_ = 1e10;
+ double peak_wrist_velocity_ = 1e10;
+ double peak_intake_velocity_ = 1e10;
+ double peak_stilts_velocity_ = 1e10;
+};
+
+// Tests that the superstructure does nothing when the goal is zero.
+TEST_F(SuperstructureTest, DoesNothing) {
+ superstructure_plant_.InitializeElevatorPosition(1.4);
+ superstructure_plant_.InitializeWristPosition(1.0);
+ superstructure_plant_.InitializeIntakePosition(1.1);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ WaitUntilZeroed();
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->elevator.unsafe_goal = 1.4;
+ goal->wrist.unsafe_goal = 1.0;
+ goal->intake.unsafe_goal = 1.1;
+ goal->stilts.unsafe_goal = 0.1;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+ // Set a reasonable goal.
+
+ superstructure_plant_.InitializeElevatorPosition(1.4);
+ superstructure_plant_.InitializeWristPosition(1.0);
+ superstructure_plant_.InitializeIntakePosition(1.1);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ WaitUntilZeroed();
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = 1.4;
+ goal->elevator.profile_params.max_velocity = 1;
+ goal->elevator.profile_params.max_acceleration = 0.5;
+
+ goal->wrist.unsafe_goal = 1.0;
+ goal->wrist.profile_params.max_velocity = 1;
+ goal->wrist.profile_params.max_acceleration = 0.5;
+
+ goal->intake.unsafe_goal = 1.1;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
+ goal->stilts.unsafe_goal = 0.1;
+ goal->stilts.profile_params.max_velocity = 1;
+ goal->stilts.profile_params.max_acceleration = 0.5;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+//
+// We are going to disable collision detection to make this easier to implement.
+TEST_F(SuperstructureTest, SaturationTest) {
+ // Zero it before we move.
+ WaitUntilZeroed();
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->stilts.unsafe_goal = constants::Values::kStiltsRange().upper;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->elevator.profile_params.max_velocity = 20.0;
+ goal->elevator.profile_params.max_acceleration = 0.1;
+
+ goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
+ goal->wrist.profile_params.max_velocity = 20.0;
+ goal->wrist.profile_params.max_acceleration = 0.1;
+
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->intake.profile_params.max_velocity = 20.0;
+ goal->intake.profile_params.max_acceleration = 0.1;
+
+ goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
+ goal->stilts.profile_params.max_velocity = 20.0;
+ goal->stilts.profile_params.max_acceleration = 0.1;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ set_peak_elevator_velocity(23.0);
+ set_peak_elevator_acceleration(0.2);
+ set_peak_wrist_velocity(23.0);
+ set_peak_wrist_acceleration(0.2);
+ set_peak_intake_velocity(23.0);
+ set_peak_intake_acceleration(0.2);
+ set_peak_stilts_velocity(23.0);
+ set_peak_stilts_acceleration(0.2);
+
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Now do a high acceleration move with a low velocity limit.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
+ goal->elevator.profile_params.max_velocity = 0.1;
+ goal->elevator.profile_params.max_acceleration = 10.0;
+
+ goal->wrist.unsafe_goal = constants::Values::kWristRange().lower;
+ goal->wrist.profile_params.max_velocity = 0.1;
+ goal->wrist.profile_params.max_acceleration = 10.0;
+
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+ goal->intake.profile_params.max_velocity = 0.1;
+ goal->intake.profile_params.max_acceleration = 10.0;
+
+ goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
+ goal->stilts.profile_params.max_velocity = 0.1;
+ goal->stilts.profile_params.max_acceleration = 10.0;
+ }
+ set_peak_elevator_velocity(0.2);
+ set_peak_elevator_acceleration(11.0);
+ set_peak_wrist_velocity(0.2);
+ set_peak_wrist_acceleration(11.0);
+ set_peak_intake_velocity(0.2);
+ set_peak_intake_acceleration(11.0);
+ set_peak_stilts_velocity(0.2);
+ set_peak_stilts_acceleration(11.0);
+
+ VerifyNearGoal();
+}
+
+// Tests if the robot zeroes properly... maybe redundant?
+TEST_F(SuperstructureTest, ZeroTest) {
+ superstructure_plant_.InitializeElevatorPosition(1.4);
+ superstructure_plant_.InitializeWristPosition(1.0);
+ superstructure_plant_.InitializeIntakePosition(1.1);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->elevator.unsafe_goal = 1.4;
+ goal->elevator.profile_params.max_velocity = 1;
+ goal->elevator.profile_params.max_acceleration = 0.5;
+
+ goal->wrist.unsafe_goal = 1.0;
+ goal->wrist.profile_params.max_velocity = 1;
+ goal->wrist.profile_params.max_acceleration = 0.5;
+
+ goal->intake.unsafe_goal = 1.1;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
+ goal->stilts.unsafe_goal = 0.1;
+ goal->stilts.profile_params.max_velocity = 1;
+ goal->stilts.profile_params.max_acceleration = 0.5;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ WaitUntilZeroed();
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+ WaitUntilZeroed();
+ RunForTime(chrono::seconds(2));
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.elevator().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.wrist().state());
+
+ EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.stilts().state());
+}
+
+// Move wrist front to back and see if we collide
+TEST_F(SuperstructureTest, CollisionTest) {
+ // Set a reasonable goal.
+ superstructure_plant_.InitializeElevatorPosition(
+ constants::Values::kElevatorRange().lower);
+ // 60 degrees forwards
+ superstructure_plant_.InitializeWristPosition(M_PI / 3.0);
+ superstructure_plant_.InitializeIntakePosition(
+ CollisionAvoidance::kIntakeOutAngle + CollisionAvoidance::kEpsIntake);
+ superstructure_plant_.InitializeStiltsPosition(0.1);
+
+ WaitUntilZeroed();
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
+ goal->wrist.unsafe_goal = -M_PI / 3.0;
+ goal->intake.unsafe_goal =
+ CollisionAvoidance::kIntakeInAngle - CollisionAvoidance::kEpsIntake;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(20), true, true);
+
+ VerifyNearGoal();
+}
+
+// Tests that the rollers spin when allowed
+TEST_F(SuperstructureTest, IntakeRollerTest) {
+ WaitUntilZeroed();
+
+ // Get the elevator and wrist out of the way and set the Intake to where
+ // we should be able to spin and verify that they do
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage,
+ superstructure_queue_.goal->roller_voltage);
+ VerifyNearGoal();
+
+ // Move the intake where we oughtn't to spin the rollers and verify they don't
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+ goal->wrist.unsafe_goal = 0.0;
+ goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+ goal->roller_voltage = 6.0;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(5), true, true);
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage, 0.0);
+ VerifyNearGoal();
+}
+
+// Tests that running disabled, ya know, works
+TEST_F(SuperstructureTest, DiasableTest) {
+ RunForTime(chrono::seconds(2), false, false);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index b944d98..e99a264 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -1,3 +1,6 @@
+load("//tools:environments.bzl", "mcu_cpus")
+load("//motors:macros.bzl", "hex_from_elf")
+
spi_crc_args = [
"$(location //third_party/pycrc:pycrc_main)",
"--width=16",
@@ -60,6 +63,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos/containers:sized_array",
+ "//aos/time",
"//third_party/eigen",
],
)
@@ -81,6 +85,21 @@
],
)
+cc_library(
+ name = "uart",
+ srcs = [
+ "uart.cc",
+ ],
+ hdrs = [
+ "uart.h",
+ ],
+ deps = [
+ ":structures",
+ "//aos/containers:sized_array",
+ "//third_party/optional",
+ ],
+)
+
cc_test(
name = "spi_test",
srcs = [
@@ -115,3 +134,34 @@
"//third_party/GSL",
],
)
+
+cc_library(
+ name = "serial",
+ srcs = ["serial.cc"],
+ hdrs = ["serial.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/logging",
+ ],
+)
+
+cc_binary(
+ name = "teensy.elf",
+ srcs = [
+ "teensy.cc",
+ ],
+ restricted_to = ["//tools:cortex-m4f"],
+ deps = [
+ "//aos/time:time_mcu",
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:configuration",
+ "//motors/peripheral:uart",
+ "//motors/print:usb",
+ ],
+)
+
+hex_from_elf(
+ name = "teensy",
+ restricted_to = ["//tools:cortex-m4f"],
+)
diff --git a/y2019/jevois/serial.cc b/y2019/jevois/serial.cc
new file mode 100644
index 0000000..7c49b4f
--- /dev/null
+++ b/y2019/jevois/serial.cc
@@ -0,0 +1,78 @@
+#include "y2019/jevois/serial.h"
+
+#include "aos/logging/logging.h"
+
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <termios.h>
+#include <unistd.h>
+
+namespace y2019 {
+namespace jevois {
+
+int open_via_terminos(const char *tty_name) {
+ int itsDev = ::open(tty_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
+ if (itsDev == -1) {
+ LOG(FATAL, "problem opening: %s\n", tty_name);
+ }
+
+ termios options = {};
+ if (tcgetattr(itsDev, &options) == -1) LOG(FATAL, "Failed to get options");
+
+ // get raw input from the port
+ options.c_cflag |= (CLOCAL // ignore modem control lines
+ | CREAD); // enable the receiver
+
+ options.c_iflag &= ~(IGNBRK // ignore BREAK condition on input
+ | BRKINT // If IGNBRK is not set, generate SIGINT on
+ // BREAK condition, else read BREAK as \0
+ | PARMRK | ISTRIP // strip off eighth bit
+ | INLCR // donot translate NL to CR on input
+ | IGNCR // ignore CR
+ | ICRNL // translate CR to newline on input
+ | IXON // disable XON/XOFF flow control on output
+ );
+
+ // disable implementation-defined output processing
+ options.c_oflag &= ~OPOST;
+ options.c_lflag &= ~(ECHO // dont echo i/p chars
+ | ECHONL // do not echo NL under any circumstance
+ | ICANON // disable cannonical mode
+ | ISIG // do not signal for INTR, QUIT, SUSP etc
+ | IEXTEN // disable platform dependent i/p processing
+ );
+
+ cfsetispeed(&options, B115200);
+ cfsetospeed(&options, B115200);
+
+ // Set the number of bits:
+ options.c_cflag &= ~CSIZE; // mask off the 'size' bits
+ options.c_cflag |= CS8;
+
+ // Set parity option:
+ options.c_cflag &= ~(PARENB | PARODD);
+
+ // Set the stop bits option:
+ options.c_cflag &= ~CSTOPB;
+
+ // Set the flow control:
+ options.c_cflag &= ~CRTSCTS;
+ options.c_iflag &= ~(IXON | IXANY | IXOFF);
+
+ options.c_cc[VMIN] = 1;
+ options.c_cc[VTIME] = 2;
+
+ // Flow control:
+ // flow soft:
+ // options.c_iflag |= (IXON | IXANY | IXOFF);
+ // flow hard:
+ // options.c_cflag |= CRTSCTS;
+
+ if (tcsetattr(itsDev, TCSANOW, &options) == -1)
+ LOG(FATAL, "Failed to set port options");
+ return itsDev;
+}
+
+} // namespace jevois
+} // namespace y2019
diff --git a/y2019/jevois/serial.h b/y2019/jevois/serial.h
new file mode 100644
index 0000000..9dd4b0c
--- /dev/null
+++ b/y2019/jevois/serial.h
@@ -0,0 +1,12 @@
+#ifndef Y2019_JEVOIS_SERIAL_H_
+#define Y2019_JEVOIS_SERIAL_H_
+
+namespace y2019 {
+namespace jevois {
+
+int open_via_terminos(const char *tty_name);
+
+} // namespace jevois
+} // namespace frc971
+
+#endif // Y2019_JEVOIS_SERIAL_H_
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index c82a089..15889c4 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -10,6 +10,7 @@
#include "Eigen/Dense"
#include "aos/containers/sized_array.h"
+#include "aos/time/time.h"
namespace frc971 {
namespace jevois {
@@ -106,6 +107,15 @@
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
+ enum class CameraCommand {
+ // Stay in normal mode.
+ kNormal,
+ // Go to camera passthrough mode.
+ kCameraPassthrough,
+ // Go to being a useful USB device.
+ kUsb,
+ };
+
bool operator==(const CameraCalibration &other) const {
if (other.calibration != calibration) {
return false;
@@ -120,6 +130,17 @@
//
// TODO(Parker): What are the details on how this is defined?
Eigen::Matrix<float, 3, 4> calibration;
+
+ // A local timestamp from the Teensy. This starts at 0 when the Teensy is
+ // powered on.
+ aos::monotonic_clock::time_point teensy_now;
+
+ // A realtime timestamp from the roboRIO. This will be min_time if the roboRIO
+ // has never sent anything.
+ aos::realtime_clock::time_point realtime_now;
+
+ // What mode the camera should transition into.
+ CameraCommand camera_command;
};
// This is all the information the Teensy sends to the RoboRIO.
@@ -160,6 +181,9 @@
// Whether the light ring for each camera should be on.
std::bitset<5> light_rings;
+
+ // The current time.
+ aos::realtime_clock::time_point realtime_now;
};
} // namespace jevois
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
new file mode 100644
index 0000000..2d3c020
--- /dev/null
+++ b/y2019/jevois/teensy.cc
@@ -0,0 +1,412 @@
+#include "aos/time/time.h"
+#include "motors/core/kinetis.h"
+#include "motors/core/time.h"
+#include "motors/peripheral/configuration.h"
+#include "motors/peripheral/uart.h"
+#include "motors/print/print.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace jevois {
+namespace {
+
+struct Uarts {
+ Uarts() {
+ DisableInterrupts disable_interrupts;
+ instance = this;
+ }
+ ~Uarts() {
+ DisableInterrupts disable_interrupts;
+ instance = nullptr;
+ }
+
+ void Initialize(int baud_rate) {
+ cam0.Initialize(baud_rate);
+ cam1.Initialize(baud_rate);
+ cam2.Initialize(baud_rate);
+ cam3.Initialize(baud_rate);
+ cam4.Initialize(baud_rate);
+ }
+
+ frc971::teensy::InterruptBufferedUart cam0{&UART1, F_CPU};
+ frc971::teensy::InterruptBufferedUart cam1{&UART0, F_CPU};
+ frc971::teensy::InterruptBufferedUart cam2{&UART2, BUS_CLOCK_FREQUENCY};
+ frc971::teensy::InterruptBufferedUart cam3{&UART3, BUS_CLOCK_FREQUENCY};
+ frc971::teensy::InterruptBufferedUart cam4{&UART4, BUS_CLOCK_FREQUENCY};
+
+ static Uarts *instance;
+};
+
+Uarts *Uarts::instance = nullptr;
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+extern char *__brkval;
+extern uint32_t __bss_ram_start__[];
+extern uint32_t __heap_start__[];
+extern uint32_t __stack_end__[];
+
+void uart0_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam1.HandleInterrupt(disable_interrupts);
+}
+
+void uart1_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam0.HandleInterrupt(disable_interrupts);
+}
+
+void uart2_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam2.HandleInterrupt(disable_interrupts);
+}
+
+void uart3_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam3.HandleInterrupt(disable_interrupts);
+}
+
+void uart4_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam4.HandleInterrupt(disable_interrupts);
+}
+
+} // extern "C"
+
+// A test program which echos characters back after adding a per-UART offset to
+// them (CAM0 adds 1, CAM1 adds 2, etc).
+__attribute__((unused)) void TestUarts() {
+ Uarts *const uarts = Uarts::instance;
+ while (true) {
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam0.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 1;
+ }
+ uarts->cam0.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam1.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 2;
+ }
+ uarts->cam1.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam2.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 3;
+ }
+ uarts->cam2.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam3.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 4;
+ }
+ uarts->cam3.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam4.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 5;
+ }
+ uarts->cam4.Write(data);
+ }
+ }
+}
+
+// Tests all the I/O pins. Cycles through each one for 1 second. While active,
+// each output is turned on, and each input has its value printed.
+__attribute__((unused)) void TestIo() {
+ // Set SPI0 pins to GPIO.
+ // SPI_OUT
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 6) = 1;
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_CS
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 0) = 0;
+ PORTD_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_IN
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 7) = 0;
+ PORTC_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_SCK
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 1) = 0;
+ PORTD_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ // Set LED pins to GPIO.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 11) = 1;
+ PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 10) = 1;
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 8) = 1;
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 9) = 1;
+ PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 18) = 1;
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 2) = 1;
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 7) = 1;
+ PORTD_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 1) = 1;
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 19) = 1;
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 5) = 1;
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ auto next = aos::monotonic_clock::now();
+ static constexpr auto kTick = std::chrono::seconds(1);
+ while (true) {
+ printf("SPI_MISO\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 0;
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_CS %d\n", (int)PERIPHERAL_BITBAND(GPIOD_PDIR, 0));
+ }
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_MOSI %d\n", (int)PERIPHERAL_BITBAND(GPIOC_PDIR, 7));
+ }
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_CLK %d\n", (int)PERIPHERAL_BITBAND(GPIOD_PDIR, 1));
+ }
+ next += kTick;
+
+ printf("CAM0\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 0;
+ next += kTick;
+
+ printf("CAM1\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 0;
+ next += kTick;
+
+ printf("CAM2\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 0;
+ next += kTick;
+
+ printf("CAM3\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 0;
+ next += kTick;
+
+ printf("CAM4\n");
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = 0;
+ next += kTick;
+
+ printf("CAM5\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 0;
+ next += kTick;
+
+ printf("CAM6\n");
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 7) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 7) = 0;
+ next += kTick;
+
+ printf("CAM7\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 0;
+ next += kTick;
+
+ printf("CAM8\n");
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 19) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 19) = 0;
+ next += kTick;
+
+ printf("CAM9\n");
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 5) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 5) = 0;
+ next += kTick;
+ }
+}
+
+int Main() {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
+ NVIC_SET_SANE_PRIORITY(IRQ_UART0_STATUS, 0xE);
+
+ // Set the LED's pin to output mode.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ frc971::motors::PrintingParameters printing_parameters;
+ printing_parameters.dedicated_usb = true;
+ const ::std::unique_ptr<frc971::motors::PrintingImplementation> printing =
+ CreatePrinting(printing_parameters);
+ printing->Initialize();
+
+ DMA.CR = M_DMA_EMLM;
+
+ SIM_SCGC4 |=
+ SIM_SCGC4_UART0 | SIM_SCGC4_UART1 | SIM_SCGC4_UART2 | SIM_SCGC4_UART3;
+ SIM_SCGC1 |= SIM_SCGC1_UART4;
+
+ // SPI0 goes to the roboRIO.
+ // SPI0_PCS0 is SPI_CS.
+ PORTD_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SOUT is SPI_MISO.
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SIN is SPI_MOSI.
+ PORTC_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SCK is SPI_CLK.
+ PORTD_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ // FTM0_CH0 is LED0 (7 in silkscreen, a beacon channel).
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH1 is LED1 (5 in silkscreen, a beacon channel).
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH7 is LED2 (6 in silkscreen, a beacon channel).
+ PORTD_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH5 is LED3 (9 in silkscreen, a vision camera).
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+
+ // FTM2_CH1 is LED4 (8 in silkscreen, a vision camera).
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM2_CH0 is LED5 (for CAM4).
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // FTM3_CH4 is LED6 (for CAM2).
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH5 is LED7 (for CAM3).
+ PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH6 is LED8 (for CAM1).
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH7 is LED9 (for CAM0).
+ PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // This hardware has been deactivated, but keep this comment for now to
+ // document which pins it is on.
+#if 0
+ // This is ODROID_EN.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 0) = 1;
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 0) = 0;
+ PORTC_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // This is CAM_EN.
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 0) = 1;
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 0) = 0;
+ PORTB_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+#endif
+ // This is 5V_PGOOD.
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 6) = 0;
+ PORTD_PCR6 = PORT_PCR_MUX(1);
+
+ // These go to CAM1.
+ // UART0_RX (peripheral) is UART1_RX (schematic).
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART0_TX (peripheral) is UART1_TX (schematic).
+ PORTA_PCR14 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM0.
+ // UART1_RX (peripheral) is UART0_RX (schematic).
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART1_TX (peripheral) is UART0_TX (schematic).
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM2.
+ // UART2_RX
+ PORTD_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART2_TX
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM3.
+ // UART3_RX
+ PORTB_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART3_TX
+ PORTB_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM4.
+ // UART4_RX
+ PORTE_PCR25 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART4_TX
+ PORTE_PCR24 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ Uarts uarts;
+
+ // Give everything a chance to get going.
+ delay(100);
+
+ printf("Ram start: %p\n", __bss_ram_start__);
+ printf("Heap start: %p\n", __heap_start__);
+ printf("Heap end: %p\n", __brkval);
+ printf("Stack start: %p\n", __stack_end__);
+
+ uarts.Initialize(115200);
+ NVIC_ENABLE_IRQ(IRQ_UART0_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART1_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART2_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART3_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART4_STATUS);
+
+ while (true) {
+ }
+}
+
+extern "C" {
+
+int main(void) {
+ return Main();
+}
+
+} // extern "C"
+
+} // namespace
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
new file mode 100644
index 0000000..f4f66e3
--- /dev/null
+++ b/y2019/jevois/uart.cc
@@ -0,0 +1,14 @@
+#include "y2019/jevois/uart.h"
+
+namespace frc971 {
+namespace jevois {
+
+UartBuffer UartPackToTeensy(const Frame & /*message*/) { return UartBuffer(); }
+
+tl::optional<CameraCalibration> UartUnpackToCamera(
+ const UartBuffer & /*message*/) {
+ return tl::nullopt;
+}
+
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
new file mode 100644
index 0000000..04a1554
--- /dev/null
+++ b/y2019/jevois/uart.h
@@ -0,0 +1,26 @@
+#ifndef Y2019_JEVOIS_UART_H_
+#define Y2019_JEVOIS_UART_H_
+
+#include "aos/containers/sized_array.h"
+#include "third_party/optional/tl/optional.hpp"
+#include "y2019/jevois/structures.h"
+
+// This file manages serializing and deserializing the various structures for
+// transport via UART.
+
+namespace frc971 {
+namespace jevois {
+
+constexpr size_t uart_max_size() {
+ // TODO(Brian): Make this real.
+ return 10;
+}
+using UartBuffer = aos::SizedArray<char, uart_max_size()>;
+
+UartBuffer UartPackToTeensy(const Frame &message);
+tl::optional<CameraCalibration> UartUnpackToCamera(const UartBuffer &message);
+
+} // namespace jevois
+} // namespace frc971
+
+#endif // Y2019_JEVOIS_UART_H_
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 7bb32b2..f718c3d 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -30,8 +30,15 @@
namespace joysticks {
// TODO(sabina): update button locations when the board is done
-const ButtonLocation kElevatorUp(0, 0);
+const ButtonLocation kElevatorUp(4, 4);
+const ButtonLocation kIntakeOut(3, 3);
const ButtonLocation kElevatorDown(0, 0);
+const ButtonLocation kElevator1(4, 1);
+const ButtonLocation kElevator2(4, 11);
+const ButtonLocation kElevator3(4, 9);
+const ButtonLocation kElevator4(4, 7);
+const ButtonLocation kElevator5(4, 5);
+
const ButtonLocation kDiskLoad(0, 0);
const ButtonLocation kDiskRocketMiddle(0, 0);
const ButtonLocation kDiskRocketTop(0, 0);
@@ -47,10 +54,12 @@
const ButtonLocation kCargoSuction(0, 0);
const ButtonLocation kDiskSuction(0, 0);
const ButtonLocation kSuctionOut(0, 0);
-const ButtonLocation kDeployStilt(0, 0);
+const ButtonLocation kDeployStilt(3, 8);
const ButtonLocation kRetractStilt(0, 0);
const ButtonLocation kBackwards(0, 0);
+const ButtonLocation kWristDown(3, 1);
+
class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
@@ -69,6 +78,7 @@
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ /*
if (data.IsPressed(kElevatorUp)) {
elevator_height_ += 0.1;
} else if (data.IsPressed(kElevatorDown)) {
@@ -105,31 +115,34 @@
wrist_angle_ = 0.0;
} else {
}
+ */
+ /*
// TODO(sabina): get accurate angle.
if (data.IsPressed(kIntakeExtend)) {
- new_superstructure_goal->intake.joint_angle = 0.5;
+ new_superstructure_goal->intake.unsafe_goal = 0.5;
} else {
- new_superstructure_goal->intake.joint_angle = 0.0;
+ new_superstructure_goal->intake.unsafe_goal = 0.0;
}
if (data.IsPressed(kIntake)) {
new_superstructure_goal->suction.bottom = true;
if (superstructure_queue.status->has_piece == false) {
- new_superstructure_goal->intake.roller_voltage = 12.0;
+ new_superstructure_goal->roller_voltage = 12.0;
} else {
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ new_superstructure_goal->roller_voltage = 0.0;
}
} else if (data.IsPressed(kSpit)) {
new_superstructure_goal->suction.bottom = false;
if (superstructure_queue.status->has_piece == false) {
- new_superstructure_goal->intake.roller_voltage = 12.0;
+ new_superstructure_goal->roller_voltage = 12.0;
} else {
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ new_superstructure_goal->roller_voltage = 0.0;
}
} else {
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ new_superstructure_goal->roller_voltage = 0.0;
}
+ */
// TODO(sabina): decide if we should really have disk suction as its own
// button
@@ -142,23 +155,43 @@
} else if (data.IsPressed(kSuctionOut)) {
new_superstructure_goal->suction.top = true;
new_superstructure_goal->suction.bottom = true;
- } else {
}
// TODO(sabina): max height please?
if (data.IsPressed(kDeployStilt)) {
- new_superstructure_goal->stilts.height = 0;
- } else if (data.IsPressed(kRetractStilt)) {
- new_superstructure_goal->stilts.height = 0;
+ new_superstructure_goal->stilts.unsafe_goal = 0.3;
} else {
+ new_superstructure_goal->stilts.unsafe_goal = 0.01;
}
- if (data.IsPressed(kBackwards)) {
- wrist_angle_ = -wrist_angle_;
+ if (data.IsPressed(kIntakeOut)) {
+ new_superstructure_goal->intake.unsafe_goal = 0.8;
+ new_superstructure_goal->roller_voltage = 5.0;
+ } else {
+ new_superstructure_goal->intake.unsafe_goal = -1.2;
+ new_superstructure_goal->roller_voltage = 0.0;
}
- new_superstructure_goal->elevator.height = elevator_height_;
- new_superstructure_goal->wrist.angle = wrist_angle_;
+ if (data.IsPressed(kElevator1)) {
+ elevator_height_ = 1.5;
+ } else if (data.IsPressed(kElevator2)) {
+ elevator_height_ = 1.2;
+ } else if (data.IsPressed(kElevator3)) {
+ elevator_height_ = 0.8;
+ } else if (data.IsPressed(kElevator4)) {
+ elevator_height_ = 0.3;
+ } else if (data.IsPressed(kElevator5)) {
+ elevator_height_ = 0.01;
+ }
+
+ if (data.IsPressed(kWristDown)) {
+ wrist_angle_ = -M_PI / 3.0;
+ } else {
+ wrist_angle_ = M_PI / 3.0;
+ }
+
+ new_superstructure_goal->elevator.unsafe_goal = elevator_height_;
+ new_superstructure_goal->wrist.unsafe_goal = wrist_angle_;
LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
if (!new_superstructure_goal.Send()) {
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
new file mode 100644
index 0000000..6736c24
--- /dev/null
+++ b/y2019/vision/BUILD
@@ -0,0 +1,80 @@
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+
+package(default_visibility = ["//visibility:public"])
+
+VISION_TARGETS = [ "//tools:k8", "//tools:armhf-debian"]
+
+cc_library(
+ name = "target_finder",
+ srcs = ["target_finder.cc", "target_geometry.cc"],
+ hdrs = ["target_finder.h", "target_types.h"],
+ deps = [
+ "@com_google_ceres_solver//:ceres",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:region_alloc",
+ "//aos/vision/blob:contour",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:overlay",
+ "//aos/vision/math:vector",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+gtk_dependent_cc_binary(
+ name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/vision/blob:move_scale",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:debug_framework",
+ "//aos/vision/math:vector",
+ ],
+ copts = ["-Wno-unused-variable"],
+ restricted_to = VISION_TARGETS,
+)
+
+cc_binary(
+ name = "target_sender",
+ srcs = ["target_sender.cc"],
+ deps = [
+ ":target_finder",
+ "//y2019/jevois:serial",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+"""
+cc_binary(
+ name = "calibration",
+ srcs = ["calibration.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+"""
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
new file mode 100644
index 0000000..4f43c9a
--- /dev/null
+++ b/y2019/vision/debug_viewer.cc
@@ -0,0 +1,269 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+#include "y2019/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::AnalysisAllocator;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+using aos::vision::Segment;
+using aos::vision::PixelRef;
+
+namespace y2019 {
+namespace vision {
+
+std::vector<PixelRef> GetNColors(size_t num_colors) {
+ std::vector<PixelRef> colors;
+ for (size_t i = 0; i < num_colors; ++i) {
+ int quadrent = i * 6 / num_colors;
+ uint8_t alpha = (256 * 6 * i - quadrent * num_colors * 256) / num_colors;
+ uint8_t inv_alpha = 255 - alpha;
+ switch (quadrent) {
+ case 0:
+ colors.push_back(PixelRef{255, alpha, 0});
+ break;
+ case 1:
+ colors.push_back(PixelRef{inv_alpha, 255, 0});
+ break;
+ case 2:
+ colors.push_back(PixelRef{0, 255, alpha});
+ break;
+ case 3:
+ colors.push_back(PixelRef{0, inv_alpha, 255});
+ break;
+ case 4:
+ colors.push_back(PixelRef{alpha, 0, 255});
+ break;
+ case 5:
+ colors.push_back(PixelRef{255, 0, inv_alpha});
+ break;
+ }
+ }
+ return colors;
+}
+
+class FilterHarness : 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.75);
+ overlays_.push_back(&overlay_);
+ overlays_.push_back(finder_.GetOverlay());
+ viewer_->view()->SetOverlays(&overlays_);
+ }
+
+ void DrawBlob(const RangeImage &blob, PixelRef color) {
+ if (viewer_) {
+ BlobList list;
+ list.push_back(blob);
+ viewer_->DrawBlobList(list, color);
+ }
+ }
+
+ bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ imgs_last_ = imgs;
+ fmt_last_ = fmt;
+ // reset for next drawing cycle
+ for (auto &overlay : overlays_) {
+ overlay->Reset();
+ }
+
+ if (draw_select_blob_ || draw_raw_poly_ || draw_components_ ||
+ draw_raw_target_ || draw_raw_IR_ || draw_results_) {
+ printf("_____ New Image _____\n");
+ }
+
+ // Remove bad blobs.
+ finder_.PreFilter(&imgs);
+
+ // Find polygons from blobs.
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon =
+ finder_.FillPolygon(blob, draw_raw_poly_);
+ if (polygon.empty()) {
+ DrawBlob(blob, {255, 0, 0});
+ } else {
+ raw_polys.push_back(polygon);
+ if (draw_select_blob_) {
+ DrawBlob(blob, {0, 0, 255});
+ }
+ if (draw_raw_poly_) {
+ std::vector<PixelRef> colors = GetNColors(polygon.size());
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(polygon[i].Intersect(polygon[(i + 1) % 4]));
+ }
+
+ for (size_t i = 0; i < 4; ++i) {
+ overlay_.AddLine(corners[i], corners[(i + 1) % 4], colors[i]);
+ }
+ }
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+ if (draw_components_) {
+ for (const TargetComponent &comp : target_component_list) {
+ DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
+ {0, 0, 255});
+ }
+ }
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list = finder_.FindTargetsFromComponents(
+ target_component_list, draw_raw_target_);
+ if (draw_raw_target_) {
+ for (const Target &target : target_list) {
+ DrawTarget(target);
+ }
+ }
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, true));
+ if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
+ }
+
+ // Check that our current results match possible solutions.
+ results = finder_.FilterResults(results);
+ if (draw_results_) {
+ for (const IntermediateResult &res : results) {
+ DrawResult(res, {0, 255, 0});
+ DrawTarget(res, {0, 255, 0});
+ }
+ }
+
+ // If the target list is not empty then we found a target.
+ return !results.empty();
+ }
+
+ std::function<void(uint32_t)> RegisterKeyPress() override {
+ return [this](uint32_t key) {
+ (void)key;
+ if (key == 'z') {
+ draw_results_ = !draw_results_;
+ } else if (key == 'x') {
+ draw_raw_IR_ = !draw_raw_IR_;
+ } else if (key == 'c') {
+ draw_raw_target_ = !draw_raw_target_;
+ } else if (key == 'v') {
+ draw_components_ = !draw_components_;
+ } else if (key == 'b') {
+ draw_raw_poly_ = !draw_raw_poly_;
+ } else if (key == 'n') {
+ draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'q') {
+ printf("User requested shutdown.\n");
+ exit(0);
+ }
+ HandleBlobs(imgs_last_, fmt_last_);
+ viewer_->Redraw();
+ };
+ }
+
+ void DrawComponent(const TargetComponent &comp, PixelRef top_color,
+ PixelRef bot_color, PixelRef in_color,
+ PixelRef out_color) {
+ overlay_.AddLine(comp.top, comp.inside, top_color);
+ overlay_.AddLine(comp.bottom, comp.outside, bot_color);
+
+ overlay_.AddLine(comp.bottom, comp.inside, in_color);
+ overlay_.AddLine(comp.top, comp.outside, out_color);
+ }
+
+ void DrawTarget(const Target &target) {
+ Vector<2> leftTop = (target.left.top + target.left.inside) * 0.5;
+ Vector<2> rightTop = (target.right.top + target.right.inside) * 0.5;
+ overlay_.AddLine(leftTop, rightTop, {255, 215, 0});
+
+ Vector<2> leftBot = (target.left.bottom + target.left.outside) * 0.5;
+ Vector<2> rightBot = (target.right.bottom + target.right.outside) * 0.5;
+ overlay_.AddLine(leftBot, rightBot, {255, 215, 0});
+
+ overlay_.AddLine(leftTop, leftBot, {255, 215, 0});
+ overlay_.AddLine(rightTop, rightBot, {255, 215, 0});
+ }
+
+ void DrawResult(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ DrawComponent(target.left, color, color, color, color);
+ DrawComponent(target.right, color, color, color, color);
+ }
+
+ void DrawTarget(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
+ (target.left.bottom + target.left.outside) * 0.5);
+ leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
+ leftAx.B() * 0.9 + leftAx.A() * 0.1);
+ overlay_.AddLine(leftAx, color);
+
+ Segment<2> rightAx((target.right.top + target.right.inside) * 0.5,
+ (target.right.bottom + target.right.outside) * 0.5);
+ rightAx.Set(rightAx.A() * 0.9 + rightAx.B() * 0.1,
+ rightAx.B() * 0.9 + rightAx.A() * 0.1);
+ overlay_.AddLine(rightAx, color);
+
+ overlay_.AddLine(leftAx.A(), rightAx.A(), color);
+ overlay_.AddLine(leftAx.B(), rightAx.B(), color);
+ Vector<3> p1(0.0, 0.0, 100.0);
+
+ Vector<3> p2 =
+ Rotate(intrinsics().mount_angle, result.extrinsics.r1, 0.0, p1);
+ Vector<2> p3(p2.x(), p2.y());
+ overlay_.AddLine(leftAx.A(), p3 + leftAx.A(), {0, 255, 0});
+ overlay_.AddLine(leftAx.B(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(rightAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(rightAx.B(), p3 + rightAx.B(), {0, 255, 0});
+
+ overlay_.AddLine(p3 + leftAx.A(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(p3 + rightAx.A(), p3 + rightAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
+ }
+
+ const IntrinsicParams &intrinsics() const { return finder_.intrinsics(); }
+
+ private:
+ // implementation of the filter pipeline.
+ TargetFinder finder_;
+ aos::vision::BlobStreamViewer *viewer_ = nullptr;
+ aos::vision::PixelLinesOverlay overlay_;
+ std::vector<aos::vision::OverlayBase *> overlays_;
+ BlobList imgs_last_;
+ ImageFormat fmt_last_;
+ bool draw_select_blob_ = false;
+ bool draw_raw_poly_ = false;
+ bool draw_components_ = false;
+ bool draw_raw_target_ = false;
+ bool draw_raw_IR_ = false;
+ bool draw_results_ = true;
+};
+
+} // namespace vision
+} // namespace y2017
+
+int main(int argc, char **argv) {
+ y2019::vision::FilterHarness filter_harness;
+ aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
+ aos::vision::CameraParams());
+}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
new file mode 100644
index 0000000..f69e987
--- /dev/null
+++ b/y2019/vision/target_finder.cc
@@ -0,0 +1,379 @@
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
+using namespace aos::vision;
+
+namespace y2019 {
+namespace vision {
+
+TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+
+aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
+ const uint8_t threshold_value = GetThresholdValue();
+ return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
+ if (px.g > threshold_value && px.b > threshold_value &&
+ px.r > threshold_value) {
+ return true;
+ }
+ return false;
+ });
+}
+
+// Filter blobs on size.
+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());
+}
+
+// TODO: Try hierarchical merge for this.
+// Convert blobs into polygons.
+std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
+ const RangeImage &blob, bool verbose) {
+ if (verbose) printf("Process Polygon.\n");
+ alloc_.reset();
+ auto *st = RangeImgToContour(blob, &alloc_);
+
+ struct Pt {
+ float x;
+ float y;
+ };
+ std::vector<Pt> pts;
+
+ // Collect all slopes from the contour.
+ auto opt = st->pt;
+ for (auto *node = st; node->next != st;) {
+ node = node->next;
+
+ auto npt = node->pt;
+
+ pts.push_back(
+ {static_cast<float>(npt.x - opt.x), static_cast<float>(npt.y - opt.y)});
+
+ opt = npt;
+ }
+
+ const int n = pts.size();
+ auto get_pt = [&](int i) { return pts[(i + n * 2) % n]; };
+
+ std::vector<Pt> pts_new = pts;
+ auto run_box_filter = [&](int window_size) {
+ for (size_t i = 0; i < pts.size(); ++i) {
+ Pt a{0.0, 0.0};
+ for (int j = -window_size; j <= window_size; ++j) {
+ Pt p = get_pt(j + i);
+ a.x += p.x;
+ a.y += p.y;
+ }
+ a.x /= (window_size * 2 + 1);
+ a.y /= (window_size * 2 + 1);
+
+ float scale = 1.0 + (i / float(pts.size() * 10));
+ a.x *= scale;
+ a.y *= scale;
+ pts_new[i] = a;
+ }
+ pts = pts_new;
+ };
+ // Three box filter makith a guassian?
+ // Run gaussian filter over the slopes.
+ run_box_filter(2);
+ run_box_filter(2);
+ run_box_filter(2);
+
+ // Heuristic which says if a particular slope is part of a corner.
+ auto is_corner = [&](size_t i) {
+ Pt a = get_pt(i - 3);
+ Pt b = get_pt(i + 3);
+ double dx = (a.x - b.x);
+ double dy = (a.y - b.y);
+ return dx * dx + dy * dy > 0.25;
+ };
+
+ bool prev_v = is_corner(-1);
+
+ // Find all centers of corners.
+ // Because they round, multiple points may be a corner.
+ std::vector<size_t> edges;
+ size_t kBad = pts.size() + 10;
+ size_t prev_up = kBad;
+ size_t wrapped_n = prev_up;
+
+ for (size_t i = 0; i < pts.size(); ++i) {
+ bool v = is_corner(i);
+ if (prev_v && !v) {
+ if (prev_up == kBad) {
+ wrapped_n = i;
+ } else {
+ edges.push_back((prev_up + i - 1) / 2);
+ }
+ }
+ if (v && !prev_v) {
+ prev_up = i;
+ }
+ prev_v = v;
+ }
+
+ if (wrapped_n != kBad) {
+ edges.push_back(((prev_up + pts.size() + wrapped_n - 1) / 2) % pts.size());
+ }
+
+ if (verbose) printf("Edge Count (%zu).\n", edges.size());
+
+ // Get all CountourNodes from the contour.
+ using aos::vision::PixelRef;
+ std::vector<ContourNode *> segments;
+ {
+ std::vector<ContourNode *> segments_all;
+
+ for (ContourNode *node = st; node->next != st;) {
+ node = node->next;
+ segments_all.push_back(node);
+ }
+ for (size_t i : edges) {
+ segments.push_back(segments_all[i]);
+ }
+ }
+ if (verbose) printf("Segment Count (%zu).\n", segments.size());
+
+ // Run best-fits over each line segment.
+ std::vector<Segment<2>> seg_list;
+ if (segments.size() == 4) {
+ for (size_t i = 0; i < segments.size(); ++i) {
+ auto *ed = segments[(i + 1) % segments.size()];
+ auto *st = segments[i];
+ float mx = 0.0;
+ float my = 0.0;
+ int n = 0;
+ for (auto *node = st; node != ed; node = node->next) {
+ mx += node->pt.x;
+ my += node->pt.y;
+ ++n;
+ // (x - [x] / N) ** 2 = [x * x] - 2 * [x] * [x] / N + [x] * [x] / N / N;
+ }
+ mx /= n;
+ my /= n;
+
+ float xx = 0.0;
+ float xy = 0.0;
+ float yy = 0.0;
+ for (auto *node = st; node != ed; node = node->next) {
+ float x = node->pt.x - mx;
+ float y = node->pt.y - my;
+ xx += x * x;
+ xy += x * y;
+ yy += y * y;
+ }
+
+ // TODO: Extract common to hierarchical merge.
+ float neg_b_over_2 = (xx + yy) / 2.0;
+ float c = (xx * yy - xy * xy);
+
+ float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
+
+ {
+ float lam = neg_b_over_2 + sqr;
+ float x = xy;
+ float y = lam - xx;
+
+ float norm = sqrt(x * x + y * y);
+ x /= norm;
+ y /= norm;
+
+ seg_list.push_back(
+ Segment<2>(Vector<2>(mx, my), Vector<2>(mx + x, my + y)));
+ }
+
+ /* Characteristic polynomial
+ 1 lam^2 - (xx + yy) lam + (xx * yy - xy * xy) = 0
+
+ [a b]
+ [c d]
+
+ // covariance matrix.
+ [xx xy] [nx]
+ [xy yy] [ny]
+ */
+ }
+ }
+ if (verbose) printf("Poly Count (%zu).\n", seg_list.size());
+ return seg_list;
+}
+
+// Convert segments into target components (left or right)
+std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+ const std::vector<std::vector<Segment<2>>> &seg_list) {
+ std::vector<TargetComponent> list;
+ TargetComponent new_target;
+ for (const auto &poly : seg_list) {
+ // Reject missized pollygons for now. Maybe rectify them here in the future;
+ if (poly.size() != 4) continue;
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(poly[i].Intersect(poly[(i + 1) % 4]));
+ }
+
+ // Select the closest two points. Short side of the rectangle.
+ double min_dist = -1;
+ std::pair<size_t, size_t> closest;
+ for (size_t i = 0; i < 4; ++i) {
+ size_t next = (i + 1) % 4;
+ double nd = corners[i].SquaredDistanceTo(corners[next]);
+ if (min_dist == -1 || nd < min_dist) {
+ min_dist = nd;
+ closest.first = i;
+ closest.second = next;
+ }
+ }
+
+ // Verify our top is above the bottom.
+ size_t bot_index = closest.first;
+ size_t top_index = (closest.first + 2) % 4;
+ if (corners[top_index].y() < corners[bot_index].y()) {
+ closest.first = top_index;
+ closest.second = (top_index + 1) % 4;
+ }
+
+ // Find the major axis.
+ size_t far_first = (closest.first + 2) % 4;
+ size_t far_second = (closest.second + 2) % 4;
+ Segment<2> major_axis(
+ (corners[closest.first] + corners[closest.second]) * 0.5,
+ (corners[far_first] + corners[far_second]) * 0.5);
+ if (major_axis.AsVector().AngleToZero() > M_PI / 180.0 * 120.0 ||
+ major_axis.AsVector().AngleToZero() < M_PI / 180.0 * 60.0) {
+ // Target is angled way too much, drop it.
+ continue;
+ }
+
+ // organize the top points.
+ Vector<2> topA = corners[closest.first] - major_axis.B();
+ new_target.major_axis = major_axis;
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a left target since we are leaning positive.
+ new_target.is_right = false;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ }
+ } else {
+ // We have a right target since we are leaning negative.
+ new_target.is_right = true;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ }
+ }
+
+ // organize the top points.
+ Vector<2> botA = corners[far_first] - major_axis.A();
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a right target since we are leaning positive.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ }
+ } else {
+ // We have a left target since we are leaning negative.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ }
+ }
+
+ // This piece of the target should be ready now.
+ list.emplace_back(new_target);
+ }
+
+ return list;
+}
+
+// Match components into targets.
+std::vector<Target> TargetFinder::FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose) {
+ std::vector<Target> target_list;
+ using namespace aos::vision;
+ if (component_list.size() < 2) {
+ // We don't enough parts for a target.
+ return target_list;
+ }
+
+ for (size_t i = 0; i < component_list.size(); i++) {
+ const TargetComponent &a = component_list[i];
+ for (size_t j = 0; j < i; j++) {
+ bool target_valid = false;
+ Target new_target;
+ const TargetComponent &b = component_list[j];
+
+ // Reject targets that are too far off vertically.
+ Vector<2> a_center = a.major_axis.Center();
+ if (a_center.y() > b.bottom.y() || a_center.y() < b.top.y()) {
+ continue;
+ }
+ Vector<2> b_center = b.major_axis.Center();
+ if (b_center.y() > a.bottom.y() || b_center.y() < a.top.y()) {
+ continue;
+ }
+
+ if (a.is_right && !b.is_right) {
+ if (a.top.x() > b.top.x()) {
+ new_target.right = a;
+ new_target.left = b;
+ target_valid = true;
+ }
+ } else if (!a.is_right && b.is_right) {
+ if (b.top.x() > a.top.x()) {
+ new_target.right = b;
+ new_target.left = a;
+ target_valid = true;
+ }
+ }
+ if (target_valid) {
+ target_list.emplace_back(new_target);
+ }
+ }
+ }
+ if (verbose) printf("Possible Target: %zu.\n", target_list.size());
+ return target_list;
+}
+
+std::vector<IntermediateResult> TargetFinder::FilterResults(
+ const std::vector<IntermediateResult> &results) {
+ std::vector<IntermediateResult> filtered;
+ for (const IntermediateResult &res : results) {
+ if (res.solver_error < 75.0) {
+ filtered.emplace_back(res);
+ }
+ }
+ return filtered;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
new file mode 100644
index 0000000..633733a
--- /dev/null
+++ b/y2019/vision/target_finder.h
@@ -0,0 +1,80 @@
+#ifndef _Y2019_VISION_TARGET_FINDER_H_
+#define _Y2019_VISION_TARGET_FINDER_H_
+
+#include "aos/vision/blob/region_alloc.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"
+#include "y2019/vision/target_types.h"
+
+namespace y2019 {
+namespace vision {
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+class TargetFinder {
+ public:
+ TargetFinder();
+ // Turn a raw image into blob range image.
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+ // Value against which we threshold.
+ uint8_t GetThresholdValue() { return 120; }
+
+ // filter out obvious or durranged blobs.
+ void PreFilter(BlobList *imgs);
+
+ // Turn a blob into a polgygon.
+ std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ bool verbose);
+
+ // Turn a bloblist into components of a target.
+ std::vector<TargetComponent> FillTargetComponentList(
+ const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list);
+
+ // Piece the compenents together into a target.
+ std::vector<Target> FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose);
+
+ // Given a target solve for the transformation of the template target.
+ IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
+
+ std::vector<IntermediateResult> FilterResults(
+ const std::vector<IntermediateResult> &results);
+
+ // Get the local overlay for debug if we are doing that.
+ aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+ // Convert target location into meters and radians.
+ void GetAngleDist(const aos::vision::Vector<2> &target, double down_angle,
+ double *dist, double *angle);
+
+ // Return the template target in a normalized space.
+ const Target &GetTemplateTarget() { return target_template_; }
+
+ const IntrinsicParams &intrinsics() const { return intrinsics_; }
+
+ private:
+ // Find a loosly connected target.
+ double DetectConnectedTarget(const RangeImage &img);
+
+ aos::vision::PixelLinesOverlay overlay_;
+
+ aos::vision::AnalysisAllocator alloc_;
+
+ // The template for the default target in the standard space.
+ Target target_template_;
+
+ IntrinsicParams intrinsics_;
+
+ ExtrinsicParams default_extrinsics_;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
new file mode 100644
index 0000000..77adc83
--- /dev/null
+++ b/y2019/vision/target_geometry.cc
@@ -0,0 +1,186 @@
+#include "y2019/vision/target_finder.h"
+
+#include "ceres/ceres.h"
+
+#include <math.h>
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+static constexpr double kInchesToMeters = 0.0254;
+
+using namespace aos::vision;
+using aos::vision::Vector;
+
+Target Target::MakeTemplate() {
+ Target out;
+ // This is how off-vertical the tape is.
+ const double theta = 14.5 * M_PI / 180.0;
+
+ const double tape_offset = 4 * kInchesToMeters;
+ const double tape_width = 2 * kInchesToMeters;
+ const double tape_length = 5.5 * kInchesToMeters;
+
+ const double s = sin(theta);
+ const double c = cos(theta);
+ out.right.top = Vector<2>(tape_offset, 0.0);
+ out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
+ out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
+ tape_width * s - tape_length * c);
+ out.right.outside =
+ Vector<2>(tape_offset + tape_length * s, -tape_length * c);
+
+ out.right.is_right = true;
+ out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
+ out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
+ out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
+ out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
+ return out;
+}
+
+std::array<Vector<2>, 8> Target::toPointList() const {
+ return std::array<Vector<2>, 8>{{right.top, right.inside, right.bottom,
+ right.outside, left.top, left.inside,
+ left.bottom, left.outside}};
+}
+
+Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ double y = extrinsics.y;
+ double z = extrinsics.z;
+ double r1 = extrinsics.r1;
+ double r2 = extrinsics.r2;
+ double rup = intrinsics.mount_angle;
+ double fl = intrinsics.focal_length;
+
+ ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
+
+ {
+ double theta = r1;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ pts(2) += z;
+
+ {
+ double theta = r2;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ double theta = rup;
+ double s = sin(theta);
+ double c = cos(theta);
+
+ pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Final image projection.
+ ::Eigen::Matrix<double, 1, 3> res = pts;
+
+ float scale = fl / res.z();
+ return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
+}
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ auto project = [&](Vector<2> pt) {
+ return Project(pt, intrinsics, extrinsics);
+ };
+ Target new_targ;
+ new_targ.right.is_right = true;
+ new_targ.right.top = project(target.right.top);
+ new_targ.right.inside = project(target.right.inside);
+ new_targ.right.bottom = project(target.right.bottom);
+ new_targ.right.outside = project(target.right.outside);
+
+ new_targ.left.top = project(target.left.top);
+ new_targ.left.inside = project(target.left.inside);
+ new_targ.left.bottom = project(target.left.bottom);
+ new_targ.left.outside = project(target.left.outside);
+
+ return new_targ;
+}
+
+// Used at runtime on a single image given camera parameters.
+struct RuntimeCostFunctor {
+ RuntimeCostFunctor(Vector<2> result, Vector<2> template_pt,
+ IntrinsicParams intrinsics)
+ : result(result), template_pt(template_pt), intrinsics(intrinsics) {}
+
+ bool operator()(const double *const x, double *residual) const {
+ auto extrinsics = ExtrinsicParams::get(x);
+ auto pt = result - Project(template_pt, intrinsics, extrinsics);
+ residual[0] = pt.x();
+ residual[1] = pt.y();
+ return true;
+ }
+
+ Vector<2> result;
+ Vector<2> template_pt;
+ IntrinsicParams intrinsics;
+};
+
+IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
+ bool verbose) {
+ // Memory for the ceres solver.
+ double params[ExtrinsicParams::kNumParams];
+ default_extrinsics_.set(¶ms[0]);
+
+ Problem problem;
+
+ auto target_value = target.toPointList();
+ auto template_value = target_template_.toPointList();
+
+ for (size_t i = 0; i < 8; ++i) {
+ auto a = template_value[i];
+ auto b = target_value[i];
+
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
+ new RuntimeCostFunctor(b, a, intrinsics_)),
+ NULL, ¶ms[0]);
+ }
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = false;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+ IntermediateResult IR;
+ IR.extrinsics = ExtrinsicParams::get(¶ms[0]);
+ IR.solver_error = summary.final_cost;
+
+ if (verbose) {
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
+ std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
+ std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
+ std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "error = " << summary.final_cost << ";\n";
+ }
+ return IR;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
new file mode 100644
index 0000000..0d53e2c
--- /dev/null
+++ b/y2019/vision/target_sender.cc
@@ -0,0 +1,136 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.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/reader.h"
+
+#include "y2019/jevois/serial.h"
+#include "y2019/vision/target_finder.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2019::jevois::open_via_terminos;
+using aos::vision::Segment;
+
+class CameraStream : public ::aos::vision::ImageStreamEvent {
+ public:
+ CameraStream(::aos::vision::CameraParams params, const ::std::string &fname)
+ : ImageStreamEvent(fname, params) {}
+
+ void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
+ LOG(INFO, "got frame: %d\n", (int)data.size());
+
+ static unsigned i = 0;
+
+ /*
+ std::ofstream ofs(std::string("/jevois/data/debug_viewer_jpeg_") +
+ std::to_string(i) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ */
+ if (on_frame) on_frame(data, monotonic_now);
+ ++i;
+
+ if (i == 200) exit(-1);
+ }
+
+ std::function<void(DataRef, monotonic_clock::time_point)> on_frame;
+};
+
+int open_terminos(const char *tty_name) { return open_via_terminos(tty_name); }
+
+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);
+}
+
+int main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ int itsDev = open_terminos("/dev/ttyS0");
+ dup2(itsDev, 1);
+ dup2(itsDev, 2);
+
+ TargetFinder finder_;
+
+ // Check that our current results match possible solutions.
+ aos::vision::CameraParams params0;
+ params0.set_exposure(0);
+ params0.set_brightness(40);
+ params0.set_width(640);
+ // params0.set_fps(10);
+ params0.set_height(480);
+
+ ::std::unique_ptr<CameraStream> camera0(
+ new CameraStream(params0, "/dev/video0"));
+ camera0->on_frame = [&](DataRef data,
+ monotonic_clock::time_point /*monotonic_now*/) {
+ aos::vision::ImageFormat fmt{640, 480};
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+ finder_.PreFilter(&imgs);
+
+ bool verbose = false;
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ if (polygon.empty()) {
+ } else {
+ raw_polys.push_back(polygon);
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list =
+ finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
+ }
+
+ results = finder_.FilterResults(results);
+ };
+
+ aos::events::EpollLoop loop;
+
+ for (int i = 0; i < 100; ++i) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ camera0->ReadEvent();
+ }
+
+ // TODO: Fix event loop on jevois:
+ // loop.Add(camera0.get());
+ // loop.Run();
+}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
new file mode 100644
index 0000000..557b04a
--- /dev/null
+++ b/y2019/vision/target_types.h
@@ -0,0 +1,128 @@
+#ifndef _Y2019_VISION_TARGET_TYPES_H_
+#define _Y2019_VISION_TARGET_TYPES_H_
+
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace y2019 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+ aos::vision::Vector<2> GetByIndex(size_t i) const {
+ switch (i) {
+ case 0:
+ return top;
+ case 1:
+ return inside;
+ case 2:
+ return bottom;
+ case 3:
+ return outside;
+ default:
+ return aos::vision::Vector<2>();
+ }
+ }
+ bool is_right;
+ aos::vision::Vector<2> top;
+ aos::vision::Vector<2> inside;
+ aos::vision::Vector<2> outside;
+ aos::vision::Vector<2> bottom;
+
+ aos::vision::Segment<2> major_axis;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+ TargetComponent left;
+ TargetComponent right;
+
+ static Target MakeTemplate();
+ // Get the points in some order (will match against the template).
+ std::array<aos::vision::Vector<2>, 8> toPointList() const;
+};
+
+struct IntrinsicParams {
+ static constexpr size_t kNumParams = 2;
+
+ double mount_angle = 10.0481 / 180.0 * M_PI;
+ double focal_length = 729.445;
+
+ void set(double *data) {
+ data[0] = mount_angle;
+ data[1] = focal_length;
+ }
+ static IntrinsicParams get(const double *data) {
+ IntrinsicParams out;
+ out.mount_angle = data[0];
+ out.focal_length = data[1];
+ return out;
+ }
+};
+
+struct ExtrinsicParams {
+ static constexpr size_t kNumParams = 4;
+
+ double y = 18.0 * 0.0254;
+ double z = 23.0 * 0.0254;
+ double r1 = 1.0 / 180 * M_PI;
+ double r2 = -1.0 / 180 * M_PI;
+
+ void set(double *data) {
+ data[0] = y;
+ data[1] = z;
+ data[2] = r1;
+ data[3] = r2;
+ }
+ static ExtrinsicParams get(const double *data) {
+ ExtrinsicParams out;
+ out.y = data[0];
+ out.z = data[1];
+ out.r1 = data[2];
+ out.r2 = data[3];
+ return out;
+ }
+};
+// Projects a point from idealized template space to camera space.
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+// An intermediate for of the results.
+// These are actual awnsers, but in a form from the solver that is not ready for
+// the wire.
+struct IntermediateResult {
+ ExtrinsicParams extrinsics;
+
+ // Error from solver calulations.
+ double solver_error;
+};
+
+// Final foramtting ready for output on the wire.
+struct TargetResult {
+ // Distance to the target in meters. Specifically, the distance from the
+ // center of the camera's image plane to the center of the target.
+ float distance;
+ // Height of the target in meters. Specifically, the distance from the floor
+ // to the center of the target.
+ float height;
+
+ // Heading of the center of the target in radians. Zero is straight out
+ // perpendicular to the camera's image plane. Images to the left (looking at a
+ // camera image) are at a positive angle.
+ float heading;
+
+ // The angle between the target and the camera's image plane. This is
+ // projected so both are assumed to be perpendicular to the floor. Parallel
+ // targets have a skew of zero. Targets rotated such that their left edge
+ // (looking at a camera image) is closer are at a positive angle.
+ float skew;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 428e723..339a388 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -31,6 +31,8 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
@@ -40,6 +42,7 @@
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
+#include "third_party/Phoenix-frc-lib/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
@@ -93,11 +96,11 @@
double elevator_pot_translate(double voltage) {
return voltage * Values::kElevatorPotRatio() *
- (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
double wrist_pot_translate(double voltage) {
- return voltage * Values::kWristPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ return voltage * Values::kWristPotRatio() * (5.0 /*turns*/ / 5.0 /*volts*/) *
(2 * M_PI /*radians*/);
}
@@ -111,10 +114,10 @@
Values::kMaxIntakeEncoderPulsesPerSecond());
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
-
constexpr double kMaxMediumEncoderPulsesPerSecond =
max(Values::kMaxElevatorEncoderPulsesPerSecond(),
Values::kMaxWristEncoderPulsesPerSecond());
+
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");
@@ -206,9 +209,6 @@
drivetrain_message.Send();
}
- }
-
- void RunDmaIteration() {
const auto values = constants::GetValues();
{
@@ -257,9 +257,6 @@
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
intake_victor_ = ::std::move(t);
}
- void set_intake_rollers_victor(::std::unique_ptr<::frc::VictorSP> t) {
- intake_rollers_victor_ = ::std::move(t);
- }
void set_wrist_victor(::std::unique_ptr<::frc::VictorSP> t) {
wrist_victor_ = ::std::move(t);
@@ -279,25 +276,20 @@
auto &queue =
::y2019::control_loops::superstructure::superstructure_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- elevator_victor_->SetSpeed(::aos::Clip(-queue->elevator_voltage,
+ elevator_victor_->SetSpeed(::aos::Clip(queue->elevator_voltage,
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- intake_victor_->SetSpeed(::aos::Clip(-queue->intake_joint_voltage,
+ intake_victor_->SetSpeed(::aos::Clip(queue->intake_joint_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- intake_rollers_victor_->SetSpeed(::aos::Clip(-queue->intake_roller_voltage,
- -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
-
wrist_victor_->SetSpeed(::aos::Clip(-queue->wrist_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- stilts_victor_->SetSpeed(::aos::Clip(-queue->stilts_voltage,
+ stilts_victor_->SetSpeed(::aos::Clip(queue->stilts_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
}
@@ -307,13 +299,92 @@
elevator_victor_->SetDisabled();
intake_victor_->SetDisabled();
- intake_rollers_victor_->SetDisabled();
wrist_victor_->SetDisabled();
stilts_victor_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
- intake_rollers_victor_, wrist_victor_, stilts_victor_;
+ wrist_victor_, stilts_victor_;
+};
+
+class SolenoidWriter {
+ public:
+ SolenoidWriter()
+ : superstructure_(
+ ".y2019.control_loops.superstructure.superstructure_queue.output") {
+ }
+
+ void set_big_suction_cup(int index) {
+ big_suction_cup_ = pcm_.MakeSolenoid(index);
+ }
+ void set_small_suction_cup(int index) {
+ small_suction_cup_ = pcm_.MakeSolenoid(index);
+ }
+
+ void set_intake_roller_talon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX> t) {
+ intake_rollers_talon_ = ::std::move(t);
+ intake_rollers_talon_->ConfigContinuousCurrentLimit(40.0, 0);
+ intake_rollers_talon_->EnableCurrentLimit(true);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("Solenoids");
+ ::aos::SetCurrentThreadRealtimePriority(27);
+
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
+ }
+ }
+
+ {
+ superstructure_.FetchLatest();
+ if (superstructure_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+
+ big_suction_cup_->Set(!superstructure_->intake_suction_top);
+ small_suction_cup_->Set(!superstructure_->intake_suction_bottom);
+
+ intake_rollers_talon_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ ::aos::Clip(superstructure_->intake_roller_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
+ }
+
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+
+ pcm_.Flush();
+ to_log.read_solenoids = pcm_.GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
+ }
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ ::frc971::wpilib::BufferedPcm pcm_;
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup_,
+ small_suction_cup_;
+
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
+ intake_rollers_talon_;
+
+ ::aos::Queue<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Output>
+ superstructure_;
+
+ ::std::atomic<bool> run_{true};
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -334,34 +405,33 @@
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
SensorReader reader;
- // TODO(Sabina): Update port numbers(Sensors and Victors)
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_elevator_encoder(make_encoder(2));
- reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(0));
- reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(0));
+ reader.set_elevator_encoder(make_encoder(4));
+ reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(4));
+ reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(4));
- reader.set_wrist_encoder(make_encoder(3));
- reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(1));
- reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(2));
+ reader.set_wrist_encoder(make_encoder(5));
+ reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(5));
+ reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(5));
- reader.set_intake_encoder(make_encoder(4));
- reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ reader.set_intake_encoder(make_encoder(2));
+ reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(2));
- reader.set_stilts_encoder(make_encoder(5));
- reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ reader.set_stilts_encoder(make_encoder(3));
+ reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(3));
reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
::std::thread reader_thread(::std::ref(reader));
- auto imu_trigger = make_unique<frc::DigitalInput>(5);
+ auto imu_trigger = make_unique<frc::DigitalInput>(0);
::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
imu.SetDummySPI(frc::SPI::Port::kOnboardCS2);
- auto imu_reset = make_unique<frc::DigitalOutput>(6);
+ auto imu_reset = make_unique<frc::DigitalOutput>(1);
imu.set_reset(imu_reset.get());
::std::thread imu_thread(::std::ref(imu));
@@ -378,19 +448,28 @@
SuperstructureWriter superstructure_writer;
superstructure_writer.set_elevator_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- superstructure_writer.set_intake_rollers_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
- superstructure_writer.set_intake_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
+ // TODO(austin): Do the vacuum
+ //superstructure_writer.set_vacuum(
+ //::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+ superstructure_writer.set_intake_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
superstructure_writer.set_wrist_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
superstructure_writer.set_stilts_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));
+ SolenoidWriter solenoid_writer;
+ solenoid_writer.set_intake_roller_talon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
+ solenoid_writer.set_big_suction_cup(0);
+ solenoid_writer.set_small_suction_cup(1);
+
+ ::std::thread solenoid_writer_thread(::std::ref(solenoid_writer));
+
// Wait forever. Not much else to do...
while (true) {
const int r = select(0, nullptr, nullptr, nullptr, nullptr);
@@ -403,6 +482,8 @@
LOG(ERROR, "Exiting WPILibRobot\n");
+ solenoid_writer.Quit();
+ solenoid_writer_thread.join();
joystick_sender.Quit();
joystick_thread.join();
pdp_fetcher.Quit();