Merge "Added Collision Avoidance."
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 0b8b447..0c13ecc 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -35,6 +35,21 @@
SizedArray &operator=(const SizedArray &) = default;
SizedArray &operator=(SizedArray &&) = default;
+ 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);
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 99ae87d..e712d54 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -1,5 +1,27 @@
package(default_visibility = ["//visibility:public"])
+cc_library(
+ name = "bitpacking",
+ hdrs = [
+ "bitpacking.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//third_party/GSL",
+ ],
+)
+
+cc_test(
+ name = "bitpacking_test",
+ srcs = [
+ "bitpacking_test.cc",
+ ],
+ deps = [
+ ":bitpacking",
+ "//aos/testing:googletest",
+ ],
+)
+
py_library(
name = "py_trapezoid_profile",
srcs = [
diff --git a/aos/util/bitpacking.h b/aos/util/bitpacking.h
new file mode 100644
index 0000000..df6ad16
--- /dev/null
+++ b/aos/util/bitpacking.h
@@ -0,0 +1,140 @@
+#ifndef AOS_UTIL_BITPACKING_H_
+#define AOS_UTIL_BITPACKING_H_
+
+#include <assert.h>
+
+#include <type_traits>
+
+#include "third_party/GSL/include/gsl/gsl"
+
+namespace aos {
+
+template <typename Integer>
+typename std::enable_if<std::is_unsigned<Integer>::value, Integer>::type
+MaskOnes(size_t bits) {
+ // Get these edge cases out of the way first, so we can subtract 1 from bits
+ // safely later without getting a negative number.
+ if (bits == 0) {
+ return 0;
+ }
+ if (bits == 1) {
+ return 1;
+ }
+ static constexpr Integer kOne = 1;
+ // Note that we shift at most by bits - 1. bits == sizeof(Integer) * 8 is
+ // valid, and shifting by the width of a type is undefined behavior, so we
+ // need to get a bit fancy to make it all work. Just ORing high_bit in
+ // explicitly at the end makes it work.
+ const Integer high_bit = kOne << (bits - 1);
+ return (high_bit - kOne) | high_bit;
+}
+
+template <typename Integer, size_t bits, size_t offset>
+typename std::enable_if<std::is_unsigned<Integer>::value &&
+ sizeof(Integer) * 8 >= bits>::type
+PackBits(const Integer value, const gsl::span<char> destination) {
+ assert(static_cast<size_t>(destination.size()) * 8u >= bits + offset);
+ size_t bits_completed = 0;
+ while (bits_completed < bits) {
+ // Which logical bit (through all the bytes) we're writing at.
+ const size_t output_bit = offset + bits_completed;
+ // The lowest-numbered bit in the current byte we're writing to.
+ const size_t output_min_bit = output_bit % 8;
+ // The number of bits we're writing in this byte.
+ const size_t new_bits = std::min(8 - output_min_bit, bits - bits_completed);
+ // The highest-numbered bit in the current byte we're writing to.
+ const size_t output_max_bit = output_min_bit + new_bits;
+ // The new bits to set in the this byte.
+ const uint8_t new_byte_part =
+ (value >> bits_completed) & MaskOnes<Integer>(new_bits);
+ // A mask of bits to keep from the current value of this byte. Start with
+ // just the low ones.
+ uint8_t existing_mask = MaskOnes<uint8_t>(output_min_bit);
+ // And then add in the high bits to keep.
+ existing_mask |= MaskOnes<uint8_t>(std::max<int>(8 - output_max_bit, 0))
+ << output_max_bit;
+ // The index of the byte we're writing to.
+ const size_t byte_index = output_bit / 8;
+ // The full new value of the current byte. Start with just the existing bits
+ // we're not touching.
+ uint8_t new_byte = destination[byte_index] & existing_mask;
+ // Add in the new part.
+ new_byte |= new_byte_part << output_min_bit;
+ destination[byte_index] = new_byte;
+ bits_completed += new_bits;
+ }
+ assert(bits_completed == bits);
+}
+
+template <typename Integer, size_t bits, size_t offset>
+typename std::enable_if<std::is_unsigned<Integer>::value &&
+ sizeof(Integer) * 8 >= bits, Integer>::type
+UnpackBits(const gsl::span<const char> source) {
+ Integer result = 0;
+ assert(static_cast<size_t>(source.size()) * 8u >= bits + offset);
+ size_t bits_completed = 0;
+ while (bits_completed < bits) {
+ // Which logical bit (through all the bytes) we're reading at.
+ const size_t input_bit = offset + bits_completed;
+ // The lowest-numbered bit in the current byte we're reading from.
+ const size_t input_min_bit = input_bit % 8;
+ // The number of bits we're reading in this byte.
+ const size_t new_bits = std::min(8 - input_min_bit, bits - bits_completed);
+ // The index of the byte we're reading from.
+ const size_t byte_index = input_bit / 8;
+ // The part of the current byte we're actually reading.
+ const uint8_t new_byte_part =
+ (source[byte_index] >> input_min_bit) & MaskOnes<Integer>(new_bits);
+ result |= static_cast<Integer>(new_byte_part) << bits_completed;
+ bits_completed += new_bits;
+ }
+ assert(bits_completed == bits);
+ return result;
+}
+
+template <int bits>
+uint32_t FloatToIntLinear(float min, float max, float value) {
+ static_assert(bits <= 31, "Only support 32-bit outputs for now");
+ static_assert(bits >= 1, "Bits must be positive");
+ // Start such that value in [0, 1) maps to [0, 2**bits) in the final
+ // result.
+ float result = (value - min) / (max - min);
+ // Multiply so that value is in [0, 2**bits).
+ // Make sure we do the shifting in a 32-bit integer, despite C++'s weird
+ // integer promotions, which is safe because bits is at most 31.
+ result *= static_cast<uint32_t>(UINT32_C(1) << bits);
+ if (result <= 0.0f) {
+ return 0;
+ }
+ const float max_result = MaskOnes<uint32_t>(bits);
+ if (result >= max_result) {
+ return max_result;
+ }
+ return static_cast<uint32_t>(result);
+}
+
+template <int bits>
+float IntToFloatLinear(float min, float max, uint32_t value) {
+ static_assert(bits <= 31, "Only support 32-bit outputs for now");
+ static_assert(bits >= 1, "Bits must be positive");
+ const float max_value = MaskOnes<uint32_t>(bits);
+ if (value > max_value) {
+ value = max_value;
+ }
+ // Start such that result in [0, 2**bits) maps to [min, max) in the final
+ // result.
+ float result = value;
+ // Offset by half a bit so we return a value in the middle of each one.
+ // This causes us to return the middle floating point value which could be
+ // represented by a given integer value.
+ result += 0.5f;
+ // Multiply so that result is in [0, 1).
+ // Make sure we do the shifting in a 32-bit integer, despite C++'s weird
+ // integer promotions, which is safe because bits is at most 31.
+ result *= 1.0f / static_cast<uint32_t>(UINT32_C(1) << bits);
+ return min + result * (max - min);
+}
+
+} // namespace aos
+
+#endif // AOS_UTIL_BITPACKING_H_
diff --git a/aos/util/bitpacking_test.cc b/aos/util/bitpacking_test.cc
new file mode 100644
index 0000000..013e911
--- /dev/null
+++ b/aos/util/bitpacking_test.cc
@@ -0,0 +1,387 @@
+#include "aos/util/bitpacking.h"
+
+#include <stdint.h>
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+// Tests MaskOnes with small arguments.
+TEST(MaskOnesTest, Small) {
+ EXPECT_EQ(0u, MaskOnes<uint8_t>(0));
+ EXPECT_EQ(0u, MaskOnes<uint64_t>(0));
+ EXPECT_EQ(1u, MaskOnes<uint8_t>(1));
+ EXPECT_EQ(1u, MaskOnes<uint64_t>(1));
+}
+
+// Tests MaskOnes with large arguments.
+TEST(MaskOnesTest, Large) {
+ EXPECT_EQ(0xFFu, MaskOnes<uint8_t>(8));
+ EXPECT_EQ(0x7Fu, MaskOnes<uint8_t>(7));
+
+ EXPECT_EQ(0xFFu, MaskOnes<uint64_t>(8));
+ EXPECT_EQ(UINT64_C(0xFFFFFFFFFFFFFFFF), MaskOnes<uint64_t>(64));
+ EXPECT_EQ(UINT64_C(0x7FFFFFFFFFFFFFFF), MaskOnes<uint64_t>(63));
+}
+
+// Tests some simple non-edge-case use cases for PackBits.
+TEST(PackBitsTest, Basic) {
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint8_t, 8, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 3>{}), buffer);
+ PackBits<uint8_t, 8, 0>(9, buffer);
+ EXPECT_EQ((std::array<char, 3>{{9, 0, 0}}), buffer);
+ PackBits<uint8_t, 8, 8>(7, buffer);
+ EXPECT_EQ((std::array<char, 3>{{9, 7, 0}}), buffer);
+ PackBits<uint8_t, 8, 16>(1, buffer);
+ EXPECT_EQ((std::array<char, 3>{{9, 7, 1}}), buffer);
+ }
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint16_t, 16, 0>(0xdead, buffer);
+ EXPECT_EQ((std::array<char, 3>{
+ {static_cast<char>(0xad), static_cast<char>(0xde), 0}}),
+ buffer);
+ }
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint8_t, 4, 0>(0xd7, buffer);
+ EXPECT_EQ((std::array<char, 3>{{0x7, 0, 0}}), buffer);
+ }
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint8_t, 4, 4>(0xd7, buffer);
+ EXPECT_EQ((std::array<char, 3>{{0x70, 0, 0}}), buffer);
+ }
+}
+
+// Verifies that PackBits puts bits in an order consistent with increasing
+// offsets.
+TEST(PackBitsTest, Consistency) {
+ {
+ std::array<char, 3> buffer1{};
+ PackBits<uint8_t, 8, 0>(0x80, buffer1);
+ std::array<char, 3> buffer2{};
+ PackBits<uint8_t, 1, 7>(0x1, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+ {
+ std::array<char, 1> buffer1{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 8, 0>(0x7F, buffer1);
+ std::array<char, 1> buffer2{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 1, 7>(0x0, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+ {
+ std::array<char, 1> buffer1{};
+ PackBits<uint8_t, 3, 5>(0x7, buffer1);
+ std::array<char, 1> buffer2{};
+ PackBits<uint8_t, 5, 3>(0x3C, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+ {
+ std::array<char, 1> buffer1{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 3, 5>(0x0, buffer1);
+ std::array<char, 1> buffer2{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 5, 3>(0x03, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+}
+
+// Tests some simple non-edge-case use cases for UnpackBits.
+TEST(UnpackBitsTest, Basic) {
+ {
+ std::array<char, 3> buffer{};
+ EXPECT_EQ(0u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ buffer = {{9, 0, 0}};
+ EXPECT_EQ(9u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ buffer = {{9, 7, 0}};
+ EXPECT_EQ(9u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 8, 8>(buffer)));
+ buffer = {{9, 7, 1}};
+ EXPECT_EQ(9u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 8, 8>(buffer)));
+ EXPECT_EQ(1u, (UnpackBits<uint8_t, 8, 16>(buffer)));
+ }
+ {
+ const std::array<char, 3> buffer = {
+ {static_cast<char>(0xad), static_cast<char>(0xde), 0}};
+ EXPECT_EQ(0xdead, (UnpackBits<uint16_t, 16, 0>(buffer)));
+ }
+ {
+ const std::array<char, 3> buffer = {{static_cast<char>(0xF7), 0, 0}};
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 4, 0>(buffer)));
+ }
+ {
+ const std::array<char, 3> buffer = {{static_cast<char>(0x7F), 0, 0}};
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 4, 4>(buffer)));
+ }
+}
+
+// Tests PackBits split across multiple bytes.
+TEST(PackBitsTest, AcrossBytes) {
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 7>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x80), static_cast<char>(0x7F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 6>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xC0), static_cast<char>(0x3F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 5>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xE0), static_cast<char>(0x1F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 4>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xF0), static_cast<char>(0x0F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 3>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xF8), static_cast<char>(0x07)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 2>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFC), static_cast<char>(0x03)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 1>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFE), static_cast<char>(0x01)}}),
+ buffer);
+ }
+}
+
+// Tests UnpackBits split across multiple bytes.
+TEST(UnpackBitsTest, AcrossBytes) {
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0x80), static_cast<char>(0x7F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 7>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xC0), static_cast<char>(0x3F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 6>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xE0), static_cast<char>(0x1F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 5>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xF0), static_cast<char>(0x0F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 4>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xF8), static_cast<char>(0x07)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 3>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xFC), static_cast<char>(0x03)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 2>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xFE), static_cast<char>(0x01)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 1>(buffer)));
+ }
+}
+
+// Verifies that PackBits avoids touching adjacent bits.
+TEST(PackBitsTest, AdjacentBits) {
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 1, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFE), static_cast<char>(0xFF)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 7, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x80), static_cast<char>(0xFF)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 8, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0xFF)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 9, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0xFE)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 14, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0xC0)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 15, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0x80)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 15, 1>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x01), static_cast<char>(0x00)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 6, 8>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFF), static_cast<char>(0xC0)}}),
+ buffer);
+ }
+ {
+ std::array<char, 4> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF),
+ static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 6, 24>(0, buffer);
+ EXPECT_EQ((std::array<char, 4>{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF),
+ static_cast<char>(0xFF), static_cast<char>(0xC0)}}),
+ buffer);
+ }
+}
+
+// Tests FloatToIntLinear with values near or outside of its boundaries.
+TEST(FloatToIntLinearTest, OutOfBounds) {
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, 0.0f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, -0.1f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, -1.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 1.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 1.1f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 2.0f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 4.0f, 0.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 4.0f, 4.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 4.0f, 10.0f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(0.0f, 4.0f, 0.0f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(0.0f, 4.0f, -100.0f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(0.0f, 4.0f, 4.0f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(0.0f, 4.0f, 4.01f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(-3.0f, 5.0f, -3.0f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(-3.0f, 5.0f, -3.1f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(-3.0f, 5.0f, 5.0f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(-3.0f, 5.0f, 5.1f)));
+}
+
+// Tests that FloatToIntLinear rounds correctly at the boundaries between output
+// values.
+TEST(FloatToIntLinearTest, Rounding) {
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, 0.49f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 0.51f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(-1.0f, 0.0f, -0.49f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(-1.0f, 0.0f, -0.51f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(-1.0f, 1.0f, 0.01f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(-1.0f, 1.0f, -0.01f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.124f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.126f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.249f)));
+ EXPECT_EQ(2u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.251f)));
+ EXPECT_EQ(2u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.374f)));
+ EXPECT_EQ(3u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.376f)));
+ EXPECT_EQ(3u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.499f)));
+ EXPECT_EQ(4u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.501f)));
+ EXPECT_EQ(4u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.624f)));
+ EXPECT_EQ(5u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.626f)));
+ EXPECT_EQ(5u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.749f)));
+ EXPECT_EQ(6u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.751f)));
+ EXPECT_EQ(6u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.874f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.876f)));
+}
+
+// Tests IntToFloatLinear with values near or outside of its boundaries.
+TEST(IntToFloatLinearTest, OutOfBounds) {
+ EXPECT_EQ(0.25f, (IntToFloatLinear<1>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<1>(0.0f, 1.0f, 1)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<1>(0.0f, 1.0f, 2)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<1>(0.0f, 1.0f, 3)));
+
+ EXPECT_EQ(1.0f, (IntToFloatLinear<1>(0.0f, 4.0f, 0)));
+ EXPECT_EQ(3.0f, (IntToFloatLinear<1>(0.0f, 4.0f, 1)));
+
+ EXPECT_EQ(0.0625f, (IntToFloatLinear<3>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.9375f, (IntToFloatLinear<3>(0.0f, 1.0f, 7)));
+ EXPECT_EQ(0.9375f, (IntToFloatLinear<3>(0.0f, 1.0f, 8)));
+}
+
+// Tests IntToFloatLinear with some specific values which are easy to calculate
+// by hand.
+TEST(IntToFloatLinearTest, Values) {
+ EXPECT_EQ(0.125f, (IntToFloatLinear<2>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.375f, (IntToFloatLinear<2>(0.0f, 1.0f, 1)));
+ EXPECT_EQ(0.625f, (IntToFloatLinear<2>(0.0f, 1.0f, 2)));
+ EXPECT_EQ(0.875f, (IntToFloatLinear<2>(0.0f, 1.0f, 3)));
+
+ EXPECT_EQ(0.0625f, (IntToFloatLinear<3>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.1875f, (IntToFloatLinear<3>(0.0f, 1.0f, 1)));
+ EXPECT_EQ(0.3125f, (IntToFloatLinear<3>(0.0f, 1.0f, 2)));
+ EXPECT_EQ(0.4375f, (IntToFloatLinear<3>(0.0f, 1.0f, 3)));
+ EXPECT_EQ(0.5625f, (IntToFloatLinear<3>(0.0f, 1.0f, 4)));
+ EXPECT_EQ(0.6875f, (IntToFloatLinear<3>(0.0f, 1.0f, 5)));
+ EXPECT_EQ(0.8125f, (IntToFloatLinear<3>(0.0f, 1.0f, 6)));
+ EXPECT_EQ(0.9375f, (IntToFloatLinear<3>(0.0f, 1.0f, 7)));
+
+ EXPECT_EQ(-0.875f, (IntToFloatLinear<2>(-1.0f, 0.0f, 0)));
+ EXPECT_EQ(-0.625f, (IntToFloatLinear<2>(-1.0f, 0.0f, 1)));
+ EXPECT_EQ(-0.375f, (IntToFloatLinear<2>(-1.0f, 0.0f, 2)));
+ EXPECT_EQ(-0.125f, (IntToFloatLinear<2>(-1.0f, 0.0f, 3)));
+
+ EXPECT_EQ(-0.75f, (IntToFloatLinear<2>(-1.0f, 1.0f, 0)));
+ EXPECT_EQ(-0.25f, (IntToFloatLinear<2>(-1.0f, 1.0f, 1)));
+ EXPECT_EQ(0.25f, (IntToFloatLinear<2>(-1.0f, 1.0f, 2)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<2>(-1.0f, 1.0f, 3)));
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index d12d3da..5871a29 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -341,3 +341,72 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "capped_test_plant",
+ testonly = True,
+ srcs = [
+ "capped_test_plant.cc",
+ ],
+ hdrs = [
+ "capped_test_plant.h",
+ ],
+ deps = [
+ ":state_feedback_loop",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "static_zeroing_single_dof_profiled_subsystem",
+ hdrs = [
+ "static_zeroing_single_dof_profiled_subsystem.h",
+ ],
+ deps = [
+ "//frc971/control_loops:profiled_subsystem",
+ ],
+)
+
+genrule(
+ name = "genrule_static_zeroing_single_dof_profiled_subsystem_test",
+ outs = [
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.h",
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.cc",
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h",
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.cc",
+ ],
+ cmd = "$(location //frc971/control_loops/python:static_zeroing_single_dof_profiled_subsystem_test) $(OUTS)",
+ tools = [
+ "//frc971/control_loops/python:static_zeroing_single_dof_profiled_subsystem_test",
+ ],
+)
+
+cc_library(
+ name = "static_zeroing_single_dof_profiled_subsystem_test_plants",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.cc",
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.cc",
+ ],
+ hdrs = [
+ "static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h",
+ "static_zeroing_single_dof_profiled_subsystem_test_plant.h",
+ ],
+ deps = [
+ ":state_feedback_loop",
+ ],
+)
+
+cc_test(
+ name = "static_zeroing_single_dof_profiled_subsystem_test",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.cc",
+ ],
+ deps = [
+ ":capped_test_plant",
+ ":position_sensor_sim",
+ ":static_zeroing_single_dof_profiled_subsystem",
+ ":static_zeroing_single_dof_profiled_subsystem_test_plants",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/frc971/control_loops/capped_test_plant.cc b/frc971/control_loops/capped_test_plant.cc
new file mode 100644
index 0000000..8f32551
--- /dev/null
+++ b/frc971/control_loops/capped_test_plant.cc
@@ -0,0 +1 @@
+#include "frc971/control_loops/capped_test_plant.h"
\ No newline at end of file
diff --git a/frc971/control_loops/capped_test_plant.h b/frc971/control_loops/capped_test_plant.h
new file mode 100644
index 0000000..65bc7cc
--- /dev/null
+++ b/frc971/control_loops/capped_test_plant.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Basic state feedback plant for use in tests.
+class CappedTestPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit CappedTestPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
+} // namespace frc971
+} // namespace control_loops
+#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
\ No newline at end of file
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 64b4612..c9582e6 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -362,31 +362,32 @@
return result;
}
-::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
- ::std::chrono::nanoseconds dt) {
+::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
double dt_float =
::std::chrono::duration_cast<::std::chrono::duration<double>>(dt).count();
- double t = 0.0;
- ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+ // TODO(austin): This feels like something that should be pulled out into
+ // a library for re-use.
+ *state = RungeKutta([this](const ::Eigen::Matrix<double, 2, 1> x) {
+ ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+ return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+ }, *state, dt_float);
+
+ ::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
+ (*state)(1) = result(1);
+ return result;
+}
+
+::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+ ::std::chrono::nanoseconds dt) {
+ ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
::std::vector<::Eigen::Matrix<double, 3, 1>> result;
result.emplace_back(FFAcceleration(0));
result.back()(1) = 0.0;
- while (state(0) < length() - 1e-4) {
- t += dt_float;
-
- // TODO(austin): This feels like something that should be pulled out into
- // a library for re-use.
- state = RungeKutta(
- [this](const ::Eigen::Matrix<double, 2, 1> x) {
- ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
- return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
- },
- state, dt_float);
-
- result.emplace_back(FFAcceleration(state(0)));
- state(1) = result.back()(1);
+ while (!is_at_end(state)) {
+ result.emplace_back(GetNextXVA(dt, &state));
}
return result;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index d3468b3..66dcba1 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -98,6 +98,11 @@
// Returns the feed forwards voltage for an explicit distance.
::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
+ // Returns whether a state represents a state at the end of the spline.
+ bool is_at_end(::Eigen::Matrix<double, 2, 1> state) const {
+ return state(0) > length() - 1e-4;
+ }
+
// Returns the length of the path in meters.
double length() const { return spline_->length(); }
@@ -151,6 +156,10 @@
const ::Eigen::DiagonalMatrix<double, 5> &Q,
const ::Eigen::DiagonalMatrix<double, 2> &R) const;
+ // Return the next position, velocity, acceleration based on the current
+ // state. Updates the passed in state for the next iteration.
+ ::Eigen::Matrix<double, 3, 1> GetNextXVA(
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state);
::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
::std::chrono::nanoseconds dt);
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index eb1664a..f65adf1 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -265,6 +265,15 @@
1.2e-2);
}
+// Tests that Iteratively computing the XVA plan is the same as precomputing it.
+TEST_P(ParameterizedSplineTest, IterativeXVA) {
+ ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+ for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
+ ::Eigen::Matrix<double, 3, 1> xva = trajectory_->GetNextXVA(kDt, &state);
+ EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
+ }
+}
+
SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
return params;
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 42433e9..38dbd53 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -150,7 +150,9 @@
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
v_observer;
v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity()));
+ Eigen::Matrix<double, 2, 7>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 7, 7>::Identity()));
StateFeedbackObserver<2, 4, 7> observer(&v_observer);
StateFeedbackLoop<2, 4, 7> test_loop(
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index d1356da..784b92f 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -153,3 +153,8 @@
// State of the estimator.
.frc971.IndexEstimatorState estimator_state;
};
+
+struct StaticZeroingSingleDOFProfiledSubsystemGoal {
+ double unsafe_goal;
+ .frc971.ProfileParameters profile_params;
+};
\ No newline at end of file
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index a0ee825..b3aa2c0 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -97,7 +97,7 @@
deps = [
":libspline",
":python_init",
- ]
+ ],
)
py_library(
@@ -166,15 +166,15 @@
py_binary(
name = "path_edit",
srcs = [
- "path_edit.py",
"basic_window.py",
"color.py",
+ "path_edit.py",
],
- visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
deps = [
- ":python_init",
":libspline",
+ ":python_init",
"@python_gtk",
],
)
@@ -183,10 +183,10 @@
name = "basic_window",
srcs = [
"basic_window.py",
- "color.py"
+ "color.py",
],
- visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
deps = [
":python_init",
"@python_gtk",
@@ -198,9 +198,25 @@
srcs = [
"color.py",
],
- visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
deps = [
":python_init",
],
)
+
+py_binary(
+ name = "static_zeroing_single_dof_profiled_subsystem_test",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":controls",
+ ":linear_system",
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ ],
+)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 9aa00df..e3e00b7 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -453,10 +453,17 @@
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
+ Q = self.Q
+ R = self.R
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
+ Q = numpy.zeros(self.A.shape)
+ R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
- ans.append(' return %s(KalmanGain);\n' % (observer_coefficient_type,))
+ ans.append(self._DumpMatrix('Q', Q, scalar_type))
+ ans.append(self._DumpMatrix('R', R, scalar_type))
+ ans.append(' return %s(KalmanGain, Q, R);\n'
+ % (observer_coefficient_type,))
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 3f9123f..c59cbab 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -362,10 +362,15 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ # If we don't have an IMU, pad various matrices with zeros so that
+ # we can still have 4 measurement outputs.
if not self.has_imu:
self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
+ Rtmp = numpy.zeros((4, 4))
+ Rtmp[0:3, 0:3] = self.R
+ self.R = Rtmp
self.L = self.A * self.KalmanGain
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 52bd8d1..9748a41 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -17,16 +17,16 @@
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-LENGTH_OF_FIELD = 323.65
+WIDTH_OF_FIELD_IN_METERS = 8.258302
PIXELS_ON_SCREEN = 300
-def pxToIn(p):
- return p * LENGTH_OF_FIELD / PIXELS_ON_SCREEN
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-def inToPx(i):
- return (i * PIXELS_ON_SCREEN / LENGTH_OF_FIELD)
+def mToPx(i):
+ return (i * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
def px(cr):
@@ -155,7 +155,7 @@
#John also wrote this
def add_point(self, x, y):
if (len(self.selected_points) < 6):
- self.selected_points.append([x, y])
+ self.selected_points.append([pxToM(x), pxToM(y)])
if (len(self.selected_points) == 6):
self.mode = Mode.kEditing
self.splines.append(np.array(self.selected_points))
@@ -188,9 +188,10 @@
cur_p = [self.x, self.y]
for index_splines, points in enumerate(self.spline):
- for index_points, i in enumerate(points.curve):
+ for index_points, point in enumerate(points.curve):
# pythagorean
- distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
+ distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
+ (cur_p[1] - mToPx(point[1])**2))
if distance < nearest:
nearest = distance
print("DISTANCE: ", distance, " | INDEX: ", index_points)
@@ -222,7 +223,7 @@
if index > 0 and index <= self.index_of_edit:
distance += np.sqrt((points[index - 1][0] - point[0])**2 +
(points[index - 1][1] - point[1])**2)
- return pxToIn(distance)
+ return distance
# Handle the expose-event by updating the Window and drawing
def handle_draw(self, cr):
@@ -255,23 +256,6 @@
cr.rectangle(-450, -150, 300, 300)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
- cr.rectangle((inToPx(140 - 161.825) - 300), inToPx(76.575), inToPx(56),
- inToPx(-153.15))
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
- cr.stroke()
- cr.rectangle((inToPx(161.825 - 24) - 300), inToPx(90), inToPx(24),
- inToPx(-180))
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
- cr.stroke()
-
- set_color(cr, Color(0.2, 0.2, 0.2))
- cr.rectangle(
- inToPx(140 - 161.825) - 300, inToPx(76.575), inToPx(56),
- inToPx(-153.15))
- cr.fill()
- cr.rectangle(
- inToPx(161.825 - 24) - 300, inToPx(90), inToPx(24), inToPx(-180))
- cr.fill()
y = 0
@@ -307,8 +291,8 @@
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)
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
+ 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.kEditing:
@@ -316,17 +300,14 @@
cr.move_to(-300, 170)
display_text(cr, "EDITING", 1, 1, 1, 1)
if len(self.splines) > 0:
- # print("Splines: " + str(len(self.splines)))
- # print("ITEMS:")
holder_spline = []
for i, points in enumerate(self.splines):
array = np.zeros(shape=(6, 2), dtype=float)
for j, point in enumerate(points):
- array[j, 0] = point[0]
- array[j, 1] = point[1]
+ array[j, 0] = mToPx(point[0])
+ array[j, 1] = mToPx(point[1])
spline = Spline(np.ascontiguousarray(np.transpose(array)))
for k in np.linspace(0.01, 1, 100):
-
cr.move_to(
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1])
@@ -336,7 +317,6 @@
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1]
]
-
holder_spline.append(holding)
self.curves.append(holder_spline)
@@ -346,13 +326,13 @@
for i, point in enumerate(points):
# print("I: " + str(i))
if spline == self.spline_edit and i == self.index_of_edit:
- draw_px_x(cr, point[0], point[1], 15,
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
self.colors[spline])
elif (spline == 0 and not i == 5) or (not i == 0
and not i == 5):
- draw_px_x(cr, point[0], point[1], 10,
+ draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
self.colors[spline])
- cr.move_to(point[0], point[1] - 15)
+ 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:
@@ -466,7 +446,7 @@
if self.index_of_edit > -1 and self.held_x != self.x:
print("INDEX OF EDIT: " + str(self.index_of_edit))
self.splines[self.spline_edit][self.index_of_edit] = [
- self.x, self.y
+ pxToM(self.x), pxToM(self.y)
]
if not self.spline_edit == len(self.splines) - 1:
@@ -494,11 +474,11 @@
# Get clicked point
# Find nearest
# Move nearest to clicked
- cur_p = [self.x, self.y]
+ cur_p = [pxToM(self.x), pxToM(self.y)]
print("CUR_P: " + str(self.x) + " " + str(self.y))
# Get the distance between each for x and y
# Save the index of the point closest
- nearest = 50
+ nearest = 1 # Max distance away a the selected point can be in meters
index_of_closest = 0
for index_splines, points in enumerate(self.splines):
for index_points, val in enumerate(points):
@@ -520,7 +500,7 @@
def do_button_press(self, event):
print("button press activated")
- #Be consistent with the scaling in the drawing_area
+ # Be consistent with the scaling in the drawing_area
self.x = event.x * 2
self.y = event.y * 2
self.button_press_action()
@@ -622,4 +602,4 @@
window = GridWindow()
-basic_window.RunApp()
\ No newline at end of file
+basic_window.RunApp()
diff --git a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
new file mode 100644
index 0000000..caa3082
--- /dev/null
+++ b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
@@ -0,0 +1,56 @@
+#!/usr/bin/python
+
+# Generates profiled subsystem for use in
+# static_zeroing_single_dof_profiled_subsystem_test
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import linear_system
+import numpy
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kIntake = linear_system.LinearSystemParams(
+ name='TestIntakeSystem',
+ motor=control_loop.Vex775Pro(),
+ # (1 / 35.0) * (20.0 / 40.0) -> 16 tooth sprocket on #25 chain
+ G=(1.0 / 35.0) * (20.0 / 40.0),
+ radius=16.0 * 0.25 / (2.0 * numpy.pi) * 0.0254,
+ mass=5.4,
+ q_pos=0.015,
+ q_vel=0.3,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.00,
+ kalman_q_voltage=40.0,
+ kalman_r_position=0.05)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[0.1], [0.0]])
+ linear_system.PlotMotion(kIntake, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the \
+ static_zeroing_single_dof_profiled_subsystem_test and integral \
+ static_zeroing_single_dof_profiled_subsystem_test.'
+ )
+ else:
+ namespaces = ['frc971', 'control_loops']
+ linear_system.WriteLinearSystem(kIntake, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 2511d98..fc3ff34 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -275,10 +275,15 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain)
- : KalmanGain(KalmanGain) {}
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &
+ KalmanGain,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R)
+ : KalmanGain(KalmanGain), Q(Q), R(R) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
new file mode 100644
index 0000000..9ad7439
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -0,0 +1,216 @@
+#ifndef FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
+#define FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
+
+#include "frc971/control_loops/profiled_subsystem.h"
+
+namespace frc971 {
+namespace control_loops {
+
+template <typename ZeroingEstimator>
+struct StaticZeroingSingleDOFProfiledSubsystemParams {
+ // Maximum voltage while the subsystem is zeroing
+ double zeroing_voltage;
+
+ // Maximum voltage while the subsystem is running
+ double operating_voltage;
+
+ // Maximum velocity (units/s) and acceleration while State::ZEROING
+ ::frc971::ProfileParameters zeroing_profile_params;
+
+ // Maximum velocity (units/s) and acceleration while State::RUNNING if max
+ // velocity or acceleration in goal profile_params is 0
+ ::frc971::ProfileParameters default_profile_params;
+
+ // Maximum range of the subsystem in meters
+ const ::frc971::constants::Range range;
+
+ // Zeroing constants for PotAndABsoluteEncoder estimator
+ const typename ZeroingEstimator::ZeroingConstants zeroing_constants;
+
+ // Function that makes the integral loop for the subsystem
+ ::std::function<StateFeedbackLoop<3, 1, 1>()> make_integral_loop;
+};
+
+// Class for controlling and motion profiling a single degree of freedom
+// subsystem with a zeroing strategy of not moving.
+template <typename ZeroingEstimator>
+class StaticZeroingSingleDOFProfiledSubsystem {
+ public:
+ StaticZeroingSingleDOFProfiledSubsystem(
+ const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ ¶ms);
+
+ // Returns the filtered goal of the profiled subsystem (R)
+ double goal(int row) const { return profiled_subsystem_.goal(row, 0); }
+
+ // Returns the zeroing voltage of the subsystem
+ double zeroing_voltage() { return params_.zeroing_voltage; }
+
+ // Returns the operating voltage of the subsystem
+ double operating_voltage() { return params_.operating_voltage; }
+
+ // Sets further constraints on the range constant
+ void set_min_position(double min_position) { min_position_ = min_position; }
+
+ void set_max_position(double max_position) { max_position_ = max_position; }
+
+ // Resets constrained min/max position
+ void clear_min_position() { min_position_ = params_.range.lower_hard; }
+
+ void clear_max_position() { max_position_ = params_.range.upper_hard; }
+
+ // Returns the current position
+ double position() const { return profiled_subsystem_.position(); }
+
+ void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+
+ // Resets the profiled subsystem and returns to uninitialized
+ void Reset();
+
+ enum class State : int32_t {
+ UNINITIALIZED,
+ DISABLED_INITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ State state_ = State::UNINITIALIZED;
+ double min_position_, max_position_;
+
+ const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator> params_;
+
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<ZeroingEstimator>
+ profiled_subsystem_;
+};
+
+template <typename ZeroingEstimator>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::
+ StaticZeroingSingleDOFProfiledSubsystem(
+ const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ ¶ms)
+ : params_(params),
+ profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(params.make_integral_loop())),
+ params.zeroing_constants, params.range,
+ params.default_profile_params.max_velocity,
+ params.default_profile_params.max_acceleration) {
+ Reset();
+};
+
+template <typename ZeroingEstimator>
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Reset() {
+ state_ = State::UNINITIALIZED;
+ clear_min_position();
+ clear_max_position();
+ profiled_subsystem_.Reset();
+}
+
+template <typename ZeroingEstimator>
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Iterate(
+ const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ bool disabled = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ if (profiled_subsystem_.initialized()) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ disabled = true;
+ break;
+ case State::DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled.
+ if (disabled) {
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ }
+ } else {
+ state_ = State::ZEROING;
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(params_.zeroing_profile_params);
+
+ // We are not ready to start doing anything yet.
+ disabled = true;
+ break;
+ case State::ZEROING:
+ // Now, zero by actively holding still.
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ } else if (disabled) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ break;
+
+ case State::RUNNING: {
+ if (disabled) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ }
+
+ if (goal) {
+ profiled_subsystem_.AdjustProfile(goal->profile_params);
+
+ double safe_goal = goal->unsafe_goal;
+ if (safe_goal < min_position_) {
+ LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
+ safe_goal = min_position_;
+ }
+ if (safe_goal > max_position_) {
+ LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
+ safe_goal = max_position_;
+ }
+ profiled_subsystem_.set_unprofiled_goal(safe_goal);
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disabled = true;
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage = (state_ == State::RUNNING)
+ ? params_.operating_voltage
+ : params_.zeroing_voltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disabled);
+
+ // Write out all the voltages.
+ if (output) {
+ *output = profiled_subsystem_.voltage();
+ }
+
+ status->zeroed = profiled_subsystem_.zeroed();
+
+ profiled_subsystem_.PopulateStatus(status);
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_STATIC_ZEROING_SINGLE_DOF_PROFILED_SUBSYSTEM_H_
\ No newline at end of file
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
new file mode 100644
index 0000000..8805842
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -0,0 +1,492 @@
+#include "gtest/gtest.h"
+
+#include "aos/controls/control_loop_test.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
+
+using ::frc971::control_loops::PositionSensorSimulator;
+
+namespace frc971 {
+namespace control_loops {
+namespace {
+constexpr double kNoiseScalar = 0.01;
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+struct TestIntakeSystemValues {
+ static const int kZeroingSampleSize = 200;
+
+ static constexpr double kEncoderRatio =
+ (16.0 * 0.25) * (20.0 / 40.0) / (2.0 * M_PI) * 0.0254;
+ static constexpr double kEncoderIndexDifference = 2.0 * M_PI * kEncoderRatio;
+ static constexpr ::frc971::constants::Range kRange{
+ .lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
+
+ static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ static const ::frc971::ProfileParameters kDefaultParams;
+ static const ::frc971::ProfileParameters kZeroingParams;
+
+ static constexpr ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ kZeroing{kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
+
+ static const StaticZeroingSingleDOFProfiledSubsystemParams<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ kParams;
+};
+
+constexpr ::frc971::constants::Range TestIntakeSystemValues::kRange;
+constexpr ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ TestIntakeSystemValues::kZeroing;
+const ::frc971::ProfileParameters TestIntakeSystemValues::kDefaultParams{0.3,
+ 5.0};
+const ::frc971::ProfileParameters TestIntakeSystemValues::kZeroingParams{0.1,
+ 1.0};
+
+const StaticZeroingSingleDOFProfiledSubsystemParams<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ TestIntakeSystemValues::kParams(
+ {.zeroing_voltage = TestIntakeSystemValues::kZeroingVoltage,
+ .operating_voltage = TestIntakeSystemValues::kOperatingVoltage,
+ .zeroing_profile_params = TestIntakeSystemValues::kZeroingParams,
+ .default_profile_params = TestIntakeSystemValues::kDefaultParams,
+ .range = TestIntakeSystemValues::kRange,
+ .zeroing_constants = TestIntakeSystemValues::kZeroing,
+ .make_integral_loop = MakeIntegralTestIntakeSystemLoop});
+
+struct TestIntakeSystemData {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+
+ ::frc971::control_loops::AbsoluteProfiledJointStatus status;
+
+ ::frc971::PotAndAbsolutePosition position;
+
+ double output;
+};
+
+} // namespace
+
+class TestIntakeSystemSimulation {
+ public:
+ TestIntakeSystemSimulation()
+ : subsystem_plant_(new CappedTestPlant(
+ ::frc971::control_loops::MakeTestIntakeSystemPlant())),
+ subsystem_pot_encoder_(
+ TestIntakeSystemValues::kEncoderIndexDifference) {
+ // Start the subsystem out in the middle by default.
+ InitializeSubsystemPosition((TestIntakeSystemValues::kRange.lower +
+ TestIntakeSystemValues::kRange.upper) /
+ 2.0);
+ }
+
+ void InitializeSubsystemPosition(double start_pos) {
+ subsystem_plant_->mutable_X(0, 0) = start_pos;
+ subsystem_plant_->mutable_X(1, 0) = 0.0;
+
+ subsystem_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues::kZeroing.measured_absolute_position);
+ }
+
+ // Updates the position message with the position of the subsystem.
+ void UpdatePositionMessage(::frc971::PotAndAbsolutePosition *position) {
+ subsystem_pot_encoder_.GetSensorValues(position);
+ }
+
+ double subsystem_position() const { return subsystem_plant_->X(0, 0); }
+ double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
+
+ // Sets the difference between the commanded and applied powers.
+ // This lets us test that the integrators work.
+ void set_subsystem_voltage_offset(double voltage_offset) {
+ subsystem_plant_->set_voltage_offset(voltage_offset);
+ }
+
+ // Simulates the subsystem for a single timestep.
+ void Simulate(const double output_voltage, const int32_t state) {
+ const double voltage_check_subsystem =
+ (static_cast<StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State>(state) ==
+ StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING)
+ ? TestIntakeSystemValues::kOperatingVoltage
+ : TestIntakeSystemValues::kZeroingVoltage;
+
+ EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
+
+ ::Eigen::Matrix<double, 1, 1> subsystem_U;
+ subsystem_U << output_voltage + subsystem_plant_->voltage_offset();
+ subsystem_plant_->Update(subsystem_U);
+
+ const double position_subsystem = subsystem_plant_->Y(0, 0);
+
+ subsystem_pot_encoder_.MoveTo(position_subsystem);
+
+ EXPECT_GE(position_subsystem, TestIntakeSystemValues::kRange.lower_hard);
+ EXPECT_LE(position_subsystem, TestIntakeSystemValues::kRange.upper_hard);
+ }
+
+ private:
+ ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
+ PositionSensorSimulator subsystem_pot_encoder_;
+};
+
+class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ IntakeSystemTest()
+ : subsystem_(TestIntakeSystemValues::kParams), subsystem_plant_() {}
+
+ void VerifyNearGoal() {
+ EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
+ subsystem_data_.status.position, 0.001);
+ EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
+ subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(subsystem_data_.status.velocity, 0, 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true, bool null_goal = false) {
+ SendMessages(enabled);
+ subsystem_plant_.UpdatePositionMessage(&subsystem_data_.position);
+
+ // Checks if the robot was reset and resets the subsystem.
+ // Required since there is no ControlLoop to reset it (ie. a superstructure)
+ ::aos::robot_state.FetchLatest();
+ if (::aos::robot_state.get() &&
+ sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ subsystem_.Reset();
+ }
+
+ sensor_reader_pid_ = ::aos::robot_state->reader_pid;
+ subsystem_goal_.unsafe_goal = subsystem_data_.goal.unsafe_goal;
+ subsystem_goal_.profile_params = subsystem_data_.goal.profile_params;
+
+ subsystem_.Iterate(null_goal ? nullptr : &subsystem_goal_,
+ &subsystem_data_.position, &subsystem_data_.output,
+ &subsystem_data_.status);
+
+ subsystem_plant_.Simulate(subsystem_data_.output,
+ subsystem_data_.status.state);
+
+ TickTime(::std::chrono::microseconds(5050));
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
+ bool null_goal = false) {
+ 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_subsystem_velocity = subsystem_plant_.subsystem_velocity();
+
+ RunIteration(enabled, null_goal);
+
+ const double loop_time = chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time)
+ .count();
+ const double subsystem_acceleration =
+ (subsystem_plant_.subsystem_velocity() - begin_subsystem_velocity) /
+ loop_time;
+ EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
+ EXPECT_NEAR(subsystem_plant_.subsystem_velocity(), 0.0,
+ peak_subsystem_velocity_);
+ }
+ }
+
+ void set_peak_subsystem_acceleration(double value) {
+ peak_subsystem_acceleration_ = value;
+ }
+ void set_peak_subsystem_velocity(double value) {
+ peak_subsystem_velocity_ = value;
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory
+ // that is no longer valid.
+ // TestIntakeSystemData subsystem_data_;
+
+ // Create a control loop and simulation.
+ StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_;
+ TestIntakeSystemSimulation subsystem_plant_;
+
+ StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
+
+ TestIntakeSystemData subsystem_data_;
+
+ private:
+ // The acceleration limits to check for while moving.
+ double peak_subsystem_acceleration_ = 1e10;
+ // The velocity limits to check for while moving.
+ double peak_subsystem_velocity_ = 1e10;
+
+ int32_t sensor_reader_pid_ = 0;
+};
+
+// Tests that the subsystem does nothing when the goal is zero.
+TEST_F(IntakeSystemTest, DoesNothing) {
+ // Intake system uses 0.05 to test for 0.
+ subsystem_data_.goal.unsafe_goal = 0.05;
+ RunForTime(chrono::seconds(5));
+
+ VerifyNearGoal();
+}
+
+// Tests that the subsystem loop can reach a goal.
+TEST_F(IntakeSystemTest, ReachesGoal) {
+ // Set a reasonable goal.
+ auto &goal = subsystem_data_.goal;
+ goal.unsafe_goal = 0.1;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+
+ // 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.
+TEST_F(IntakeSystemTest, SaturationTest) {
+ // Zero it before we move.
+ auto &goal = subsystem_data_.goal;
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.lower;
+ goal.profile_params.max_velocity = 20.0;
+ goal.profile_params.max_acceleration = 0.1;
+ }
+ set_peak_subsystem_velocity(23.0);
+ set_peak_subsystem_acceleration(0.2);
+
+ RunForTime(chrono::seconds(8));
+ VerifyNearGoal();
+
+ // Now do a high acceleration move with a low velocity limit.
+ {
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.profile_params.max_velocity = 0.1;
+ goal.profile_params.max_acceleration = 100;
+ }
+
+ set_peak_subsystem_velocity(0.2);
+ set_peak_subsystem_acceleration(103);
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Tests that the subsystem loop doesn't try and go beyond it's physical range
+// of the mechanisms.
+TEST_F(IntakeSystemTest, RespectsRange) {
+ auto &goal = subsystem_data_.goal;
+ // Set some ridiculous goals to test upper limits.
+ {
+ goal.unsafe_goal = 100.0;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+ }
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ EXPECT_NEAR(TestIntakeSystemValues::kRange.upper,
+ subsystem_data_.status.position, 0.001);
+
+ // Set some ridiculous goals to test lower limits.
+ {
+ goal.unsafe_goal = -100.0;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+ }
+
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ EXPECT_NEAR(TestIntakeSystemValues::kRange.lower,
+ subsystem_data_.status.position, 0.001);
+}
+
+// Tests that the subsystem loop zeroes when run for a while.
+TEST_F(IntakeSystemTest, ZeroTest) {
+ auto &goal = subsystem_data_.goal;
+ {
+ goal.unsafe_goal = TestIntakeSystemValues::kRange.upper;
+ goal.profile_params.max_velocity = 1;
+ goal.profile_params.max_acceleration = 0.5;
+ }
+
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(IntakeSystemTest, ZeroNoGoal) {
+ RunForTime(chrono::seconds(5));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
+ subsystem_.state());
+}
+
+TEST_F(IntakeSystemTest, LowerHardstopStartup) {
+ subsystem_plant_.InitializeSubsystemPosition(
+ TestIntakeSystemValues::kRange.lower_hard);
+ { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper; }
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(IntakeSystemTest, UpperHardstopStartup) {
+ subsystem_plant_.InitializeSubsystemPosition(
+ TestIntakeSystemValues::kRange.upper_hard);
+ { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper; }
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(IntakeSystemTest, ResetTest) {
+ subsystem_plant_.InitializeSubsystemPosition(
+ TestIntakeSystemValues::kRange.upper);
+ {
+ subsystem_data_.goal.unsafe_goal =
+ TestIntakeSystemValues::kRange.upper - 0.1;
+ }
+ RunForTime(chrono::seconds(10));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
+ subsystem_.state());
+
+ VerifyNearGoal();
+ SimulateSensorReset();
+ RunForTime(chrono::milliseconds(100));
+
+ EXPECT_EQ(
+ StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::UNINITIALIZED,
+ subsystem_.state());
+
+ RunForTime(chrono::seconds(10));
+
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
+ subsystem_.state());
+ VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(IntakeSystemTest, DisabledGoalTest) {
+ {
+ subsystem_data_.goal.unsafe_goal =
+ TestIntakeSystemValues::kRange.lower + 0.03;
+ }
+
+ // Checks that the subsystem has not moved from its starting position at 0
+ RunForTime(chrono::milliseconds(100), false);
+ EXPECT_EQ(0.0, subsystem_.goal(0));
+
+ // Now make sure they move correctly
+ RunForTime(chrono::seconds(4), true);
+ EXPECT_NE(0.0, subsystem_.goal(0));
+}
+
+// Tests that zeroing while disabled works.
+TEST_F(IntakeSystemTest, DisabledZeroTest) {
+ { subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower; }
+
+ // Run disabled for 2 seconds
+ RunForTime(chrono::seconds(2), false);
+ EXPECT_EQ(StaticZeroingSingleDOFProfiledSubsystem<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>::State::RUNNING,
+ subsystem_.state());
+
+ RunForTime(chrono::seconds(4), true);
+
+ VerifyNearGoal();
+}
+
+// Tests that set_min_position limits range properly
+TEST_F(IntakeSystemTest, MinPositionTest) {
+ subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.lower_hard;
+ RunForTime(chrono::seconds(2), true);
+
+ // Check that kRange.lower is used as the default min position
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.lower, 0.001);
+
+ // Set min position and check that the subsystem increases to that position
+ subsystem_.set_min_position(TestIntakeSystemValues::kRange.lower + 0.05);
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower + 0.05);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.lower + 0.05, 0.001);
+
+ // Clear min position and check that the subsystem returns to kRange.lower
+ subsystem_.clear_min_position();
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.lower);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.lower, 0.001);
+}
+
+// Tests that set_max_position limits range properly
+TEST_F(IntakeSystemTest, MaxPositionTest) {
+ subsystem_data_.goal.unsafe_goal = TestIntakeSystemValues::kRange.upper_hard;
+ RunForTime(chrono::seconds(2), true);
+
+ // Check that kRange.upper is used as the default max position
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.upper, 0.001);
+
+ // Set max position and check that the subsystem lowers to that position
+ subsystem_.set_max_position(TestIntakeSystemValues::kRange.upper - 0.05);
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper - 0.05);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.upper - 0.05, 0.001);
+
+ // Clear max position and check that the subsystem returns to kRange.upper
+ subsystem_.clear_max_position();
+ RunForTime(chrono::seconds(2), true);
+ EXPECT_EQ(subsystem_.goal(0), TestIntakeSystemValues::kRange.upper);
+ EXPECT_NEAR(subsystem_data_.status.position,
+ TestIntakeSystemValues::kRange.upper, 0.001);
+}
+
+// Tests that the subsystem maintains its current position when sent a null goal
+TEST_F(IntakeSystemTest, NullGoalTest) {
+ subsystem_data_.goal.unsafe_goal =
+ TestIntakeSystemValues::kRange.lower + 0.05;
+ RunForTime(chrono::seconds(2), true);
+
+ VerifyNearGoal();
+
+ // Run with a null goal
+ RunForTime(chrono::seconds(2), true, true);
+
+ // Check that the subsystem has not moved
+ VerifyNearGoal();
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index c843735..be96eea 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -141,6 +141,27 @@
encoder_ratio * (2.0 * M_PI);
}
+ void CopyPosition(
+ const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
+ ::frc971::PotAndIndexPosition *position,
+ ::std::function<double(int32_t)> encoder_translate,
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double pot_offset) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.polled_encoder_value());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.polled_potentiometer_voltage()) +
+ pot_offset;
+ position->latched_encoder =
+ multiplier * encoder_translate(encoder.last_encoder_value());
+ position->latched_pot =
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ pot_offset;
+ position->index_pulses = encoder.index_posedge_count();
+ }
+
double encoder_translate(int32_t value, double counts_per_revolution,
double ratio) {
return static_cast<double>(value) / counts_per_revolution * ratio *
diff --git a/third_party/pycrc/BUILD b/third_party/pycrc/BUILD
new file mode 100644
index 0000000..50a21e1
--- /dev/null
+++ b/third_party/pycrc/BUILD
@@ -0,0 +1,25 @@
+licenses(["notice"])
+
+genrule(
+ name = "copy_main",
+ srcs = [
+ "pycrc.py",
+ ],
+ outs = [
+ "pycrc_main.py",
+ ],
+ cmd = "cp $< $@",
+)
+
+py_binary(
+ name = "pycrc_main",
+ srcs = glob([
+ "pycrc/*.py",
+ ]) + [
+ "pycrc_main.py",
+ ],
+ imports = ["."],
+ legacy_create_init = False,
+ main = "pycrc_main.py",
+ visibility = ["//visibility:public"],
+)
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index ee4b897..787d9a5 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -1,13 +1,13 @@
+#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
-#include <inttypes.h>
-#include <chrono>
-#include <thread>
-#include <mutex>
-#include <functional>
#include <array>
+#include <chrono>
+#include <functional>
+#include <mutex>
+#include <thread>
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Compressor.h"
@@ -234,7 +234,6 @@
autonomous_modes_.at(i) = ::std::move(sensor);
}
-
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
@@ -278,16 +277,15 @@
{
auto superstructure_message = superstructure_queue.position.MakeMessage();
- CopyPotAndIndexPosition(intake_encoder_, &superstructure_message->intake,
- intake_translate, intake_pot_translate, false,
- values.intake.pot_offset);
- CopyPotAndIndexPosition(shoulder_encoder_,
- &superstructure_message->shoulder,
- shoulder_translate, shoulder_pot_translate, false,
- values.shoulder.pot_offset);
- CopyPotAndIndexPosition(wrist_encoder_, &superstructure_message->wrist,
- wrist_translate, wrist_pot_translate, true,
- values.wrist.pot_offset);
+ CopyPosition(intake_encoder_, &superstructure_message->intake,
+ intake_translate, intake_pot_translate, false,
+ values.intake.pot_offset);
+ CopyPosition(shoulder_encoder_, &superstructure_message->shoulder,
+ shoulder_translate, shoulder_pot_translate, false,
+ values.shoulder.pot_offset);
+ CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ wrist_translate, wrist_pot_translate, true,
+ values.wrist.pot_offset);
superstructure_message.Send();
}
@@ -314,27 +312,6 @@
}
private:
- void CopyPotAndIndexPosition(
- const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
- ::frc971::PotAndIndexPosition *position,
- ::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> potentiometer_translate, bool reverse,
- double pot_offset) {
- const double multiplier = reverse ? -1.0 : 1.0;
- position->encoder =
- multiplier * encoder_translate(encoder.polled_encoder_value());
- position->pot = multiplier * potentiometer_translate(
- encoder.polled_potentiometer_voltage()) +
- pot_offset;
- position->latched_encoder =
- multiplier * encoder_translate(encoder.last_encoder_value());
- position->latched_pot =
- multiplier *
- potentiometer_translate(encoder.last_potentiometer_voltage()) +
- pot_offset;
- position->index_pulses = encoder.index_posedge_count();
- }
-
::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
@@ -353,9 +330,7 @@
: pcm_(pcm),
drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
shooter_(".y2016.control_loops.shooter.shooter_queue.output"),
- superstructure_(
- ".y2016.control_loops.superstructure_queue.output") {
- }
+ superstructure_(".y2016.control_loops.superstructure_queue.output") {}
void set_compressor(::std::unique_ptr<Compressor> compressor) {
compressor_ = ::std::move(compressor);
@@ -371,8 +346,7 @@
climber_trigger_ = ::std::move(s);
}
- void set_traverse(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ void set_traverse(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
traverse_ = ::std::move(s);
}
@@ -391,8 +365,7 @@
shooter_pusher_ = ::std::move(s);
}
- void set_lights(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ void set_lights(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
lights_ = ::std::move(s);
}
@@ -462,9 +435,7 @@
{
::frc971::wpilib::PneumaticsToLog to_log;
- {
- to_log.compressor_on = compressor_->Enabled();
- }
+ { to_log.compressor_on = compressor_->Enabled(); }
pcm_->Flush();
to_log.read_solenoids = pcm_->GetAll();
@@ -486,8 +457,7 @@
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
::aos::Queue<::y2016::control_loops::shooter::ShooterQueue::Output> shooter_;
- ::aos::Queue<
- ::y2016::control_loops::SuperstructureQueue::Output>
+ ::aos::Queue<::y2016::control_loops::SuperstructureQueue::Output>
superstructure_;
::std::atomic<bool> run_{true};
diff --git a/y2019/BUILD b/y2019/BUILD
index ad35ead..2eb5e24 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,3 +1,13 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+
+robot_downloader(
+ start_binaries = [
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//y2019/control_loops/drivetrain:drivetrain",
+ ],
+)
+
cc_library(
name = "constants",
srcs = [
@@ -55,7 +65,8 @@
"//third_party/Phoenix-frc-lib:phoenix",
],
)
-cc_library(
+
+cc_binary(
name = "joystick_reader",
srcs = [
":joystick_reader.cc",
@@ -63,9 +74,9 @@
deps = [
"//aos:init",
"//aos/actions:action_lib",
+ "//aos/input:action_joystick_input",
"//aos/input:drivetrain_input",
"//aos/input:joystick_input",
- "//aos/input:action_joystick_input",
"//aos/logging",
"//aos/network:team_number",
"//aos/stl_mutex",
diff --git a/y2019/constants.h b/y2019/constants.h
index e7c178b..e68143c 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -1,8 +1,8 @@
#ifndef Y2019_CONSTANTS_H_
#define Y2019_CONSTANTS_H_
-#include <stdint.h>
#include <math.h>
+#include <stdint.h>
#include "frc971/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
@@ -28,7 +28,7 @@
return kDrivetrainCyclesPerRevolution() * 4;
}
static constexpr double kDrivetrainEncoderRatio() {
- return (52.0 / 24.0);
+ return (24.0 / 52.0);
}
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
diff --git a/y2019/control_loops/python/BUILD b/y2019/control_loops/python/BUILD
index f3e4ca1..b038507 100644
--- a/y2019/control_loops/python/BUILD
+++ b/y2019/control_loops/python/BUILD
@@ -105,9 +105,9 @@
)
py_binary(
- name = "lift",
+ name = "stilts",
srcs = [
- "lift.py",
+ "stilts.py",
],
legacy_create_init = False,
restricted_to = ["//tools:k8"],
diff --git a/y2019/control_loops/python/lift.py b/y2019/control_loops/python/stilts.py
similarity index 82%
rename from y2019/control_loops/python/lift.py
rename to y2019/control_loops/python/stilts.py
index dab2754..8faa5d4 100755
--- a/y2019/control_loops/python/lift.py
+++ b/y2019/control_loops/python/stilts.py
@@ -14,8 +14,8 @@
except gflags.DuplicateFlagError:
pass
-kLift = linear_system.LinearSystemParams(
- name='Lift',
+kStilts = linear_system.LinearSystemParams(
+ name='Stilts',
motor=control_loop.Vex775Pro(),
G=(12.0 / 100.0) * (14.0 / 52.0),
# 5mm pitch, 18 tooth
@@ -34,8 +34,8 @@
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[1.5], [0.0]])
- linear_system.PlotKick(kLift, R)
- linear_system.PlotMotion(kLift, R, max_velocity=5.0)
+ linear_system.PlotKick(kStilts, R)
+ linear_system.PlotMotion(kStilts, R, max_velocity=5.0)
# Write the generated constants out to a file.
if len(argv) != 5:
@@ -43,8 +43,8 @@
'Expected .h file name and .cc file name for the elevator and integral elevator.'
)
else:
- namespaces = ['y2019', 'control_loops', 'superstructure', 'lift']
- linear_system.WriteLinearSystem(kLift, argv[1:3], argv[3:5],
+ namespaces = ['y2019', 'control_loops', 'superstructure', 'stilts']
+ linear_system.WriteLinearSystem(kStilts, argv[1:3], argv[3:5],
namespaces)
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 8a8bca3..871ee15 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -1,3 +1,57 @@
+spi_crc_args = [
+ "$(location //third_party/pycrc:pycrc_main)",
+ "--width=16",
+ # This is the recommendation from
+ # http://users.ece.cmu.edu/~koopman/roses/dsn04/koopman04_crc_poly_embedded.pdf
+ # for messages of 242 - 2048 bits, which covers what we want.
+ # That's an analysis from an exhaustive search of all polynomials for
+ # various CRCs to find the best ones. This is 0xBAAD, converted from the
+ # weird format used there to the standard one used by pycrc.
+ "--poly=0x755b",
+ "--reflect-in=False",
+ "--xor-in=0xffff",
+ "--reflect-out=False",
+ "--xor-out=0xffff",
+ "--std=C99",
+ "--algorithm=table-driven",
+ "--symbol-prefix=jevois_spi_crc_",
+ "--crc-type=uint16_t",
+]
+
+genrule(
+ name = "gen_spi_crc",
+ outs = [
+ "spi_crc.h",
+ "spi_crc.c",
+ ],
+ cmd = " && ".join([
+ " ".join(spi_crc_args + [
+ "--generate=h",
+ "--output=$(location spi_crc.h)",
+ ]),
+ " ".join(spi_crc_args + [
+ "--generate=c",
+ "--output=$(location spi_crc.c)",
+ ]),
+ ]),
+ tools = [
+ "//third_party/pycrc:pycrc_main",
+ ],
+)
+
+cc_library(
+ name = "spi_crc",
+ srcs = [
+ "spi_crc.c",
+ ],
+ hdrs = [
+ "spi_crc.h",
+ ],
+ deps = [
+ "//third_party/GSL",
+ ],
+)
+
cc_library(
name = "structures",
hdrs = [
@@ -9,3 +63,31 @@
"//third_party/eigen",
],
)
+
+cc_library(
+ name = "spi",
+ srcs = [
+ "spi.cc",
+ ],
+ hdrs = [
+ "spi.h",
+ ],
+ deps = [
+ ":spi_crc",
+ ":structures",
+ "//aos/util:bitpacking",
+ "//third_party/GSL",
+ "//third_party/optional",
+ ],
+)
+
+cc_test(
+ name = "spi_test",
+ srcs = [
+ "spi_test.cc",
+ ],
+ deps = [
+ ":spi",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
new file mode 100644
index 0000000..0500860
--- /dev/null
+++ b/y2019/jevois/spi.cc
@@ -0,0 +1,237 @@
+#include "y2019/jevois/spi.h"
+
+#include <assert.h>
+
+#include "aos/util/bitpacking.h"
+#include "third_party/GSL/include/gsl/gsl"
+#include "y2019/jevois/spi_crc.h"
+
+// SPI transfer format (6x 8 bit frames):
+// 1. 1-byte brightness for each beacon channel.
+// 2. 1-byte specifying on/off for each light ring.
+// 3. 2-byte CRC
+//
+// SPI transfer format (41x 8 bit frames):
+// 1. Camera frame 0
+// 2. Camera frame 1
+// 3. Camera frame 2
+// 4. 2-byte CRC-16
+// Each camera frame (13x 8 bit frames):
+// 1. Duration for how old the frame is. This is a value received from the
+// camera, added to the time between the first character being received
+// by the MCU to the CS line being asserted. Specifically it's an 8 bit
+// unsigned number of ms.
+// 2. Target 0
+// 3. Target 1
+// 4. Target 2
+// Each target (4x 8 bit frames):
+// 1. 10 bits heading
+// 2. 8 bits distance
+// 3. 6 bits skew
+// 4. 6 bits height
+// 5. 1 bit target valid (a present frame has all-valid targets)
+// 6. 1 bit target present (a present frame can have from 0 to 3
+// targets, depending on how many were found)
+// Note that empty frames are still sent to indicate that the camera is
+// still working even though it doesn't see any targets.
+
+namespace frc971 {
+namespace jevois {
+namespace {
+
+constexpr float heading_min() { return -3; }
+constexpr float heading_max() { return 3; }
+constexpr int heading_bits() { return 10; }
+constexpr int heading_offset() { return 0; }
+void heading_pack(float heading, gsl::span<char> destination) {
+ const auto integer = aos::FloatToIntLinear<heading_bits()>(
+ heading_min(), heading_max(), heading);
+ aos::PackBits<uint32_t, heading_bits(), heading_offset()>(integer,
+ destination);
+}
+float heading_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, heading_bits(), heading_offset()>(source);
+ return aos::IntToFloatLinear<heading_bits()>(heading_min(), heading_max(),
+ integer);
+}
+
+constexpr float distance_min() { return 0; }
+constexpr float distance_max() {
+ // The field is 18.4m diagonally.
+ return 18.4;
+}
+constexpr int distance_bits() { return 8; }
+constexpr int distance_offset() { return heading_offset() + heading_bits(); }
+void distance_pack(float distance, gsl::span<char> destination) {
+ const auto integer = aos::FloatToIntLinear<distance_bits()>(
+ distance_min(), distance_max(), distance);
+ aos::PackBits<uint32_t, distance_bits(), distance_offset()>(integer,
+ destination);
+}
+float distance_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, distance_bits(), distance_offset()>(source);
+ return aos::IntToFloatLinear<distance_bits()>(distance_min(), distance_max(),
+ integer);
+}
+
+constexpr float skew_min() { return -3; }
+constexpr float skew_max() { return 3; }
+constexpr int skew_bits() { return 6; }
+constexpr int skew_offset() { return distance_offset() + distance_bits(); }
+void skew_pack(float skew, gsl::span<char> destination) {
+ const auto integer =
+ aos::FloatToIntLinear<skew_bits()>(skew_min(), skew_max(), skew);
+ aos::PackBits<uint32_t, skew_bits(), skew_offset()>(integer, destination);
+}
+float skew_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, skew_bits(), skew_offset()>(source);
+ return aos::IntToFloatLinear<skew_bits()>(skew_min(), skew_max(), integer);
+}
+
+constexpr float height_min() { return 0; }
+constexpr float height_max() { return 1.5; }
+constexpr int height_bits() { return 6; }
+constexpr int height_offset() { return skew_offset() + skew_bits(); }
+void height_pack(float height, gsl::span<char> destination) {
+ const auto integer =
+ aos::FloatToIntLinear<height_bits()>(height_min(), height_max(), height);
+ aos::PackBits<uint32_t, height_bits(), height_offset()>(integer, destination);
+}
+float height_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, height_bits(), height_offset()>(source);
+ return aos::IntToFloatLinear<height_bits()>(height_min(), height_max(),
+ integer);
+}
+
+constexpr int valid_bits() { return 1; }
+constexpr int valid_offset() { return height_offset() + height_bits(); }
+void valid_pack(bool valid, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, valid_bits(), valid_offset()>(valid, destination);
+}
+bool valid_unpack(gsl::span<const char> source) {
+ return aos::UnpackBits<uint32_t, valid_bits(), valid_offset()>(source);
+}
+
+constexpr int present_bits() { return 1; }
+constexpr int present_offset() { return valid_offset() + valid_bits(); }
+void present_pack(bool present, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, present_bits(), present_offset()>(present,
+ destination);
+}
+bool present_unpack(gsl::span<const char> source) {
+ return aos::UnpackBits<uint32_t, present_bits(), present_offset()>(source);
+}
+
+constexpr int next_offset() { return present_offset() + present_bits(); }
+static_assert(next_offset() <= 32, "Target is too big");
+
+} // namespace
+
+SpiTransfer SpiPackToRoborio(const TeensyToRoborio &message) {
+ SpiTransfer transfer;
+ gsl::span<char> remaining_space = transfer;
+ for (int frame = 0; frame < 3; ++frame) {
+ for (int target = 0; target < 3; ++target) {
+ remaining_space[0] = 0;
+ remaining_space[1] = 0;
+ remaining_space[2] = 0;
+ remaining_space[3] = 0;
+
+ if (static_cast<int>(message.frames.size()) > frame) {
+ valid_pack(true, remaining_space);
+ if (static_cast<int>(message.frames[frame].targets.size()) > target) {
+ heading_pack(message.frames[frame].targets[target].heading,
+ remaining_space);
+ distance_pack(message.frames[frame].targets[target].distance,
+ remaining_space);
+ skew_pack(message.frames[frame].targets[target].skew,
+ remaining_space);
+ height_pack(message.frames[frame].targets[target].height,
+ remaining_space);
+ present_pack(true, remaining_space);
+ } else {
+ present_pack(false, remaining_space);
+ }
+ } else {
+ valid_pack(false, remaining_space);
+ }
+
+ remaining_space = remaining_space.subspan(4);
+ }
+ if (static_cast<int>(message.frames.size()) > frame) {
+ const uint8_t age_count = message.frames[frame].age.count();
+ memcpy(&remaining_space[0], &age_count, 1);
+ } else {
+ remaining_space[0] = 0;
+ }
+ remaining_space = remaining_space.subspan(1);
+ }
+ {
+ uint16_t crc = jevois_spi_crc_init();
+ crc = jevois_spi_crc_update(crc, transfer.data(),
+ transfer.size() - remaining_space.size());
+ crc = jevois_spi_crc_finalize(crc);
+ assert(static_cast<size_t>(remaining_space.size()) >= sizeof(crc));
+ memcpy(&remaining_space[0], &crc, sizeof(crc));
+ remaining_space = remaining_space.subspan(sizeof(crc));
+ }
+ assert(remaining_space.empty());
+ return transfer;
+}
+
+tl::optional<TeensyToRoborio> SpiUnpackToRoborio(const SpiTransfer &transfer) {
+ TeensyToRoborio message;
+ gsl::span<const char> remaining_input = transfer;
+ for (int frame = 0; frame < 3; ++frame) {
+ const bool have_frame = valid_unpack(remaining_input);
+ if (have_frame) {
+ message.frames.push_back({});
+ }
+ for (int target = 0; target < 3; ++target) {
+ if (present_unpack(remaining_input)) {
+ if (have_frame) {
+ message.frames.back().targets.push_back({});
+ message.frames.back().targets.back().heading =
+ heading_unpack(remaining_input);
+ message.frames.back().targets.back().distance =
+ distance_unpack(remaining_input);
+ message.frames.back().targets.back().skew =
+ skew_unpack(remaining_input);
+ message.frames.back().targets.back().height =
+ height_unpack(remaining_input);
+ }
+ }
+
+ remaining_input = remaining_input.subspan(4);
+ }
+ if (have_frame) {
+ uint8_t age_count;
+ memcpy(&age_count, &remaining_input[0], 1);
+ message.frames.back().age = camera_duration(age_count);
+ }
+ remaining_input = remaining_input.subspan(1);
+ }
+ {
+ uint16_t calculated_crc = jevois_spi_crc_init();
+ calculated_crc =
+ jevois_spi_crc_update(calculated_crc, transfer.data(),
+ transfer.size() - remaining_input.size());
+ calculated_crc = jevois_spi_crc_finalize(calculated_crc);
+ uint16_t received_crc;
+ assert(static_cast<size_t>(remaining_input.size()) >= sizeof(received_crc));
+ memcpy(&received_crc, &remaining_input[0], sizeof(received_crc));
+ remaining_input = remaining_input.subspan(sizeof(received_crc));
+ if (calculated_crc != received_crc) {
+ return tl::nullopt;
+ }
+ }
+ assert(remaining_input.empty());
+ return message;
+}
+
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/jevois/spi.h b/y2019/jevois/spi.h
new file mode 100644
index 0000000..e0c4d90
--- /dev/null
+++ b/y2019/jevois/spi.h
@@ -0,0 +1,34 @@
+#ifndef Y2019_JEVOIS_SPI_H_
+#define Y2019_JEVOIS_SPI_H_
+
+#include <stdint.h>
+
+#include <array>
+
+#include "third_party/optional/tl/optional.hpp"
+#include "y2019/jevois/structures.h"
+
+// This file manages serializing and deserializing the various structures for
+// transport via SPI.
+//
+// Our SPI transfers are fixed-size to simplify everything.
+
+namespace frc971 {
+namespace jevois {
+
+constexpr size_t spi_transfer_size() {
+ // The teensy->RoboRIO side is way bigger, so just calculate that.
+ return 3 /* 3 frames */ *
+ (1 /* age */ + 3 /* targets */ * 4 /* target size */) +
+ 2 /* CRC-16 */;
+}
+static_assert(spi_transfer_size() == 41, "hand math is wrong");
+using SpiTransfer = std::array<char, spi_transfer_size()>;
+
+SpiTransfer SpiPackToRoborio(const TeensyToRoborio &message);
+tl::optional<TeensyToRoborio> SpiUnpackToRoborio(const SpiTransfer &transfer);
+
+} // namespace jevois
+} // namespace frc971
+
+#endif // Y2019_JEVOIS_SPI_H_
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
new file mode 100644
index 0000000..de5158f
--- /dev/null
+++ b/y2019/jevois/spi_test.cc
@@ -0,0 +1,110 @@
+#include "y2019/jevois/spi.h"
+
+#include <stdint.h>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace jevois {
+namespace testing {
+
+// Tests packing and then unpacking an empty message.
+TEST(SpiToRoborioPackTest, Empty) {
+ TeensyToRoborio input_message;
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests that unpacking after the message has been modified results in a
+// checksum failure.
+TEST(SpiToRoborioPackTest, CorruptChecksum) {
+ TeensyToRoborio input_message;
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[0]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[0] ^= 0xFF;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[transfer.size() - 1]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ input_message.frames.push_back({});
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[0]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[3]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ input_message.frames.back().targets.push_back({});
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[3]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+}
+
+// Tests packing and then unpacking a full message.
+TEST(SpiToRoborioPackTest, Full) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().age = camera_duration(9);
+ input_message.frames.push_back({});
+ input_message.frames.back().age = camera_duration(7);
+ input_message.frames.push_back({});
+ input_message.frames.back().age = camera_duration(1);
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests that packing and unpacking a target results in values close to before.
+TEST(SpiToRoborioPackTest, Target) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 9;
+ input_message.frames.back().targets.back().height = 1;
+ input_message.frames.back().targets.back().heading = 0.5;
+ input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.push_back({});
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ ASSERT_EQ(3u, output_message->frames.size());
+ ASSERT_EQ(1u, output_message->frames[0].targets.size());
+ ASSERT_EQ(2u, output_message->frames[1].targets.size());
+ ASSERT_EQ(3u, output_message->frames[2].targets.size());
+ EXPECT_NEAR(input_message.frames.back().targets.back().distance,
+ output_message->frames.back().targets.back().distance, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets.back().height,
+ output_message->frames.back().targets.back().height, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets.back().heading,
+ output_message->frames.back().targets.back().heading, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets.back().skew,
+ output_message->frames.back().targets.back().skew, 0.1);
+}
+
+} // namespace testing
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index d2ab3e5..c82a089 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -41,6 +41,25 @@
// just use floats and not worry about it.
struct Target {
+ bool operator==(const Target &other) const {
+ if (other.distance != distance) {
+ return false;
+ }
+ if (other.height != height) {
+ return false;
+ }
+ if (other.heading != heading) {
+ return false;
+ }
+ if (other.skew != skew) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const Target &other) const {
+ return !(*this == other);
+ }
+
// 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;
@@ -65,6 +84,19 @@
//
// This is all the information sent from each camera to the Teensy.
struct Frame {
+ bool operator==(const Frame &other) const {
+ if (other.targets != targets) {
+ return false;
+ }
+ if (other.age != age) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const Frame &other) const {
+ return !(*this == other);
+ }
+
// The top most interesting targets found in this frame.
aos::SizedArray<Target, 3> targets;
@@ -74,14 +106,34 @@
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
+ bool operator==(const CameraCalibration &other) const {
+ if (other.calibration != calibration) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const CameraCalibration &other) const {
+ return !(*this == other);
+ }
+
// The calibration matrix. This defines where the camera is pointing.
//
- // TODO(Parker): What are the details on how this is defined.
+ // TODO(Parker): What are the details on how this is defined?
Eigen::Matrix<float, 3, 4> calibration;
};
// This is all the information the Teensy sends to the RoboRIO.
struct TeensyToRoborio {
+ bool operator==(const TeensyToRoborio &other) const {
+ if (other.frames != frames) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const TeensyToRoborio &other) const {
+ return !(*this == other);
+ }
+
// The newest frames received from up to three cameras. These will be the
// three earliest-received of all buffered frames.
aos::SizedArray<Frame, 3> frames;
@@ -89,6 +141,19 @@
// This is all the information the RoboRIO sends to the Teensy.
struct RoborioToTeensy {
+ bool operator==(const RoborioToTeensy &other) const {
+ if (other.beacon_brightness != beacon_brightness) {
+ return false;
+ }
+ if (other.light_rings != light_rings) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const RoborioToTeensy &other) const {
+ return !(*this == other);
+ }
+
// Brightnesses for each of the beacon light channels. 0 is off, 255 is fully
// on.
std::array<uint8_t, 3> beacon_brightness;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index c696a37..bc6c25f 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -169,9 +169,9 @@
::frc971::wpilib::DrivetrainWriter drivetrain_writer;
drivetrain_writer.set_left_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), false);
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), false);
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
// Wait forever. Not much else to do...