Merge "Add imu to /etc/hosts as pi5"
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index e9f8262..9fdc93f 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -46,6 +46,9 @@
hdrs = [
"sized_array.h",
],
+ deps = [
+ "@com_google_absl//absl/container:inlined_vector",
+ ],
)
cc_test(
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 4264cf6..8a8d2de 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -1,134 +1,28 @@
#ifndef AOS_CONTAINERS_SIZED_ARRAY_H_
#define AOS_CONTAINERS_SIZED_ARRAY_H_
-#include <array>
-#include <cstddef>
+#include "absl/container/inlined_vector.h"
namespace aos {
-// An array along with a variable size. This is a simple variable-size container
-// with a fixed maximum size.
-//
-// Note that it default-constructs N T instances at construction time. This
-// simplifies the internal bookkeeping a lot (I believe this can be
-// all-constexpr in C++17), but makes it a poor choice for complex T.
-template <typename T, size_t N>
-class SizedArray {
- private:
- using array = std::array<T, N>;
-
+// Minimal compliant allocator whose allocating operations are all fatal.
+template <typename T>
+class FatalAllocator {
public:
- using value_type = typename array::value_type;
- using size_type = typename array::size_type;
- using difference_type = typename array::difference_type;
- using reference = typename array::reference;
- using const_reference = typename array::const_reference;
- using pointer = typename array::pointer;
- using const_pointer = typename array::const_pointer;
- using iterator = typename array::iterator;
- using const_iterator = typename array::const_iterator;
- using reverse_iterator = typename array::reverse_iterator;
- using const_reverse_iterator = typename array::const_reverse_iterator;
+ using value_type = T;
- constexpr SizedArray() = default;
- SizedArray(const SizedArray &) = default;
- SizedArray(SizedArray &&) = default;
- SizedArray &operator=(const SizedArray &) = default;
- SizedArray &operator=(SizedArray &&) = default;
+ [[nodiscard, noreturn]] T *allocate(std::size_t) { __builtin_trap(); }
- bool operator==(const SizedArray &other) const {
- if (other.size() != size()) {
- return false;
- }
- for (size_t i = 0; i < size(); ++i) {
- if (other[i] != (*this)[i]) {
- return false;
- }
- }
- return true;
- }
- bool operator!=(const SizedArray &other) const {
- return !(*this == other);
- }
-
- reference at(size_t i) {
- check_index(i);
- return array_.at(i);
- }
- const_reference at(size_t i) const {
- check_index(i);
- return array_.at(i);
- }
-
- reference operator[](size_t i) { return array_[i]; }
- const_reference operator[](size_t i) const { return array_[i]; }
-
- reference front() { return array_.front(); }
- const_reference front() const { return array_.front(); }
-
- reference back() { return array_[size_ - 1]; }
- const_reference back() const { return array_[size_ - 1]; }
-
- T *data() { return array_.data(); }
- const T *data() const { return array_.data(); }
-
- iterator begin() { return array_.begin(); }
- const_iterator begin() const { return array_.begin(); }
- const_iterator cbegin() const { return array_.cbegin(); }
-
- iterator end() { return array_.begin() + size_; }
- const_iterator end() const { return array_.begin() + size_; }
- const_iterator cend() const { return array_.cbegin() + size_; }
-
- reverse_iterator rbegin() { return array_.rend() - size_; }
- const_reverse_iterator rbegin() const { return array_.rend() - size_; }
- const_reverse_iterator crbegin() const { return array_.crend() - size_; }
-
- reverse_iterator rend() { return array_.rend(); }
- const_reverse_iterator rend() const { return array_.rend(); }
- const_reverse_iterator crend() const { return array_.crend(); }
-
- bool empty() const { return size_ == 0; }
- bool full() const { return size() == max_size(); }
-
- size_t size() const { return size_; }
- constexpr size_t max_size() const { return array_.max_size(); }
-
- void push_back(const T &t) {
- array_.at(size_) = t;
- ++size_;
- }
- void push_back(T &&t) {
- array_.at(size_) = std::move(t);
- ++size_;
- }
-
- void pop_back() {
- if (empty()) {
- __builtin_trap();
- }
- --size_;
- }
-
- void clear() { size_ = 0; }
-
- // These allow access to the underlying storage. The data here may be outside
- // the current logical extents of the container.
- const array &backing_array() const { return array_; }
- array *mutable_backing_array() { return &array_; }
- void set_size(size_t size) { size_ = size; }
-
- private:
- void check_index(size_t i) const {
- if (i >= size_) {
- __builtin_trap();
- }
- }
-
- array array_;
- size_t size_ = 0;
+ [[noreturn]] void deallocate(T *, std::size_t) { __builtin_trap(); }
};
+// Reuse the logic from absl::InlinedVector for a statically allocated,
+// dynamically sized list of values. InlinedVector's default behavior is to
+// allocate from the heap when growing beyond the static capacity, make this
+// fatal instead to enforce RT guarantees.
+template <typename T, size_t N>
+using SizedArray = absl::InlinedVector<T, N, FatalAllocator<T>>;
+
} // namespace aos
#endif // AOS_CONTAINERS_SIZED_ARRAY_H_
diff --git a/aos/containers/sized_array_test.cc b/aos/containers/sized_array_test.cc
index bfaba06..d182ad2 100644
--- a/aos/containers/sized_array_test.cc
+++ b/aos/containers/sized_array_test.cc
@@ -32,39 +32,39 @@
TEST(SizedArrayTest, Accessors) {
SizedArray<int, 5> a;
EXPECT_TRUE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
EXPECT_EQ(0u, a.size());
- EXPECT_EQ(5u, a.max_size());
+ EXPECT_EQ(5u, a.capacity());
a.push_back(9);
EXPECT_FALSE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
EXPECT_EQ(1u, a.size());
- EXPECT_EQ(5u, a.max_size());
+ EXPECT_EQ(5u, a.capacity());
a.push_back(9);
EXPECT_FALSE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
EXPECT_EQ(2u, a.size());
- EXPECT_EQ(5u, a.max_size());
+ EXPECT_EQ(5u, a.capacity());
a.push_back(9);
EXPECT_FALSE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
EXPECT_EQ(3u, a.size());
- EXPECT_EQ(5u, a.max_size());
+ EXPECT_EQ(5u, a.capacity());
a.push_back(9);
EXPECT_FALSE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
EXPECT_EQ(4u, a.size());
- EXPECT_EQ(5u, a.max_size());
+ EXPECT_EQ(5u, a.capacity());
a.push_back(9);
EXPECT_FALSE(a.empty());
- EXPECT_TRUE(a.full());
+ EXPECT_EQ(a.size(), a.capacity());
EXPECT_EQ(5u, a.size());
- EXPECT_EQ(5u, a.max_size());
+ EXPECT_EQ(5u, a.capacity());
}
// Tests the various kinds of iterator.
@@ -139,20 +139,35 @@
TEST(SizedArrayTest, FillEmpty) {
SizedArray<int, 2> a;
EXPECT_TRUE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
a.push_back(9);
EXPECT_FALSE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
a.push_back(7);
EXPECT_FALSE(a.empty());
- EXPECT_TRUE(a.full());
+ EXPECT_EQ(a.size(), a.capacity());
a.clear();
EXPECT_TRUE(a.empty());
- EXPECT_FALSE(a.full());
+ EXPECT_NE(a.size(), a.capacity());
a.push_back(1);
EXPECT_EQ(1, a.back());
}
+TEST(SizedArrayTest, OverflowTest) {
+ SizedArray<int, 4> a;
+ EXPECT_EQ(a.capacity(), 4u);
+ EXPECT_TRUE(a.empty());
+
+ const int* const pre_front = a.data();
+ a.assign({1, 2, 3, 4});
+
+ EXPECT_EQ(a.capacity(), 4u);
+ // Verify that we didn't reallocate
+ EXPECT_EQ(pre_front, a.data());
+
+ EXPECT_DEATH(a.emplace_back(5), "SIGILL");
+}
+
} // namespace testing
} // namespace aos
diff --git a/motors/peripheral/uart.cc b/motors/peripheral/uart.cc
index 6e48320..0617e3f 100644
--- a/motors/peripheral/uart.cc
+++ b/motors/peripheral/uart.cc
@@ -70,7 +70,7 @@
// 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()) {
+ while (DataAvailable() && result.size() != result.capacity()) {
result.push_back(ReadCharacter());
}
return result;
diff --git a/motors/print/usb.h b/motors/print/usb.h
index 1ed1bee..c2224d2 100644
--- a/motors/print/usb.h
+++ b/motors/print/usb.h
@@ -30,7 +30,7 @@
aos::SizedArray<char, 4> ReadStdin() override {
aos::SizedArray<char, 4> result;
- result.set_size(stdout_tty_->Read(result.data(), result.max_size()));
+ result.resize(stdout_tty_->Read(result.data(), result.capacity()));
return result;
}
@@ -58,7 +58,7 @@
aos::SizedArray<char, 4> ReadStdin() override {
aos::SizedArray<char, 4> result;
- result.set_size(stdout_tty_.Read(result.data(), result.max_size()));
+ result.resize(stdout_tty_.Read(result.data(), result.capacity()));
return result;
}
diff --git a/y2019/jevois/cobs.h b/y2019/jevois/cobs.h
index 979cc15..c429622 100644
--- a/y2019/jevois/cobs.h
+++ b/y2019/jevois/cobs.h
@@ -23,9 +23,8 @@
// output_buffer is where to store the result.
// Returns a span in output_buffer which has no 0 bytes.
template <size_t max_decoded_size>
-absl::Span<char> CobsEncode(
- absl::Span<const char> input,
- std::array<char, CobsMaxEncodedSize(max_decoded_size)> *output_buffer);
+absl::Span<char> CobsEncode(absl::Span<const char> input,
+ absl::Span<char> output_buffer);
// Decodes some COBS-encoded data.
// input is the data to decide. Its size may be at most
@@ -90,20 +89,19 @@
};
template <size_t max_decoded_size>
-absl::Span<char> CobsEncode(
- absl::Span<const char> input,
- std::array<char, CobsMaxEncodedSize(max_decoded_size)> *output_buffer) {
+absl::Span<char> CobsEncode(absl::Span<const char> input,
+ absl::Span<char> output_buffer) {
static_assert(max_decoded_size > 0, "Empty buffers not supported");
if (static_cast<size_t>(input.size()) > max_decoded_size) {
__builtin_trap();
}
auto input_pointer = input.begin();
- auto output_pointer = output_buffer->begin();
+ auto output_pointer = output_buffer.begin();
auto code_pointer = output_pointer;
++output_pointer;
uint8_t code = 1;
while (input_pointer < input.end()) {
- if (output_pointer >= output_buffer->end()) {
+ if (output_pointer >= output_buffer.end()) {
__builtin_trap();
}
if (*input_pointer == 0u) {
@@ -125,11 +123,11 @@
++input_pointer;
}
*code_pointer = code;
- if (output_pointer > output_buffer->end()) {
+ if (output_pointer > output_buffer.end()) {
__builtin_trap();
}
- return absl::Span<char>(*output_buffer)
- .subspan(0, output_pointer - output_buffer->begin());
+ return absl::Span<char>(output_buffer)
+ .subspan(0, output_pointer - output_buffer.begin());
}
template <size_t max_decoded_size>
diff --git a/y2019/jevois/cobs_test.cc b/y2019/jevois/cobs_test.cc
index fd312f0..fa4742f 100644
--- a/y2019/jevois/cobs_test.cc
+++ b/y2019/jevois/cobs_test.cc
@@ -32,8 +32,8 @@
template <size_t max_decoded_size>
void EncodeAndDecode(const absl::Span<const char> decoded_input) {
std::array<char, CobsMaxEncodedSize(max_decoded_size)> encoded_buffer;
- const auto encoded =
- CobsEncode<max_decoded_size>(decoded_input, &encoded_buffer);
+ const auto encoded = CobsEncode<max_decoded_size>(
+ decoded_input, absl::Span<char>(encoded_buffer));
ASSERT_LE(encoded.size(), encoded_buffer.size());
ASSERT_EQ(encoded.data(), &encoded_buffer.front());
diff --git a/y2019/jevois/serial.cc b/y2019/jevois/serial.cc
index 5febbb4..cd0bb99 100644
--- a/y2019/jevois/serial.cc
+++ b/y2019/jevois/serial.cc
@@ -1,13 +1,13 @@
#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>
+#include "aos/logging/logging.h"
+
namespace y2019 {
namespace jevois {
@@ -33,7 +33,7 @@
| 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;
@@ -42,7 +42,7 @@
| 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);
diff --git a/y2019/jevois/serial.h b/y2019/jevois/serial.h
index 9dd4b0c..14e2f98 100644
--- a/y2019/jevois/serial.h
+++ b/y2019/jevois/serial.h
@@ -7,6 +7,6 @@
int open_via_terminos(const char *tty_name);
} // namespace jevois
-} // namespace frc971
+} // namespace y2019
#endif // Y2019_JEVOIS_SERIAL_H_
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index 2857bd6..9cded62 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -49,8 +49,9 @@
}
AOS_CHECK(remaining_space.empty());
UartToTeensyBuffer result;
- result.set_size(
- CobsEncode<uart_to_teensy_size()>(buffer, result.mutable_backing_array())
+ result.resize(result.capacity());
+ result.resize(
+ CobsEncode<uart_to_teensy_size()>(buffer, absl::Span<char>(result))
.size());
return result;
}
@@ -137,8 +138,9 @@
}
AOS_CHECK(remaining_space.empty());
UartToCameraBuffer result;
- result.set_size(
- CobsEncode<uart_to_camera_size()>(buffer, result.mutable_backing_array())
+ result.resize(result.capacity());
+ result.resize(
+ CobsEncode<uart_to_camera_size()>(buffer, absl::Span<char>(result))
.size());
return result;
}
diff --git a/y2019/jevois/uart_test.cc b/y2019/jevois/uart_test.cc
index ff02f08..5a3a330 100644
--- a/y2019/jevois/uart_test.cc
+++ b/y2019/jevois/uart_test.cc
@@ -88,7 +88,7 @@
}
{
UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
- buffer.set_size(buffer.size() - 1);
+ buffer.resize(buffer.size() - 1);
EXPECT_FALSE(UartUnpackToTeensy(buffer));
}
{
@@ -113,7 +113,7 @@
}
{
UartToCameraBuffer buffer = UartPackToCamera(input_message);
- buffer.set_size(buffer.size() - 1);
+ buffer.resize(buffer.size() - 1);
EXPECT_FALSE(UartUnpackToCamera(buffer));
}
{
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 4aa4b54..650693c 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -1,5 +1,3 @@
-#include "y2019/vision/target_finder.h"
-
#include <condition_variable>
#include <fstream>
#include <mutex>
@@ -16,19 +14,20 @@
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
#include "y2019/vision/image_writer.h"
+#include "y2019/vision/target_finder.h"
// This has to be last to preserve compatibility with other headers using AOS
// logging.
#include "glog/logging.h"
+using ::aos::monotonic_clock;
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;
+using ::y2019::jevois::open_via_terminos;
class CameraStream : public ::y2019::camera::ImageStreamEvent {
public:
@@ -41,8 +40,9 @@
if (on_frame_) on_frame_(data, monotonic_now);
}
- void set_on_frame(const std::function<
- void(DataRef, monotonic_clock::time_point)> &on_frame) {
+ void set_on_frame(
+ const std::function<void(DataRef, monotonic_clock::time_point)>
+ &on_frame) {
on_frame_ = on_frame;
}
@@ -67,12 +67,12 @@
exit(-1);
}
+using aos::vision::ImageFormat;
using aos::vision::ImageRange;
using aos::vision::RangeImage;
-using aos::vision::ImageFormat;
-using y2019::vision::TargetFinder;
using y2019::vision::IntermediateResult;
using y2019::vision::Target;
+using y2019::vision::TargetFinder;
class TargetProcessPool {
public:
@@ -271,7 +271,7 @@
frc971::jevois::CameraFrame frame{};
- for (size_t i = 0; i < results.size() && i < frame.targets.max_size();
+ for (size_t i = 0; i < results.size() && i < frame.targets.capacity();
++i) {
const auto &result = results[i].extrinsics;
frame.targets.push_back(frc971::jevois::Target{
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index f70f107..b7d57b4 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -225,9 +225,8 @@
subsystems_not_ready.push_back(Subsystem::TURRET);
}
- subsystems_not_ready_offset =
- status->fbb()->CreateVector(subsystems_not_ready.backing_array().data(),
- subsystems_not_ready.size());
+ subsystems_not_ready_offset = status->fbb()->CreateVector(
+ subsystems_not_ready.data(), subsystems_not_ready.size());
}
Status::Builder status_builder = status->MakeBuilder<Status>();
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 1109b58..3f08be9 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -32,8 +32,8 @@
pi_(pi),
pi_number_(aos::network::ParsePiNumber(pi)),
camera_id_(camera_id),
- H_camera_board_(Eigen::Affine3d()),
prev_H_camera_board_(Eigen::Affine3d()),
+ prev_image_H_camera_board_(Eigen::Affine3d()),
charuco_extractor_(
event_loop, pi,
[this](cv::Mat rgb_image,
@@ -73,12 +73,12 @@
}
// Calibration calculates rotation and translation delta from last image
- // captured to automatically capture next image
+ // stored to automatically capture next image
Eigen::Affine3d H_board_camera =
Eigen::Translation3d(tvec_eigen) *
Eigen::AngleAxisd(rvec_eigen.norm(), rvec_eigen / rvec_eigen.norm());
- H_camera_board_ = H_board_camera.inverse();
+ Eigen::Affine3d H_camera_board_ = H_board_camera.inverse();
Eigen::Affine3d H_delta = H_board_camera * prev_H_camera_board_;
Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(H_delta.rotation());
@@ -88,8 +88,30 @@
double r_norm = std::abs(delta_r.angle());
double t_norm = delta_t.norm();
- int keystroke = cv::waitKey(1);
+ bool store_image = false;
+ // Verify that camera has moved enough from last stored image
if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
+ // frame_ refers to deltas between current and last captured image
+ Eigen::Affine3d frame_H_delta =
+ H_board_camera * prev_image_H_camera_board_;
+
+ Eigen::AngleAxisd frame_delta_r =
+ Eigen::AngleAxisd(frame_H_delta.rotation());
+
+ Eigen::Vector3d frame_delta_t = frame_H_delta.translation();
+
+ double frame_r_norm = std::abs(frame_delta_r.angle());
+ double frame_t_norm = frame_delta_t.norm();
+
+ // Make sure camera has stopped moving before storing image
+ store_image =
+ frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
+ }
+
+ prev_image_H_camera_board_ = H_camera_board_;
+
+ int keystroke = cv::waitKey(1);
+ if (store_image) {
if (valid) {
prev_H_camera_board_ = H_camera_board_;
@@ -101,7 +123,7 @@
<< kDeltaRThreshold;
}
if (t_norm > kDeltaTThreshold) {
- LOG(INFO) << "Trigerred by translation delta = " << t_norm << " > "
+ LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
<< kDeltaTThreshold;
}
@@ -199,6 +221,9 @@
static constexpr double kDeltaRThreshold = M_PI / 6.0;
static constexpr double kDeltaTThreshold = 0.3;
+ static constexpr double kFrameDeltaRLimit = M_PI / 60;
+ static constexpr double kFrameDeltaTLimit = 0.01;
+
aos::ShmEventLoop *event_loop_;
std::string pi_;
const std::optional<uint16_t> pi_number_;
@@ -209,6 +234,7 @@
Eigen::Affine3d H_camera_board_;
Eigen::Affine3d prev_H_camera_board_;
+ Eigen::Affine3d prev_image_H_camera_board_;
CharucoExtractor charuco_extractor_;
};
diff --git a/y2022/BUILD b/y2022/BUILD
index 02d1bb1..fd82eb5 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -155,6 +155,7 @@
"//y2022/control_loops/superstructure:superstructure_goal_fbs",
"//y2022/control_loops/superstructure:superstructure_output_fbs",
"//y2022/control_loops/superstructure:superstructure_position_fbs",
+ "//y2022/control_loops/superstructure:superstructure_can_position_fbs",
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -238,6 +239,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2022/control_loops/superstructure:superstructure_can_position_fbs",
"//y2022/control_loops/superstructure:superstructure_output_fbs",
"//y2022/control_loops/superstructure:superstructure_position_fbs",
],
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index f08bb80..cd3f7c3 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -24,6 +24,14 @@
)
flatbuffer_cc_library(
+ name = "superstructure_can_position_fbs",
+ srcs = [
+ "superstructure_can_position.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
name = "superstructure_status_fbs",
srcs = [
"superstructure_status.fbs",
diff --git a/y2022/control_loops/superstructure/superstructure_can_position.fbs b/y2022/control_loops/superstructure/superstructure_can_position.fbs
new file mode 100644
index 0000000..e521d8c
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_can_position.fbs
@@ -0,0 +1,10 @@
+namespace y2022.control_loops.superstructure;
+
+// CAN readings from the CAN sensor reader loop
+table CANPosition {
+ // Velocity of the flipper arms (rad/s), obtained from the integrated sensor
+ // in the falcon.
+ flipper_arm_integrated_sensor_velocity:double (id: 0);
+}
+
+root_type CANPosition;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 2868539..b95a4ec 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -49,6 +49,7 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -412,8 +413,8 @@
}
void set_flipper_arms_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
- flipper_arms_falcon_ = ::std::move(t);
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ flipper_arms_falcon_ = t;
flipper_arms_falcon_->ConfigSupplyCurrentLimit(
{true, Values::kFlipperArmSupplyCurrentLimit(),
Values::kFlipperArmSupplyCurrentLimit(), 0});
@@ -422,6 +423,11 @@
Values::kFlipperArmStatorCurrentLimit(), 0});
}
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ flipper_arms_falcon() {
+ return flipper_arms_falcon_;
+ }
+
void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
transfer_roller_victor_ = ::std::move(t);
}
@@ -477,13 +483,60 @@
::std::unique_ptr<frc::TalonFX> intake_falcon_front_, intake_falcon_back_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- roller_falcon_front_, roller_falcon_back_, flipper_arms_falcon_;
+ roller_falcon_front_, roller_falcon_back_;
+
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ flipper_arms_falcon_;
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
catapult_falcon_2_, climber_falcon_;
::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
};
+class CANSensorReader {
+ public:
+ CANSensorReader(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ can_position_sender_(
+ event_loop->MakeSender<superstructure::CANPosition>(
+ "/superstructure")) {
+ event_loop->SetRuntimeRealtimePriority(16);
+
+ phased_loop_handler_ =
+ event_loop_->AddPhasedLoop([this](int) { Loop(); }, kPeriod);
+ phased_loop_handler_->set_name("CAN SensorReader Loop");
+
+ event_loop->OnRun([this]() { Loop(); });
+ }
+
+ void set_flipper_arms_falcon(
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ flipper_arms_falcon_ = std::move(t);
+ }
+
+ private:
+ void Loop() {
+ auto builder = can_position_sender_.MakeBuilder();
+ superstructure::CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<superstructure::CANPosition>();
+ can_position_builder.add_flipper_arm_integrated_sensor_velocity(
+ flipper_arms_falcon_->GetSelectedSensorVelocity() *
+ kVelocityConversion);
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ }
+
+ static constexpr std::chrono::milliseconds kPeriod =
+ std::chrono::milliseconds(20);
+ // 2048 encoder counts / 100 ms to rad/sec
+ static constexpr double kVelocityConversion = (2.0 * M_PI / 2048) * 0.100;
+ aos::EventLoop *event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handler_;
+
+ ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ flipper_arms_falcon_;
+ aos::Sender<superstructure::CANPosition> can_position_sender_;
+};
+
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<frc::Encoder> make_encoder(int index) {
@@ -544,8 +597,10 @@
sensor_reader.set_catapult_encoder(
make_unique<frc::Encoder>(0, 1, false, frc::Encoder::k4X));
- sensor_reader.set_catapult_absolute_pwm(std::make_unique<frc::DigitalInput>(2));
- sensor_reader.set_catapult_potentiometer(std::make_unique<frc::AnalogInput>(2));
+ sensor_reader.set_catapult_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_catapult_potentiometer(
+ std::make_unique<frc::AnalogInput>(2));
AddLoop(&sensor_reader_event_loop);
@@ -577,6 +632,13 @@
AddLoop(&output_event_loop);
+ // Thread 5
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
+ can_sensor_reader.set_flipper_arms_falcon(
+ superstructure_writer.flipper_arms_falcon());
+ AddLoop(&can_sensor_reader_event_loop);
+
RunLoops();
}
};
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
index 6ded52a..35f1ab0 100644
--- a/y2022/y2022_imu.json
+++ b/y2022/y2022_imu.json
@@ -390,10 +390,10 @@
"name": "imu",
"hostname": "imu",
"hostnames": [
- "pi-971-7",
- "pi-7971-7",
- "pi-8971-7",
- "pi-9971-7"
+ "pi-971-5",
+ "pi-7971-5",
+ "pi-8971-5",
+ "pi-9971-5"
],
"port": 9971
},
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index edbb258..f3bf5d0 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -238,6 +238,14 @@
"max_size": 448
},
{
+ "name": "/superstructure",
+ "type": "y2022.control_loops.superstructure.CANPosition",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 72
+ },
+ {
"name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",