Merge "Change the event-loop restriction"
diff --git a/README.md b/README.md
index eef71a6..e572ea9 100644
--- a/README.md
+++ b/README.md
@@ -98,7 +98,7 @@
```console
# Freshly imaged roboRIOs need to be configured to run the 971 code
# at startup. This is done by using the setup_roborio.sh script.
-setup_roborio.sh roboRIO-971-frc.local
+bazel run -c opt //aos/config:setup_roborio -- roboRIO-XXX-frc.local
```
### Some other useful packages
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/input/BUILD b/aos/input/BUILD
index dd79c5e..0da8d01 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -48,3 +48,18 @@
"//aos/robot_state",
],
)
+
+cc_library(
+ name = "action_joystick_input",
+ srcs = ["action_joystick_input.cc"],
+ hdrs = ["action_joystick_input.h"],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/logging",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
+ ],
+)
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
new file mode 100644
index 0000000..991ea17
--- /dev/null
+++ b/aos/input/action_joystick_input.cc
@@ -0,0 +1,50 @@
+#include "aos/input/action_joystick_input.h"
+
+#include "aos/input/driver_station_data.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+
+using ::aos::input::driver_station::ControlBit;
+
+namespace aos {
+namespace input {
+
+void ActionJoystickInput::RunIteration(
+ const ::aos::input::driver_station::Data &data) {
+ const bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ if (!auto_running_) {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+}
+
+void ActionJoystickInput::StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+ action_queue_.EnqueueAction(::frc971::autonomous::MakeAutonomousAction(0));
+}
+
+void ActionJoystickInput::StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+}
+
+} // namespace input
+} // namespace aos
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
new file mode 100644
index 0000000..d4023ca
--- /dev/null
+++ b/aos/input/action_joystick_input.h
@@ -0,0 +1,48 @@
+#ifndef AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+#define AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+
+namespace aos {
+namespace input {
+
+// Class to abstract out managing actions, autonomous mode, and drivetrains.
+// Turns out we do the same thing every year, so let's stop copying it.
+class ActionJoystickInput : public ::aos::input::JoystickInput {
+ public:
+ ActionJoystickInput(
+ ::aos::EventLoop *event_loop,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config)
+ : ::aos::input::JoystickInput(event_loop),
+ drivetrain_input_reader_(DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kPistol, dt_config)) {}
+
+ virtual ~ActionJoystickInput() {}
+
+ private:
+ // Handles any year specific superstructure code.
+ virtual void HandleTeleop(const ::aos::input::driver_station::Data &data) = 0;
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override;
+
+ void StartAuto();
+ void StopAuto();
+
+ // True if the internal state machine thinks auto is running right now.
+ bool auto_running_ = false;
+ // True if an action was running last cycle.
+ bool was_running_ = false;
+
+ ::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+} // namespace input
+} // namespace aos
+
+#endif // AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 1ba8bf8..67ee9f3 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -64,8 +64,6 @@
new_drivetrain_goal->controller_type = is_control_loop_driving ? 1 : 0;
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
- new_drivetrain_goal->left_velocity_goal = 0;
- new_drivetrain_goal->right_velocity_goal = 0;
new_drivetrain_goal->linear.max_velocity = 3.0;
new_drivetrain_goal->linear.max_acceleration = 20.0;
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/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 04ee334..ac1b3b7 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -34,9 +34,7 @@
.wheel(0.0)
.throttle(0.0)
.left_goal(initial_drivetrain_.left)
- .left_velocity_goal(0)
.right_goal(initial_drivetrain_.right)
- .right_velocity_goal(0)
.max_ss_voltage(max_drivetrain_voltage_)
.Send();
}
@@ -63,9 +61,7 @@
drivetrain_message->wheel = 0.0;
drivetrain_message->throttle = 0.0;
drivetrain_message->left_goal = initial_drivetrain_.left;
- drivetrain_message->left_velocity_goal = 0;
drivetrain_message->right_goal = initial_drivetrain_.right;
- drivetrain_message->right_velocity_goal = 0;
drivetrain_message->max_ss_voltage = max_drivetrain_voltage_;
drivetrain_message->linear = linear;
drivetrain_message->angular = angular;
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/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 2e78e51..c66b03e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -40,14 +40,14 @@
message Goal {
// Position of the steering wheel (positive = turning left when going
// forwards).
- double wheel;
- double wheel_velocity;
- double wheel_torque;
+ float wheel;
+ float wheel_velocity;
+ float wheel_torque;
// Position of the throttle (positive forwards).
- double throttle;
- double throttle_velocity;
- double throttle_torque;
+ float throttle;
+ float throttle_velocity;
+ float throttle_torque;
// True to shift into high, false to shift into low.
bool highgear;
@@ -66,12 +66,7 @@
double left_goal;
double right_goal;
- // Velocity goal for each drivetrain side (in m/s) when the closed-loop
- // controller is active.
- double left_velocity_goal;
- double right_velocity_goal;
-
- double max_ss_voltage;
+ float max_ss_voltage;
// Motion profile parameters.
// The control loop will profile if these are all non-zero.
@@ -127,10 +122,6 @@
double uncapped_left_voltage;
double uncapped_right_voltage;
- // The goal velocities for the polydrive controller.
- double left_velocity_goal;
- double right_velocity_goal;
-
// The voltage error for the left and right sides.
double left_voltage_error;
double right_voltage_error;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 5c1a8b4..17da3ab 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -392,8 +392,6 @@
goal->controller_type = 1;
goal->left_goal = 4.0;
goal->right_goal = 4.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 1.0;
goal->linear.max_acceleration = 3.0;
goal->angular.max_velocity = 1.0;
@@ -423,8 +421,6 @@
goal->controller_type = 1;
goal->left_goal = -1.0;
goal->right_goal = 1.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 1.0;
goal->linear.max_acceleration = 3.0;
goal->angular.max_velocity = 1.0;
@@ -454,8 +450,6 @@
goal->controller_type = 1;
goal->left_goal = 5.0;
goal->right_goal = 4.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 6.0;
goal->linear.max_acceleration = 4.0;
goal->angular.max_velocity = 2.0;
@@ -510,8 +504,6 @@
goal->controller_type = 1;
goal->left_goal = 5.0;
goal->right_goal = 4.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 1.0;
goal->linear.max_acceleration = 2.0;
goal->angular.max_velocity = 1.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
index aed0773..4101993 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
@@ -37,8 +37,6 @@
control_loop_driving = false;
left_goal = 0.0f;
right_goal = 0.0f;
- left_velocity_goal = 0.0f;
- right_velocity_goal = 0.0f;
max_ss_voltage = 0.0f;
//linear.max_velocity = 0.0f;
//linear.max_acceleration = 0.0f;
@@ -80,8 +78,6 @@
estimated_right_velocity = 0.0f;
uncapped_left_voltage = 0.0f;
uncapped_right_voltage = 0.0f;
- left_velocity_goal = 0.0f;
- right_velocity_goal = 0.0f;
left_voltage_error = 0.0f;
right_voltage_error = 0.0f;
profiled_left_position_goal = 0.0f;
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.h b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
index 7fcdea1..f72b4be 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.h
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
@@ -44,8 +44,6 @@
bool control_loop_driving;
float left_goal;
float right_goal;
- float left_velocity_goal;
- float right_velocity_goal;
float max_ss_voltage;
};
@@ -86,8 +84,6 @@
float estimated_right_velocity;
float uncapped_left_voltage;
float uncapped_right_voltage;
- float left_velocity_goal;
- float right_velocity_goal;
float left_voltage_error;
float right_voltage_error;
float profiled_left_position_goal;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index cc3468a..f8f1a39 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -427,8 +427,6 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) {
- status->left_velocity_goal = goal_left_velocity_;
- status->right_velocity_goal = goal_right_velocity_;
status->cim_logging.left_in_gear = IsInGear(left_gear_);
status->cim_logging.left_motor_speed = left_motor_speed_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 0d01622..292f291 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -146,8 +146,7 @@
void DrivetrainMotorsSS::SetGoal(
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- unprofiled_goal_ << goal.left_goal, goal.left_velocity_goal, goal.right_goal,
- goal.right_velocity_goal, 0.0, 0.0, 0.0;
+ unprofiled_goal_ << goal.left_goal, 0.0, goal.right_goal, 0.0, 0.0, 0.0, 0.0;
if (::std::abs(goal.max_ss_voltage) < 0.01) {
max_voltage_ = kMaxVoltage;
} else {
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..f3015c6 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -78,7 +78,7 @@
.frc971.HallEffectAndPositionEstimatorState estimator_state;
};
-struct AbsoluteProfiledJointStatus {
+struct PotAndAbsoluteEncoderProfiledJointStatus {
// Is the subsystem zeroed?
bool zeroed;
@@ -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/angular_system.py b/frc971/control_loops/python/angular_system.py
index 8d375f8..a31390a 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -371,6 +371,10 @@
angular_system = AngularSystem(params, params.name)
loop_writer = control_loop.ControlLoopWriter(
angular_system.name, [angular_system], namespaces=year_namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', angular_system.G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', angular_system.motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
integral_angular_system = IntegralAngularSystem(params,
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 9edbb47..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):
@@ -144,7 +144,6 @@
self.selected_points = []
self.splines = []
- self.spline = []
self.reinit_extents()
self.inStart = None
@@ -156,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))
@@ -189,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)
@@ -204,8 +204,6 @@
self.startSet = True
else:
self.inEnd = [self.index_of_edit, self.findDistance()]
- self.spline[self.spline_edit].addConstraint(
- self.inStart, self.inEnd, self.inConstraint, self.inValue)
self.startSet = False
self.mode = Mode.kEditing
self.spline_edit = -1
@@ -225,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):
@@ -237,7 +235,7 @@
set_color(cr, palette["GREY"])
cr.paint()
#Scale the field to fit within drawing area
- cr.scale(0.5,0.5)
+ cr.scale(0.5, 0.5)
# Draw a extents rectangle
set_color(cr, palette["WHITE"])
@@ -258,33 +256,8 @@
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
- for x, i in enumerate(self.spline):
- for j in i.constraints:
- cr.move_to(-650, -y * 10 + 320)
- set_color(cr, palette["BLACK"])
- display_text(
- cr, str("Spline " + str(x) + ": " + str(j.toString())),
- 0.5, 0.5, 2, 2)
- y += 1
# update all the things
@@ -318,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:
@@ -327,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])
@@ -347,7 +317,6 @@
spline.Point(k - 0.01)[0],
spline.Point(k - 0.01)[1]
]
-
holder_spline.append(holding)
self.curves.append(holder_spline)
@@ -357,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:
@@ -477,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:
@@ -489,9 +458,6 @@
self.splines[spline_edit][1] = f * 2 + e * -1
self.splines[spline_edit][2] = d + f * 4 + e * -4
- self.spline[spline_edit].point = self.splines[spline_edit]
- self.spline[spline_edit].math()
-
if not self.spline_edit == 0:
spline_edit = self.spline_edit - 1
a = self.splines[self.spline_edit][0]
@@ -501,11 +467,6 @@
self.splines[spline_edit][4] = a * 2 + b * -1
self.splines[spline_edit][3] = c + a * 4 + b * -4
- self.spline[spline_edit].point = self.splines[spline_edit]
- self.spline[spline_edit].math()
-
- self.spline[self.spline_edit].edit(self.index_of_edit,
- [self.x, self.y])
self.index_of_edit = -1
self.spline_edit = -1
else:
@@ -513,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):
@@ -539,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()
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..2709315
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -0,0 +1,217 @@
+#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::PotAndAbsoluteEncoderProfiledJointStatus
+ *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::PotAndAbsoluteEncoderProfiledJointStatus *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..df26c56
--- /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::PotAndAbsoluteEncoderProfiledJointStatus 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/BUILD b/frc971/wpilib/BUILD
index 9cf14ad..c2707ec 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -299,3 +299,22 @@
"//third_party:wpilib",
],
)
+
+cc_library(
+ name = "drivetrain_writer",
+ srcs = [
+ "drivetrain_writer.cc",
+ ],
+ hdrs = [
+ "drivetrain_writer.h",
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ ":loop_output_handler",
+ "//aos:math",
+ "//aos/logging",
+ "//aos/logging:queue_logging",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//third_party:wpilib",
+ ],
+)
diff --git a/frc971/wpilib/ahal/Spark.cc b/frc971/wpilib/ahal/Spark.cc
new file mode 100644
index 0000000..9127931
--- /dev/null
+++ b/frc971/wpilib/ahal/Spark.cc
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc971/wpilib/ahal/Spark.h"
+
+#include <HAL/HAL.h>
+
+using namespace frc;
+
+Spark::Spark(int channel) : PWM(channel) {
+ /* Note that the Spark uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Spark User Manual available
+ * from REV Robotics.
+ *
+ * 2.003ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.46ms = the "low end" of the deadband range
+ * 0.999ms = full "reverse"
+ */
+ SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+}
diff --git a/frc971/wpilib/ahal/Spark.h b/frc971/wpilib/ahal/Spark.h
new file mode 100644
index 0000000..f33130b
--- /dev/null
+++ b/frc971/wpilib/ahal/Spark.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc971/wpilib/ahal/PWM.h"
+
+namespace frc {
+
+/**
+ * Vex Robotics Victor SP Speed Controller
+ */
+class Spark : public PWM {
+ public:
+ explicit Spark(int channel);
+ virtual ~Spark() = default;
+};
+
+} // namespace frc
diff --git a/frc971/wpilib/drivetrain_writer.cc b/frc971/wpilib/drivetrain_writer.cc
new file mode 100644
index 0000000..8d388dc
--- /dev/null
+++ b/frc971/wpilib/drivetrain_writer.cc
@@ -0,0 +1,48 @@
+#include "frc971/wpilib/drivetrain_writer.h"
+
+#include "aos/commonmath.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/wpilib/ahal/PWM.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void DrivetrainWriter::Write() {
+ auto &queue = ::frc971::control_loops::drivetrain_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, queue->left_voltage));
+ right_controller0_->SetSpeed(
+ SafeSpeed(reversed_right0_, queue->right_voltage));
+
+ if (left_controller1_) {
+ left_controller1_->SetSpeed(
+ SafeSpeed(reversed_left1_, queue->left_voltage));
+ }
+ if (right_controller1_) {
+ right_controller1_->SetSpeed(
+ SafeSpeed(reversed_right1_, queue->right_voltage));
+ }
+}
+
+void DrivetrainWriter::Read() {
+ ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
+}
+
+void DrivetrainWriter::Stop() {
+ LOG(WARNING, "drivetrain output too old\n");
+ left_controller0_->SetDisabled();
+ right_controller0_->SetDisabled();
+
+ if (left_controller1_) {
+ left_controller1_->SetDisabled();
+ }
+ if (right_controller1_) {
+ right_controller1_->SetDisabled();
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
new file mode 100644
index 0000000..61fa0ff
--- /dev/null
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -0,0 +1,53 @@
+#ifndef FRC971_WPILIB_DRIVETRAIN_WRITER_H_
+#define FRC971_WPILIB_DRIVETRAIN_WRITER_H_
+
+#include "aos/commonmath.h"
+
+#include "frc971/wpilib/ahal/PWM.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ left_controller0_ = ::std::move(t);
+ reversed_left0_ = reversed;
+ }
+
+ void set_right_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ right_controller0_ = ::std::move(t);
+ reversed_right0_ = reversed;
+ }
+
+ void set_left_controller1(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ left_controller1_ = ::std::move(t);
+ reversed_left1_ = reversed;
+ }
+
+ void set_right_controller1(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ right_controller1_ = ::std::move(t);
+ reversed_right1_ = reversed;
+ }
+
+ private:
+ void Write() override;
+ void Read() override;
+ void Stop() override;
+
+ double SafeSpeed(bool reversed, double voltage) {
+ return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) /
+ 12.0);
+ }
+
+ ::std::unique_ptr<::frc::PWM> left_controller0_, right_controller0_,
+ left_controller1_, right_controller1_;
+
+ bool reversed_right0_, reversed_left0_, reversed_right1_, reversed_left1_;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_DRIVETRAIN_WRITER_H_
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index 3516168..42fb86b 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -271,6 +271,24 @@
::std::unique_ptr<frc::AnalogInput> potentiometer_;
};
+class AbsoluteEncoder {
+ public:
+ void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
+ duty_cycle_.set_input(::std::move(input));
+ }
+
+ void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ encoder_ = ::std::move(encoder);
+ }
+
+ double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
+ int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
+
+ private:
+ DutyCycleReader duty_cycle_;
+ ::std::unique_ptr<frc::Encoder> encoder_;
+};
+
} // namespace wpilib
} // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index dc33698..f5372bf 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -42,15 +42,6 @@
drivetrain_right_encoder_->SetMaxPeriod(0.005);
}
-// All of the DMA-related set_* calls must be made before this, and it
-// doesn't hurt to do all of them.
-// TODO(austin): Does anyone actually do anything other than set this? Or can
-// we just take care of that automatically?
-void SensorReader::set_dma(::std::unique_ptr<DMA> dma) {
- dma_synchronizer_.reset(
- new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
-}
-
void SensorReader::set_pwm_trigger(
::std::unique_ptr<frc::DigitalInput> pwm_trigger) {
fast_encoder_filter_.Add(pwm_trigger.get());
@@ -130,7 +121,9 @@
int32_t my_pid = getpid();
Start();
- dma_synchronizer_->Start();
+ if (dma_synchronizer_) {
+ dma_synchronizer_->Start();
+ }
if (pwm_trigger_) {
last_period_ = chrono::microseconds(5050);
@@ -160,8 +153,10 @@
::frc971::wpilib::SendRobotState(my_pid);
RunIteration();
- dma_synchronizer_->RunIteration();
- RunDmaIteration();
+ if (dma_synchronizer_) {
+ dma_synchronizer_->RunIteration();
+ RunDmaIteration();
+ }
if (pwm_trigger_) {
monotonic_clock::time_point last_tick_timepoint;
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index fb25360..be96eea 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -34,10 +34,12 @@
// Sets the right drivetrain encoder.
void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder);
- // Sets the dma.
- void set_dma(::std::unique_ptr<DMA> dma);
-
+ // Adds a sensor to DMA.
void AddToDMA(DMASampleHandlerInterface *handler) {
+ if (!dma_synchronizer_) {
+ dma_synchronizer_.reset(
+ new ::frc971::wpilib::DMASynchronizer(std::make_unique<DMA>()));
+ }
dma_synchronizer_->Add(handler);
}
@@ -48,6 +50,8 @@
void Quit() { run_ = false; }
virtual void RunIteration() = 0;
+ // Runs the DMA iteration after the synchronizer. This only gets run if a
+ // sensor has been added to DMA.
virtual void RunDmaIteration() {}
void operator()();
@@ -118,6 +122,46 @@
encoder_ratio);
}
+ // Copies a Absolute Encoder with the correct unit
+ // and direction changes.
+ void CopyPosition(
+ const ::frc971::wpilib::AbsoluteEncoder &encoder,
+ ::frc971::AbsolutePosition *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+
+ position->absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
+ : encoder.ReadAbsoluteEncoder()) *
+ 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/y2012/BUILD b/y2012/BUILD
index 159632b..2841ee4 100644
--- a/y2012/BUILD
+++ b/y2012/BUILD
@@ -51,6 +51,7 @@
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 46b20f6..849f4a5 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -105,8 +105,6 @@
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index 27e8b77..0fa0f45 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -34,6 +34,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
@@ -181,38 +182,6 @@
::std::atomic<bool> run_{true};
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
- left_drivetrain_talon_ = ::std::move(t);
- }
-
- void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
- right_drivetrain_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- left_drivetrain_talon_->SetSpeed(-queue->left_voltage / 12.0);
- right_drivetrain_talon_->SetSpeed(queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- left_drivetrain_talon_->SetDisabled();
- right_drivetrain_talon_->SetDisabled();
- }
-
- ::std::unique_ptr<Talon> left_drivetrain_talon_;
- ::std::unique_ptr<Talon> right_drivetrain_talon_;
-};
-
class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
void set_talon1(::std::unique_ptr<Talon> t) {
@@ -259,18 +228,17 @@
::std::thread joystick_thread(::std::ref(joystick_sender));
SensorReader reader;
- reader.set_dma(make_unique<DMA>());
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
::std::thread reader_thread(::std::ref(reader));
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_left_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(3)));
- drivetrain_writer.set_right_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(4)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<Talon>(new Talon(3)), true);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<Talon>(new Talon(4)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
::y2012::wpilib::AccessoriesWriter accessories_writer;
diff --git a/y2014/BUILD b/y2014/BUILD
index d83a477..bf8f689 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -93,6 +93,7 @@
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 9bf98a2..7275d0d 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -129,8 +129,6 @@
.highgear(true)
.left_goal(left_goal_state(0, 0) + params.left_initial_position)
.right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
.Send();
}
if (ShouldCancel()) return true;
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index 3ed2cd1..ef59ba4 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -55,9 +55,7 @@
.controller_type(1)
.highgear(true)
.left_goal(left_initial_position)
- .left_velocity_goal(0)
.right_goal(right_initial_position)
- .right_velocity_goal(0)
.quickturn(false)
.Send();
}
@@ -70,9 +68,7 @@
.wheel(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
- .left_velocity_goal(0)
.right_goal(right_initial_position)
- .right_velocity_goal(0)
.Send();
}
@@ -103,8 +99,6 @@
.highgear(true)
.left_goal(left_goal)
.right_goal(right_goal)
- .left_velocity_goal(0.0)
- .right_velocity_goal(0.0)
.Send();
left_initial_position = left_goal;
right_initial_position = right_goal;
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index cc29bcb..9c32aa4 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -233,8 +233,6 @@
.controller_type(is_control_loop_driving ? 1 : 0)
.left_goal(left_goal)
.right_goal(right_goal)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index d7aa8ee..0ad900e 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -34,6 +34,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
@@ -513,38 +514,6 @@
::std::atomic<bool> run_{true};
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
- left_drivetrain_talon_ = ::std::move(t);
- }
-
- void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
- right_drivetrain_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- left_drivetrain_talon_->SetSpeed(-queue->left_voltage / 12.0);
- right_drivetrain_talon_->SetSpeed(queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- left_drivetrain_talon_->SetDisabled();
- right_drivetrain_talon_->SetDisabled();
- }
-
- ::std::unique_ptr<Talon> left_drivetrain_talon_;
- ::std::unique_ptr<Talon> right_drivetrain_talon_;
-};
-
class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
void set_shooter_talon(::std::unique_ptr<Talon> t) {
@@ -677,17 +646,16 @@
reader.set_shooter_plunger(make_unique<DigitalInput>(8)); // S3
reader.set_shooter_latch(make_unique<DigitalInput>(9)); // S4
- reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
::frc971::wpilib::GyroSender gyro_sender;
::std::thread gyro_thread(::std::ref(gyro_sender));
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_left_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
- drivetrain_writer.set_right_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(2)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<Talon>(new Talon(5)), true);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<Talon>(new Talon(2)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
::y2014::wpilib::ClawWriter claw_writer;
diff --git a/y2014_bot3/BUILD b/y2014_bot3/BUILD
index 1118d09..25a6445 100644
--- a/y2014_bot3/BUILD
+++ b/y2014_bot3/BUILD
@@ -54,6 +54,7 @@
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:joystick_sender",
"//frc971/wpilib:logging_queue",
diff --git a/y2014_bot3/actions/drivetrain_action.cc b/y2014_bot3/actions/drivetrain_action.cc
deleted file mode 100644
index 22c668e..0000000
--- a/y2014_bot3/actions/drivetrain_action.cc
+++ /dev/null
@@ -1,166 +0,0 @@
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/util/phased_loop.h"
-#include "aos/logging/logging.h"
-#include "aos/util/trapezoid_profile.h"
-#include "aos/commonmath.h"
-
-#include "bot3/actions/drivetrain_action.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-namespace bot3 {
-namespace actions {
-
-namespace chrono = ::std::chrono;
-
-DrivetrainAction::DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s)
- : ::frc971::actions::ActionBase
- <::frc971::actions::DrivetrainActionQueueGroup>(s) {}
-
-void DrivetrainAction::RunAction() {
- static const auto K = control_loops::MakeDrivetrainLoop().K();
-
- const double yoffset = action_q_->goal->y_offset;
- const double turn_offset =
- action_q_->goal->theta_offset * control_loops::kBot3TurnWidth / 2.0;
- LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(chrono::milliseconds(10));
- ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(10));
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
- ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
-
- profile.set_maximum_acceleration(action_q_->goal->maximum_acceleration);
- profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
- turn_profile.set_maximum_acceleration(
- 10.0 * control_loops::kBot3TurnWidth / 2.0);
- turn_profile.set_maximum_velocity(3.0 * control_loops::kBot3TurnWidth / 2.0);
-
- while (true) {
- // wait until next 10ms tick
- ::aos::time::PhasedLoop10MS(5000);
-
- control_loops::drivetrain.status.FetchLatest();
- if (control_loops::drivetrain.status.get()) {
- const auto &status = *control_loops::drivetrain.status;
- if (::std::abs(status.uncapped_left_voltage -
- status.uncapped_right_voltage) >
- 24) {
- LOG(DEBUG, "spinning in place\n");
- // They're more than 24V apart, so stop moving forwards and let it deal
- // with spinning first.
- profile.SetGoal(
- (status.estimated_left_position + status.estimated_right_position) /
- 2.0);
- } else {
- static const double divisor = K(0, 0) + K(0, 2);
- double dx_left, dx_right;
- if (status.uncapped_left_voltage > 12.0) {
- dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
- double dx;
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(FATAL, "no drivetrain status!\n");
- }
-
- const auto drive_profile_goal_state =
- profile.Update(yoffset, goal_velocity);
- const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
- left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
- right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
-
- if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
- ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
- break;
- }
- if (ShouldCancel()) return;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + action_q_->goal->left_initial_position,
- right_goal_state(0, 0) + action_q_->goal->right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
- .right_goal(right_goal_state(0, 0) + action_q_->goal->right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
- .Send();
- }
- if (ShouldCancel()) return;
- control_loops::drivetrain.status.FetchLatest();
- while (!control_loops::drivetrain.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- control_loops::drivetrain.status.FetchNextBlocking();
- if (ShouldCancel()) return;
- }
- while (true) {
- if (ShouldCancel()) return;
- const double kPositionThreshold = 0.05;
-
- const double left_error = ::std::abs(
- control_loops::drivetrain.status->estimated_left_position -
- (left_goal_state(0, 0) + action_q_->goal->left_initial_position));
- const double right_error = ::std::abs(
- control_loops::drivetrain.status->estimated_right_position -
- (right_goal_state(0, 0) + action_q_->goal->right_initial_position));
- const double velocity_error =
- ::std::abs(control_loops::drivetrain.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- control_loops::drivetrain.status.FetchNextBlocking();
- }
- LOG(INFO, "Done moving\n");
-}
-
-::std::unique_ptr<::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
-MakeDrivetrainAction() {
- return ::std::unique_ptr<
- ::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
- new ::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
- &::frc971::actions::drivetrain_action));
-}
-
-} // namespace actions
-} // namespace bot3
diff --git a/y2014_bot3/actions/drivetrain_action.h b/y2014_bot3/actions/drivetrain_action.h
deleted file mode 100644
index fc4b837..0000000
--- a/y2014_bot3/actions/drivetrain_action.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef BOT3_ACTIONS_DRIVETRAIN_ACTION_H_
-#define BOT3_ACTIONS_DRIVETRAIN_ACTION_H_
-
-#include <memory>
-
-#include "frc971/actions/action.h"
-#include "frc971/actions/action_client.h"
-#include "frc971/actions/drivetrain_action.q.h"
-
-namespace bot3 {
-namespace actions {
-
-class DrivetrainAction : public
- ::frc971::actions::ActionBase<::frc971::actions::DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s);
-
- virtual void RunAction();
-};
-
-// Makes a new DrivetrainAction action.
-::std::unique_ptr<::frc971::TypedAction
- < ::frc971::actions::DrivetrainActionQueueGroup>>
-MakeDrivetrainAction();
-
-} // namespace actions
-} // namespace bot3
-
-#endif
diff --git a/y2014_bot3/actions/drivetrain_action_main.cc b/y2014_bot3/actions/drivetrain_action_main.cc
deleted file mode 100644
index 84042c6..0000000
--- a/y2014_bot3/actions/drivetrain_action_main.cc
+++ /dev/null
@@ -1,17 +0,0 @@
-#include <stdio.h>
-
-#include "aos/logging/logging.h"
-#include "aos/init.h"
-#include "bot3/actions/drivetrain_action.h"
-#include "frc971/actions/drivetrain_action.q.h"
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
-
- bot3::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
-
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index 943d809..56d72f1 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
@@ -205,39 +206,6 @@
::std::atomic<bool> run_{true};
};
-// Writes out drivetrain voltages.
-class DrivetrainWriter : public LoopOutputHandler {
- public:
- void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
- left_drivetrain_talon_ = ::std::move(t);
- }
-
- void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
- right_drivetrain_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- left_drivetrain_talon_->SetSpeed(-queue->left_voltage / 12.0);
- right_drivetrain_talon_->SetSpeed(queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- left_drivetrain_talon_->SetDisabled();
- right_drivetrain_talon_->SetDisabled();
- }
-
- ::std::unique_ptr<Talon> left_drivetrain_talon_;
- ::std::unique_ptr<Talon> right_drivetrain_talon_;
-};
-
// Writes out rollers voltages.
class RollersWriter : public LoopOutputHandler {
public:
@@ -319,11 +287,11 @@
::std::thread gyro_thread(::std::ref(gyro_sender));
// Outputs
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_left_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
- drivetrain_writer.set_right_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(2)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<Talon>(new Talon(5)), true);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<Talon>(new Talon(2)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
RollersWriter rollers_writer;
diff --git a/y2016/BUILD b/y2016/BUILD
index 9c9afeb..1a00044 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -92,6 +92,7 @@
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 0716a97..b805747 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -67,8 +67,6 @@
.controller_type(1)
.left_goal(left_current + side_distance_change)
.right_goal(right_current - side_distance_change)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending drivetrain goal failed\n");
}
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 6887a53..19cd065 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -167,8 +167,6 @@
.controller_type(is_control_loop_driving ? 1 : 0)
.left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
.right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 52d0b1b..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"
@@ -38,6 +38,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
@@ -233,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.
@@ -277,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();
}
@@ -313,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_;
@@ -352,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);
@@ -370,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);
}
@@ -390,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);
}
@@ -461,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();
@@ -485,44 +457,12 @@
::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};
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left_talon(::std::unique_ptr<Talon> t) {
- drivetrain_left_talon_ = ::std::move(t);
- }
-
- void set_drivetrain_right_talon(::std::unique_ptr<Talon> t) {
- drivetrain_right_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_talon_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_right_talon_->SetSpeed(-queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_talon_->SetDisabled();
- drivetrain_right_talon_->SetDisabled();
- }
-
- ::std::unique_ptr<Talon> drivetrain_left_talon_, drivetrain_right_talon_;
-};
-
class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
void set_shooter_left_talon(::std::unique_ptr<Talon> t) {
@@ -659,7 +599,6 @@
reader.set_autonomous_mode(2, make_unique<DigitalInput>(7));
reader.set_autonomous_mode(3, make_unique<DigitalInput>(6));
- reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
// TODO(Brian): This interacts poorly with the SpiRxClearer in ADIS16448.
@@ -670,11 +609,11 @@
::frc971::wpilib::ADIS16448 imu(SPI::Port::kMXP, imu_trigger.get());
::std::thread imu_thread(::std::ref(imu));
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
- drivetrain_writer.set_drivetrain_right_talon(
- ::std::unique_ptr<Talon>(new Talon(4)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<Talon>(new Talon(5)), false);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<Talon>(new Talon(4)), true);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
ShooterWriter shooter_writer;
diff --git a/y2017/BUILD b/y2017/BUILD
index e88c725..8637eed 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -73,6 +73,7 @@
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index 89145a4..76c44a4 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -32,7 +32,7 @@
void Intake::Iterate(
const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index 40f49fc..db1442a 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -34,7 +34,8 @@
void Iterate(const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *status);
void Reset();
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 60c5e16..5c0771c 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -200,7 +200,7 @@
bool estopped;
// Each subsystems status.
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
.frc971.control_loops.IndexProfiledJointStatus hood;
ShooterStatus shooter;
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 8bcb9b5..a5235c9 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -41,6 +41,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
@@ -330,38 +331,6 @@
::std::atomic<bool> run_{true};
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left_victor_ = ::std::move(t);
- }
-
- void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(-queue->left_voltage / 12.0);
- drivetrain_right_victor_->SetSpeed(queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_victor_->SetDisabled();
- drivetrain_right_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
- drivetrain_right_victor_;
-};
-
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
@@ -498,7 +467,6 @@
reader.set_pwm_trigger(make_unique<DigitalInput>(7));
- reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
auto imu_trigger = make_unique<DigitalInput>(3);
@@ -508,11 +476,11 @@
imu.set_reset(imu_reset.get());
::std::thread imu_thread(::std::ref(imu));
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(7)));
- drivetrain_writer.set_drivetrain_right_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(7)), true);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
SuperstructureWriter superstructure_writer;
diff --git a/y2017_bot3/BUILD b/y2017_bot3/BUILD
deleted file mode 100644
index b1c68ce..0000000
--- a/y2017_bot3/BUILD
+++ /dev/null
@@ -1,89 +0,0 @@
-load("//aos/downloader:downloader.bzl", "aos_downloader")
-
-cc_binary(
- name = "joystick_reader",
- srcs = [
- "joystick_reader.cc",
- ],
- deps = [
- "//aos:init",
- "//aos/actions:action_lib",
- "//aos/input:drivetrain_input",
- "//aos/input:joystick_input",
- "//aos/logging",
- "//aos/time",
- "//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//y2017_bot3/control_loops/drivetrain:drivetrain_base",
- "//y2017_bot3/control_loops/superstructure:superstructure_lib",
- "//y2017_bot3/control_loops/superstructure:superstructure_queue",
- ],
-)
-
-cc_binary(
- name = "wpilib_interface",
- srcs = [
- "wpilib_interface.cc",
- ],
- restricted_to = ["//tools:roborio"],
- deps = [
- "//aos:init",
- "//aos:make_unique",
- "//aos/controls:control_loop",
- "//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/time",
- "//aos/util:log_interval",
- "//aos/util:phased_loop",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/wpilib:buffered_pcm",
- "//frc971/wpilib:gyro_sender",
- "//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
- "//frc971/wpilib:loop_output_handler",
- "//frc971/wpilib:pdp_fetcher",
- "//frc971/wpilib:sensor_reader",
- "//frc971/wpilib:wpilib_robot_base",
- "//third_party:wpilib",
- "//y2017_bot3/control_loops/drivetrain:polydrivetrain_plants",
- "//y2017_bot3/control_loops/superstructure:superstructure_queue",
- ],
-)
-
-aos_downloader(
- name = "download",
- srcs = [
- "//aos:prime_binaries",
- ],
- restricted_to = ["//tools:roborio"],
- start_srcs = [
- ":wpilib_interface",
- ":joystick_reader",
- "//aos:prime_start_binaries",
- "//y2017_bot3/control_loops/drivetrain:drivetrain",
- "//y2017_bot3/control_loops/superstructure:superstructure",
- ],
-)
-
-aos_downloader(
- name = "download_stripped",
- srcs = [
- "//aos:prime_binaries_stripped",
- ],
- restricted_to = ["//tools:roborio"],
- start_srcs = [
- ":wpilib_interface.stripped",
- ":joystick_reader.stripped",
- "//aos:prime_start_binaries_stripped",
- "//y2017_bot3/control_loops/drivetrain:drivetrain.stripped",
- "//y2017_bot3/control_loops/superstructure:superstructure.stripped",
- ],
-)
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- visibility = ["//visibility:public"],
-)
diff --git a/y2017_bot3/__init__.py b/y2017_bot3/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2017_bot3/__init__.py
+++ /dev/null
diff --git a/y2017_bot3/control_loops/BUILD b/y2017_bot3/control_loops/BUILD
deleted file mode 100644
index a98593b..0000000
--- a/y2017_bot3/control_loops/BUILD
+++ /dev/null
@@ -1,6 +0,0 @@
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- visibility = ["//visibility:public"],
- deps = ["//y2017_bot3:python_init"],
-)
diff --git a/y2017_bot3/control_loops/__init__.py b/y2017_bot3/control_loops/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2017_bot3/control_loops/__init__.py
+++ /dev/null
diff --git a/y2017_bot3/control_loops/drivetrain/BUILD b/y2017_bot3/control_loops/drivetrain/BUILD
deleted file mode 100644
index 08aaab2..0000000
--- a/y2017_bot3/control_loops/drivetrain/BUILD
+++ /dev/null
@@ -1,83 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-load("//aos/build:queues.bzl", "queue_library")
-
-genrule(
- name = "genrule_drivetrain",
- outs = [
- "drivetrain_dog_motor_plant.h",
- "drivetrain_dog_motor_plant.cc",
- "kalman_drivetrain_motor_plant.h",
- "kalman_drivetrain_motor_plant.cc",
- ],
- cmd = "$(location //y2017_bot3/control_loops/python:drivetrain) $(OUTS)",
- tools = [
- "//y2017_bot3/control_loops/python:drivetrain",
- ],
- visibility = ["//visibility:private"],
-)
-
-genrule(
- name = "genrule_polydrivetrain",
- outs = [
- "polydrivetrain_dog_motor_plant.h",
- "polydrivetrain_dog_motor_plant.cc",
- "polydrivetrain_cim_plant.h",
- "polydrivetrain_cim_plant.cc",
- "hybrid_velocity_drivetrain.h",
- "hybrid_velocity_drivetrain.cc",
- ],
- cmd = "$(location //y2017_bot3/control_loops/python:polydrivetrain) $(OUTS)",
- tools = [
- "//y2017_bot3/control_loops/python:polydrivetrain",
- ],
- visibility = ["//visibility:private"],
-)
-
-cc_library(
- name = "polydrivetrain_plants",
- srcs = [
- "drivetrain_dog_motor_plant.cc",
- "hybrid_velocity_drivetrain.cc",
- "kalman_drivetrain_motor_plant.cc",
- "polydrivetrain_dog_motor_plant.cc",
- ],
- hdrs = [
- "drivetrain_dog_motor_plant.h",
- "hybrid_velocity_drivetrain.h",
- "kalman_drivetrain_motor_plant.h",
- "polydrivetrain_dog_motor_plant.h",
- ],
- deps = [
- "//frc971/control_loops:hybrid_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- ],
-)
-
-cc_library(
- name = "drivetrain_base",
- srcs = [
- "drivetrain_base.cc",
- ],
- hdrs = [
- "drivetrain_base.h",
- ],
- deps = [
- ":polydrivetrain_plants",
- "//frc971:shifter_hall_effect",
- "//frc971/control_loops/drivetrain:drivetrain_config",
- ],
-)
-
-cc_binary(
- name = "drivetrain",
- srcs = [
- "drivetrain_main.cc",
- ],
- deps = [
- ":drivetrain_base",
- "//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/control_loops/drivetrain:drivetrain_lib",
- ],
-)
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
deleted file mode 100644
index da4aafb..0000000
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ /dev/null
@@ -1,56 +0,0 @@
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-
-#include <chrono>
-
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017_bot3/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
-#include "y2017_bot3/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2017_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-
-namespace chrono = ::std::chrono;
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace drivetrain {
-
-using ::frc971::constants::ShifterHallEffect;
-
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
-
-const DrivetrainConfig<double> &GetDrivetrainConfig() {
- static DrivetrainConfig<double> kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- ::frc971::control_loops::drivetrain::IMUType::IMU_X,
-
- drivetrain::MakeDrivetrainLoop,
- drivetrain::MakeVelocityDrivetrainLoop,
- drivetrain::MakeKFDrivetrainLoop,
- drivetrain::MakeHybridVelocityDrivetrainLoop,
-
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(drivetrain::kDt)),
- drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
-
- drivetrain::kHighGearRatio, drivetrain::kHighGearRatio,
- drivetrain::kJ,
- drivetrain::kMass,
- kThreeStateDriveShifter, kThreeStateDriveShifter,
- // TODO(Neil): Find out whigh position is default in pneumatics for the
- // gearing
- true /* default_high_gear */, 0 /* down_offset */,
- 0.4 /* wheel_non_linearity */, 1.0 /* quickturn_wheel_multiplier */,
- 1.0 /* wheel_multiplier */,
- };
-
- return kDrivetrainConfig;
-};
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2017_bot3
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.h b/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
deleted file mode 100644
index 4fff031..0000000
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef Y2017_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-#define Y2017_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace drivetrain {
-
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2017_bot3
-
-#endif // Y2017_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
deleted file mode 100644
index ac581eb..0000000
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "aos/init.h"
-
-#include "aos/events/shm-event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-
-using ::frc971::control_loops::drivetrain::DrivetrainLoop;
-
-int main() {
- ::aos::Init();
- ::aos::ShmEventLoop event_loop;
- DrivetrainLoop drivetrain(
- ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- &event_loop);
- drivetrain.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2017_bot3/control_loops/python/BUILD b/y2017_bot3/control_loops/python/BUILD
deleted file mode 100644
index ed5cc69..0000000
--- a/y2017_bot3/control_loops/python/BUILD
+++ /dev/null
@@ -1,53 +0,0 @@
-package(default_visibility = ["//y2017_bot3:__subpackages__"])
-
-py_binary(
- name = "drivetrain",
- srcs = [
- "drivetrain.py",
- ],
- legacy_create_init = False,
- restricted_to = ["//tools:k8"],
- deps = [
- ":python_init",
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:drivetrain",
- ],
-)
-
-py_binary(
- name = "polydrivetrain",
- srcs = [
- "drivetrain.py",
- "polydrivetrain.py",
- ],
- legacy_create_init = False,
- restricted_to = ["//tools:k8"],
- deps = [
- ":python_init",
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:polydrivetrain",
- ],
-)
-
-py_library(
- name = "polydrivetrain_lib",
- srcs = [
- "drivetrain.py",
- "polydrivetrain.py",
- ],
- restricted_to = ["//tools:k8"],
- deps = [
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:controls",
- ],
-)
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- visibility = ["//visibility:public"],
- deps = ["//y2017_bot3/control_loops:python_init"],
-)
diff --git a/y2017_bot3/control_loops/python/__init__.py b/y2017_bot3/control_loops/python/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2017_bot3/control_loops/python/__init__.py
+++ /dev/null
diff --git a/y2017_bot3/control_loops/python/drivetrain.py b/y2017_bot3/control_loops/python/drivetrain.py
deleted file mode 100755
index 0272232..0000000
--- a/y2017_bot3/control_loops/python/drivetrain.py
+++ /dev/null
@@ -1,39 +0,0 @@
-#!/usr/bin/python
-
-from frc971.control_loops.python import drivetrain
-import sys
-
-import gflags
-import glog
-
-FLAGS = gflags.FLAGS
-
-gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-
-#TODO(Neil): Update robot moment of inertia, mass, and robot radius
-kDrivetrain = drivetrain.DrivetrainParams(J = 6.0,
- mass = 52,
- robot_radius = 0.59055 / 2.0,
- wheel_radius = 4 * 0.0254 / 2,
- G_high = 14.0 / 40.0 * 34.0 / 50.0 * 52.0 / 60.0,
- G_low = 14.0 / 40.0 * 24.0 / 60.0 * 52.0 / 60.0,
- q_pos_low = 0.12,
- q_pos_high = 0.14,
- q_vel_low = 1.0,
- q_vel_high = 0.95,
- has_imu = False)
-
-def main(argv):
- argv = FLAGS(argv)
- glog.init()
-
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print "Expected .h file name and .cc file name"
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2017_bot3', kDrivetrain)
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/y2017_bot3/control_loops/python/polydrivetrain.py b/y2017_bot3/control_loops/python/polydrivetrain.py
deleted file mode 100755
index 955f78f..0000000
--- a/y2017_bot3/control_loops/python/polydrivetrain.py
+++ /dev/null
@@ -1,31 +0,0 @@
-#!/usr/bin/python
-
-import sys
-from y2017_bot3.control_loops.python import drivetrain
-from frc971.control_loops.python import polydrivetrain
-
-import gflags
-import glog
-
-__author__ = 'Austin Schuh (austin.linux@gmail.com)'
-
-FLAGS = gflags.FLAGS
-
-try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-except gflags.DuplicateFlagError:
- pass
-
-def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
- 'y2017_bot3', drivetrain.kDrivetrain)
-
-if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
diff --git a/y2017_bot3/control_loops/superstructure/BUILD b/y2017_bot3/control_loops/superstructure/BUILD
deleted file mode 100644
index 9c9f750..0000000
--- a/y2017_bot3/control_loops/superstructure/BUILD
+++ /dev/null
@@ -1,48 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-load("//aos/build:queues.bzl", "queue_library")
-
-queue_library(
- name = "superstructure_queue",
- srcs = [
- "superstructure.q",
- ],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:profiled_subsystem_queue",
- "//frc971/control_loops:queues",
- ],
-)
-
-cc_library(
- name = "superstructure_lib",
- srcs = [
- "superstructure.cc",
- ],
- hdrs = [
- "superstructure.h",
- ],
- deps = [
- ":superstructure_queue",
- "//aos:math",
- "//aos/controls:control_loop",
- "//aos/util:trapezoid_profile",
- "//frc971/control_loops:profiled_subsystem",
- "//frc971/control_loops:simple_capped_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- "//frc971/zeroing",
- ],
-)
-
-cc_binary(
- name = "superstructure",
- srcs = [
- "superstructure_main.cc",
- ],
- deps = [
- ":superstructure_lib",
- ":superstructure_queue",
- "//aos:init",
- "//aos/events:shm-event-loop",
- ],
-)
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.cc b/y2017_bot3/control_loops/superstructure/superstructure.cc
deleted file mode 100644
index e75ffed..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure.cc
+++ /dev/null
@@ -1,39 +0,0 @@
-#include "y2017_bot3/control_loops/superstructure/superstructure.h"
-
-#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
-#include "aos/logging/logging.h"
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-Superstructure::Superstructure(::aos::EventLoop *event_loop,
- const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name) {}
-
-void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position * /*position*/,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status * /*status*/) {
- // Write out all the voltages.
- if (output) {
- output->voltage_rollers = 0.0;
- output->hanger_voltage = 0.0;
- output->fingers_out = false;
- if (unsafe_goal) {
- // Intake.
- output->voltage_rollers = unsafe_goal->voltage_rollers;
- // Fire piston to release gear.
- output->fingers_out = unsafe_goal->fingers_out;
- // Spin Hanger.
- output->hanger_voltage = unsafe_goal->hanger_voltage;
- }
- }
-}
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017_bot3
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.h b/y2017_bot3/control_loops/superstructure/superstructure.h
deleted file mode 100644
index a9378eb..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
-#define Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
-
-#include <memory>
-
-#include "aos/controls/control_loop.h"
-#include "aos/util/trapezoid_profile.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/zeroing/zeroing.h"
-#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
-
-namespace y2017_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
- public:
- explicit Superstructure(::aos::EventLoop *event_loop,
- const ::std::string &name =
- ".y2017_bot3.control_loops.superstructure_queue");
-
- static constexpr double kOperatingVoltage = 12.0;
-
- protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position * /*position*/,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status * /*status*/) override;
-
- private:
- DISALLOW_COPY_AND_ASSIGN(Superstructure);
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2017_bot3
-
-#endif // Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.q b/y2017_bot3/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 10a0fca..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,39 +0,0 @@
-package y2017_bot3.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Voltage from -12 to 12 to send to the rollers. Positive is front surface
- // of the roller is moving down when gear is moving inwards.
- float voltage_rollers;
- // State of finger pistons. True is out, false is in.
- bool fingers_out;
- // Voltage from -12 to 12 sent to the hanger roller. Positive is front
- // surface of the hanger is moving down.
- float hanger_voltage;
- };
-
- message Status {
- };
-
- message Position {
- };
-
- message Output {
- // see above
- float voltage_rollers;
- bool fingers_out;
- float hanger_voltage;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
-
-queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2017_bot3/control_loops/superstructure/superstructure_main.cc b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
deleted file mode 100644
index 38df40e..0000000
--- a/y2017_bot3/control_loops/superstructure/superstructure_main.cc
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "y2017_bot3/control_loops/superstructure/superstructure.h"
-
-#include "aos/init.h"
-
-int main() {
- ::aos::Init();
- ::aos::ShmEventLoop event_loop;
- ::y2017_bot3::control_loops::superstructure::Superstructure superstructure(
- &event_loop);
- superstructure.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2017_bot3/joystick_reader.cc b/y2017_bot3/joystick_reader.cc
deleted file mode 100644
index c3cc9cb..0000000
--- a/y2017_bot3/joystick_reader.cc
+++ /dev/null
@@ -1,157 +0,0 @@
-#include <math.h>
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "aos/actions/actions.h"
-#include "aos/input/driver_station_data.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/logging.h"
-#include "aos/util/log_interval.h"
-#include "aos/input/drivetrain_input.h"
-#include "aos/input/joystick_input.h"
-#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-
-using ::frc971::control_loops::drivetrain_queue;
-using ::y2017_bot3::control_loops::superstructure_queue;
-
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::ControlBit;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::POVLocation;
-using ::aos::input::DrivetrainInputReader;
-
-namespace y2017_bot3 {
-namespace input {
-namespace joysticks {
-
-const ButtonLocation kHangerOn(2, 11);
-const ButtonLocation kGearOut(2, 10);
-const ButtonLocation kRollerOn(2, 7);
-const ButtonLocation kRollerSpit(2, 6);
-
-std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
-
-class Reader : public ::aos::input::JoystickInput {
- public:
- Reader(::aos::EventLoop *event_loop)
- : ::aos::input::JoystickInput(event_loop) {
- // Setting driver station type to Steering Wheel
- drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel,
- ::y2017_bot3::control_loops::drivetrain::GetDrivetrainConfig());
- }
-
- void RunIteration(const ::aos::input::driver_station::Data &data) override {
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
-
- const bool last_auto_running = auto_running_;
- auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.GetControlBit(ControlBit::kEnabled);
- if (auto_running_ != last_auto_running) {
- if (auto_running_) {
- StartAuto();
- } else {
- StopAuto();
- }
- }
-
- if (!auto_running_) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
-
- // Process pending actions.
- action_queue_.Tick();
- was_running_ = action_queue_.Running();
-
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
- }
- void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- drivetrain_input_reader_->HandleDrivetrain(data);
- robot_velocity_ = drivetrain_input_reader_->robot_velocity();
- }
-
- void HandleTeleop(const ::aos::input::driver_station::Data &data) {
- superstructure_queue.status.FetchLatest();
- if (!superstructure_queue.status.get()) {
- LOG(ERROR, "Got no superstructure status packet.\n");
- return;
- }
-
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
- new_superstructure_goal->voltage_rollers = 0.0;
-
- if (data.IsPressed(kRollerOn)) {
- new_superstructure_goal->voltage_rollers = 12.0;
- }
-
- if (data.IsPressed(kRollerSpit)) {
- new_superstructure_goal->voltage_rollers = -12.0;
- }
-
- if (data.IsPressed(kHangerOn)) {
- new_superstructure_goal->hanger_voltage = 12.0;
- }
-
-
- if (data.IsPressed(kGearOut)) {
- new_superstructure_goal->fingers_out = true;
- } else {
- new_superstructure_goal->fingers_out = false;
- }
-
- LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
- LOG(ERROR, "Sending superstructure goal failed.\n");
- }
- }
-
- private:
- void StartAuto() {
- LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::AutonomousActionParams params;
- ::frc971::autonomous::auto_mode.FetchLatest();
- if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode;
- } else {
- LOG(WARNING, "no auto mode values\n");
- params.mode = 0;
- }
- action_queue_.EnqueueAction(
- ::frc971::autonomous::MakeAutonomousAction(params));
- }
-
- void StopAuto() {
- LOG(INFO, "Stopping auto mode\n");
- action_queue_.CancelAllActions();
- }
- double robot_velocity_ = 0.0;
- ::aos::common::actions::ActionQueue action_queue_;
-
- bool was_running_ = false;
- bool auto_running_ = false;
-};
-
-} // namespace joysticks
-} // namespace input
-} // namespace y2017_bot3
-
-int main() {
- ::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
- ::y2017_bot3::input::joysticks::Reader reader(&event_loop);
- reader.Run();
- ::aos::Cleanup();
-}
diff --git a/y2017_bot3/wpilib_interface.cc b/y2017_bot3/wpilib_interface.cc
deleted file mode 100644
index e191e1f..0000000
--- a/y2017_bot3/wpilib_interface.cc
+++ /dev/null
@@ -1,417 +0,0 @@
-#include <inttypes.h>
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#include <array>
-#include <chrono>
-#include <cmath>
-#include <functional>
-#include <thread>
-
-#include "frc971/wpilib/ahal/AnalogInput.h"
-#include "frc971/wpilib/ahal/Compressor.h"
-#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
-#include "frc971/wpilib/ahal/DriverStation.h"
-#include "frc971/wpilib/ahal/Encoder.h"
-#include "frc971/wpilib/ahal/VictorSP.h"
-#undef ERROR
-
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/make_unique.h"
-#include "aos/time/time.h"
-#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/wpilib/buffered_pcm.h"
-#include "frc971/wpilib/buffered_solenoid.h"
-#include "frc971/wpilib/dma.h"
-#include "frc971/wpilib/gyro_sender.h"
-#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
-#include "frc971/wpilib/loop_output_handler.h"
-#include "frc971/wpilib/pdp_fetcher.h"
-#include "frc971/wpilib/sensor_reader.h"
-#include "frc971/wpilib/wpilib_robot_base.h"
-#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::frc971::control_loops::drivetrain_queue;
-using ::y2017_bot3::control_loops::superstructure_queue;
-using ::aos::monotonic_clock;
-namespace chrono = ::std::chrono;
-using namespace frc;
-using aos::make_unique;
-
-namespace y2017_bot3 {
-namespace wpilib {
-namespace {
-
-constexpr double kMaxBringupPower = 12.0;
-
-constexpr double kDrivetrainCyclesPerRevolution = 128.0;
-constexpr double kDrivetrainEncoderCountsPerRevolution =
- kDrivetrainCyclesPerRevolution * 4;
-constexpr double kDrivetrainEncoderRatio = 1.0;
-constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
- control_loops::drivetrain::kFreeSpeed *
- control_loops::drivetrain::kHighOutputRatio /
- kDrivetrainEncoderRatio *
- kDrivetrainEncoderCountsPerRevolution;
-
-// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
-// DMA stuff and then removing the * 2.0 in *_translate.
-// The low bit is direction.
-
-// TODO(brian): Use ::std::max instead once we have C++14 so that can be
-// constexpr.
-template <typename T>
-constexpr T max(T a, T b) {
- return (a > b) ? a : b;
-}
-template <typename T, typename... Rest>
-constexpr T max(T a, T b, T c, Rest... rest) {
- return max(max(a, b), c, rest...);
-}
-
-double hall_translate(double in) {
- // Turn voltage from our 3-state halls into a ratio that the loop can use.
- return in / 5.0;
-}
-
-double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (kDrivetrainCyclesPerRevolution /*cpr*/ * 4.0 /*4x*/) *
- kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius *
- 2.0 * M_PI;
-}
-
-double drivetrain_velocity_translate(double in) {
- return (1.0 / in) / 256.0 /*cpr*/ *
- kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
-}
-
-constexpr double kMaxFastEncoderPulsesPerSecond =
- kMaxDrivetrainEncoderPulsesPerSecond;
-static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
- "fast encoders are too fast");
-
-// Class to send position messages with sensor readings to our loops.
-class SensorReader : public ::frc971::wpilib::SensorReader {
- public:
- SensorReader() {
- // Set to filter out anything shorter than 1/4 of the minimum pulse width
- // we should ever see.
- UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
- }
-
- void set_drivetrain_left_hall(::std::unique_ptr<AnalogInput> analog) {
- drivetrain_left_hall_ = ::std::move(analog);
- }
-
- void set_drivetrain_right_hall(::std::unique_ptr<AnalogInput> analog) {
- drivetrain_right_hall_ = ::std::move(analog);
- }
-
- void RunIteration() {
- {
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
-
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
-
- drivetrain_message->left_shifter_position =
- hall_translate(drivetrain_left_hall_->GetVoltage());
- drivetrain_message->right_shifter_position =
- hall_translate(drivetrain_right_hall_->GetVoltage());
-
- drivetrain_message.Send();
- }
-
- {
- auto superstructure_message = superstructure_queue.position.MakeMessage();
- superstructure_message.Send();
- }
- }
-
- private:
- ::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
-};
-
-class SolenoidWriter {
- public:
- SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
- : pcm_(pcm),
- drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
- superstructure_(
- ".y2017_bot3.control_loops.superstructure_queue.output") {}
-
- void set_compressor(::std::unique_ptr<Compressor> compressor) {
- compressor_ = ::std::move(compressor);
- }
-
- void set_left_drivetrain_shifter(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- left_drivetrain_shifter_ = ::std::move(s);
- }
-
- void set_right_drivetrain_shifter(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- right_drivetrain_shifter_ = ::std::move(s);
- }
-
- void set_fingers(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- fingers_ = ::std::move(s);
- }
-
- void operator()() {
- compressor_->Start();
- ::aos::SetCurrentThreadName("Solenoids");
- ::aos::SetCurrentThreadRealtimePriority(27);
-
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
- ::std::chrono::milliseconds(1));
-
- while (run_) {
- {
- int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
- }
- }
-
- {
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- left_drivetrain_shifter_->Set(!drivetrain_->left_high);
- right_drivetrain_shifter_->Set(!drivetrain_->right_high);
- }
- }
-
- {
- superstructure_.FetchLatest();
- if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- fingers_->Set(superstructure_->fingers_out);
- }
- }
-
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
- { to_log.compressor_on = compressor_->Enabled(); }
-
- pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
- }
- }
- }
-
- void Quit() { run_ = false; }
-
- private:
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
- left_drivetrain_shifter_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
- right_drivetrain_shifter_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> fingers_;
-
- ::std::unique_ptr<Compressor> compressor_;
-
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Queue<::y2017_bot3::control_loops::SuperstructureQueue::Output>
- superstructure_;
-
- ::std::atomic<bool> run_{true};
-};
-
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left0_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left0_victor_ = ::std::move(t);
- }
- void set_drivetrain_left1_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left1_victor_ = ::std::move(t);
- }
-
- void set_drivetrain_right0_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right0_victor_ = ::std::move(t);
- }
- void set_drivetrain_right1_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right1_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left0_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_left1_victor_->SetSpeed(queue->left_voltage / 12.0);
- drivetrain_right0_victor_->SetSpeed(-queue->right_voltage / 12.0);
- drivetrain_right1_victor_->SetSpeed(-queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left0_victor_->SetDisabled();
- drivetrain_left1_victor_->SetDisabled();
- drivetrain_right0_victor_->SetDisabled();
- drivetrain_right1_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left0_victor_,
- drivetrain_left1_victor_, drivetrain_right0_victor_,
- drivetrain_right1_victor_;
-};
-
-class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_rollers_victor(::std::unique_ptr<::frc::VictorSP> t) {
- rollers_victor_ = ::std::move(t);
- }
-
- void set_hanger0_victor(::std::unique_ptr<::frc::VictorSP> t) {
- hanger0_victor_ = ::std::move(t);
- }
- void set_hanger1_victor(::std::unique_ptr<::frc::VictorSP> t) {
- hanger1_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::y2017_bot3::control_loops::superstructure_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2017_bot3::control_loops::superstructure_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- rollers_victor_->SetSpeed(queue->voltage_rollers / 12.0);
- hanger0_victor_->SetSpeed(queue->hanger_voltage / 12.0);
- hanger1_victor_->SetSpeed(queue->hanger_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "Superstructure output too old.\n");
- rollers_victor_->SetDisabled();
- hanger0_victor_->SetDisabled();
- hanger1_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> rollers_victor_;
- ::std::unique_ptr<::frc::VictorSP> hanger0_victor_, hanger1_victor_;
-};
-
-class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
- public:
- ::std::unique_ptr<Encoder> make_encoder(int index) {
- return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
- Encoder::k4X);
- }
-
- void Run() override {
- ::aos::InitNRT();
- ::aos::SetCurrentThreadName("StartCompetition");
-
- ::frc971::wpilib::JoystickSender joystick_sender;
- ::std::thread joystick_thread(::std::ref(joystick_sender));
-
- ::frc971::wpilib::PDPFetcher pdp_fetcher;
- ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
-
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_drivetrain_left_hall(make_unique<AnalogInput>(0));
- reader.set_drivetrain_right_hall(make_unique<AnalogInput>(1));
-
- reader.set_pwm_trigger(make_unique<DigitalInput>(0));
- reader.set_dma(make_unique<DMA>());
- ::std::thread reader_thread(::std::ref(reader));
-
- ::frc971::wpilib::GyroSender gyro_sender;
- ::std::thread gyro_thread(::std::ref(gyro_sender));
-
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left0_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
- drivetrain_writer.set_drivetrain_left1_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
- drivetrain_writer.set_drivetrain_right0_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
- drivetrain_writer.set_drivetrain_right1_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
-
- SuperstructureWriter superstructure_writer;
- superstructure_writer.set_rollers_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- superstructure_writer.set_hanger0_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)));
- superstructure_writer.set_hanger1_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
- ::std::thread superstructure_writer_thread(::std::ref(superstructure_writer));
-
- ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
- new ::frc971::wpilib::BufferedPcm());
- SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_left_drivetrain_shifter(pcm->MakeSolenoid(3));
- solenoid_writer.set_right_drivetrain_shifter(pcm->MakeSolenoid(1));
- solenoid_writer.set_fingers(pcm->MakeSolenoid(2));
-
- solenoid_writer.set_compressor(make_unique<Compressor>());
-
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-
- // Wait forever. Not much else to do...
- while (true) {
- const int r = select(0, nullptr, nullptr, nullptr, nullptr);
- if (r != 0) {
- PLOG(WARNING, "infinite select failed");
- } else {
- PLOG(WARNING, "infinite select succeeded??\n");
- }
- }
-
- LOG(ERROR, "Exiting WPILibRobot\n");
-
- joystick_sender.Quit();
- joystick_thread.join();
- pdp_fetcher.Quit();
- pdp_fetcher_thread.join();
- reader.Quit();
- reader_thread.join();
- gyro_sender.Quit();
- gyro_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- solenoid_writer.Quit();
- solenoid_thread.join();
-
- ::aos::Cleanup();
- }
-};
-
-} // namespace
-} // namespace wpilib
-} // namespace y2017_bot3
-
-AOS_ROBOT_CLASS(::y2017_bot3::wpilib::WPILibRobot);
diff --git a/y2018/BUILD b/y2018/BUILD
index 1fc9d1b..18a8ba0 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -87,6 +87,7 @@
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:joystick_sender",
"//frc971/wpilib:logging_queue",
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index efe0c1d..a646ab6 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -36,6 +36,7 @@
cc_test(
name = "superstructure_lib_test",
+ timeout = "long",
srcs = [
"superstructure_lib_test.cc",
],
@@ -44,9 +45,9 @@
":superstructure_queue",
"//aos:math",
"//aos:queues",
- "//aos/time:time",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
+ "//aos/time",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
"//y2018/control_loops/superstructure/intake:intake_plants",
@@ -67,12 +68,12 @@
cc_library(
name = "debouncer",
- hdrs = [
- "debouncer.h",
- ],
srcs = [
"debouncer.cc",
],
+ hdrs = [
+ "debouncer.h",
+ ],
)
cc_test(
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 02aeeaa..2d5e556 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -39,6 +39,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
@@ -544,40 +545,6 @@
int light_flash_ = 0;
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left_victor_ = ::std::move(t);
- }
-
- void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(
- ::aos::Clip(queue->left_voltage, -12.0, 12.0) / 12.0);
- drivetrain_right_victor_->SetSpeed(
- ::aos::Clip(-queue->right_voltage, -12.0, 12.0) / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_victor_->SetDisabled();
- drivetrain_right_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
- drivetrain_right_victor_;
-};
-
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
void set_proximal_victor(::std::unique_ptr<::frc::VictorSP> t) {
@@ -720,7 +687,6 @@
reader.set_lidar_lite_input(make_unique<frc::DigitalInput>(22));
- reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
auto imu_trigger = make_unique<frc::DigitalInput>(5);
@@ -735,11 +701,11 @@
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- drivetrain_writer.set_drivetrain_right_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), false);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), true);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
SuperstructureWriter superstructure_writer;
diff --git a/y2019/BUILD b/y2019/BUILD
index 2b9d39b..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 = [
@@ -41,7 +51,7 @@
"//frc971/control_loops:queues",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//frc971/wpilib:ADIS16448",
- "//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
@@ -56,6 +66,31 @@
],
)
+cc_binary(
+ name = "joystick_reader",
+ srcs = [
+ ":joystick_reader.cc",
+ ],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:action_joystick_input",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/logging",
+ "//aos/network:team_number",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//aos/vision/events:udp",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2019/control_loops/drivetrain:drivetrain_base",
+ "//y2019/control_loops/superstructure:superstructure_queue",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
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/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 5d07e17..453c680 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -39,3 +39,31 @@
"//aos/events:shm-event-loop",
],
)
+
+cc_library(
+ name = "collision_avoidance",
+ srcs = [
+ "collision_avoidance.cc",
+ ],
+ hdrs = [
+ "collision_avoidance.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop_queues",
+ "//frc971:constants",
+ ],
+)
+
+cc_test(
+ name = "collision_avoidance_tests",
+ srcs = [
+ "collision_avoidance_tests.cc",
+ ],
+ deps = [
+ ":collision_avoidance",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..7a26b90
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,162 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double CollisionAvoidance::kElevatorClearHeight;
+constexpr double CollisionAvoidance::kElevatorClearWristDownHeight;
+constexpr double CollisionAvoidance::kElevatorClearIntakeHeight;
+constexpr double CollisionAvoidance::kWristMaxAngle;
+constexpr double CollisionAvoidance::kWristMinAngle;
+constexpr double CollisionAvoidance::kIntakeOutAngle;
+constexpr double CollisionAvoidance::kIntakeInAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMinAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMaxAngle;
+constexpr double CollisionAvoidance::kEps;
+constexpr double CollisionAvoidance::kEpsIntake;
+constexpr double CollisionAvoidance::kEpsWrist;
+
+CollisionAvoidance::CollisionAvoidance() {
+ clear_min_wrist_goal();
+ clear_max_wrist_goal();
+ clear_min_elevator_goal();
+ clear_min_intake_goal();
+ clear_max_intake_goal();
+}
+
+bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Elevator is down, so the wrist can't be close to vertical.
+ if (elevator_position < kElevatorClearHeight) {
+ if (wrist_position < kWristElevatorCollisionMaxAngle &&
+ wrist_position > kWristElevatorCollisionMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so wrist can't go below horizontal in either direction.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ if (wrist_position > kWristMaxAngle) {
+ return true;
+ }
+ if (wrist_position < kWristMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so the intake has to be at either extreme.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
+ return true;
+ }
+ }
+
+ // Nothing is hitting, we must be good.
+ return false;
+}
+
+void CollisionAvoidance::UpdateGoal(
+ const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Start with our constraints being wide open.
+ clear_max_wrist_goal();
+ clear_min_wrist_goal();
+ clear_max_intake_goal();
+ clear_min_intake_goal();
+
+ // If the elevator is low enough, we also can't transition the wrist.
+ if (elevator_position < kElevatorClearHeight) {
+ // Figure out which side the wrist is on and stay there.
+ if (wrist_position < 0.0) {
+ update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
+ } else {
+ update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
+ }
+ }
+
+ // If the elevator is too low, the wrist needs to be above the clearance
+ // angles to avoid crashing the frame.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ update_min_wrist_goal(kWristMinAngle + kEpsWrist);
+ update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+ }
+
+ constexpr double kIntakeMiddleAngle =
+ (kIntakeOutAngle + kIntakeInAngle) / 2.0;
+
+ // If the elevator is too low, the intake can't transition from in to out or
+ // back.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ // Figure out if the intake is in our out and keep it there.
+ if (intake_position < kIntakeMiddleAngle) {
+ update_max_intake_goal(kIntakeInAngle - kEpsIntake);
+ } else {
+ update_min_intake_goal(kIntakeOutAngle + kEpsIntake);
+ }
+ }
+
+ // Start with an unconstrained elevator.
+ clear_min_elevator_goal();
+
+ // If the intake is within the collision range, don't let the elevator down.
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+
+ // If the wrist is within the elevator collision range, don't let the elevator
+ // go down.
+ if (wrist_position > kWristElevatorCollisionMinAngle &&
+ wrist_position < kWristElevatorCollisionMaxAngle) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // If the wrist is far enough down that we are going to hit the frame, don't
+ // let the elevator go too far down.
+ if (wrist_position > kWristMaxAngle || wrist_position < kWristMinAngle) {
+ update_min_elevator_goal(kElevatorClearWristDownHeight + kEps);
+ }
+
+ if (unsafe_goal) {
+ const double wrist_goal = unsafe_goal->wrist.angle;
+ const double intake_goal = unsafe_goal->intake.joint_angle;
+
+ // Compute if we need to move the intake.
+ const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
+ (intake_goal < kIntakeMiddleAngle);
+
+ // Compute if we need to move the wrist across 0.
+ const bool wrist_needs_to_move =
+ (wrist_position < 0.0) ^ (wrist_goal < 0.0);
+
+ // If we need to move the intake, we've got to shove the elevator up. The
+ // intake is already constrained so it can't hit anything until it's clear.
+ if (intake_needs_to_move && wrist_position > 0) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+ // If we need to move the wrist, we've got to shove the elevator up too. The
+ // wrist is already constrained so it can't hit anything until it's clear.
+ // If both the intake and wrist need to move, figure out which one will
+ // require the higher motion and move that.
+ if (wrist_needs_to_move) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // TODO(austin): We won't shove the elevator up if the wrist is asked to go
+ // down below horizontal. I think that's fine.
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..1ad9582
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,111 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+
+#include <cmath>
+#include "aos/controls/control_loops.q.h"
+#include "frc971/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+// CollisionAvoidance computes the min and max allowable ranges for various
+// subsystems to avoid collisions. It also shoves the elevator up to let the
+// intake go in and out, and to let the wrist switch sides.
+class CollisionAvoidance {
+ public:
+ CollisionAvoidance();
+
+ // Reports if the superstructure is collided.
+ bool IsCollided(const SuperstructureQueue::Status *status);
+
+ // Checks and alters goals to make sure they're safe.
+ // TODO(austin): Either we will have a unit delay because this has to happen
+ // after the controls, or we need to be more clever about restructuring.
+ void UpdateGoal(const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal);
+
+ // Returns the goals to give to the respective control loops in
+ // superstructure.
+ double min_wrist_goal() const { return min_wrist_goal_; }
+ double max_wrist_goal() const { return max_wrist_goal_; }
+ double min_elevator_goal() const { return min_elevator_goal_; }
+ double min_intake_goal() const { return min_intake_goal_; }
+ double max_intake_goal() const { return max_intake_goal_; }
+
+ void update_max_wrist_goal(double max_wrist_goal) {
+ max_wrist_goal_ = ::std::min(max_wrist_goal, max_wrist_goal_);
+ }
+ void update_min_wrist_goal(double min_wrist_goal) {
+ min_wrist_goal_ = ::std::max(min_wrist_goal, min_wrist_goal_);
+ }
+ void update_max_intake_goal(double max_intake_goal) {
+ max_intake_goal_ = ::std::min(max_intake_goal, max_intake_goal_);
+ }
+ void update_min_intake_goal(double min_intake_goal) {
+ min_intake_goal_ = ::std::max(min_intake_goal, min_intake_goal_);
+ }
+ void update_min_elevator_goal(double min_elevator_goal) {
+ min_elevator_goal_ = ::std::max(min_elevator_goal, min_elevator_goal_);
+ }
+
+ // TODO(sabina): set all the numbers to correctly match the robot.
+
+ // Height above which we can move the wrist freely.
+ static constexpr double kElevatorClearHeight = 0.5;
+
+ // Height above which we can move the wrist down.
+ static constexpr double kElevatorClearWristDownHeight = 0.3;
+ // Height the carriage needs to be above to move the intake.
+ static constexpr double kElevatorClearIntakeHeight = 0.4;
+
+ // Angle constraints for the wrist when below kElevatorClearDownHeight
+ static constexpr double kWristMaxAngle = M_PI / 2.0;
+ static constexpr double kWristMinAngle = -M_PI / 2.0;
+
+ // Angles outside of which the intake is fully clear of the wrist.
+ static constexpr double kIntakeOutAngle = M_PI / 6.0;
+ static constexpr double kIntakeInAngle = -M_PI / 3.0;
+
+ // Angles within which we will crash the wrist into the elevator if the
+ // elevator is below kElevatorClearHeight.
+ static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
+ static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
+
+ // Tolerance for the elevator.
+ static constexpr double kEps = 0.02;
+ // Tolerance for the intake.
+ static constexpr double kEpsIntake = 0.05;
+ // Tolerance for the wrist.
+ static constexpr double kEpsWrist = 0.05;
+
+ private:
+ void clear_min_wrist_goal() {
+ min_wrist_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_wrist_goal() {
+ max_wrist_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_elevator_goal() {
+ min_elevator_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_intake_goal() {
+ min_intake_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_goal() {
+ max_intake_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+
+ double min_wrist_goal_;
+ double max_wrist_goal_;
+ double min_elevator_goal_;
+ double min_intake_goal_;
+ double max_intake_goal_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
new file mode 100644
index 0000000..834a251
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -0,0 +1,281 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include "aos/commonmath.h"
+#include "gtest/gtest.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+/*
+Test List:
+ FullClockwiseRotationFromBottomBackIntakeIn
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle
+ QuarterClockwiseRotationFromMiddleFrontIntakeMoving
+
+ FullCounterClockwiseRotationFromBottomFrontIntakeIn
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeOut
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
+*/
+
+class CollisionAvoidanceTests : public ::testing::Test {
+ public:
+ void Iterate() {
+ SuperstructureQueue::Goal safe_goal;
+ while (true) {
+ avoidance.UpdateGoal(&status, &unsafe_goal);
+
+ EXPECT_FALSE(avoidance.IsCollided(&status));
+ safe_goal.wrist.angle =
+ ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+ avoidance.max_wrist_goal());
+
+ safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+ avoidance.min_elevator_goal());
+
+ safe_goal.intake.joint_angle =
+ ::aos::Clip(unsafe_goal.intake.joint_angle,
+ avoidance.min_intake_goal(), avoidance.max_intake_goal());
+
+ LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
+ LimitedMove(&status.elevator.position, safe_goal.elevator.height);
+ LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+ if (IsMoving()) {
+ break;
+ }
+ past_status = status;
+ }
+ }
+
+ bool IsMoving() {
+ if ((past_status.wrist.position == status.wrist.position) &&
+ (past_status.elevator.position == status.elevator.position) &&
+ (past_status.intake.position == status.intake.position)) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // provide goals and status messages
+ SuperstructureQueue::Goal unsafe_goal;
+ SuperstructureQueue::Status status;
+ SuperstructureQueue::Status past_status;
+
+ protected:
+ // setup for all tests
+ CollisionAvoidance avoidance;
+
+ void CheckGoals() {
+ // check to see if we reached the goals
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ }
+
+ private:
+ void LimitedMove(float *position, double goal) {
+ if (*position + kIterationMove < goal) {
+ *position += kIterationMove;
+ } else if (*position - kIterationMove > goal) {
+ *position -= kIterationMove;
+ } else {
+ *position = goal;
+ }
+ }
+
+ static constexpr double kIterationMove = 0.001;
+};
+
+// It is trying to rotate from far back to front low.
+TEST_F(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle;
+
+ Iterate();
+
+ CheckGoals();
+}
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ status.wrist.position = avoidance.kWristMaxAngle / 2.0;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from front low to far back.
+TEST_F(CollisionAvoidanceTests,
+ FullCounterClockwiseRotationFromBottomBackIntakeIn) {
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// Unreasonable Elevator Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = 4.0;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.45;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Unreasonable Wrist Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableWristGoal) {
+ // changes the goals to be in the position where the angle is low front and
+ // the elevator is all the way at the bottom with the intake attempting to be
+ // out.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ // sets the status position messgaes to be have the elevator at the half way
+ // with the intake in and the wrist middle front
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.45;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index 2cd7238..fef9ced 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -67,10 +67,10 @@
bool has_piece;
// Status of each subsystem.
- .frc971.control_loops.AbsoluteProfiledJointStatus elevator;
- .frc971.control_loops.AbsoluteProfiledJointStatus wrist;
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
- .frc971.control_loops.AbsoluteProfiledJointStatus stilts;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
};
message Position {
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/joystick_reader.cc b/y2019/joystick_reader.cc
new file mode 100644
index 0000000..7bb32b2
--- /dev/null
+++ b/y2019/joystick_reader.cc
@@ -0,0 +1,185 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/logging.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+using ::y2019::control_loops::superstructure::superstructure_queue;
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+
+// TODO(sabina): update button locations when the board is done
+const ButtonLocation kElevatorUp(0, 0);
+const ButtonLocation kElevatorDown(0, 0);
+const ButtonLocation kDiskLoad(0, 0);
+const ButtonLocation kDiskRocketMiddle(0, 0);
+const ButtonLocation kDiskRocketTop(0, 0);
+const ButtonLocation kCargoLoad(0, 0);
+const ButtonLocation kCargoBay(0, 0);
+const ButtonLocation kCargoRocketBase(0, 0);
+const ButtonLocation kCargoRocketMiddle(0, 0);
+const ButtonLocation kCargoRocketTop(0, 0);
+const ButtonLocation kStow(0, 0);
+const ButtonLocation kIntakeExtend(0, 0);
+const ButtonLocation kIntake(0, 0);
+const ButtonLocation kSpit(0, 0);
+const ButtonLocation kCargoSuction(0, 0);
+const ButtonLocation kDiskSuction(0, 0);
+const ButtonLocation kSuctionOut(0, 0);
+const ButtonLocation kDeployStilt(0, 0);
+const ButtonLocation kRetractStilt(0, 0);
+const ButtonLocation kBackwards(0, 0);
+
+class Reader : public ::aos::input::ActionJoystickInput {
+ public:
+ Reader(::aos::EventLoop *event_loop)
+ : ::aos::input::ActionJoystickInput(
+ event_loop,
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ superstructure_queue.position.FetchLatest();
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get() ||
+ !superstructure_queue.position.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+
+ if (data.IsPressed(kElevatorUp)) {
+ elevator_height_ += 0.1;
+ } else if (data.IsPressed(kElevatorDown)) {
+ elevator_height_ -= 0.1;
+ } else if (data.IsPressed(kDiskLoad)) {
+ elevator_height_ = 0.48;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kDiskRocketMiddle)) {
+ elevator_height_ = 1.19;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kDiskRocketTop)) {
+ elevator_height_ = 1.90;
+ wrist_angle_ = M_PI;
+ }
+
+ // TODO(sabina): do we need an angle here?
+ else if (data.IsPressed(kCargoLoad)) {
+ elevator_height_ = 1.12;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kCargoBay)) {
+ elevator_height_ = 0.0;
+ wrist_angle_ = M_PI / 3;
+ } else if (data.IsPressed(kCargoRocketBase)) {
+ elevator_height_ = 0.7;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kCargoRocketMiddle)) {
+ elevator_height_ = 1.41;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kCargoRocketTop)) {
+ elevator_height_ = 2.12;
+ wrist_angle_ = M_PI;
+ } else if (data.IsPressed(kStow)) {
+ elevator_height_ = 0.5;
+ wrist_angle_ = 0.0;
+ } else {
+ }
+
+ // TODO(sabina): get accurate angle.
+ if (data.IsPressed(kIntakeExtend)) {
+ new_superstructure_goal->intake.joint_angle = 0.5;
+ } else {
+ new_superstructure_goal->intake.joint_angle = 0.0;
+ }
+
+ if (data.IsPressed(kIntake)) {
+ new_superstructure_goal->suction.bottom = true;
+ if (superstructure_queue.status->has_piece == false) {
+ new_superstructure_goal->intake.roller_voltage = 12.0;
+ } else {
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+ } else if (data.IsPressed(kSpit)) {
+ new_superstructure_goal->suction.bottom = false;
+ if (superstructure_queue.status->has_piece == false) {
+ new_superstructure_goal->intake.roller_voltage = 12.0;
+ } else {
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+ } else {
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+
+ // TODO(sabina): decide if we should really have disk suction as its own
+ // button
+ if (data.IsPressed(kCargoSuction)) {
+ new_superstructure_goal->suction.top = false;
+ new_superstructure_goal->suction.bottom = true;
+ } else if (data.IsPressed(kDiskSuction)) {
+ new_superstructure_goal->suction.top = true;
+ new_superstructure_goal->suction.bottom = true;
+ } else if (data.IsPressed(kSuctionOut)) {
+ new_superstructure_goal->suction.top = true;
+ new_superstructure_goal->suction.bottom = true;
+ } else {
+ }
+
+ // TODO(sabina): max height please?
+ if (data.IsPressed(kDeployStilt)) {
+ new_superstructure_goal->stilts.height = 0;
+ } else if (data.IsPressed(kRetractStilt)) {
+ new_superstructure_goal->stilts.height = 0;
+ } else {
+ }
+
+ if (data.IsPressed(kBackwards)) {
+ wrist_angle_ = -wrist_angle_;
+ }
+
+ new_superstructure_goal->elevator.height = elevator_height_;
+ new_superstructure_goal->wrist.angle = wrist_angle_;
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+
+ private:
+ // Current goals here.
+ double elevator_height_ = 0.0;
+ double wrist_angle_ = 0.0;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2019
+
+int main() {
+ ::aos::Init(-1);
+ ::aos::ShmEventLoop event_loop;
+ ::y2019::input::joysticks::Reader reader(&event_loop);
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 7a6a360..bc6c25f 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
@@ -128,40 +129,6 @@
}
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left_victor_ = ::std::move(t);
- }
-
- void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right_victor_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_victor_->SetSpeed(
- ::aos::Clip(queue->left_voltage, -12.0, 12.0) / 12.0);
- drivetrain_right_victor_->SetSpeed(
- ::aos::Clip(-queue->right_voltage, -12.0, 12.0) / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_victor_->SetDisabled();
- drivetrain_right_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
- drivetrain_right_victor_;
-};
-
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<frc::Encoder> make_encoder(int index) {
@@ -186,7 +153,6 @@
reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
- reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
auto imu_trigger = make_unique<frc::DigitalInput>(5);
@@ -201,11 +167,11 @@
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- drivetrain_writer.set_drivetrain_right_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
+ drivetrain_writer.set_right_controller0(
+ ::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...