Merge "Full calibration files for 971 bot"
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 9b03907..8a30408 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -228,9 +228,6 @@
srcs = [
"irq_affinity.cc",
],
- data = [
- "//aos/starter:rockpi_config.json",
- ],
visibility = ["//visibility:public"],
deps = [
":irq_affinity_lib",
diff --git a/aos/util/error_counter.h b/aos/util/error_counter.h
index cf6cb7f..9fbe242 100644
--- a/aos/util/error_counter.h
+++ b/aos/util/error_counter.h
@@ -67,5 +67,40 @@
private:
flatbuffers::Vector<flatbuffers::Offset<Count>> *vector_ = nullptr;
};
+
+// The ArrayErrorCounter serves the same purpose as the ErrorCounter class,
+// except that:
+// (a) It owns its own memory, rather than modifying a flatbuffer in-place.
+// (b) Because of this, the user has greater flexibility in choosing when to
+// reset the error counters.
+template <typename Error, typename Count>
+class ArrayErrorCounter {
+ public:
+ static constexpr size_t kNumErrors = ErrorCounter<Error, Count>::kNumErrors;
+ ArrayErrorCounter() { ResetCounts(); }
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Count>>>
+ PopulateCounts(flatbuffers::FlatBufferBuilder *fbb) {
+ const flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Count>>>
+ offset = ErrorCounter<Error, Count>::Initialize(fbb);
+ flatbuffers::Vector<flatbuffers::Offset<Count>> *vector =
+ flatbuffers::GetMutableTemporaryPointer(*fbb, offset);
+ for (size_t ii = 0; ii < kNumErrors; ++ii) {
+ vector->GetMutableObject(ii)->mutate_count(error_counts_.at(ii));
+ }
+ return offset;
+ }
+
+ void IncrementError(Error error) {
+ DCHECK_LT(static_cast<size_t>(error), error_counts_.size());
+ error_counts_.at(static_cast<size_t>(error))++;
+ }
+
+ // Sets all the error counts to zero.
+ void ResetCounts() { error_counts_.fill(0); }
+
+ private:
+ std::array<size_t, kNumErrors> error_counts_;
+};
} // namespace aos::util
#endif // AOS_UTIL_ERROR_COUNTER_H_
diff --git a/aos/util/error_counter_test.cc b/aos/util/error_counter_test.cc
index 2166cea..567d71d 100644
--- a/aos/util/error_counter_test.cc
+++ b/aos/util/error_counter_test.cc
@@ -34,4 +34,45 @@
EXPECT_EQ(0u, message.message().error_counts()->Get(0)->count());
EXPECT_EQ(0u, message.message().error_counts()->Get(1)->count());
}
+
+// Tests the ArrayErrorCounter
+TEST(ErrorCounterTest, ARrayErrorCounter) {
+ ArrayErrorCounter<aos::timing::SendError, aos::timing::SendErrorCount>
+ counter;
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ counter.IncrementError(aos::timing::SendError::MESSAGE_SENT_TOO_FAST);
+ counter.IncrementError(aos::timing::SendError::MESSAGE_SENT_TOO_FAST);
+ counter.IncrementError(aos::timing::SendError::INVALID_REDZONE);
+ {
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<aos::timing::SendErrorCount>>>
+ counts_offset = counter.PopulateCounts(&fbb);
+ aos::timing::Sender::Builder builder(fbb);
+ builder.add_error_counts(counts_offset);
+ fbb.Finish(builder.Finish());
+ aos::FlatbufferDetachedBuffer<aos::timing::Sender> message = fbb.Release();
+ ASSERT_EQ(2u, message.message().error_counts()->size());
+ EXPECT_EQ(aos::timing::SendError::MESSAGE_SENT_TOO_FAST,
+ message.message().error_counts()->Get(0)->error());
+ EXPECT_EQ(2u, message.message().error_counts()->Get(0)->count());
+ EXPECT_EQ(aos::timing::SendError::INVALID_REDZONE,
+ message.message().error_counts()->Get(1)->error());
+ EXPECT_EQ(1u, message.message().error_counts()->Get(1)->count());
+ }
+
+ counter.ResetCounts();
+ {
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<aos::timing::SendErrorCount>>>
+ counts_offset = counter.PopulateCounts(&fbb);
+ aos::timing::Sender::Builder builder(fbb);
+ builder.add_error_counts(counts_offset);
+ fbb.Finish(builder.Finish());
+ aos::FlatbufferDetachedBuffer<aos::timing::Sender> message = fbb.Release();
+ ASSERT_EQ(2u, message.message().error_counts()->size());
+ EXPECT_EQ(0u, message.message().error_counts()->Get(0)->count());
+ EXPECT_EQ(0u, message.message().error_counts()->Get(1)->count());
+ }
+}
} // namespace aos::util::testing
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7e0500e..86c1ec0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -831,17 +831,3 @@
"//aos/network/www:proxy",
],
)
-
-cc_library(
- name = "localization_utils",
- srcs = ["localization_utils.cc"],
- hdrs = ["localization_utils.h"],
- deps = [
- ":drivetrain_output_fbs",
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//frc971/input:joystick_state_fbs",
- "//frc971/vision:calibration_fbs",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
new file mode 100644
index 0000000..06c8d57
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -0,0 +1,66 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+cc_library(
+ name = "utils",
+ srcs = ["utils.cc"],
+ hdrs = ["utils.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/input:joystick_state_fbs",
+ "//frc971/vision:calibration_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "puppet_localizer",
+ srcs = ["puppet_localizer.cc"],
+ hdrs = ["puppet_localizer.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ ],
+)
+
+cc_test(
+ name = "puppet_localizer_test",
+ srcs = ["puppet_localizer_test.cc"],
+ data = ["//y2022/control_loops/drivetrain:simulation_config"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":puppet_localizer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/network:team_number",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_output_fbs",
+ srcs = [
+ "localizer_output.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "localizer_output_ts_fbs",
+ srcs = ["localizer_output.fbs"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2022/localizer/localizer_output.fbs b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
similarity index 100%
rename from y2022/localizer/localizer_output.fbs
rename to frc971/control_loops/drivetrain/localization/localizer_output.fbs
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
similarity index 82%
rename from y2022/control_loops/drivetrain/localizer.cc
rename to frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 0aa4456..3125793 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -1,10 +1,10 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-Localizer::Localizer(
+PuppetLocalizer::PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config)
@@ -15,8 +15,6 @@
localizer_output_fetcher_(
event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
"/localizer")),
- joystick_state_fetcher_(
- event_loop_->MakeFetcher<aos::JoystickState>("/aos")),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")) {
@@ -30,7 +28,7 @@
target_selector_.set_has_target(false);
}
-void Localizer::Reset(
+void PuppetLocalizer::Reset(
aos::monotonic_clock::time_point t,
const frc971::control_loops::drivetrain::HybridEkf<double>::State &state) {
// Go through and clear out all of the fetchers so that we don't get behind.
@@ -38,19 +36,12 @@
ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
}
-void Localizer::Update(const Eigen::Matrix<double, 2, 1> &U,
+void PuppetLocalizer::Update(const Eigen::Matrix<double, 2, 1> &U,
aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
- joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() != nullptr &&
- joystick_state_fetcher_->autonomous()) {
- // TODO(james): This is an inelegant way to avoid having the localizer mess
- // up splines. Do better.
- // return;
- }
if (localizer_output_fetcher_.Fetch()) {
clock_offset_fetcher_.Fetch();
bool message_bridge_connected = true;
@@ -79,8 +70,6 @@
std::chrono::nanoseconds(
localizer_output_fetcher_->monotonic_timestamp_ns()) -
monotonic_offset);
- // TODO: Finish implementing simple x/y/theta updater with state_at_capture.
- // TODO: Implement turret/camera processing logic on pi side.
std::optional<State> state_at_capture =
ekf_.LastStateBeforeTime(capture_time);
if (!state_at_capture.has_value()) {
@@ -104,4 +93,4 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
diff --git a/y2022/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
similarity index 78%
rename from y2022/control_loops/drivetrain/localizer.h
rename to frc971/control_loops/drivetrain/localization/puppet_localizer.h
index 77b29eb..4f8f4f3 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
@@ -1,5 +1,5 @@
-#ifndef Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
-#define Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
#include <string_view>
@@ -7,20 +7,20 @@
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/input/joystick_state_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-// This class handles the localization for the 2022 robot. Rather than actually
-// doing any work on the roborio, we farm all the localization out to a
+// This class handles the localization for the 2022/2023 robots. Rather than
+// actually doing any work on the roborio, we farm all the localization out to a
// raspberry pi and it then sends out LocalizerOutput messages that we treat as
-// measurement updates. See //y2022/localizer.
-// TODO(james): Needs tests. Should refactor out some of the code from the 2020
-// localizer test.
-class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
+// measurement updates. See //y202*/localizer.
+// TODO(james): Needs more tests. Should refactor out some of the code from the
+// 2020 localizer test.
+class PuppetLocalizer
+ : public frc971::control_loops::drivetrain::LocalizerInterface {
public:
typedef frc971::control_loops::TypedPose<float> Pose;
typedef frc971::control_loops::drivetrain::HybridEkf<float> HybridEkf;
@@ -29,9 +29,10 @@
typedef typename HybridEkf::StateSquare StateSquare;
typedef typename HybridEkf::Input Input;
typedef typename HybridEkf::Output Output;
- Localizer(aos::EventLoop *event_loop,
- const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config);
+ PuppetLocalizer(
+ aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
const override {
return ekf_.X_hat().cast<double>();
@@ -93,7 +94,6 @@
HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
// Target selector to allow us to satisfy the LocalizerInterface requirements.
@@ -102,6 +102,6 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
-#endif // Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
similarity index 92%
rename from y2022/control_loops/drivetrain/localizer_test.cc
rename to frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
index 77f3988..d64c419 100644
--- a/y2022/control_loops/drivetrain/localizer_test.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
@@ -1,4 +1,4 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include <queue>
@@ -8,17 +8,17 @@
#include "aos/network/testing_time_converter.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-#include "y2022/localizer/localizer_output_generated.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
DECLARE_bool(die_on_malloc);
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
namespace testing {
@@ -29,7 +29,8 @@
namespace {
DrivetrainConfig<double> GetTest2022DrivetrainConfig() {
- DrivetrainConfig<double> config = GetDrivetrainConfig();
+ DrivetrainConfig<double> config =
+ y2022::control_loops::drivetrain::GetDrivetrainConfig();
return config;
}
} // namespace
@@ -39,11 +40,13 @@
using frc971::control_loops::drivetrain::DrivetrainLoop;
using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
+
// TODO(james): Make it so this actually tests the full system of the localizer.
class LocalizedDrivetrainTest : public frc971::testing::ControlLoopTest {
protected:
- // We must use the 2022 drivetrain config so that we don't have to deal
- // with shifting:
+ // We must use the 2022 drivetrain config so that we actually have a multi-nde
+ // config with a LocalizerOutput message.
+ // TODO(james): Refactor this test to be year-agnostic.
LocalizedDrivetrainTest()
: frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig(
@@ -108,7 +111,8 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ Eigen::Matrix<double, PuppetLocalizer::HybridEkf::kNStates, 1>
+ localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
localizer_.Reset(monotonic_now(), localizer_state);
@@ -169,7 +173,7 @@
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- Localizer localizer_;
+ PuppetLocalizer localizer_;
DrivetrainLoop drivetrain_;
std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
@@ -208,4 +212,4 @@
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/localization_utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
similarity index 97%
rename from frc971/control_loops/drivetrain/localization_utils.cc
rename to frc971/control_loops/drivetrain/localization/utils.cc
index c7968e1..ff027d0 100644
--- a/frc971/control_loops/drivetrain/localization_utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/drivetrain/localization_utils.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
namespace frc971::control_loops::drivetrain {
diff --git a/frc971/control_loops/drivetrain/localization_utils.h b/frc971/control_loops/drivetrain/localization/utils.h
similarity index 100%
rename from frc971/control_loops/drivetrain/localization_utils.h
rename to frc971/control_loops/drivetrain/localization/utils.h
diff --git a/frc971/rockpi/BUILD b/frc971/rockpi/BUILD
new file mode 100644
index 0000000..f6d1840
--- /dev/null
+++ b/frc971/rockpi/BUILD
@@ -0,0 +1 @@
+exports_files(["rockpi_config.json"])
diff --git a/frc971/rockpi/contents/root/bin/change_hostname.sh b/frc971/rockpi/contents/root/bin/change_hostname.sh
index a784bb1..43e6528 100755
--- a/frc971/rockpi/contents/root/bin/change_hostname.sh
+++ b/frc971/rockpi/contents/root/bin/change_hostname.sh
@@ -40,23 +40,10 @@
done
fi
-# Put corret team number in roborio's address, or add it if missing
+# Put correct team number in roborio's address, or add it if missing
if grep '^10\.[0-9]*\.[0-9]*\.2\s*roborio$' /etc/hosts >/dev/null;
then
sed -i "s/^10\.[0-9]*\.[0-9]*\(\.2\s*roborio\)$/${IP_BASE}\1/" /etc/hosts
else
echo -e "${IP_BASE}.2\troborio" >> /etc/hosts
fi
-
-# Put corret team number in imu's address, or add it if missing
-if grep '^10\.[0-9]*\.[0-9]*\.105\s.*\s*imu$' /etc/hosts >/dev/null;
-then
- sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi5 imu\)$/${IP_BASE}\1${TEAM_NUMBER}\2/" /etc/hosts
-else
- if grep '^10\.[0-9]*\.[0-9]*\.105\s*pi-[0-9]*-[0-9]*\s*pi5$' /etc/hosts
- then
- sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi5\)$/${IP_BASE}\1${TEAM_NUMBER}\2 imu/" /etc/hosts
- else
- echo -e "${IP_BASE}.105\tpi-${TEAM_NUMBER}-5 pi5 imu" >> /etc/hosts
- fi
-fi
diff --git a/aos/starter/rockpi_config.json b/frc971/rockpi/rockpi_config.json
similarity index 100%
rename from aos/starter/rockpi_config.json
rename to frc971/rockpi/rockpi_config.json
diff --git a/y2022/BUILD b/y2022/BUILD
index e978f7a..1a04c2e 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -102,7 +102,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//frc971/vision:vision_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//y2022/vision:target_estimate_fbs",
"//y2022/vision:ball_color_fbs",
@@ -132,7 +132,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/control_loops/drivetrain/BUILD b/y2022/control_loops/drivetrain/BUILD
index 18855a6..148a998 100644
--- a/y2022/control_loops/drivetrain/BUILD
+++ b/y2022/control_loops/drivetrain/BUILD
@@ -71,19 +71,6 @@
],
)
-cc_library(
- name = "localizer",
- srcs = ["localizer.cc"],
- hdrs = ["localizer.h"],
- deps = [
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//frc971/control_loops/drivetrain:hybrid_ekf",
- "//frc971/control_loops/drivetrain:localizer",
- "//y2022/localizer:localizer_output_fbs",
- ],
-)
-
cc_binary(
name = "drivetrain",
srcs = [
@@ -93,10 +80,10 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
- ":localizer",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
],
)
@@ -111,25 +98,6 @@
],
)
-cc_test(
- name = "localizer_test",
- srcs = ["localizer_test.cc"],
- data = [":simulation_config"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":drivetrain_base",
- ":localizer",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_writer",
- "//aos/network:team_number",
- "//frc971/control_loops:control_loop_test",
- "//frc971/control_loops:team_number_test_environment",
- "//frc971/control_loops/drivetrain:drivetrain_lib",
- "//frc971/control_loops/drivetrain:drivetrain_test_lib",
- "//y2022/localizer:localizer_output_fbs",
- ],
-)
-
cc_binary(
name = "trajectory_generator",
srcs = [
diff --git a/y2022/control_loops/drivetrain/drivetrain_main.cc b/y2022/control_loops/drivetrain/drivetrain_main.cc
index a422eaa..6e02cc7 100644
--- a/y2022/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_main.cc
@@ -3,8 +3,8 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-#include "y2022/control_loops/drivetrain/localizer.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -15,10 +15,11 @@
aos::configuration::ReadConfig("aos_config.json");
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<::y2022::control_loops::drivetrain::Localizer> localizer =
- std::make_unique<y2022::control_loops::drivetrain::Localizer>(
- &event_loop,
- y2022::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer =
+ std::make_unique<frc971::control_loops::drivetrain::PuppetLocalizer>(
+ &event_loop,
+ y2022::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2022::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index a085049..e0db64c 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -193,10 +193,10 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/queues:gyro_fbs",
"//third_party:phoenix",
"//third_party:wpilib",
- "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index c058254..71bc73b 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -8,11 +8,11 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/queues/gyro_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
namespace y2022::control_loops::superstructure {
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index a3b1774..dd0fb67 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -17,22 +17,6 @@
)
flatbuffer_cc_library(
- name = "localizer_output_fbs",
- srcs = [
- "localizer_output.fbs",
- ],
- gen_reflections = True,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_ts_library(
- name = "localizer_output_ts_fbs",
- srcs = ["localizer_output.fbs"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_cc_library(
name = "localizer_status_fbs",
srcs = [
"localizer_status.fbs",
@@ -94,7 +78,6 @@
hdrs = ["localizer.h"],
visibility = ["//visibility:public"],
deps = [
- ":localizer_output_fbs",
":localizer_status_fbs",
":localizer_visualization_fbs",
"//aos/containers:ring_buffer",
@@ -107,8 +90,9 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:improved_down_estimator",
- "//frc971/control_loops/drivetrain:localization_utils",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:utils",
"//frc971/imu_reader:imu_watcher",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:calibration_fbs",
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 46eff94..ec81573 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -479,21 +479,6 @@
}
namespace {
-// Converts a flatbuffer TransformationMatrix to an Eigen matrix. Technically,
-// this should be able to do a single memcpy, but the extra verbosity here seems
-// appropriate.
-Eigen::Matrix<double, 4, 4> FlatbufferToTransformationMatrix(
- const frc971::vision::calibration::TransformationMatrix &flatbuffer) {
- CHECK_EQ(16u, CHECK_NOTNULL(flatbuffer.data())->size());
- Eigen::Matrix<double, 4, 4> result;
- result.setIdentity();
- for (int row = 0; row < 4; ++row) {
- for (int col = 0; col < 4; ++col) {
- result(row, col) = (*flatbuffer.data())[row * 4 + col];
- }
- }
- return result;
-}
// Node names of the pis to listen for cameras from.
constexpr std::array<std::string_view, ModelBasedLocalizer::kNumPis> kPisToUse{
@@ -521,7 +506,8 @@
}
CHECK(calibration->has_fixed_extrinsics());
const Eigen::Matrix<double, 4, 4> fixed_extrinsics =
- FlatbufferToTransformationMatrix(*calibration->fixed_extrinsics());
+ control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->fixed_extrinsics());
// Calculate the pose of the camera relative to the robot origin.
Eigen::Matrix<double, 4, 4> H_robot_camera = fixed_extrinsics;
@@ -530,7 +516,8 @@
H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw<double>(
state.turret_position) *
- FlatbufferToTransformationMatrix(*calibration->turret_extrinsics());
+ control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->turret_extrinsics());
}
return H_robot_camera;
}
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index a403ca8..59ad75c 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -9,18 +9,18 @@
#include "aos/network/message_bridge_server_generated.h"
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
-#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
-#include "frc971/control_loops/drivetrain/localization_utils.h"
+#include "frc971/imu_reader/imu_watcher.h"
+#include "frc971/input/joystick_state_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/localizer/localizer_status_generated.h"
#include "y2022/localizer/localizer_visualization_generated.h"
#include "y2022/vision/target_estimate_generated.h"
-#include "frc971/imu_reader/imu_watcher.h"
namespace frc971::controls {
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 8d8a43c..99fb95d 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -98,11 +98,11 @@
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//aos/network:team_number",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:v4l2_reader",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index c2d4f5f..7f07704 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -11,10 +11,10 @@
#include "aos/events/shm_event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/v4l2_reader.h"
#include "frc971/vision/vision_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/vision/calibration_data.h"
#include "y2022/vision/gpio.h"
#include "y2022/vision/target_estimate_generated.h"
diff --git a/y2022/www/BUILD b/y2022/www/BUILD
index 386ca90..e806404 100644
--- a/y2022/www/BUILD
+++ b/y2022/www/BUILD
@@ -24,8 +24,8 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
"//y2022/control_loops/superstructure:superstructure_status_ts_fbs",
- "//y2022/localizer:localizer_output_ts_fbs",
"//y2022/localizer:localizer_status_ts_fbs",
"//y2022/localizer:localizer_visualization_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
index ec25607..d20637f 100644
--- a/y2022/www/field_handler.ts
+++ b/y2022/www/field_handler.ts
@@ -1,7 +1,7 @@
import {ByteBuffer} from 'flatbuffers';
import {Connection} from '../../aos/network/www/proxy';
import {IntakeState, Status as SuperstructureStatus, SuperstructureState} from '../control_loops/superstructure/superstructure_status_generated'
-import {LocalizerOutput} from '../localizer/localizer_output_generated';
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated';
import {RejectionReason} from '../localizer/localizer_status_generated';
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
import {LocalizerVisualization, TargetEstimateDebug} from '../localizer/localizer_visualization_generated';
diff --git a/y2023/BUILD b/y2023/BUILD
index 16273e5..b9842aa 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -50,6 +50,7 @@
],
data = [
":aos_config",
+ "//frc971/rockpi:rockpi_config.json",
"//y2023/constants:constants.json",
"//y2023/www:www_files",
],
@@ -102,7 +103,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2023/constants:constants_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
@@ -135,7 +136,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
"//frc971/vision:target_map_fbs",
],
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
new file mode 100644
index 0000000..beb81e1
--- /dev/null
+++ b/y2023/constants/9971.json
@@ -0,0 +1,16 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json' %}
+ }
+ ]
+}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 9f8d1a4..09e57c7 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -21,6 +21,7 @@
includes = [
"7971.json",
"971.json",
+ "9971.json",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
],
diff --git a/y2023/constants/constants.jinja2.json b/y2023/constants/constants.jinja2.json
index 2c8fce9..4fb9df7 100644
--- a/y2023/constants/constants.jinja2.json
+++ b/y2023/constants/constants.jinja2.json
@@ -10,7 +10,7 @@
},
{
"team": 9971,
- "data": {}
+ "data": {% include 'y2023/constants/9971.json' %}
}
]
}
diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index 9a5a156..561bb77 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -124,3 +124,19 @@
"@pip//python_gflags",
],
)
+
+py_binary(
+ name = "turret",
+ srcs = [
+ "turret.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 69e2497..054a32d 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -13,6 +13,8 @@
def add_edge(cc_file, name, segment, index, reverse):
+ segment.VerifyPoints()
+
cc_file.append(" // Adding edge %d" % index)
vmax = "vmax"
if segment.vmax:
@@ -71,7 +73,6 @@
def main(argv):
cc_file = []
- cc_file.append("")
cc_file.append("#include <memory>")
cc_file.append("")
cc_file.append(
@@ -84,6 +85,7 @@
cc_file.append(
"#include \"y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h\""
)
+ cc_file.append("")
cc_file.append("using frc971::control_loops::arm::SearchGraph;")
cc_file.append(
@@ -187,28 +189,24 @@
path_function_name(name))
cc_file.append("::std::unique_ptr<Path> %s() {" %
path_function_name(name))
- cc_file.append(
- " return ::std::unique_ptr<Path>(new Path(CosSpline{NSpline<4, 2>((Eigen::Matrix<double, 2, 4>() << "
- )
+ cc_file.append(" return ::std::unique_ptr<Path>(new Path(CosSpline{")
+ cc_file.append(" NSpline<4, 2>((Eigen::Matrix<double, 2, 4>() <<")
points = [
segment.start, segment.control1, segment.control2, segment.end
]
- for i in range(len(points)):
- cc_file.append("%.12f," % (points[i][0]))
- for i in range(len(points)):
- cc_file.append(
- "%.12f%s" %
- (points[i][1], ", " if i != len(points) - 1 else ""))
+ cc_file.append(" " +
+ " ".join(["%.12f," % (p[0]) for p in points]))
+ cc_file.append(" " +
+ ", ".join(["%.12f" % (p[1]) for p in points]))
- cc_file.append(").finished()), {")
+ cc_file.append(" ).finished()), {")
for alpha, roll in segment.alpha_rolls:
cc_file.append(
- "CosSpline::AlphaTheta{.alpha = %.12f, .theta = %.12f}" %
- (alpha, roll))
- if alpha != segment.alpha_rolls[-1][0]:
- cc_file.append(", ")
+ " CosSpline::AlphaTheta{.alpha = %.12f, .theta = %.12f},"
+ % (alpha, roll))
cc_file.append(" }}));")
cc_file.append("}")
+ cc_file.append("")
# Matrix of nodes
h_file.append("::std::vector<::Eigen::Matrix<double, 3, 1>> PointList();")
@@ -229,22 +227,25 @@
h_file.append("SearchGraph MakeSearchGraph("
"const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
- h_file.append(" "
- "const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,")
- h_file.append(" "
- "double vmax,")
+ h_file.append(
+ " const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,"
+ )
+ h_file.append(" double vmax,")
h_file.append(
"const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop);"
)
- cc_file.append("SearchGraph MakeSearchGraph("
- "const frc971::control_loops::arm::Dynamics *dynamics, "
- "::std::vector<TrajectoryAndParams> *trajectories,")
- cc_file.append(" "
- "const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,")
- cc_file.append(" "
- "double vmax,")
+ cc_file.append("")
+ cc_file.append("SearchGraph MakeSearchGraph(")
+ cc_file.append(" const frc971::control_loops::arm::Dynamics *dynamics,")
+ cc_file.append(" std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(
- "const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop) {"
+ " const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double vmax,"
+ )
+ cc_file.append(
+ " const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,"
+ )
+ cc_file.append(
+ " HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop) {"
)
cc_file.append(" ::std::vector<SearchGraph::Edge> edges;")
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index b21f548..87ced83 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -6,22 +6,23 @@
joint_center[1] + l2 - l1,
0.0,
circular_index=1)
-
-neutral_to_pickup_1 = to_theta_with_circular_index(0.3, 0.6, circular_index=1)
-neutral_to_pickup_2 = to_theta_with_circular_index(0.3, 0.4, circular_index=1)
+neutral_to_pickup_1 = np.array([2.396694, 0.508020])
+neutral_to_pickup_2 = np.array([2.874513, 0.933160])
pickup_pos = to_theta_with_circular_index_and_roll(0.6,
- 0.1,
- 0.0,
- circular_index=1)
-neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
+ 0.4,
+ np.pi / 2.0,
+ circular_index=0)
-neutral_to_score_1 = to_theta_with_circular_index(-0.4, 1.2, circular_index=1)
-neutral_to_score_2 = to_theta_with_circular_index(-0.7, 1.2, circular_index=1)
+neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.95, np.pi / 2.0)]
+
+neutral_to_score_1 = np.array([0.994244, -1.417442])
+neutral_to_score_2 = np.array([1.711325, -0.679748])
+
score_pos = to_theta_with_circular_index_and_roll(-1.0,
1.2,
- 0.0,
- circular_index=1)
-neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
+ np.pi / 2.0,
+ circular_index=0)
+neutral_to_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
# TODO(Max): Add real paths for arm.
points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index cf4fd7b..fef1083 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -409,12 +409,11 @@
self.DoDrawTo(cr, theta_version)
- def ToThetaPoints(self):
+ def VerifyPoints(self):
points = self.DoToThetaPoints()
if self.roll_joint_collision(points, verbose=True) or \
self.arm_past_limit(points, verbose=True):
sys.exit(1)
- return points
class SplineSegmentBase(Path):
diff --git a/y2023/control_loops/python/turret.py b/y2023/control_loops/python/turret.py
new file mode 100644
index 0000000..4e8f117
--- /dev/null
+++ b/y2023/control_loops/python/turret.py
@@ -0,0 +1,54 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kTurret = angular_system.AngularSystemParams(name='Turret',
+ motor=control_loop.Falcon(),
+ G=0.01,
+ J=3.1,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=25 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kTurret, R)
+ angular_system.PlotMotion(kTurret, R)
+ return
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2023', 'control_loops', 'superstructure', 'turret']
+ angular_system.WriteAngularSystem(kTurret, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
index 1aa8ce6..306bea1 100755
--- a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi2",
+ "node_name": "pi3",
"team_number": 971,
"intrinsics": [
891.026001,
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
index e7367cc..393beef 100755
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi3",
+ "node_name": "pi2",
"team_number": 971,
"intrinsics": [
894.002502,
@@ -21,4 +21,4 @@
],
"calibration_timestamp": 1358503360377380613,
"camera_id": "23-10"
-}
\ No newline at end of file
+}
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
index 68c2b94..e119c43 100755
--- a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi4",
+ "node_name": "pi1",
"team_number": 971,
"intrinsics": [
893.617798,
@@ -21,4 +21,4 @@
],
"calibration_timestamp": 1358499779650270322,
"camera_id": "23-09"
-}
\ No newline at end of file
+}