Merge "Add clear button"
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index 1383aed..9a5d038 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -201,7 +201,7 @@
const UUID &source_node_boot_uuid = state[node_index].boot_uuid;
const Node *const source_node =
configuration::GetNode(configuration_, node_index);
- CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 24u);
+ CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 28u);
flatbuffers::FlatBufferBuilder fbb;
fbb.ForceDefaults(true);
@@ -258,41 +258,32 @@
std::vector<flatbuffers::Offset<flatbuffers::String>> boot_uuid_offsets;
boot_uuid_offsets.reserve(state.size());
- for (const NewDataWriter::State &state : state) {
- if (state.boot_uuid != UUID::Zero()) {
- boot_uuid_offsets.emplace_back(state.boot_uuid.PackString(&fbb));
- } else {
- boot_uuid_offsets.emplace_back(fbb.CreateString(""));
- }
- }
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<flatbuffers::String>>>
- boot_uuids_offset = fbb.CreateVector(boot_uuid_offsets);
-
- int64_t *oldest_remote_monotonic_timestamps;
+ int64_t *unused;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_remote_monotonic_timestamps_offset = fbb.CreateUninitializedVector(
- state.size(), &oldest_remote_monotonic_timestamps);
+ state.size(), &unused);
- int64_t *oldest_local_monotonic_timestamps;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_local_monotonic_timestamps_offset = fbb.CreateUninitializedVector(
- state.size(), &oldest_local_monotonic_timestamps);
+ state.size(), &unused);
- int64_t *oldest_remote_unreliable_monotonic_timestamps;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_remote_unreliable_monotonic_timestamps_offset =
fbb.CreateUninitializedVector(
- state.size(), &oldest_remote_unreliable_monotonic_timestamps);
+ state.size(), &unused);
- int64_t *oldest_local_unreliable_monotonic_timestamps;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_local_unreliable_monotonic_timestamps_offset =
fbb.CreateUninitializedVector(
- state.size(), &oldest_local_unreliable_monotonic_timestamps);
+ state.size(), &unused);
for (size_t i = 0; i < state.size(); ++i) {
+ if (state[i].boot_uuid != UUID::Zero()) {
+ boot_uuid_offsets.emplace_back(state[i].boot_uuid.PackString(&fbb));
+ } else {
+ boot_uuid_offsets.emplace_back(fbb.CreateString(""));
+ }
if (state[i].boot_uuid == UUID::Zero()) {
CHECK_EQ(state[i].oldest_remote_monotonic_timestamp,
monotonic_clock::max_time);
@@ -304,20 +295,32 @@
monotonic_clock::max_time);
}
- oldest_remote_monotonic_timestamps[i] =
- state[i].oldest_remote_monotonic_timestamp.time_since_epoch().count();
- oldest_local_monotonic_timestamps[i] =
- state[i].oldest_local_monotonic_timestamp.time_since_epoch().count();
- oldest_remote_unreliable_monotonic_timestamps[i] =
- state[i]
- .oldest_remote_unreliable_monotonic_timestamp.time_since_epoch()
- .count();
- oldest_local_unreliable_monotonic_timestamps[i] =
- state[i]
- .oldest_local_unreliable_monotonic_timestamp.time_since_epoch()
- .count();
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_remote_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_remote_monotonic_timestamp.time_since_epoch()
+ .count());
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_local_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_local_monotonic_timestamp.time_since_epoch()
+ .count());
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_remote_unreliable_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_remote_unreliable_monotonic_timestamp.time_since_epoch()
+ .count());
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_local_unreliable_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_local_unreliable_monotonic_timestamp.time_since_epoch()
+ .count());
}
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<flatbuffers::String>>>
+ boot_uuids_offset = fbb.CreateVector(boot_uuid_offsets);
+
aos::logger::LogFileHeader::Builder log_file_header_builder(fbb);
log_file_header_builder.add_name(name_offset);
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 3f637f8..aeced9f 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -89,7 +89,7 @@
}
bool ConfigOnly(const LogFileHeader *header) {
- CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 24u);
+ CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 28u);
if (header->has_monotonic_start_time()) return false;
if (header->has_realtime_start_time()) return false;
if (header->has_max_out_of_order_duration()) return false;
@@ -1238,8 +1238,15 @@
return a.second < b.second;
});
new_parts.parts.reserve(parts.second.parts.size());
- for (std::pair<std::string, int> &p : parts.second.parts) {
- new_parts.parts.emplace_back(std::move(p.first));
+ {
+ int last_parts_index = -1;
+ for (std::pair<std::string, int> &p : parts.second.parts) {
+ CHECK_LT(last_parts_index, p.second)
+ << ": Found duplicate parts in '" << new_parts.parts.back()
+ << "' and '" << p.first << "'";
+ last_parts_index = p.second;
+ new_parts.parts.emplace_back(std::move(p.first));
+ }
}
CHECK(!parts.second.config_sha256.empty());
diff --git a/aos/events/logging/logger.fbs b/aos/events/logging/logger.fbs
index c71baad..6d9f7cb 100644
--- a/aos/events/logging/logger.fbs
+++ b/aos/events/logging/logger.fbs
@@ -136,13 +136,17 @@
// For all channels sent from a specific node, these vectors hold the
// timestamp of the oldest known message from that node, and the
// corresponding monotonic timestamp for when that was received on our node.
- oldest_remote_monotonic_timestamps:[int64] (id: 20);
- oldest_local_monotonic_timestamps:[int64] (id: 21);
+ corrupted_oldest_remote_monotonic_timestamps:[int64] (id: 20, deprecated);
+ corrupted_oldest_local_monotonic_timestamps:[int64] (id: 21, deprecated);
+ oldest_remote_monotonic_timestamps:[int64] (id: 24);
+ oldest_local_monotonic_timestamps:[int64] (id: 25);
// For all channels *excluding* the reliable channels (ttl == 0), record the
// same quantity.
- oldest_remote_unreliable_monotonic_timestamps:[int64] (id: 22);
- oldest_local_unreliable_monotonic_timestamps:[int64] (id: 23);
+ corrupted_oldest_remote_unreliable_monotonic_timestamps:[int64] (id: 22, deprecated);
+ corrupted_oldest_local_unreliable_monotonic_timestamps:[int64] (id: 23, deprecated);
+ oldest_remote_unreliable_monotonic_timestamps:[int64] (id: 26);
+ oldest_local_unreliable_monotonic_timestamps:[int64] (id: 27);
}
// Table holding a message.
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 226c084..8d8a2a6 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -2501,6 +2501,30 @@
ConfirmReadable(pi1_single_direction_logfiles_);
}
+// Tests that we explode if someone passes in a part file twice with a better
+// error than an out of order error.
+TEST_P(MultinodeLoggerTest, DuplicateLogFiles) {
+ time_converter_.AddMonotonic(
+ {BootTimestamp::epoch(),
+ BootTimestamp::epoch() + chrono::seconds(1000)});
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(10000));
+ }
+
+ std::vector<std::string> duplicates;
+ for (const std::string &f : pi1_single_direction_logfiles_) {
+ duplicates.emplace_back(f);
+ duplicates.emplace_back(f);
+ }
+ EXPECT_DEATH({ SortParts(duplicates); }, "Found duplicate parts in");
+}
+
// Tests that we properly handle a dead node. Do this by just disconnecting it
// and only using one nodes of logs.
TEST_P(MultinodeLoggerTest, DeadNode) {
diff --git a/aos/network/www/colors.ts b/aos/network/www/colors.ts
index 1a1c0b3..b173e13 100644
--- a/aos/network/www/colors.ts
+++ b/aos/network/www/colors.ts
@@ -5,3 +5,4 @@
export const PINK: number[] = [1, 0.3, 0.6];
export const CYAN: number[] = [0.3, 1, 1];
export const WHITE: number[] = [1, 1, 1];
+export const BLACK: number[] = [0, 0, 0];
diff --git a/debian/matplotlib.BUILD b/debian/matplotlib.BUILD
index 881521c..ad44398 100644
--- a/debian/matplotlib.BUILD
+++ b/debian/matplotlib.BUILD
@@ -2,10 +2,5 @@
build_matplotlib(
"3",
- tkinter_py_version = "3.5",
-)
-
-build_matplotlib(
- "2.7",
- copy_shared_files = False,
+ tkinter_py_version = "3.7",
)
diff --git a/debian/matplotlib.bzl b/debian/matplotlib.bzl
index d94efd9..3ae0786 100644
--- a/debian/matplotlib.bzl
+++ b/debian/matplotlib.bzl
@@ -185,11 +185,19 @@
name = "matplotlib" + version,
srcs = _src_copied + [
version + "/matplotlib/__init__.py",
- ],
+ ] + native.glob(
+ include = ["usr/lib/python" + tkinter_py_version + "/**/*.py"],
+ ),
data = _data_files + _builtin_so_copied + _system_so_copied + [
":usr/share/matplotlib/mpl-data/matplotlibrc",
- ] + native.glob(["etc/**"]),
- imports = ["usr/lib/python" + version + "/dist-packages", version, "."],
+ ] + native.glob(["etc/**", "usr/share/fonts/**"]),
+ imports = [
+ "rpathed3/usr/lib/python" + version + "/dist-packages",
+ "rpathed3/usr/lib/python" + version + ".7/lib-dynload",
+ version,
+ ".",
+ "usr/lib/python" + tkinter_py_version,
+ ],
target_compatible_with = ["@platforms//cpu:x86_64"],
visibility = ["//visibility:public"],
)
diff --git a/debian/matplotlib_init.patch b/debian/matplotlib_init.patch
index 4cc155a..2818cd6 100644
--- a/debian/matplotlib_init.patch
+++ b/debian/matplotlib_init.patch
@@ -26,7 +26,7 @@
+
+# Tell fontconfig where to find matplotlib's sandboxed font files.
+os.environ["FONTCONFIG_PATH"] = os.path.join(_matplotlib_base, "etc/fonts")
-+os.environ["FONTCONFIG_FILE"] = os.path.join(_matplotlib_base, "etc/fonts/fonts.conf")
++os.environ["FONTCONFIG_FILE"] = "fonts.conf"
+os.environ["FONTCONFIG_SYSROOT"] = _matplotlib_base
+
try:
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 2a945bf..5457405 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -47,6 +47,7 @@
"//frc971/control_loops/drivetrain:robot_state_plotter",
"//frc971/control_loops/drivetrain:spline_plotter",
"//frc971/wpilib:imu_plotter",
+ "//y2020/control_loops/drivetrain:localizer_plotter",
"//y2020/control_loops/superstructure:accelerator_plotter",
"//y2020/control_loops/superstructure:finisher_plotter",
"//y2020/control_loops/superstructure:hood_plotter",
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 8b8c951..7b46a70 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -32,6 +32,8 @@
'org_frc971/y2020/control_loops/superstructure/finisher_plotter'
import {plotTurret} from
'org_frc971/y2020/control_loops/superstructure/turret_plotter'
+import {plotLocalizer} from
+ 'org_frc971/y2020/control_loops/drivetrain/localizer_plotter'
import {plotAccelerator} from
'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
import {plotHood} from
@@ -100,6 +102,7 @@
['Accelerator', new PlotState(plotDiv, plotAccelerator)],
['Hood', new PlotState(plotDiv, plotHood)],
['Turret', new PlotState(plotDiv, plotTurret)],
+ ['2020 Localizer', new PlotState(plotDiv, plotLocalizer)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 04ffa4e..ecdabb8 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -137,7 +137,7 @@
};
static constexpr int kNInputs = 4;
// Number of previous samples to save.
- static constexpr int kSaveSamples = 50;
+ static constexpr int kSaveSamples = 80;
// Whether we should completely rerun the entire stored history of
// kSaveSamples on every correction. Enabling this will increase overall CPU
// usage substantially; however, leaving it disabled makes it so that we are
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index b6290d4..a4b8fb8 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -112,6 +112,7 @@
":drivetrain",
":python_init",
"//external:python-glog",
+ "@matplotlib_repo//:matplotlib3",
],
)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index cea1fdd..72d3297 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -588,6 +588,9 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.0001634
+
class NMotor(object):
def __init__(self, motor, n):
diff --git a/frc971/control_loops/python/field_images/2020.png b/frc971/control_loops/python/field_images/2020.png
index 166a1bb..2397fa0 100644
--- a/frc971/control_loops/python/field_images/2020.png
+++ b/frc971/control_loops/python/field_images/2020.png
Binary files differ
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index b7e0246..bf54f60 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -453,6 +453,12 @@
# scale from point in field coordinates
point = self.mousex, self.mousey
+ # This restricts the amount it can be scaled.
+ if self.transform.xx <= 0.4:
+ scale = max(scale, 1)
+ elif self.transform.xx >= 4:
+ scale = min(scale, 1)
+
# move the origin to point
self.transform.translate(point[0], point[1])
diff --git a/frc971/wpilib/dma.cc b/frc971/wpilib/dma.cc
index 432b77b..6cc8bf0 100644
--- a/frc971/wpilib/dma.cc
+++ b/frc971/wpilib/dma.cc
@@ -7,6 +7,7 @@
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/Encoder.h"
+#include "glog/logging.h"
#include "hal/HAL.h"
// Interface to the roboRIO FPGA's DMA features.
@@ -142,6 +143,7 @@
void DMA::SetExternalTrigger(frc::DigitalSource *input, bool rising,
bool falling) {
+ CHECK(input);
tRioStatusCode status = 0;
if (manager_) {
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index 1b7bee9..ca2a10e 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -32,6 +32,25 @@
prev_sample_ = sample;
}
+void DMAPulseSeparationReader::UpdateFromSample(const DMASample &sample) {
+ // save the time of the falling edge of the input one
+ if (have_prev_sample_ && !sample.Get(input_one_) &&
+ prev_sample_.Get(input_one_)) {
+ input_one_time_ = sample.GetTimestamp();
+ }
+
+ // take the difference in time between the falling edge of the input one and
+ // the falling edge of the input two
+ if (!sample.Get(input_two_) && input_one_time_.has_value()) {
+ last_width_ = sample.GetTimestamp() - input_one_time_.value();
+ pulses_detected_++;
+ input_one_time_.reset();
+ }
+
+ have_prev_sample_ = true;
+ prev_sample_ = sample;
+}
+
void DMASynchronizer::CheckDMA() {
DMASample current_sample;
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index a6f2da4..d8fe476 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -2,6 +2,7 @@
#define FRC971_WPILIB_DMA_EDGE_COUNTING_H_
#include <memory>
+#include <optional>
#include <vector>
#include "aos/macros.h"
@@ -69,6 +70,53 @@
DISALLOW_COPY_AND_ASSIGN(DMAPulseWidthReader);
};
+// Takes two digital inputs and times the difference between the first one going
+// low and the second one going low.
+class DMAPulseSeparationReader : public DMASampleHandlerInterface {
+ public:
+ DMAPulseSeparationReader(frc::DigitalInput *input_one,
+ frc::DigitalInput *input_two)
+ : input_one_(input_one), input_two_(input_two) {}
+ DMAPulseSeparationReader() = default;
+
+ void set_input_one(frc::DigitalInput *input) { input_one_ = input; }
+ void set_input_two(frc::DigitalInput *input) { input_two_ = input; }
+
+ double last_width() const { return last_width_; }
+ double pulses_detected() const { return pulses_detected_; }
+
+ private:
+ void UpdateFromSample(const DMASample & /*sample*/) override;
+ void UpdatePolledValue() override {}
+
+ void PollFromSample(const DMASample & /*sample*/) override {}
+ void AddToDMA(DMA *dma) override {
+ dma->Add(input_one_);
+ dma->SetExternalTrigger(input_one_, true, true);
+ dma->Add(input_two_);
+ dma->SetExternalTrigger(input_two_, false, true);
+ }
+
+ static constexpr double kSampleTimeoutSeconds = 0.1;
+
+ frc::DigitalInput *input_one_ = nullptr;
+ frc::DigitalInput *input_two_ = nullptr;
+
+ // The last DMA reading we got.
+ DMASample prev_sample_;
+ // Whether or not we actually have anything in prev_sample_.
+ bool have_prev_sample_ = false;
+
+ // the time when the input one went low.
+ std::optional<double> input_one_time_;
+
+ int pulses_detected_ = 0;
+
+ double last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+
+ DISALLOW_COPY_AND_ASSIGN(DMAPulseSeparationReader);
+};
+
// Counts edges on a sensor using DMA data and latches encoder values
// corresponding to those edges.
class DMAEdgeCounter : public DMASampleHandlerInterface {
diff --git a/y2020/BUILD b/y2020/BUILD
index 45164b4..dfe09f9 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -116,6 +116,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
],
@@ -230,8 +231,11 @@
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_params_fbs",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2020/control_loops/drivetrain:localizer_debug_fbs",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2020/actors/BUILD b/y2020/actors/BUILD
index 4ec7b08..3555531 100644
--- a/y2020/actors/BUILD
+++ b/y2020/actors/BUILD
@@ -77,3 +77,22 @@
"//frc971/autonomous:auto_fbs",
],
)
+
+cc_binary(
+ name = "shooter_tuning_action",
+ srcs = [
+ "shooter_tuning_actor.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:shm_event_loop",
+ "//aos:init",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_params_fbs",
+ "//y2020/control_loops/superstructure/shooter:shooter_tuning_readings_fbs",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ ],
+)
diff --git a/y2020/actors/shooter_tuning_actor.cc b/y2020/actors/shooter_tuning_actor.cc
new file mode 100644
index 0000000..f9d4123
--- /dev/null
+++ b/y2020/actors/shooter_tuning_actor.cc
@@ -0,0 +1,184 @@
+#include <algorithm>
+#include <cstdlib>
+#include <ctime>
+#include <fstream>
+#include <sstream>
+#include <utility>
+
+#include "aos/init.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "glog/logging.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_params_generated.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_readings_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+
+namespace y2020::actors {
+
+namespace superstructure = y2020::control_loops::superstructure;
+
+// Actor for tuning the shooter flywheel velocities.
+// We found that the shooter is most precise when the ball velocity varies the
+// least. To figure out which finisher and accelerator velocities cause the ball
+// velocity to vary the least, this does a sweep of a range of those velocities
+// in randomized order, and shoots balls at each set of velocities. It also
+// fetches the ball velocity at each iteration, which we will use to calculate
+// each iteration's deviation in ball velocity and figure out which flywheel
+// velocities are the best with the csv of velocities that this outputs.
+class ShooterTuningActor : public frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit ShooterTuningActor(aos::EventLoop *event_loop);
+
+ bool RunAction(
+ const frc971::autonomous::AutonomousActionParams *params) override;
+
+ private:
+ void SendSuperstructureGoal();
+ void WaitAndWriteBallData();
+
+ aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+ aos::Fetcher<superstructure::shooter::TuningReadings>
+ tuning_readings_fetcher_;
+ const superstructure::shooter::TuningParams *tuning_params_;
+
+ // Vector of (velocity_accelerator, velocity finisher) containing all velocity
+ // values in the sweep in randomized order.
+ std::vector<std::pair<double, double>> velocities_;
+
+ bool shooting_ = false;
+ double velocity_finisher_ = 0.0;
+ double velocity_accelerator_ = 0.0;
+
+ std::ofstream fout_;
+};
+
+ShooterTuningActor::ShooterTuningActor(aos::EventLoop *event_loop)
+ : frc971::autonomous::BaseAutonomousActor(
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<superstructure::Goal>("/superstructure")),
+ tuning_readings_fetcher_(
+ event_loop->MakeFetcher<superstructure::shooter::TuningReadings>(
+ "/superstructure")) {
+ auto tuning_params_fetcher =
+ event_loop->MakeFetcher<superstructure::shooter::TuningParams>(
+ "/superstructure");
+ CHECK(tuning_params_fetcher.Fetch());
+ tuning_params_ = tuning_params_fetcher.get();
+ const auto finisher = tuning_params_->finisher();
+ const auto accelerator = tuning_params_->accelerator();
+
+ LOG(INFO) << "Tuning with fininisher from " << finisher->velocity_initial()
+ << " to " << finisher->velocity_final() << " by "
+ << finisher->velocity_increment();
+ LOG(INFO) << "Tuning with accelerator from "
+ << accelerator->velocity_initial() << " to "
+ << accelerator->velocity_final() << " by "
+ << accelerator->velocity_increment();
+ // Add the velocities for the sweep
+ for (velocity_finisher_ = finisher->velocity_initial();
+ velocity_finisher_ <= finisher->velocity_final();
+ velocity_finisher_ += finisher->velocity_increment()) {
+ for (velocity_accelerator_ = accelerator->velocity_initial();
+ velocity_accelerator_ <= accelerator->velocity_final();
+ velocity_accelerator_ += accelerator->velocity_increment()) {
+ for (int i = 0; i < tuning_params_->balls_per_iteration(); i++) {
+ velocities_.emplace_back(velocity_accelerator_, velocity_finisher_);
+ }
+ }
+ }
+ // Randomize the ordering of the velocities
+ std::srand(std::time(nullptr));
+ std::random_shuffle(velocities_.begin(), velocities_.end());
+}
+
+bool ShooterTuningActor::RunAction(
+ const frc971::autonomous::AutonomousActionParams * /*params*/) {
+ fout_.open("shooter_tuning_data.csv", std::ios_base::app);
+ LOG(INFO) << "velocity_accelerator,velocity_finisher,velocity_ball";
+
+ shooting_ = true;
+ for (size_t i = 0; i < velocities_.size(); i++) {
+ LOG(INFO) << "Shooting ball " << (i + 1) << " out of "
+ << velocities_.size();
+ velocity_accelerator_ = velocities_[i].first;
+ velocity_finisher_ = velocities_[i].second;
+ SendSuperstructureGoal();
+ WaitAndWriteBallData();
+
+ if (i % 100 == 0 && i != 0) {
+ LOG(INFO) << "Pausing to cool for 2 minutes";
+ shooting_ = false;
+ velocity_accelerator_ = 0.0;
+ velocity_finisher_ = 0.0;
+ SendSuperstructureGoal();
+ std::this_thread::sleep_for(std::chrono::seconds(120));
+ shooting_ = true;
+ }
+ }
+
+ shooting_ = false;
+ velocity_finisher_ = 0.0;
+ velocity_accelerator_ = 0.0;
+ SendSuperstructureGoal();
+ fout_.close();
+ LOG(INFO) << "Done!";
+ return true;
+}
+
+void ShooterTuningActor::WaitAndWriteBallData() {
+ aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ frc971::controls::kLoopFrequency / 2);
+ bool ball_detected = false;
+
+ while (!ball_detected) {
+ phased_loop.SleepUntilNext();
+
+ if (tuning_readings_fetcher_.FetchNext()) {
+ ball_detected = true;
+ std::stringstream output;
+ output << velocity_accelerator_ << ',' << velocity_finisher_ << ','
+ << tuning_readings_fetcher_->velocity_ball() << std::endl;
+ const auto output_str = output.str();
+ LOG(INFO) << output_str;
+ fout_ << output_str;
+ }
+ }
+}
+
+void ShooterTuningActor::SendSuperstructureGoal() {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ auto shooter_builder = builder.MakeBuilder<superstructure::ShooterGoal>();
+ shooter_builder.add_velocity_finisher(velocity_finisher_);
+ shooter_builder.add_velocity_accelerator(velocity_accelerator_);
+ auto shooter_offset = shooter_builder.Finish();
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_shooter(shooter_offset);
+ superstructure_builder.add_shooting(shooting_);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+}
+
+} // namespace y2020::actors
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+ y2020::actors::ShooterTuningActor actor(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2020/actors/splines/README.md b/y2020/actors/splines/README.md
new file mode 100644
index 0000000..058f5bc
--- /dev/null
+++ b/y2020/actors/splines/README.md
@@ -0,0 +1,24 @@
+# Spline Descriptions
+This folder contains reference material for what each spline does
+
+<br>
+
+# Target Aligned - Infinite Recharge
+## After shooting balls initially, collects power cells and returns to shooting position
+
+### [Target Aligned 1](target_aligned_1.json)
+After shooting the pre-loaded balls, this spline directs the robot to the power cells, allowing the robot to collect all three balls available
+
+### [Target Aligned 2](target_aligned_2.json)
+After collecting the 3 balls using the target_aligned_1 spline, this spline directs the robot back towards the shooter tower, setting it up to shoot the balls it collected
+
+<br>
+
+# Target Offset - Infinite Recharge
+## After starting with 3 balls, collects power cells and heads to primary shooting position
+
+### [Target Offset 1](target_offset_1.json)
+When we start offset from the target with the 3 pre-loaded balls in the robot, this spline directs the robot to the additional 2 power cells, allowing the robot to collect both balls available
+
+### [Target Offset 2](target_offset_2.json)
+After collecting the 2 balls using the target_offset_1 spline, this spline directs the robot towards the shooter tower, setting it up to shoot the 5 balls.
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_1.json b/y2020/actors/splines/target_aligned_1.json
new file mode 100644
index 0000000..5a1d082
--- /dev/null
+++ b/y2020/actors/splines/target_aligned_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [3.340495531674842, 3.3229239110760016, 4.483964997108113, 6.058132772881773, 7.084092118999326, 8.012654807002901], "spline_y": [5.730681749413974, 6.367337321705237, 7.14109740201818, 7.627912651837696, 7.449284138182865, 7.4135680036085025], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_2.json b/y2020/actors/splines/target_aligned_2.json
new file mode 100644
index 0000000..bca8965
--- /dev/null
+++ b/y2020/actors/splines/target_aligned_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [8.02107576923077, 7.157915384615384, 6.042122692307692, 5.010540769230768, 4.2315911538461535, 3.368430769230769], "spline_y": [7.389495, 7.431600384615384, 6.652650769230769, 6.1473861538461545, 5.7684376923076925, 5.7473849999999995], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_offset_1.json b/y2020/actors/splines/target_offset_1.json
new file mode 100644
index 0000000..7b56061
--- /dev/null
+++ b/y2020/actors/splines/target_offset_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [3.31276940587483, 3.2907022466964535, 4.149941560222061, 3.9732949238578668, 4.6679269035533, 5.751552791878172], "spline_y": [2.721120947225566, 1.4302342949805793, 1.6475505418770833, 1.4726197969543149, 0.8891289340101522, 0.8335583756345177], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_offset_2.json b/y2020/actors/splines/target_offset_2.json
new file mode 100644
index 0000000..6292a7f
--- /dev/null
+++ b/y2020/actors/splines/target_offset_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [5.709874873096447, 4.5984637055837565, 4.112221319796954, 3.8482611675126903, 3.7232274111675125, 3.3481261421319797], "spline_y": [0.8057730964467005, 0.7779878172588832, 1.4170492385786801, 2.875776395939086, 4.167791878172588, 5.862693908629441], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 893613a..3f5cd2a 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -38,14 +38,26 @@
// Rounded up from exact estimate, since I'm not sure if the original estimate
// includes bumpers.
constexpr double kRobotLength = 0.9;
- // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
- // Current settings based on
- // https://docs.google.com/document/d/1NR9F-ntlSoqZ9LqDzLjn-c14t8ZrppawCCG7wQy47RU/edit
+ // We found that the finisher velocity does not change ball velocity much, so
+ // keep it constant.
+ constexpr double kVelocityFinisher = 350.0;
r->shot_interpolation_table = InterpolationTable<Values::ShotParams>(
- {{7.6 * kFeetToMeters - kRobotLength, {0.115, 197.0, 175.0}},
- {7.6 * kFeetToMeters + kRobotLength, {0.31, 265.0, 235.0}},
- {12.6 * kFeetToMeters + kRobotLength, {0.4, 292.0, 260.0}},
- {17.6 * kFeetToMeters + kRobotLength, {0.52, 365.0, 325.0}}});
+ {{7.6 * kFeetToMeters + kRobotLength, {0.31, 16.9}},
+ {12.6 * kFeetToMeters + kRobotLength, {0.4, 20.4}}});
+
+ r->flywheel_shot_interpolation_table =
+ InterpolationTable<Values::FlywheelShotParams>(
+ {{10.6, {250.0, kVelocityFinisher}},
+ {12.0, {275.0, kVelocityFinisher}},
+ {13.2, {300.0, kVelocityFinisher}},
+ {14.0, {325.0, kVelocityFinisher}},
+ {14.6, {350.0, kVelocityFinisher}},
+ {15.2, {375.0, kVelocityFinisher}},
+ {15.6, {400.0, kVelocityFinisher}},
+ {16.1, {425.0, kVelocityFinisher}},
+ {16.3, {450.0, kVelocityFinisher}},
+ {16.6, {475.0, kVelocityFinisher}},
+ {17.0, {500.0, kVelocityFinisher}}});
// Hood constants.
hood->zeroing_voltage = 2.0;
diff --git a/y2020/constants.h b/y2020/constants.h
index 77f78f4..4483948 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -190,24 +190,37 @@
struct ShotParams {
// Measured in radians
double hood_angle;
- // Angular velocity in radians per second of the slowest (lowest) wheel in
- // the kicker. Positive is shooting the ball.
- double accelerator_power;
- // Angular velocity in radians per seconds of the flywheel. Positive is
- // shooting.
- double finisher_power;
+ // Muzzle velocity (m/s) of the ball as it is shot out of the shooter.
+ double velocity_ball;
static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
using ::frc971::shooter_interpolation::Blend;
- return ShotParams{
- Blend(coefficient, a1.hood_angle, a2.hood_angle),
- Blend(coefficient, a1.accelerator_power, a2.accelerator_power),
- Blend(coefficient, a1.finisher_power, a2.finisher_power)};
+ return ShotParams{Blend(coefficient, a1.hood_angle, a2.hood_angle),
+ Blend(coefficient, a1.velocity_ball, a2.velocity_ball)};
}
};
- // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
+ struct FlywheelShotParams {
+ // Angular velocity in radians per second of the slowest (lowest) wheel in
+ // the kicker. Positive is shooting the ball.
+ double velocity_accelerator;
+ // Angular velocity in radians per seconds of the flywheel. Positive is
+ // shooting.
+ double velocity_finisher;
+
+ static FlywheelShotParams BlendY(double coefficient, FlywheelShotParams a1,
+ FlywheelShotParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return FlywheelShotParams{
+ Blend(coefficient, a1.velocity_accelerator, a2.velocity_accelerator),
+ Blend(coefficient, a1.velocity_finisher, a2.velocity_finisher)};
+ }
+ };
+
+ // { distance_to_target, { hood_angle, velocity_ball }}
InterpolationTable<ShotParams> shot_interpolation_table;
+ // { velocity_ball, { velocity_accelerator, velocity_finisher }}
+ InterpolationTable<FlywheelShotParams> flywheel_shot_interpolation_table;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 89d8044..7c4f99f 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -1,6 +1,7 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
genrule(
name = "genrule_drivetrain",
@@ -56,13 +57,22 @@
],
)
+flatbuffer_cc_library(
+ name = "localizer_debug_fbs",
+ srcs = ["localizer_debug.fbs"],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "localizer",
srcs = ["localizer.cc"],
hdrs = ["localizer.h"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ ":localizer_debug_fbs",
"//aos/containers:ring_buffer",
+ "//aos/containers:sized_array",
"//aos/network:message_bridge_server_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
@@ -195,3 +205,19 @@
"@com_github_google_glog//:glog",
],
)
+
+ts_library(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration_ts_fbs",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:plotter",
+ "//aos/network/www:proxy",
+ "//aos/network/www:reflection_ts",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index 65fc0c4..913fbda 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -75,6 +75,8 @@
"frc971.control_loops.drivetrain.Status");
reader.RemapLoggedChannel("/drivetrain",
"frc971.control_loops.drivetrain.Output");
+ reader.RemapLoggedChannel("/drivetrain",
+ "y2020.control_loops.drivetrain.LocalizerDebug");
reader.RemapLoggedChannel("/superstructure",
"y2020.control_loops.superstructure.Status");
reader.RemapLoggedChannel("/superstructure",
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 906bb4b..9e0e83f 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -23,6 +23,10 @@
return result;
}
+// Offset to add to the pi's yaw in its extrinsics, to account for issues in the
+// calibrated extrinsics.
+constexpr double kTurretPiOffset = 0.0;
+
// Indices of the pis to use.
const std::array<std::string, 5> kPisToUse{"pi1", "pi2", "pi3", "pi4", "pi5"};
@@ -72,7 +76,11 @@
ekf_(dt_config),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
- "/aos")) {
+ "/aos")),
+ debug_sender_(
+ event_loop_
+ ->MakeSender<y2020::control_loops::drivetrain::LocalizerDebug>(
+ "/drivetrain")) {
// TODO(james): The down estimator has trouble handling situations where the
// robot is constantly wiggling but not actually moving much, and can cause
// drift when using accelerometer readings.
@@ -145,17 +153,32 @@
double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
+ auto builder = debug_sender_.MakeBuilder();
+ aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 25> debug_offsets;
for (size_t ii = 0; ii < kPisToUse.size(); ++ii) {
auto &image_fetcher = image_fetchers_[ii];
while (image_fetcher.FetchNext()) {
- HandleImageMatch(kPisToUse[ii], *image_fetcher, now);
+ const auto offsets = HandleImageMatch(ii, kPisToUse[ii], *image_fetcher,
+ now, builder.fbb());
+ for (const auto offset : offsets) {
+ debug_offsets.push_back(offset);
+ }
}
}
+ const auto vector_offset =
+ builder.fbb()->CreateVector(debug_offsets.data(), debug_offsets.size());
+ LocalizerDebug::Builder debug_builder = builder.MakeBuilder<LocalizerDebug>();
+ debug_builder.add_matches(vector_offset);
+ CHECK(builder.Send(debug_builder.Finish()));
}
-void Localizer::HandleImageMatch(
- std::string_view pi, const frc971::vision::sift::ImageMatchResult &result,
- aos::monotonic_clock::time_point now) {
+aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5>
+Localizer::HandleImageMatch(
+ size_t camera_index, std::string_view pi,
+ const frc971::vision::sift::ImageMatchResult &result,
+ aos::monotonic_clock::time_point now, flatbuffers::FlatBufferBuilder *fbb) {
+ aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> debug_offsets;
+
std::chrono::nanoseconds monotonic_offset(0);
clock_offset_fetcher_.Fetch();
if (clock_offset_fetcher_.get() != nullptr) {
@@ -177,11 +200,21 @@
<< capture_time;
if (capture_time > now) {
LOG(WARNING) << "Got camera frame from the future.";
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::IMAGE_FROM_FUTURE);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
if (!result.has_camera_calibration()) {
LOG(WARNING) << "Got camera frame without calibration data.";
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::NO_CALIBRATION);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
// Per the ImageMatchResult specification, we can actually determine whether
// the camera is the turret camera just from the presence of the
@@ -194,7 +227,12 @@
// seems reasonable, but may be unnecessarily low or high.
constexpr float kMaxTurretVelocity = 1.0;
if (is_turret && std::abs(turret_data.velocity) > kMaxTurretVelocity) {
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::TURRET_TOO_FAST);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
CHECK(result.camera_calibration()->has_fixed_extrinsics());
const Eigen::Matrix<float, 4, 4> fixed_extrinsics =
@@ -206,19 +244,38 @@
if (is_turret) {
H_robot_camera = H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw<float>(
- turret_data.position) *
+ turret_data.position + kTurretPiOffset) *
FlatbufferToTransformationMatrix(
*result.camera_calibration()->turret_extrinsics());
}
if (!result.has_camera_poses()) {
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::NO_RESULTS);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
+ int index = -1;
for (const frc971::vision::sift::CameraPose *vision_result :
*result.camera_poses()) {
+ ++index;
+
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_pose_index(index);
+ builder.add_local_image_capture_time_ns(result.image_monotonic_timestamp_ns());
+ builder.add_roborio_image_capture_time_ns(
+ capture_time.time_since_epoch().count());
+ builder.add_image_age_sec(aos::time::DurationInSeconds(now - capture_time));
+
if (!vision_result->has_camera_to_target() ||
!vision_result->has_field_to_target()) {
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::NO_TRANSFORMS);
+ debug_offsets.push_back(builder.Finish());
continue;
}
const Eigen::Matrix<float, 4, 4> H_camera_target =
@@ -257,7 +314,7 @@
// populating some cross-correlation terms.
// Note that these are the noise standard deviations (they are squared below
// to get variances).
- Eigen::Matrix<float, 3, 1> noises(2.0, 2.0, 0.2);
+ Eigen::Matrix<float, 3, 1> noises(2.0, 2.0, 0.5);
// Augment the noise by the approximate rotational speed of the
// camera. This should help account for the fact that, while we are
// spinning, slight timing errors in the camera/turret data will tend to
@@ -271,9 +328,7 @@
H.setZero();
H(0, StateIdx::kX) = 1;
H(1, StateIdx::kY) = 1;
- // This is currently set to zero because we ignore the heading implied by
- // the camera.
- H(2, StateIdx::kTheta) = 0;
+ H(2, StateIdx::kTheta) = 1;
VLOG(1) << "Pose implied by target: " << Z.transpose()
<< " and current pose " << x() << ", " << y() << ", " << theta()
<< " Heading/dist/skew implied by target: "
@@ -282,6 +337,9 @@
// and don't use the correction.
if (std::abs(aos::math::DiffAngle<float>(theta(), Z(2))) > M_PI_2) {
AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::HIGH_THETA_DIFFERENCE);
+ debug_offsets.push_back(builder.Finish());
continue;
}
// In order to do the EKF correction, we determine the expected state based
@@ -293,24 +351,42 @@
ekf_.LastStateBeforeTime(capture_time);
if (!state_at_capture.has_value()) {
AOS_LOG(WARNING, "Dropped image match due to age of image.\n");
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::IMAGE_TOO_OLD);
+ debug_offsets.push_back(builder.Finish());
continue;
}
+
+ builder.add_implied_robot_x(Z(0));
+ builder.add_implied_robot_y(Z(1));
+ builder.add_implied_robot_theta(Z(2));
+
+ // Turret is zero when pointed backwards.
+ builder.add_implied_turret_goal(
+ aos::math::NormalizeAngle(M_PI + pose_robot_target.heading()));
+
+ std::optional<RejectionReason> correction_rejection;
const Input U = ekf_.MostRecentInput();
// For the correction step, instead of passing in the measurement directly,
// we pass in (0, 0, 0) as the measurement and then for the expected
// measurement (Zhat) we calculate the error between the implied and actual
// poses. This doesn't affect any of the math, it just makes the code a bit
// more convenient to write given the Correct() interface we already have.
+ // Note: If we start going back to doing back-in-time rewinds, then we can't
+ // get away with passing things by reference.
ekf_.Correct(
Eigen::Vector3f::Zero(), &U, {},
- [H, H_field_target, pose_robot_target, state_at_capture](
+ [H, H_field_target, pose_robot_target, state_at_capture, Z, &correction_rejection](
const State &, const Input &) -> Eigen::Vector3f {
- const Eigen::Vector3f Z =
+ // TODO(james): Figure out how to use the implied pose for...
+ const Eigen::Vector3f Z_implied =
CalculateImpliedPose(H_field_target, pose_robot_target);
+ (void)Z_implied;
// Just in case we ever do encounter any, drop measurements if they
// have non-finite numbers.
if (!Z.allFinite()) {
AOS_LOG(WARNING, "Got measurement with infinites or NaNs.\n");
+ correction_rejection = RejectionReason::NONFINITE_MEASUREMENT;
return Eigen::Vector3f::Zero();
}
Eigen::Vector3f Zhat = H * state_at_capture.value() - Z;
@@ -324,12 +400,21 @@
// because I primarily introduced it to make sure that any grossly
// invalid measurements get thrown out.
if (Zhat.squaredNorm() > std::pow(10.0, 2)) {
+ correction_rejection = RejectionReason::CORRECTION_TOO_LARGE;
return Eigen::Vector3f::Zero();
}
return Zhat;
},
[H](const State &) { return H; }, R, now);
+ if (correction_rejection) {
+ builder.add_accepted(false);
+ builder.add_rejection_reason(*correction_rejection);
+ } else {
+ builder.add_accepted(true);
+ }
+ debug_offsets.push_back(builder.Finish());
}
+ return debug_offsets;
}
} // namespace drivetrain
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index c3b6464..19ee4ad 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -4,11 +4,13 @@
#include <string_view>
#include "aos/containers/ring_buffer.h"
+#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/control_loops/drivetrain/localizer_debug_generated.h"
#include "y2020/vision/sift/sift_generated.h"
namespace y2020 {
@@ -77,9 +79,11 @@
};
// Processes new image data from the given pi and updates the EKF.
- void HandleImageMatch(std::string_view pi,
- const frc971::vision::sift::ImageMatchResult &result,
- aos::monotonic_clock::time_point now);
+ aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> HandleImageMatch(
+ size_t camera_index, std::string_view pi,
+ const frc971::vision::sift::ImageMatchResult &result,
+ aos::monotonic_clock::time_point now,
+ flatbuffers::FlatBufferBuilder *fbb);
// Processes the most recent turret position and stores it in the turret_data_
// buffer.
@@ -98,6 +102,8 @@
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+ aos::Sender<y2020::control_loops::drivetrain::LocalizerDebug> debug_sender_;
+
// Buffer of recent turret data--this is used so that when we receive a camera
// frame from the turret, we can back out what the turret angle was at that
// time.
diff --git a/y2020/control_loops/drivetrain/localizer_debug.fbs b/y2020/control_loops/drivetrain/localizer_debug.fbs
new file mode 100644
index 0000000..89aea68
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer_debug.fbs
@@ -0,0 +1,34 @@
+namespace y2020.control_loops.drivetrain;
+
+enum RejectionReason : byte {
+ IMAGE_FROM_FUTURE = 0,
+ NO_CALIBRATION = 1,
+ TURRET_TOO_FAST = 2,
+ NO_RESULTS = 3,
+ NO_TRANSFORMS = 4,
+ HIGH_THETA_DIFFERENCE = 5,
+ IMAGE_TOO_OLD = 6,
+ NONFINITE_MEASUREMENT = 7,
+ CORRECTION_TOO_LARGE = 8,
+}
+
+table ImageMatchDebug {
+ camera:uint8 (id: 0);
+ pose_index: uint8 (id: 1);
+ local_image_capture_time_ns:long (id: 2);
+ roborio_image_capture_time_ns:long (id: 3);
+ implied_robot_x:float (id: 4);
+ implied_robot_y:float (id: 5);
+ implied_robot_theta:float (id: 6);
+ implied_turret_goal:float (id: 7);
+ accepted:bool (id: 8);
+ rejection_reason:RejectionReason (id: 9);
+ // Image age (more human-readable than trying to interpret roborio_image_capture_time_ns).
+ image_age_sec:float (id: 10);
+}
+
+table LocalizerDebug {
+ matches:[ImageMatchDebug] (id: 0);
+}
+
+root_type LocalizerDebug;
diff --git a/y2020/control_loops/drivetrain/localizer_plotter.ts b/y2020/control_loops/drivetrain/localizer_plotter.ts
new file mode 100644
index 0000000..4de2856
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer_plotter.ts
@@ -0,0 +1,100 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import * as configuration from 'org_frc971/aos/configuration_generated';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {Point} from 'org_frc971/aos/network/www/plotter';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
+
+import Connection = proxy.Connection;
+import Schema = configuration.reflection.Schema;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+
+
+export function plotLocalizer(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const localizerDebug =
+ aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
+ const imageMatch =
+ aosPlotter.addMessageSource('/pi1/camera', 'frc971.vision.sift.ImageMatchResult');
+
+ var currentTop = 0;
+
+ const imageAcceptedPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
+ imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
+ imageAcceptedPlot.plot.setDefaultYRange([-0.05, 1.05]);
+
+ imageAcceptedPlot.addMessageLine(localizerDebug, ['matches[]', 'accepted'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ const impliedXPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedXPlot.plot.getAxisLabels().setTitle('Implied Robot X');
+ impliedXPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedXPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ impliedXPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_x'])
+ .setColor(RED)
+ .setDrawLine(false);
+ impliedXPlot.addMessageLine(imageMatch, ['camera_poses[]', 'field_to_camera', 'data[3]'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const impliedYPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedYPlot.plot.getAxisLabels().setTitle('Implied Robot Y');
+ impliedYPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedYPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ impliedYPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_y'])
+ .setColor(RED)
+ .setDrawLine(false);
+ impliedYPlot.addMessageLine(imageMatch, ['camera_poses[]', 'field_to_camera', 'data[7]'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const impliedHeadingPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedHeadingPlot.plot.getAxisLabels().setTitle('Implied Robot Theta');
+ impliedHeadingPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedHeadingPlot.plot.getAxisLabels().setYLabel('[rad]');
+
+ impliedHeadingPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_theta'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ const impliedTurretGoalPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedTurretGoalPlot.plot.getAxisLabels().setTitle('Implied Turret Goal');
+ impliedTurretGoalPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedTurretGoalPlot.plot.getAxisLabels().setYLabel('[rad]');
+
+ impliedTurretGoalPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_turret_goal'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ const imageTimingPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageTimingPlot.plot.getAxisLabels().setTitle('Timing Plot');
+ imageTimingPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageTimingPlot.plot.getAxisLabels().setYLabel('[ns]');
+
+ imageTimingPlot.addMessageLine(localizerDebug, ['matches[]', 'image_age_sec'])
+ .setColor(RED)
+ .setDrawLine(false);
+}
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 35ff48f..7ccccd6 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -33,7 +33,7 @@
# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
- motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.01),
+ motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
G=G,
J=J,
q_pos=0.01,
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index acddae3..6f15057 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -20,14 +20,14 @@
kTurret = angular_system.AngularSystemParams(
name='Turret',
- motor=control_loop.Vex775Pro(),
- G=(6.0 / 60.0) * (26.0 / 150.0),
+ motor=control_loop.MiniCIM(),
+ G=(26.0 / 150.0) * (14.0 / 60.0) * (20.0 / 60.0),
J=0.20,
q_pos=0.30,
q_vel=4.5,
kalman_q_pos=0.12,
kalman_q_vel=10.0,
- kalman_q_voltage=12.0,
+ kalman_q_voltage=20.0,
kalman_r_position=0.05)
def main(argv):
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 2d47cf4..db8a060 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -154,9 +154,13 @@
srcs = ["turret_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ "//aos:configuration_ts_fbs",
"//aos/network/www:aos_plotter",
"//aos/network/www:colors",
+ "//aos/network/www:plotter",
"//aos/network/www:proxy",
+ "//aos/network/www:reflection_ts",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
)
diff --git a/y2020/control_loops/superstructure/accelerator_plotter.ts b/y2020/control_loops/superstructure/accelerator_plotter.ts
index eaca1b8..b625a1c 100644
--- a/y2020/control_loops/superstructure/accelerator_plotter.ts
+++ b/y2020/control_loops/superstructure/accelerator_plotter.ts
@@ -1,7 +1,7 @@
// Provides a plot for debugging robot state-related issues.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
-import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE, BLACK} from 'org_frc971/aos/network/www/colors';
import Connection = proxy.Connection;
@@ -26,13 +26,14 @@
velocityPlot.plot.getAxisLabels().setTitle('Velocity');
velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('rad/s');
- velocityPlot.plot.setDefaultYRange([0.0, 450.0]);
+ velocityPlot.plot.setDefaultYRange([0.0, 600.0]);
velocityPlot.addMessageLine(goal, ['shooter', 'velocity_accelerator']).setColor(BLUE).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'avg_angular_velocity']).setColor(RED).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'avg_angular_velocity']).setColor(PINK).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'angular_velocity']).setColor(GREEN).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'angular_velocity']).setColor(CYAN).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'ready']).setColor(BLACK).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'dt_angular_velocity']).setColor(PINK).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'dt_angular_velocity']).setColor(BLUE).setPointSize(0.0);
diff --git a/y2020/control_loops/superstructure/finisher_plotter.ts b/y2020/control_loops/superstructure/finisher_plotter.ts
index 18052cc..6ec4be4 100644
--- a/y2020/control_loops/superstructure/finisher_plotter.ts
+++ b/y2020/control_loops/superstructure/finisher_plotter.ts
@@ -1,7 +1,7 @@
// Provides a plot for debugging robot state-related issues.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
-import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE, BLACK} from 'org_frc971/aos/network/www/colors';
import Connection = proxy.Connection;
@@ -26,11 +26,12 @@
velocityPlot.plot.getAxisLabels().setTitle('Velocity');
velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('rad/s');
- velocityPlot.plot.setDefaultYRange([0.0, 450.0]);
+ velocityPlot.plot.setDefaultYRange([0.0, 600.0]);
velocityPlot.addMessageLine(goal, ['shooter', 'velocity_finisher']).setColor(BLUE).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'avg_angular_velocity']).setColor(RED).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'angular_velocity']).setColor(GREEN).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'ready']).setColor(BLACK).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'dt_angular_velocity']).setColor(PINK).setPointSize(0.0);
const voltagePlot =
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index 0694319..d3b055c 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -1,5 +1,7 @@
package(default_visibility = ["//visibility:public"])
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
cc_library(
name = "shooter_plants",
target_compatible_with = ["@platforms//os:linux"],
@@ -46,3 +48,35 @@
"//y2020/control_loops/superstructure/finisher:finisher_plants",
],
)
+
+flatbuffer_cc_library(
+ name = "shooter_tuning_readings_fbs",
+ srcs = [
+ "shooter_tuning_readings.fbs",
+ ],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+flatbuffer_cc_library(
+ name = "shooter_tuning_params_fbs",
+ srcs = [
+ "shooter_tuning_params.fbs",
+ ],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+cc_binary(
+ name = "shooter_tuning_params_setter",
+ srcs = ["shooter_tuning_params_setter.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":shooter_tuning_params_fbs",
+ ":shooter_tuning_readings_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index d94573b..36edf30 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -41,7 +41,7 @@
// And we have a quadratic!
const double a = 1;
const double b = -bemf_voltage;
- const double c = -60.0 * 12.0 * resistance_;
+ const double c = -70.0 * 12.0 * resistance_;
// Root is always positive.
const double root = std::sqrt(b * b - 4.0 * a * c);
@@ -50,7 +50,7 @@
// Limit to the battery voltage and the current limit voltage.
mutable_U(0, 0) = std::clamp(U(0, 0), lower_limit, upper_limit);
- mutable_U(0, 0) = std::clamp(U(0, 0), -12.0, 12.0);
+ mutable_U(0, 0) = std::clamp(U(0, 0), 0.0, 12.0);
}
private:
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 9db79b0..900d0f1 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -12,7 +12,7 @@
namespace shooter {
namespace {
-const double kVelocityTolerance = 20.0;
+const double kVelocityTolerance = 2.0;
} // namespace
Shooter::Shooter()
@@ -86,6 +86,7 @@
status_builder.add_finisher(finisher_status_offset);
status_builder.add_accelerator_left(accelerator_left_status_offset);
status_builder.add_accelerator_right(accelerator_right_status_offset);
+ status_builder.add_ready(ready());
if (output) {
output->finisher_voltage = finisher_.voltage();
diff --git a/y2020/control_loops/superstructure/shooter/shooter_tuning_params.fbs b/y2020/control_loops/superstructure/shooter/shooter_tuning_params.fbs
new file mode 100644
index 0000000..7fe2dc7
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter_tuning_params.fbs
@@ -0,0 +1,26 @@
+namespace y2020.control_loops.superstructure.shooter;
+
+// Parameters for tuning a flywheel's velocity.
+// Using this for the parameters of the accelerators and the finisher.
+table FlywheelTuningParams {
+ // Shoot balls at
+ // velocity_initial, velocity_initial + velocity_increment, ...,
+ // up to velocity_final, randomizing the order of these velocities.
+ velocity_initial:double (id: 0);
+ velocity_final:double (id: 1);
+ velocity_increment:double (id: 2);
+}
+
+// Parameters for the auto mode that tunes the shooter
+table TuningParams {
+ // Parameters for the flywheels
+ accelerator:FlywheelTuningParams (id: 0);
+ finisher:FlywheelTuningParams (id: 1);
+
+ // Number of balls to shooter at each iteration in
+ // the sweep of all possible accelerator and finisher
+ // velocities.
+ balls_per_iteration:int (id: 2);
+}
+
+root_type TuningParams;
\ No newline at end of file
diff --git a/y2020/control_loops/superstructure/shooter/shooter_tuning_params_setter.cc b/y2020/control_loops/superstructure/shooter/shooter_tuning_params_setter.cc
new file mode 100644
index 0000000..3c6158e
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter_tuning_params_setter.cc
@@ -0,0 +1,58 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_params_generated.h"
+
+DEFINE_double(velocity_initial_finisher, 300.0, "Initial finisher velocity");
+DEFINE_double(velocity_final_finisher, 500.0, "Final finisher velocity");
+DEFINE_double(velocity_finisher_increment, 25.0, "Finisher velocity increment");
+
+DEFINE_double(velocity_initial_accelerator, 180.0,
+ "Initial accelerator velocity");
+DEFINE_double(velocity_final_accelerator, 250.0, "Final accelerator velocity");
+DEFINE_double(velocity_accelerator_increment, 20.0,
+ "Accelerator velocity increment");
+
+DEFINE_int32(balls_per_iteration, 10,
+ "Balls to shoot per iteration in the velocity sweep");
+
+namespace shooter = y2020::control_loops::superstructure::shooter;
+
+flatbuffers::Offset<shooter::FlywheelTuningParams> BuildFlywheelTuningParams(
+ aos::Sender<shooter::TuningParams>::Builder &builder,
+ double velocity_initial, double velocity_final, double velocity_increment) {
+ auto flywheel_tuning_params_builder =
+ builder.MakeBuilder<shooter::FlywheelTuningParams>();
+ flywheel_tuning_params_builder.add_velocity_initial(velocity_initial);
+ flywheel_tuning_params_builder.add_velocity_final(velocity_final);
+ flywheel_tuning_params_builder.add_velocity_increment(velocity_increment);
+ return flywheel_tuning_params_builder.Finish();
+}
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ auto sender = event_loop.MakeSender<shooter::TuningParams>("/superstructure");
+ auto builder = sender.MakeBuilder();
+
+ auto finisher_params = BuildFlywheelTuningParams(
+ builder, FLAGS_velocity_initial_finisher, FLAGS_velocity_final_finisher,
+ FLAGS_velocity_finisher_increment);
+ auto accelerator_params = BuildFlywheelTuningParams(
+ builder, FLAGS_velocity_initial_accelerator,
+ FLAGS_velocity_final_accelerator, FLAGS_velocity_accelerator_increment);
+
+ auto tuning_params_builder = builder.MakeBuilder<shooter::TuningParams>();
+ tuning_params_builder.add_finisher(finisher_params);
+ tuning_params_builder.add_accelerator(accelerator_params);
+ tuning_params_builder.add_balls_per_iteration(FLAGS_balls_per_iteration);
+ CHECK(builder.Send(tuning_params_builder.Finish()));
+
+ return 0;
+}
diff --git a/y2020/control_loops/superstructure/shooter/shooter_tuning_readings.fbs b/y2020/control_loops/superstructure/shooter/shooter_tuning_readings.fbs
new file mode 100644
index 0000000..ae0c379
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter_tuning_readings.fbs
@@ -0,0 +1,14 @@
+namespace y2020.control_loops.superstructure.shooter;
+
+// Contains the readings used to tune the accelerator
+// and finisher velocities to minimize variation in ball velocity.
+// We have two sensors, and find the time that the ball takes to
+// pass between the two to find its velocity.
+// This will be sent each time a ball is detected.
+table TuningReadings {
+ // The velocity (m/s) of the last ball passing between the two
+ // sensors
+ velocity_ball:double (id: 0);
+}
+
+root_type TuningReadings;
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index ce816a0..70ea319 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -72,15 +72,18 @@
aos::FlatbufferFixedAllocatorArray<ShooterGoal, 64> shooter_goal;
constants::Values::ShotParams shot_params;
+ constants::Values::FlywheelShotParams flywheel_shot_params;
if (constants::GetValues().shot_interpolation_table.GetInRange(
- distance_to_goal, &shot_params)) {
+ distance_to_goal, &shot_params) &&
+ constants::GetValues().flywheel_shot_interpolation_table.GetInRange(
+ shot_params.velocity_ball, &flywheel_shot_params)) {
hood_goal.Finish(frc971::control_loops::
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*hood_goal.fbb(), shot_params.hood_angle));
- shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(),
- shot_params.accelerator_power,
- shot_params.finisher_power));
+ shooter_goal.Finish(CreateShooterGoal(
+ *shooter_goal.fbb(), flywheel_shot_params.velocity_accelerator,
+ flywheel_shot_params.velocity_finisher));
} else {
hood_goal.Finish(
frc971::control_loops::
@@ -164,11 +167,16 @@
if (output != nullptr) {
// Friction is a pain and putting a really high burden on the integrator.
+ // TODO(james): I'm not sure how helpful this gain is.
const double turret_velocity_sign =
turret_status->velocity() * kTurretFrictionGain;
output_struct.turret_voltage +=
std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);
+ const double time_sec =
+ aos::time::DurationInSeconds(position_timestamp.time_since_epoch());
+ output_struct.turret_voltage +=
+ kTurretDitherGain * std::sin(2.0 * M_PI * time_sec * 30.0);
output_struct.turret_voltage =
std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
turret_.operating_voltage());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index c03f7a7..59e976e 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -26,8 +26,9 @@
// Terms to control the velocity gain for the friction compensation, and the
// voltage cap.
- static constexpr double kTurretFrictionGain = 10.0;
+ static constexpr double kTurretFrictionGain = 0.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
+ static constexpr double kTurretDitherGain = 0.0;
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 22dd40e..a54c89c 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -286,7 +286,7 @@
// Confirm that we aren't drawing too much current.
CHECK_NEAR(accelerator_left_plant_->battery_current(accelerator_left_U),
- 0.0, 70.0);
+ 0.0, 75.0);
::Eigen::Matrix<double, 1, 1> accelerator_right_U;
accelerator_right_U
@@ -295,7 +295,7 @@
// Confirm that we aren't drawing too much current.
CHECK_NEAR(accelerator_right_plant_->battery_current(accelerator_right_U),
- 0.0, 70.0);
+ 0.0, 75.0);
::Eigen::Matrix<double, 1, 1> finisher_U;
finisher_U << superstructure_output_fetcher_->finisher_voltage() +
@@ -742,7 +742,7 @@
superstructure_plant_.set_peak_intake_acceleration(0.2);
superstructure_plant_.set_peak_turret_velocity(23.0);
- superstructure_plant_.set_peak_turret_acceleration(0.2);
+ superstructure_plant_.set_peak_turret_acceleration(6.0);
// Intake needs over 9 seconds to reach the goal
RunFor(chrono::seconds(10));
@@ -819,7 +819,7 @@
}
// Give it a lot of time to get there.
- RunFor(chrono::seconds(15));
+ RunFor(chrono::seconds(25));
VerifyNearGoal();
}
@@ -971,7 +971,7 @@
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(kShotAngle, superstructure_status_fetcher_->turret()->position(),
- 1e-4);
+ 5e-4);
EXPECT_FLOAT_EQ(kShotAngle,
superstructure_status_fetcher_->aimer()->turret_position());
EXPECT_FLOAT_EQ(0,
@@ -1085,7 +1085,9 @@
constants::Values::kHoodRange().upper, 0.001);
}
-TEST_P(SuperstructureAllianceTest, ShooterInterpolationInRange) {
+// TODO(milind): Add correct values and enable this once interpolation tables
+// have been filled out
+TEST_P(SuperstructureAllianceTest, DISABLED_ShooterInterpolationInRange) {
SetEnabled(true);
const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
WaitUntilZeroed();
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index adca326..e55e19b 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -33,6 +33,10 @@
// Velocity is the fastest (top) wheel
accelerator_left:FlywheelControllerStatus (id: 1);
accelerator_right:FlywheelControllerStatus (id: 2);
+
+ // If the shooter is ready, this is true. Note: don't use this for status
+ // checking, only for plotting. Changes in goal take time to impact this.
+ ready:bool (id: 3);
}
table AimerStatus {
diff --git a/y2020/control_loops/superstructure/turret_plotter.ts b/y2020/control_loops/superstructure/turret_plotter.ts
index 3d975d8..948ca97 100644
--- a/y2020/control_loops/superstructure/turret_plotter.ts
+++ b/y2020/control_loops/superstructure/turret_plotter.ts
@@ -1,29 +1,82 @@
// Provides a plot for debugging robot state-related issues.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
+import * as configuration from 'org_frc971/aos/configuration_generated';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {Point} from 'org_frc971/aos/network/www/plotter';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
import Connection = proxy.Connection;
+import Schema = configuration.reflection.Schema;
const TIME = AosPlotter.TIME;
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+class DerivativeMessageHandler extends MessageHandler {
+ // Calculated magnitude of the measured acceleration from the IMU.
+ private acceleration_magnitudes: Point[] = [];
+ constructor(private readonly schema: Schema) {
+ super(schema);
+ }
+ private readScalar(table: Table, fieldName: string): number {
+ return this.parser.readScalar(table, fieldName);
+ }
+
+ // Computes a numerical derivative for a given input.
+ private derivative(input: Point[]): Point[] {
+ const num_measurements = input.length;
+ const results = [];
+ for (let ii = 0; ii < num_measurements - 1; ++ii) {
+ const x0 = input[ii].x;
+ const x1 = input[ii + 1].x;
+ const y0 = input[ii].y;
+ const y1 = input[ii + 1].y;
+ results.push(new Point((x0 + x1) / 2.0, (y1 - y0) / (x1 - x0)));
+ }
+ return results;
+ }
+
+ getField(field: string[]): Point[] {
+ // Any requested input that ends with "_derivative" will get a derivative
+ // calculated for the provided field.
+ const derivative_suffix = "_derivative";
+ const num_fields = field.length;
+ const end_field = field[num_fields - 1];
+ if (end_field.endsWith(derivative_suffix)) {
+ const field_copy = [];
+ for (let ii = 0; ii < num_fields - 1; ++ii) {
+ field_copy.push(field[ii]);
+ }
+ field_copy.push(end_field.slice(0, end_field.length - derivative_suffix.length));
+ return this.derivative(this.getField(field_copy));
+ } else {
+ return super.getField(field);
+ }
+ }
+}
+
export function plotTurret(conn: Connection, element: Element) : void {
const aosPlotter = new AosPlotter(conn);
const goal = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Goal');
const output = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Output');
- const status = aosPlotter.addMessageSource(
- '/superstructure', 'y2020.control_loops.superstructure.Status');
+ const status = aosPlotter.addRawMessageSource(
+ '/superstructure', 'y2020.control_loops.superstructure.Status',
+ new DerivativeMessageHandler(conn.getSchema('y2020.control_loops.superstructure.Status'))
+ );
const pdpValues =
aosPlotter.addMessageSource('/roborio/aos', 'frc971.PDPValues');
+ const localizerDebug =
+ aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
var currentTop = 0;
const turretPosPlot = aosPlotter.addPlot(
element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
currentTop += DEFAULT_HEIGHT;
- turretPosPlot.plot.getAxisLabels().setTitle('Turret Goal Position');
+ turretPosPlot.plot.getAxisLabels().setTitle('Turret Position');
turretPosPlot.plot.getAxisLabels().setXLabel(TIME);
turretPosPlot.plot.getAxisLabels().setYLabel('rad');
@@ -33,8 +86,39 @@
turretPosPlot.addMessageLine(status, ['turret', 'position'])
.setColor(GREEN)
.setPointSize(0.0);
+ turretPosPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_turret_goal'])
+ .setColor(GREEN)
+ .setDrawLine(false);
turretPosPlot.addMessageLine(status, ['turret', 'unprofiled_goal_position'])
.setColor(BLUE)
+ .setDrawLine(false);
+
+ const turretVelPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ turretVelPlot.plot.getAxisLabels().setTitle('Turret Velocity');
+ turretVelPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretVelPlot.plot.getAxisLabels().setYLabel('rad / sec');
+
+ turretVelPlot.addMessageLine(status, ['aimer', 'turret_velocity'])
+ .setColor(RED)
+ .setPointSize(0.0);
+ turretVelPlot.addMessageLine(status, ['turret', 'velocity'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+ turretVelPlot.addMessageLine(status, ['turret', 'unprofiled_goal_velocity'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const turretAccelPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ turretAccelPlot.plot.getAxisLabels().setTitle('Turret Acceleration');
+ turretAccelPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretAccelPlot.plot.getAxisLabels().setYLabel('rad / sec / sec');
+
+ turretAccelPlot.addMessageLine(status, ['aimer', 'turret_velocity_derivative'])
+ .setColor(RED)
.setPointSize(0.0);
const turretVoltagePlot = aosPlotter.addPlot(
@@ -44,6 +128,15 @@
turretVoltagePlot.plot.getAxisLabels().setXLabel(TIME);
turretVoltagePlot.plot.getAxisLabels().setYLabel('V');
+ turretVoltagePlot.addMessageLine(status, ['turret', 'voltage_error'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+ turretVoltagePlot.addMessageLine(status, ['turret', 'position_power'])
+ .setColor(BLUE)
+ .setPointSize(0.0);
+ turretVoltagePlot.addMessageLine(status, ['turret', 'velocity_power'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
turretVoltagePlot.addMessageLine(output, ['turret_voltage'])
.setColor(RED)
.setPointSize(0.0);
@@ -54,7 +147,7 @@
currentPlot.plot.getAxisLabels().setTitle('Current');
currentPlot.plot.getAxisLabels().setXLabel(TIME);
currentPlot.plot.getAxisLabels().setYLabel('Amps');
- currentPlot.plot.setDefaultYRange([0.0, 80.0]);
+ currentPlot.plot.setDefaultYRange([0.0, 40.0]);
currentPlot.addMessageLine(pdpValues, ['currents[6]'])
.setColor(GREEN)
@@ -83,4 +176,16 @@
targetChoicePlot.addMessageLine(status, ['aimer', 'aiming_for_inner_port'])
.setColor(RED)
.setPointSize(0.0);
+
+ const imageAcceptedPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
+ imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
+ imageAcceptedPlot.plot.setDefaultYRange([-0.05, 1.05]);
+
+ imageAcceptedPlot.addMessageLine(localizerDebug, ['matches[]', 'accepted'])
+ .setColor(RED)
+ .setDrawLine(false);
}
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 44d8a42..bb57997 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -101,6 +101,7 @@
double hood_pos = constants::Values::kHoodRange().middle();
double intake_pos = -0.89;
+ double turret_pos = 0.0;
float roller_speed = 0.0f;
float roller_speed_compensation = 0.0f;
double accelerator_speed = 0.0;
@@ -119,6 +120,12 @@
}
}
+ if (setpoint_fetcher_.get()) {
+ turret_pos = setpoint_fetcher_->turret();
+ } else {
+ turret_pos = 0.0;
+ }
+
if (data.IsPressed(kShootFast)) {
if (setpoint_fetcher_.get()) {
accelerator_speed = setpoint_fetcher_->accelerator();
@@ -175,7 +182,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.0,
+ *builder.fbb(), turret_pos,
CreateProfileParameters(*builder.fbb(), 6.0, 20.0));
flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
diff --git a/y2020/setpoint.fbs b/y2020/setpoint.fbs
index 74164e5..9a9a0be 100644
--- a/y2020/setpoint.fbs
+++ b/y2020/setpoint.fbs
@@ -6,6 +6,8 @@
finisher:double (id: 1);
hood:double (id: 2);
+
+ turret:double (id: 3);
}
root_type Setpoint;
diff --git a/y2020/setpoint_setter.cc b/y2020/setpoint_setter.cc
index 3f493c9..1e1a7b8 100644
--- a/y2020/setpoint_setter.cc
+++ b/y2020/setpoint_setter.cc
@@ -6,6 +6,7 @@
DEFINE_double(accelerator, 250.0, "Accelerator speed");
DEFINE_double(finisher, 500.0, "Finsher speed");
DEFINE_double(hood, 0.45, "Hood setpoint");
+DEFINE_double(turret, 0.0, "Turret setpoint");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -27,6 +28,7 @@
setpoint_builder.add_accelerator(FLAGS_accelerator);
setpoint_builder.add_finisher(FLAGS_finisher);
setpoint_builder.add_hood(FLAGS_hood);
+ setpoint_builder.add_turret(FLAGS_turret);
builder.Send(setpoint_builder.Finish());
return 0;
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index f5b0612..26f65b7 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -32,12 +32,12 @@
public:
CameraReader(aos::EventLoop *event_loop,
const sift::TrainingData *training_data, V4L2Reader *reader,
- cv::FlannBasedMatcher *matcher)
+ const cv::Ptr<cv::flann::IndexParams> &index_params,
+ const cv::Ptr<cv::flann::SearchParams> &search_params)
: event_loop_(event_loop),
training_data_(training_data),
camera_calibration_(FindCameraCalibration()),
reader_(reader),
- matcher_(matcher),
image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
result_sender_(
event_loop->MakeSender<sift::ImageMatchResult>("/camera")),
@@ -46,10 +46,16 @@
read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })),
prev_R_camera_field_vec_(cv::Mat::zeros(3, 1, CV_32F)),
prev_T_camera_field_(cv::Mat::zeros(3, 1, CV_32F)) {
+
+ for (int ii = 0; ii < number_training_images(); ++ii) {
+ matchers_.push_back(cv::FlannBasedMatcher(index_params, search_params));
+ }
+
CopyTrainingFeatures();
- // Technically we don't need to do this, but doing it now avoids the first
- // match attempt being slow.
- matcher_->train();
+
+ for (auto &matcher : matchers_) {
+ matcher.train();
+ }
event_loop->OnRun(
[this]() { read_image_timer_->Setup(event_loop_->monotonic_now()); });
@@ -82,6 +88,7 @@
const std::vector<cv::Mat> &field_camera_list,
const std::vector<cv::Point2f> &target_point_vector,
const std::vector<float> &target_radius_vector,
+ const std::vector<int> &training_image_indices,
aos::Sender<sift::ImageMatchResult> *result_sender,
bool send_details);
@@ -154,7 +161,7 @@
const sift::TrainingData *const training_data_;
const sift::CameraCalibration *const camera_calibration_;
V4L2Reader *const reader_;
- cv::FlannBasedMatcher *const matcher_;
+ std::vector<cv::FlannBasedMatcher> matchers_;
aos::Sender<CameraImage> image_sender_;
aos::Sender<sift::ImageMatchResult> result_sender_;
aos::Sender<sift::ImageMatchResult> detailed_result_sender_;
@@ -188,6 +195,7 @@
}
void CameraReader::CopyTrainingFeatures() {
+ int training_image_index = 0;
for (const sift::TrainingImage *training_image : *training_data_->images()) {
cv::Mat features(training_image->features()->size(), 128, CV_32F);
for (size_t i = 0; i < training_image->features()->size(); ++i) {
@@ -206,7 +214,8 @@
const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
in_mat.convertTo(out_mat, CV_32F);
}
- matcher_->add(features);
+ matchers_[training_image_index].add(features);
+ ++training_image_index;
}
}
@@ -218,6 +227,7 @@
const std::vector<cv::Mat> &field_camera_list,
const std::vector<cv::Point2f> &target_point_vector,
const std::vector<float> &target_radius_vector,
+ const std::vector<int> &training_image_indices,
aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
auto builder = result_sender->MakeBuilder();
const auto camera_calibration_offset =
@@ -252,8 +262,8 @@
sift::CreateTransformationMatrix(*builder.fbb(), fc_data_offset);
const flatbuffers::Offset<sift::TransformationMatrix>
- field_to_target_offset =
- aos::RecursiveCopyFlatBuffer(FieldToTarget(i), builder.fbb());
+ field_to_target_offset = aos::RecursiveCopyFlatBuffer(
+ FieldToTarget(training_image_indices[i]), builder.fbb());
sift::CameraPose::Builder pose_builder(*builder.fbb());
pose_builder.add_camera_to_target(transform_offset);
@@ -302,14 +312,8 @@
sift_->detectAndCompute(image_mat, cv::noArray(), keypoints, descriptors);
}
- // Then, match those features against our training data.
- std::vector<std::vector<cv::DMatch>> matches;
- if (!FLAGS_skip_sift) {
- matcher_->knnMatch(/* queryDescriptors */ descriptors, matches, /* k */ 2);
- }
-
struct PerImageMatches {
- std::vector<const std::vector<cv::DMatch> *> matches;
+ std::vector<std::vector<cv::DMatch>> matches;
std::vector<cv::Point3f> training_points_3d;
std::vector<cv::Point2f> query_points;
std::vector<cv::Point2f> training_points;
@@ -317,32 +321,43 @@
};
std::vector<PerImageMatches> per_image_matches(number_training_images());
- // Pull out the good matches which we want for each image.
- // Discard the bad matches per Lowe's ratio test.
- // (Lowe originally proposed 0.7 ratio, but 0.75 was later proposed as a
- // better option. We'll go with the more conservative (fewer, better matches)
- // for now).
- for (const std::vector<cv::DMatch> &match : matches) {
- CHECK_EQ(2u, match.size());
- CHECK_LE(match[0].distance, match[1].distance);
- CHECK_LT(match[0].imgIdx, number_training_images());
- CHECK_LT(match[1].imgIdx, number_training_images());
- CHECK_EQ(match[0].queryIdx, match[1].queryIdx);
- if (!(match[0].distance < 0.7 * match[1].distance)) {
- continue;
+ for (int image_idx = 0; image_idx < number_training_images(); ++image_idx) {
+ // Then, match those features against our training data.
+ std::vector<std::vector<cv::DMatch>> matches;
+ if (!FLAGS_skip_sift) {
+ matchers_[image_idx].knnMatch(/* queryDescriptors */ descriptors, matches,
+ /* k */ 2);
}
- const int training_image = match[0].imgIdx;
- CHECK_LT(training_image, static_cast<int>(per_image_matches.size()));
- PerImageMatches *const per_image = &per_image_matches[training_image];
- per_image->matches.push_back(&match);
- per_image->training_points.push_back(
- Training2dPoint(training_image, match[0].trainIdx));
- per_image->training_points_3d.push_back(
- Training3dPoint(training_image, match[0].trainIdx));
+ // Pull out the good matches which we want for each image.
+ // Discard the bad matches per Lowe's ratio test.
+ // (Lowe originally proposed 0.7 ratio, but 0.75 was later proposed as a
+ // better option. We'll go with the more conservative (fewer, better
+ // matches) for now).
+ for (const std::vector<cv::DMatch> &match : matches) {
+ CHECK_EQ(2u, match.size());
+ CHECK_LE(match[0].distance, match[1].distance);
+ CHECK_EQ(match[0].imgIdx, 0);
+ CHECK_EQ(match[1].imgIdx, 0);
+ CHECK_EQ(match[0].queryIdx, match[1].queryIdx);
+ if (!(match[0].distance < 0.7 * match[1].distance)) {
+ continue;
+ }
- const cv::KeyPoint &keypoint = keypoints[match[0].queryIdx];
- per_image->query_points.push_back(keypoint.pt);
+ const int training_image = image_idx;
+ CHECK_LT(training_image, static_cast<int>(per_image_matches.size()));
+ PerImageMatches *const per_image = &per_image_matches[training_image];
+ per_image->matches.push_back(match);
+ per_image->matches.back()[0].imgIdx = image_idx;
+ per_image->matches.back()[1].imgIdx = image_idx;
+ per_image->training_points.push_back(
+ Training2dPoint(training_image, match[0].trainIdx));
+ per_image->training_points_3d.push_back(
+ Training3dPoint(training_image, match[0].trainIdx));
+
+ const cv::KeyPoint &keypoint = keypoints[match[0].queryIdx];
+ per_image->query_points.push_back(keypoint.pt);
+ }
}
// The minimum number of matches in a training image for us to use it.
@@ -355,12 +370,13 @@
// Build list of target point and radius for each good match
std::vector<cv::Point2f> target_point_vector;
std::vector<float> target_radius_vector;
+ std::vector<int> training_image_indices;
// Iterate through matches for each training image
for (size_t i = 0; i < per_image_matches.size(); ++i) {
const PerImageMatches &per_image = per_image_matches[i];
- VLOG(2) << "Number of matches to start: " << per_image_matches.size()
+ VLOG(2) << "Number of matches to start: " << per_image.matches.size()
<< "\n";
// If we don't have enough matches to start, skip this set of matches
if (per_image.matches.size() < kMinimumMatchCount) {
@@ -392,8 +408,7 @@
}
// Add this to our collection of all matches that passed our criteria
- all_good_matches.push_back(
- static_cast<std::vector<cv::DMatch>>(*per_image.matches[j]));
+ all_good_matches.push_back(per_image.matches[j]);
// Fill out the data for matches per image that made it past
// homography check, for later use
@@ -415,8 +430,7 @@
// Collect training target location, so we can map it to matched image
cv::Point2f target_point;
float target_radius;
- TargetLocation((*(per_image_good_match.matches[0]))[0].imgIdx, target_point,
- target_radius);
+ TargetLocation(i, target_point, target_radius);
// Store target_point in vector for use by perspectiveTransform
std::vector<cv::Point2f> src_target_pt;
@@ -494,6 +508,8 @@
const_cast<void *>(static_cast<const void *>(
FieldToTarget(i)->data()->data())));
+ training_image_indices.push_back(i);
+
const cv::Mat R_field_target =
H_field_target(cv::Range(0, 3), cv::Range(0, 3));
const cv::Mat T_field_target =
@@ -532,11 +548,11 @@
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
target_point_vector, target_radius_vector,
- &detailed_result_sender_, true);
+ training_image_indices, &detailed_result_sender_, true);
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
target_point_vector, target_radius_vector,
- &result_sender_, false);
+ training_image_indices, &result_sender_, false);
}
void CameraReader::ReadImage() {
@@ -656,7 +672,7 @@
V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
CameraReader camera_reader(&event_loop, &training_data.message(),
- &v4l2_reader, &matcher);
+ &v4l2_reader, index_params, search_params);
event_loop.Run();
}
diff --git a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
index a144853..39c7911 100644
--- a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
+++ b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
@@ -1,6 +1,6 @@
{
"node_name": "pi1",
- "team_number": 7971,
+ "team_number": 971,
"intrinsics": [
392.276093,
0.0,
diff --git a/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json b/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json
new file mode 100644
index 0000000..8c9472e
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "reference",
+ "team_number": 1971,
+ "intrinsics": [
+ 388.0,
+ 0.0,
+ 320.0,
+ 0.0,
+ 388.0,
+ 240.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "calibration_timestamp": 1597994992500905688
+}
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 33eb1a9..6172ae7 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -7,7 +7,7 @@
import define_training_data as dtd
-glog.setLevel("WARN")
+glog.setLevel("INFO")
class CameraIntrinsics:
@@ -36,24 +36,14 @@
def load_camera_definitions():
### CAMERA DEFINITIONS
+ # We only load in cameras that have a calibration file
+ # These are stored in y2020/vision/tools/python_code/calib_files
+ # See reference_calibration_pi_cam.json for an example default file
+ #
+ # Or better yet, use //y2020/vision:calibration to calibrate the camera
+ # using a Charuco target board
- # Robot camera has:
- # FOV_H = 93.*math.pi()/180.
- # FOV_V = 70.*math.pi()/180.
-
- # Create fake camera (based on USB webcam params)
- fx = 810.
- fy = 810.
- cx = 320.
- cy = 240.
-
- # Define a web_cam
- web_cam_int = CameraIntrinsics()
- web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy],
- [0, 0, 1]])
- web_cam_int.dist_coeffs = np.zeros((5, 1))
-
- web_cam_ext = CameraExtrinsics()
+ # Extrinsic definition
# Camera rotation from robot x,y,z to opencv (z, -x, -y)
# This is extrinsics for the turret camera
# camera pose relative to center, base of the turret
@@ -63,37 +53,30 @@
[[np.cos(camera_pitch), 0.0, -np.sin(camera_pitch)], [0.0, 1.0, 0.0],
[np.sin(camera_pitch), 0.0,
np.cos(camera_pitch)]])
- web_cam_ext.R = np.array(
+ turret_cam_ext = CameraExtrinsics()
+ turret_cam_ext.R = np.array(
camera_pitch_matrix *
np.matrix([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]]))
- #web_cam_ext.T = np.array([0., 0., 0.])
- web_cam_ext.T = np.array([2.0 * 0.0254, -6.0 * 0.0254, 41.0 * 0.0254])
- fixed_ext = CameraExtrinsics()
- fixed_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
- [0.0, 0.0, 1.0]])
- fixed_ext.T = np.array([0.0, 0.0, 0.0])
+ turret_cam_ext.T = np.array([15.0 * 0.0254, 15.0 * 0.0254, 41.0 * 0.0254])
+ default_cam_ext = CameraExtrinsics()
+ default_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ default_cam_ext.T = np.array([0.0, 0.0, 0.0])
- web_cam_params = CameraParameters()
- web_cam_params.camera_int = web_cam_int
- # Fixed extrinsics are for the turret, which is centered on the robot and
- # pointed straight backwards.
- web_cam_params.camera_ext = fixed_ext
- web_cam_params.turret_ext = web_cam_ext
+ default_cam_params = CameraParameters()
+ # Currently, all cameras have this same set of extrinsics
+ default_cam_params.camera_ext = default_cam_ext
+ default_cam_params.turret_ext = turret_cam_ext
camera_list = []
- # TODO<Jim>: Should probably make this a dict to make replacing easier
- for team_number in (971, 7971, 8971, 9971):
- for node_name in ("pi0", "pi1", "pi2", "pi3", "pi4", "pi5"):
- camera_base = copy.deepcopy(web_cam_params)
- camera_base.node_name = node_name
- camera_base.team_number = team_number
- camera_list.append(camera_base)
-
dir_name = dtd.bazel_name_fix('calib_files')
+ glog.debug("Searching for calibration files in " + dir_name)
for filename in os.listdir(dir_name):
glog.debug("Inspecting %s", filename)
- if ("cam-calib-int" in filename or 'calibration' in filename) and filename.endswith(".json"):
+ if ("cam-calib-int" in filename
+ or 'calibration' in filename) and filename.endswith(".json"):
+
# Extract intrinsics from file
calib_file = open(dir_name + "/" + filename, 'r')
calib_dict = json.loads(calib_file.read())
@@ -104,6 +87,8 @@
# JSON.
# See which one we have and parse accordingly.
if 'hostname' in calib_dict:
+ glog.warn("WARNING: Using older calibration file.")
+ glog.warn("Preferred usage is y2020/vision:calibration")
hostname_split = calib_dict["hostname"].split("-")
team_number = int(hostname_split[1])
node_name = hostname_split[0] + hostname_split[2]
@@ -117,14 +102,13 @@
dist_coeffs = np.asarray(calib_dict["dist_coeffs"]).reshape(
(1, 5))
- # Look for match, and replace camera_intrinsics
- for camera_calib in camera_list:
- if camera_calib.node_name == node_name and camera_calib.team_number == team_number:
- glog.info("Found calib for %s, team #%d" % (node_name,
- team_number))
- camera_calib.camera_int.camera_matrix = copy.copy(
- camera_matrix)
- camera_calib.camera_int.dist_coeffs = copy.copy(
- dist_coeffs)
+ glog.info("Found calib for " + node_name + ", team #" +
+ str(team_number))
+ camera_base = copy.deepcopy(default_cam_params)
+ camera_base.node_name = node_name
+ camera_base.team_number = team_number
+ camera_base.camera_int.camera_matrix = copy.copy(camera_matrix)
+ camera_base.camera_int.dist_coeffs = copy.copy(dist_coeffs)
+ camera_list.append(camera_base)
return camera_list
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index bb19b9e..900c72a 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -10,7 +10,7 @@
import train_and_match as tam
# TODO<Jim>: Allow command-line setting of logging level
-glog.setLevel("WARN")
+glog.setLevel("INFO")
global VISUALIZE_KEYPOINTS
VISUALIZE_KEYPOINTS = False
@@ -62,8 +62,8 @@
# Filter and project points for each polygon in the list
filtered_keypoints, _, _, _, keep_list = dtd.filter_keypoints_by_polygons(
keypoint_list, None, [self.polygon_list[poly_ind]])
- glog.info("Filtering kept %d of %d features" %
- (len(keep_list), len(keypoint_list)))
+ glog.debug("Filtering kept %d of %d features" %
+ (len(keep_list), len(keypoint_list)))
filtered_point_array = np.asarray([
(keypoint.pt[0], keypoint.pt[1])
for keypoint in filtered_keypoints
@@ -131,28 +131,28 @@
# NOTE: Since we don't have an "ideal" (e.g., graphic) version, we're
# just using the same image as the training image.
ideal_power_port_taped = TargetData(
- 'test_images/train_power_port_taped-2020-08-20-13-27-50.png')
+ 'test_images/partial_971_power_port_red_daytime.png')
# Start at lower left corner, and work around clockwise
# These are taken by manually finding the points in gimp for this image
- power_port_taped_main_panel_polygon_points_2d = [(181, 423), (186, 192),
- (45, 191), (188, 93),
- (310, 99), (314, 196),
- (321, 414)]
+ power_port_taped_main_panel_polygon_points_2d = [(198, 473), (203, 154),
+ (23, 156), (204, 19),
+ (374, 16), (381, 152),
+ (397, 467)]
# These are "virtual" 3D points based on the expected geometry
power_port_taped_main_panel_polygon_points_3d = [
- (field_length / 2., -c_power_port_edge_y, 0.),
- (field_length / 2., -c_power_port_edge_y,
+ (-field_length / 2., c_power_port_edge_y, 0.),
+ (-field_length / 2., c_power_port_edge_y,
c_power_port_bottom_wing_height),
- (field_length / 2., -c_power_port_edge_y + c_power_port_wing_width,
+ (-field_length / 2., c_power_port_edge_y - c_power_port_wing_width,
c_power_port_bottom_wing_height),
- (field_length / 2., -c_power_port_edge_y, c_power_port_total_height),
- (field_length / 2., -c_power_port_edge_y - c_power_port_width,
+ (-field_length / 2., c_power_port_edge_y, c_power_port_total_height),
+ (-field_length / 2., c_power_port_edge_y + c_power_port_width,
c_power_port_total_height),
- (field_length / 2., -c_power_port_edge_y - c_power_port_width,
+ (-field_length / 2., c_power_port_edge_y + c_power_port_width,
c_power_port_bottom_wing_height),
- (field_length / 2., -c_power_port_edge_y - c_power_port_width, 0.)
+ (-field_length / 2., c_power_port_edge_y + c_power_port_width, 0.)
]
power_port_taped_wing_panel_polygon_points_2d = [(312, 99), (438, 191),
@@ -176,22 +176,22 @@
ideal_power_port_taped.polygon_list_3d.append(
power_port_taped_main_panel_polygon_points_3d)
# Including the wing panel
- ideal_power_port_taped.polygon_list.append(
- power_port_taped_wing_panel_polygon_points_2d)
- ideal_power_port_taped.polygon_list_3d.append(
- power_port_taped_wing_panel_polygon_points_3d)
+ #ideal_power_port_taped.polygon_list.append(
+ # power_port_taped_wing_panel_polygon_points_2d)
+ #ideal_power_port_taped.polygon_list_3d.append(
+ # power_port_taped_wing_panel_polygon_points_3d)
# Location of target. Rotation is pointing in -x direction
- ideal_power_port_taped.target_rotation = np.identity(3, np.double)
+ ideal_power_port_taped.target_rotation = -np.identity(3, np.double)
ideal_power_port_taped.target_position = np.array([
- field_length / 2., -c_power_port_edge_y - c_power_port_width / 2.,
+ -field_length / 2., c_power_port_edge_y + c_power_port_width / 2.,
c_power_port_target_height
])
- ideal_power_port_taped.target_point_2d = np.float32([[250, 146]]).reshape(
- -1, 1, 2) # train_power_port_taped-2020-08-20-13-27-50.png
+ ideal_power_port_taped.target_point_2d = np.float32([[290, 87]]).reshape(
+ -1, 1, 2) # partial_971_power_port_red_daytime.png
training_target_power_port_taped = TargetData(
- 'test_images/train_power_port_taped-2020-08-20-13-27-50.png')
+ 'test_images/partial_971_power_port_red_daytime.png')
training_target_power_port_taped.target_rotation = ideal_power_port_taped.target_rotation
training_target_power_port_taped.target_position = ideal_power_port_taped.target_position
training_target_power_port_taped.target_radius = target_radius_default
@@ -267,7 +267,7 @@
# And add the training image we'll actually use to the training list
training_target_power_port_red = TargetData(
- 'test_images/train_power_port_red_webcam.png')
+ 'test_images/train_power_port_red.png')
#'test_images/train_power_port_red_pi-7971-3.png')
training_target_power_port_red.target_rotation = ideal_power_port_red.target_rotation
training_target_power_port_red.target_position = ideal_power_port_red.target_position
@@ -354,10 +354,10 @@
]
# Populate the blue power port
- # ideal_power_port_blue.polygon_list.append(
- # power_port_blue_main_panel_polygon_points_2d)
- # ideal_power_port_blue.polygon_list_3d.append(
- # power_port_blue_main_panel_polygon_points_3d)
+ ideal_power_port_blue.polygon_list.append(
+ power_port_blue_main_panel_polygon_points_2d)
+ ideal_power_port_blue.polygon_list_3d.append(
+ power_port_blue_main_panel_polygon_points_3d)
# Including the wing panel
ideal_power_port_blue.polygon_list.append(
power_port_blue_wing_panel_polygon_points_2d)
@@ -375,7 +375,6 @@
training_target_power_port_blue = TargetData(
'test_images/train_power_port_blue.png')
- # 'test_images/image_from_ios-scaled.jpg')
training_target_power_port_blue.target_rotation = ideal_power_port_blue.target_rotation
training_target_power_port_blue.target_position = ideal_power_port_blue.target_position
training_target_power_port_blue.target_radius = target_radius_default
@@ -427,10 +426,10 @@
# targets based on all the definitions above
######################################################################
- ### Taped power port (not currently used)
- #glog.info("Adding hacked/taped up power port to the model list")
- #ideal_target_list.append(ideal_power_port_taped)
- #training_target_list.append(training_target_power_port_taped)
+ ### Taped power port
+ glog.info("Adding hacked/taped up power port to the model list")
+ ideal_target_list.append(ideal_power_port_taped)
+ training_target_list.append(training_target_power_port_taped)
### Red Power Port
glog.info("Adding red power port to the model list")
@@ -438,19 +437,19 @@
training_target_list.append(training_target_power_port_red)
### Red Loading Bay
- glog.info("Adding red loading bay to the model list")
- ideal_target_list.append(ideal_loading_bay_red)
- training_target_list.append(training_target_loading_bay_red)
+ #glog.info("Adding red loading bay to the model list")
+ #ideal_target_list.append(ideal_loading_bay_red)
+ #training_target_list.append(training_target_loading_bay_red)
### Blue Power Port
- #glog.info("Adding blue power port to the model list")
- #ideal_target_list.append(ideal_power_port_blue)
- #training_target_list.append(training_target_power_port_blue)
+ glog.info("Adding blue power port to the model list")
+ ideal_target_list.append(ideal_power_port_blue)
+ training_target_list.append(training_target_power_port_blue)
### Blue Loading Bay
- glog.info("Adding blue loading bay to the model list")
- ideal_target_list.append(ideal_loading_bay_blue)
- training_target_list.append(training_target_loading_bay_blue)
+ #glog.info("Adding blue loading bay to the model list")
+ #ideal_target_list.append(ideal_loading_bay_blue)
+ #training_target_list.append(training_target_loading_bay_blue)
return ideal_target_list, training_target_list
@@ -461,12 +460,12 @@
# Create feature extractor
feature_extractor = tam.load_feature_extractor()
- # Use webcam parameters for now
+ # Use default parameters for now
camera_params = camera_definition.load_camera_definitions()[0]
for ideal_target in ideal_target_list:
- glog.info("\nPreparing target for image %s" %
- ideal_target.image_filename)
+ glog.debug("\nPreparing target for image %s" %
+ ideal_target.image_filename)
ideal_target.extract_features(feature_extractor)
ideal_target.filter_keypoints_by_polygons()
ideal_target.compute_reprojection_maps()
@@ -515,8 +514,8 @@
AUTO_PROJECTION = True
if AUTO_PROJECTION:
- glog.info(
- "\n\nAuto projection of training keypoints to 3D using ideal images"
+ glog.debug(
+ "Doing auto projection of training keypoints to 3D using ideal images"
)
# Match the captured training image against the "ideal" training image
# and use those matches to pin down the 3D locations of the keypoints
@@ -526,8 +525,8 @@
training_target = training_target_list[target_ind]
ideal_target = ideal_target_list[target_ind]
- glog.info("\nPreparing target for image %s" %
- training_target.image_filename)
+ glog.debug("\nPreparing target for image %s" %
+ training_target.image_filename)
# Extract keypoints and descriptors for model
training_target.extract_features(feature_extractor)
@@ -562,14 +561,14 @@
training_target.target_point_2d = training_target_point_2d.reshape(
-1, 1, 2)
- glog.info("Started with %d keypoints" %
- len(training_target.keypoint_list))
+ glog.debug("Started with %d keypoints" %
+ len(training_target.keypoint_list))
training_target.keypoint_list, training_target.descriptor_list, rejected_keypoint_list, rejected_descriptor_list, _ = dtd.filter_keypoints_by_polygons(
training_target.keypoint_list, training_target.descriptor_list,
training_target.polygon_list)
- glog.info("After filtering by polygons, had %d keypoints" %
- len(training_target.keypoint_list))
+ glog.debug("After filtering by polygons, had %d keypoints" %
+ len(training_target.keypoint_list))
if VISUALIZE_KEYPOINTS:
tam.show_keypoints(training_target.image,
training_target.keypoint_list)
diff --git a/y2020/vision/tools/python_code/test_images/partial_971_power_port_red.png b/y2020/vision/tools/python_code/test_images/partial_971_power_port_red.png
new file mode 100644
index 0000000..ec402c0
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/partial_971_power_port_red.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/test_images/partial_971_power_port_red_daytime.png b/y2020/vision/tools/python_code/test_images/partial_971_power_port_red_daytime.png
new file mode 100644
index 0000000..be771b1
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/partial_971_power_port_red_daytime.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/test_images/partial_971_power_port_red_nighttime.png b/y2020/vision/tools/python_code/test_images/partial_971_power_port_red_nighttime.png
new file mode 100644
index 0000000..ec402c0
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/partial_971_power_port_red_nighttime.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index ad1bbde..5f895bb 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -183,7 +183,7 @@
matches_mask_list.append([])
continue
- glog.info(
+ glog.debug(
"Got good number of matches for model %d: %d (needed only %d)" %
(i, len(good_matches), MIN_MATCH_COUNT))
# Extract and bundle keypoint locations for computations
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 914c9c6..bdc16fa 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -48,10 +48,16 @@
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
+#include "gflags/gflags.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/shooter/shooter_tuning_readings_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
+DEFINE_bool(shooter_tuning, true,
+ "If true, reads from ball beambreak sensors and sends shooter "
+ "tuning readings");
+
using ::aos::monotonic_clock;
using ::y2020::constants::Values;
namespace superstructure = ::y2020::control_loops::superstructure;
@@ -119,7 +125,10 @@
drivetrain_position_sender_(
event_loop
->MakeSender<::frc971::control_loops::drivetrain::Position>(
- "/drivetrain")) {
+ "/drivetrain")),
+ shooter_tuning_readings_sender_(
+ event_loop->MakeSender<superstructure::shooter::TuningReadings>(
+ "/superstructure")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -186,13 +195,6 @@
right_accelerator_encoder_ = ::std::move(encoder);
}
- // Control Panel
-
- void set_control_panel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- control_panel_encoder_ = ::std::move(encoder);
- }
-
// Auto mode switches.
void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
medium_encoder_filter_.Add(sensor.get());
@@ -201,6 +203,20 @@
void set_imu(frc971::wpilib::ADIS16470 *imu) { imu_ = imu; }
+ void set_ball_beambreak_inputs(::std::unique_ptr<frc::DigitalInput> sensor1,
+ ::std::unique_ptr<frc::DigitalInput> sensor2) {
+ ball_beambreak_inputs_[0] = ::std::move(sensor1);
+ ball_beambreak_inputs_[1] = ::std::move(sensor2);
+ ball_beambreak_reader_.set_input_one(ball_beambreak_inputs_[0].get());
+ ball_beambreak_reader_.set_input_two(ball_beambreak_inputs_[1].get());
+ }
+
+ void Start() override {
+ if (FLAGS_shooter_tuning) {
+ AddToDMA(&ball_beambreak_reader_);
+ }
+ }
+
void RunIteration() override {
CHECK_NOTNULL(imu_)->DoReads();
@@ -252,14 +268,6 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
- // Control Panel
- frc971::RelativePositionT control_panel;
- CopyPosition(*control_panel_encoder_, &control_panel,
- Values::kControlPanelEncoderCountsPerRevolution(),
- Values::kControlPanelEncoderRatio(), false);
- flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
- frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
-
// Shooter
y2020::control_loops::superstructure::ShooterPositionT shooter;
shooter.theta_finisher =
@@ -286,7 +294,6 @@
position_builder.add_intake_joint(intake_joint_offset);
position_builder.add_turret(turret_offset);
position_builder.add_shooter(shooter_offset);
- position_builder.add_control_panel(control_panel_offset);
builder.Send(position_builder.Finish());
}
@@ -308,6 +315,22 @@
builder.Send(auto_mode_builder.Finish());
}
+
+ if (FLAGS_shooter_tuning) {
+ // Distance between beambreak sensors, in meters.
+ constexpr double kDistanceBetweenBeambreaks = 0.4813;
+
+ if (ball_beambreak_reader_.pulses_detected() > balls_detected_) {
+ balls_detected_ = ball_beambreak_reader_.pulses_detected();
+
+ auto builder = shooter_tuning_readings_sender_.MakeBuilder();
+ auto shooter_tuning_readings_builder =
+ builder.MakeBuilder<superstructure::shooter::TuningReadings>();
+ shooter_tuning_readings_builder.add_velocity_ball(
+ kDistanceBetweenBeambreaks / ball_beambreak_reader_.last_width());
+ builder.Send(shooter_tuning_readings_builder.Finish());
+ }
+ }
}
private:
@@ -315,6 +338,8 @@
::aos::Sender<superstructure::Position> superstructure_position_sender_;
::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
+ ::aos::Sender<superstructure::shooter::TuningReadings>
+ shooter_tuning_readings_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer turret_encoder_;
@@ -323,12 +348,18 @@
::frc971::wpilib::AbsoluteEncoder intake_joint_encoder_;
::std::unique_ptr<::frc::Encoder> finisher_encoder_,
- left_accelerator_encoder_, right_accelerator_encoder_,
- control_panel_encoder_;
-
+ left_accelerator_encoder_, right_accelerator_encoder_;
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
frc971::wpilib::ADIS16470 *imu_ = nullptr;
+
+ // Used to interface with the two beam break sensors that the ball for tuning
+ // shooter parameters has to pass through.
+ // We will time how long it takes to pass between the two sensors to get its
+ // velocity.
+ std::array<std::unique_ptr<frc::DigitalInput>, 2> ball_beambreak_inputs_;
+ frc971::wpilib::DMAPulseSeparationReader ball_beambreak_reader_;
+ int balls_detected_ = 0;
};
class SuperstructureWriter
@@ -384,11 +415,6 @@
}
}
- void set_washing_machine_control_panel_victor(
- ::std::unique_ptr<::frc::VictorSP> t) {
- washing_machine_control_panel_victor_ = ::std::move(t);
- }
-
void set_accelerator_left_falcon(::std::unique_ptr<::frc::TalonFX> t) {
accelerator_left_falcon_ = ::std::move(t);
}
@@ -430,7 +456,7 @@
kMaxBringupPower) /
12.0);
- turret_victor_->SetSpeed(std::clamp(-output.turret_voltage(),
+ turret_victor_->SetSpeed(std::clamp(output.turret_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
@@ -438,12 +464,6 @@
std::clamp(output.feeder_voltage(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
- if (washing_machine_control_panel_victor_) {
- washing_machine_control_panel_victor_->SetSpeed(
- std::clamp(-output.washing_machine_spinner_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- }
accelerator_left_falcon_->SetSpeed(
std::clamp(-output.accelerator_left_voltage(), -kMaxBringupPower,
@@ -478,9 +498,6 @@
hood_victor_->SetDisabled();
intake_joint_victor_->SetDisabled();
turret_victor_->SetDisabled();
- if (washing_machine_control_panel_victor_) {
- washing_machine_control_panel_victor_->SetDisabled();
- }
accelerator_left_falcon_->SetDisabled();
accelerator_right_falcon_->SetDisabled();
finisher_falcon0_->SetDisabled();
@@ -492,7 +509,7 @@
}
::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
- turret_victor_, washing_machine_control_panel_victor_;
+ turret_victor_;
::std::unique_ptr<::frc::TalonFX> accelerator_left_falcon_,
accelerator_right_falcon_, finisher_falcon0_, finisher_falcon1_;
@@ -550,8 +567,10 @@
sensor_reader.set_left_accelerator_encoder(make_encoder(4));
sensor_reader.set_right_accelerator_encoder(make_encoder(3));
- sensor_reader.set_control_panel_encoder(
- make_unique<frc::Encoder>(5, 6, false, frc::Encoder::k1X));
+ if (FLAGS_shooter_tuning) {
+ sensor_reader.set_ball_beambreak_inputs(
+ make_unique<frc::DigitalInput>(6), make_unique<frc::DigitalInput>(7));
+ }
// Note: If ADIS16470 is plugged in directly to the roboRIO SPI port without
// the Spartan Board, then trigger is on 26, reset 27, and chip select is
@@ -592,8 +611,6 @@
superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(7));
superstructure_writer.set_feeder_falcon(
make_unique<ctre::phoenix::motorcontrol::can::TalonFX>(1));
- superstructure_writer.set_washing_machine_control_panel_victor(
- make_unique<frc::VictorSP>(6));
superstructure_writer.set_accelerator_left_falcon(
make_unique<::frc::TalonFX>(5));
superstructure_writer.set_accelerator_right_falcon(
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 8760fde..be3bfac 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -78,6 +78,7 @@
":camera_main_bundle",
":field_main_bundle",
":files",
+ "//frc971/analysis:plot_index_bundle.min.js",
],
dir = "www",
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index 4aaa05f..1a0b56a 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -191,6 +191,8 @@
ctx.beginPath();
ctx.moveTo(0.5, 0.5);
ctx.lineTo(0, 0);
+ ctx.lineTo(100.0, 0);
+ ctx.lineTo(0, 0);
ctx.lineTo(0.5, -0.5);
ctx.stroke();
ctx.beginPath();
@@ -218,13 +220,13 @@
// Draw line in circle to show forwards.
ctx.beginPath();
ctx.moveTo(0, 0);
- ctx.lineTo(turretRadius, 0);
+ ctx.lineTo(1000.0 * turretRadius, 0);
ctx.stroke();
ctx.restore();
}
ctx.beginPath();
ctx.moveTo(0, 0);
- ctx.lineTo(ROBOT_LENGTH / 2, 0);
+ ctx.lineTo(100.0 * ROBOT_LENGTH / 2, 0);
ctx.stroke();
ctx.restore();
}
diff --git a/y2020/www/index.html b/y2020/www/index.html
index f9ceca2..a105832 100644
--- a/y2020/www/index.html
+++ b/y2020/www/index.html
@@ -1,6 +1,7 @@
<html>
<body>
<a href="camera_viewer.html">Camera Viewer</a><br>
- <a href="field.html">Field Visualization</a>
+ <a href="field.html">Field Visualization</a><br>
+ <a href="plotter.html">Plots</a>
</body>
</html>
diff --git a/y2020/www/plotter.html b/y2020/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2020/www/plotter.html
@@ -0,0 +1,7 @@
+<html>
+ <head>
+ <script src="plot_index_bundle.min.js" defer></script>
+ </head>
+ <body>
+ </body>
+</html>
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 5243d5a..67916b6 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -190,12 +190,36 @@
},
{
"name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.shooter.TuningReadings",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 64
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.shooter.TuningParams",
+ "source_node": "roborio",
+ "frequency": 5,
+ "num_senders": 2,
+ "max_size": 256
+ },
+ {
+ "name": "/superstructure",
"type": "y2020.joysticks.Setpoint",
"source_node": "roborio",
"num_senders": 2
},
{
"name": "/drivetrain",
+ "type": "y2020.control_loops.drivetrain.LocalizerDebug",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 512,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.IMUValuesBatch",
"source_node": "roborio",
"frequency": 250,
diff --git a/y2021_bot3/control_loops/python/BUILD b/y2021_bot3/control_loops/python/BUILD
index 7ade878..15dacaa 100644
--- a/y2021_bot3/control_loops/python/BUILD
+++ b/y2021_bot3/control_loops/python/BUILD
@@ -5,8 +5,10 @@
srcs = [
"drivetrain.py",
],
+ legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
+ ":python_init",
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:drivetrain",
@@ -19,8 +21,10 @@
"drivetrain.py",
"polydrivetrain.py",
],
+ legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
+ ":python_init",
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:polydrivetrain",
@@ -43,3 +47,11 @@
"//frc971/control_loops/python:polydrivetrain",
],
)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2020/control_loops:python_init"],
+)
diff --git a/y2021_bot3/control_loops/python/__init__.py b/y2021_bot3/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2021_bot3/control_loops/python/__init__.py
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index 404555f..77cdf45 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -76,3 +76,29 @@
"//aos/events:shm_event_loop",
],
)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ data = [
+ "//y2021_bot3:config",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_lib",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:math",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
\ No newline at end of file
diff --git a/y2021_bot3/control_loops/superstructure/superstructure.cc b/y2021_bot3/control_loops/superstructure/superstructure.cc
index aaf3def..22c7c44 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,7 @@
event_loop->SetRuntimeRealtimePriority(30);
}
-void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
+void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position * /*position*/,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
@@ -24,8 +24,13 @@
AOS_LOG(ERROR, "WPILib reset, restarting\n");
}
- if (output != nullptr) {
+ if (output != nullptr && unsafe_goal != nullptr) {
OutputT output_struct;
+
+ output_struct.intake_volts =
+ std::clamp(unsafe_goal->intake_speed(), -12.0, 12.0);
+ output_struct.outtake_volts =
+ std::clamp(unsafe_goal->outtake_speed(), -12.0, 12.0);
output->Send(Output::Pack(*output->fbb(), &output_struct));
}
@@ -34,6 +39,11 @@
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
+ if (unsafe_goal != nullptr) {
+ status_builder.add_intake_speed(unsafe_goal->intake_speed());
+ status_builder.add_outtake_speed(unsafe_goal->outtake_speed());
+ }
+
status->Send(status_builder.Finish());
}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..cd031c3
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,158 @@
+#include <chrono>
+#include <memory>
+
+#include "aos/events/logging/log_writer.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+ SuperstructureTest()
+ : ::frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2021_bot3/config.json"),
+ std::chrono::microseconds(5050)),
+ superstructure_event_loop(MakeEventLoop("Superstructure")),
+ superstructure_(superstructure_event_loop.get()),
+ test_event_loop_(MakeEventLoop("test")),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_position_sender_(
+ test_event_loop_->MakeSender<Position>("/superstructure")) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+ SetEnabled(true);
+
+ phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
+ [this](int) { SendPositionMessage(); }, dt());
+ }
+
+ void VerifyResults(double intake_voltage, double outtake_voltage,
+ double intake_speed, double outtake_speed) {
+ superstructure_output_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_volts(), intake_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->outtake_volts(), outtake_voltage);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_speed(), intake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->outtake_speed(), outtake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->estopped(), false);
+ EXPECT_EQ(superstructure_status_fetcher_->zeroed(), true);
+ }
+
+ void SendPositionMessage() {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ builder.Send(position_builder.Finish());
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2021_bot3::control_loops::superstructure::Superstructure superstructure_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+};
+
+// Tests running the intake and outtake separately
+TEST_F(SuperstructureTest, RunIntakeOrOuttake) {
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(10.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ SendPositionMessage();
+ RunFor(dt() * 2);
+ VerifyResults(10.0, 0.0, 10.0, 0.0);
+ }
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_outtake_speed(10.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 10.0, 0.0, 10.0);
+ }
+}
+
+// Tests running both the intake and the outtake simultaneously
+TEST_F(SuperstructureTest, RunIntakeAndOuttake) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(10.0);
+ goal_builder.add_outtake_speed(5.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(10.0, 5.0, 10.0, 5.0);
+}
+
+// Tests for an invalid voltage (over 12 or under -12) to check that it defaults
+// to 12 or -12
+TEST_F(SuperstructureTest, InvalidVoltage) {
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(20.0);
+ goal_builder.add_outtake_speed(15.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(12.0, 12.0, 20.0, 15.0);
+ }
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(-20.0);
+ goal_builder.add_outtake_speed(-15.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(-12.0, -12.0, -20.0, -15.0);
+ }
+}
+
+// Tests that there is no output when the goal is null
+TEST_F(SuperstructureTest, GoalIsNull) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 0.0, 0.0);
+}
+
+// Tests that the robot behaves properly when disabled
+TEST_F(SuperstructureTest, Disabled) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(6.0);
+ goal_builder.add_outtake_speed(5.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ SetEnabled(false);
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 6.0, 5.0);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2021_bot3
\ No newline at end of file
