Merge "A couple scripts to help with extrinsic calibration logging"
diff --git a/BUILD b/BUILD
index d2c1658..5427fd5 100644
--- a/BUILD
+++ b/BUILD
@@ -69,6 +69,8 @@
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule_response //scouting/webserver/requests/messages:submit_shift_schedule_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking //scouting/webserver/requests/messages:submit_driver_ranking_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking_response //scouting/webserver/requests/messages:submit_driver_ranking_response_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting //scouting/webserver/requests/messages:delete_2023_data_scouting_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting_response //scouting/webserver/requests/messages:delete_2023_data_scouting_response_go_fbs
gazelle(
name = "gazelle",
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index 3126265..2fbb445 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -159,7 +159,7 @@
}
}
- bool FetchNext() { return FetchNextIf(std::ref(should_fetch_)); }
+ bool FetchNext() { return FetchNextIf(should_fetch_); }
bool FetchNextIf(std::function<bool(const Context &)> fn) {
const ipc_lib::LocklessQueueReader::Result read_result =
@@ -192,7 +192,7 @@
return read_result == ipc_lib::LocklessQueueReader::Result::GOOD;
}
- bool Fetch() { return FetchIf(std::ref(should_fetch_)); }
+ bool Fetch() { return FetchIf(should_fetch_); }
Context context() const { return context_; }
diff --git a/frc971/constants.h b/frc971/constants.h
index 4f7cb36..cf53287 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -70,6 +70,28 @@
struct RelativeEncoderZeroingConstants {};
+struct ContinuousAbsoluteEncoderZeroingConstants {
+ // The number of samples in the moving average filter.
+ size_t average_filter_size;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ // It is presumed that this will always be 2 * pi for any subsystem using this
+ // class, unless you have a continuous system that for some reason doesn't
+ // have a logical period of 1 revolution in radians.
+ double one_revolution_distance;
+ // Measured absolute position of the encoder when at zero.
+ double measured_absolute_position;
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ double zeroing_threshold;
+ // Buffer size for deciding if we are moving.
+ size_t moving_buffer_size;
+
+ // Value between 0 and 1 indicating what fraction of a revolution
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
+};
+
struct AbsoluteEncoderZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 3cdb6ab..31b245b 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -416,6 +416,7 @@
"//aos/util:trapezoid_profile",
"//frc971/control_loops:control_loop",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_index",
],
)
@@ -661,6 +662,9 @@
":static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_fbs",
"//aos/testing:googletest",
"//frc971/control_loops:control_loop_test",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
],
)
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 9823f0c..355710c 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -257,7 +257,7 @@
relative_pose_.rel_pos().y(), relative_pose_.rel_theta(),
abs_state(3, 0), abs_state(4, 0))
.finished();
- if (velocity_sign_ * goal_velocity_ < 0) {
+ if (velocity_sign_ * goal_velocity_ < -0.1) {
goal_theta = rel_state(2, 0);
}
controls_goal_ << goal_theta, goal_velocity_, 0.0;
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index dff5aad..1fa8ac2 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -15,6 +15,7 @@
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/pot_and_index.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index c886775..ace528e 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -16,6 +16,8 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_position_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_generated.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
using ::frc971::control_loops::PositionSensorSimulator;
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 7778da1..c4af163 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -224,7 +224,7 @@
: target_constraints_(target_constraints),
T_frozen_actual_(Eigen::Vector3d::Zero()),
R_frozen_actual_(Eigen::Quaterniond::Identity()),
- vis_robot_(cv::Size(1280, 1000)) {
+ vis_robot_(cv::Size(kImageWidth_, kImageHeight_)) {
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
@@ -243,7 +243,7 @@
target_constraints_(target_constraints),
T_frozen_actual_(Eigen::Vector3d::Zero()),
R_frozen_actual_(Eigen::Quaterniond::Identity()),
- vis_robot_(cv::Size(1280, 1000)) {
+ vis_robot_(cv::Size(kImageWidth_, kImageHeight_)) {
CountConstraints();
}
@@ -369,9 +369,10 @@
TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
// Set up robot visualization.
vis_robot_.ClearImage();
- constexpr int kImageWidth = 1280;
- constexpr double kFocalLength = 500.0;
- vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ // Compute focal length so that image shows field with viewpoint at 10m above
+ // it (default for viewer)
+ const double kFocalLength = kImageWidth_ * 10.0 / kFieldWidth_;
+ vis_robot_.SetDefaultViewpoint(kImageWidth_, kFocalLength);
const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id;
// Translation and rotation error for each target
@@ -428,6 +429,7 @@
CHECK(SolveOptimizationProblem(&target_pose_problem_2))
<< "The target pose solve 2 was not successful, exiting.";
+ LOG(INFO) << "Solving the overall map's best alignment to the previous map";
ceres::Problem map_fitting_problem(
{.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP});
std::unique_ptr<ceres::CostFunction> map_fitting_cost_function =
@@ -503,7 +505,6 @@
}
namespace {
-
// Hacks to extract a double from a scalar, which is either a ceres jet or a
// double. Only used for debugging and displaying.
template <typename S>
@@ -587,8 +588,11 @@
kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().z();
if (FLAGS_visualize_solver) {
+ LOG(INFO) << std::to_string(id) + std::string("-est") << " at "
+ << ScalarAffineToDouble(H_world_actual).matrix();
vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_actual),
- std::to_string(id), cv::Scalar(0, 255, 0));
+ std::to_string(id) + std::string("-est"),
+ cv::Scalar(0, 255, 0));
vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_ideal),
std::to_string(id), cv::Scalar(255, 255, 255));
}
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index cfefcc7..dc4a85a 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -116,6 +116,10 @@
Eigen::Translation3d T_frozen_actual_;
Eigen::Quaterniond R_frozen_actual_;
+ const double kFieldWidth_ = 20.0; // 20 meters across
+ const int kImageWidth_ = 1000;
+ const int kImageHeight_ =
+ kImageWidth_ * 3.0 / 4.0; // Roughly matches field aspect ratio
mutable VisualizeRobot vis_robot_;
Stats stats_with_outliers_;
diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/falcon.cc
index 6be83aa..c3ff26d 100644
--- a/frc971/wpilib/falcon.cc
+++ b/frc971/wpilib/falcon.cc
@@ -3,11 +3,12 @@
using frc971::wpilib::Falcon;
using frc971::wpilib::kMaxBringupPower;
-Falcon::Falcon(int device_id, std::string canbus,
+Falcon::Falcon(int device_id, bool inverted, std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit)
: talon_(device_id, canbus),
device_id_(device_id),
+ inverted_(inverted),
device_temp_(talon_.GetDeviceTemp()),
supply_voltage_(talon_.GetSupplyVoltage()),
supply_current_(talon_.GetSupplyCurrent()),
@@ -37,6 +38,12 @@
signals->push_back(&duty_cycle_);
}
+Falcon::Falcon(FalconParams params, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit)
+ : Falcon(params.device_id, params.inverted, canbus, signals,
+ stator_current_limit, supply_current_limit) {}
+
void Falcon::PrintConfigs() {
ctre::phoenix6::configs::TalonFXConfiguration configuration;
ctre::phoenix::StatusCode status =
@@ -48,9 +55,7 @@
AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
}
-void Falcon::WriteConfigs(ctre::phoenix6::signals::InvertedValue invert) {
- inverted_ = invert;
-
+void Falcon::WriteConfigs() {
ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
current_limits.StatorCurrentLimit = stator_current_limit_;
current_limits.StatorCurrentLimitEnable = true;
@@ -94,7 +99,8 @@
return status;
}
-void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb) {
+void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ double gear_ratio) {
control_loops::CANFalcon::Builder builder(*fbb);
builder.add_id(device_id_);
builder.add_device_temp(device_temp());
@@ -102,7 +108,7 @@
builder.add_supply_current(supply_current());
builder.add_torque_current(torque_current());
builder.add_duty_cycle(duty_cycle());
- builder.add_position(position());
+ builder.add_position(position() * gear_ratio);
last_position_offset_ = builder.Finish();
}
diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/falcon.h
index 8f0f1f0..6ea8735 100644
--- a/frc971/wpilib/falcon.h
+++ b/frc971/wpilib/falcon.h
@@ -18,24 +18,35 @@
namespace frc971 {
namespace wpilib {
+struct FalconParams {
+ int device_id;
+ bool inverted;
+};
+
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
static constexpr double kMaxBringupPower = 12.0;
// Gets info from and writes to falcon motors using the TalonFX controller.
class Falcon {
public:
- Falcon(int device_id, std::string canbus,
+ Falcon(int device_id, bool inverted, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit);
+
+ Falcon(FalconParams params, std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit);
void PrintConfigs();
- void WriteConfigs(ctre::phoenix6::signals::InvertedValue invert);
+ void WriteConfigs();
ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
- void SerializePosition(flatbuffers::FlatBufferBuilder *fbb);
+ // The position of the Falcon output shaft is multiplied by gear_ratio
+ void SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ double gear_ratio);
std::optional<flatbuffers::Offset<control_loops::CANFalcon>> TakeOffset();
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
index 2b6ef9e..bfc5d73 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
@@ -37,8 +37,8 @@
void DrivetrainWriter::WriteConfigs() {
for (auto module : {front_left_, front_right_, back_left_, back_right_}) {
- module->rotation->WriteConfigs(false);
- module->translation->WriteConfigs(false);
+ module->rotation->WriteConfigs();
+ module->translation->WriteConfigs();
}
}
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index 534f0ce..f449afa 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -8,14 +8,15 @@
namespace swerve {
struct SwerveModule {
- SwerveModule(int rotation_id, int translation_id, std::string canbus,
+ SwerveModule(FalconParams rotation_params, FalconParams translation_params,
+ std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit)
- : rotation(std::make_shared<Falcon>(rotation_id, canbus, signals,
+ : rotation(std::make_shared<Falcon>(rotation_params, canbus, signals,
stator_current_limit,
supply_current_limit)),
- translation(std::make_shared<Falcon>(translation_id, canbus, signals,
- stator_current_limit,
+ translation(std::make_shared<Falcon>(translation_params, canbus,
+ signals, stator_current_limit,
supply_current_limit)) {}
void WriteModule(
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index d50f181..8ca7f67 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -61,20 +61,8 @@
cc_library(
name = "zeroing",
srcs = [
- "absolute_and_absolute_encoder.cc",
- "absolute_encoder.cc",
- "hall_effect_and_position.cc",
- "pot_and_absolute_encoder.cc",
- "pot_and_index.cc",
- "pulse_index.cc",
],
hdrs = [
- "absolute_and_absolute_encoder.h",
- "absolute_encoder.h",
- "hall_effect_and_position.h",
- "pot_and_absolute_encoder.h",
- "pot_and_index.h",
- "pulse_index.h",
"zeroing.h",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -88,19 +76,10 @@
],
)
-cc_test(
- name = "zeroing_test",
- srcs = [
- "absolute_and_absolute_encoder_test.cc",
- "absolute_encoder_test.cc",
- "hall_effect_and_position_test.cc",
- "pot_and_absolute_encoder_test.cc",
- "pot_and_index_test.cc",
- "pulse_index_test.cc",
- "relative_encoder_test.cc",
- "zeroing_test.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
+cc_library(
+ name = "zeroing_test_lib",
+ testonly = True,
+ hdrs = ["zeroing_test.h"],
deps = [
":zeroing",
"//aos/testing:googletest",
@@ -109,6 +88,46 @@
],
)
+[
+ (
+ cc_library(
+ name = lib,
+ srcs = [lib + ".cc"],
+ hdrs = [lib + ".h"],
+ deps = [
+ ":wrap",
+ ":zeroing",
+ "//aos/containers:error_list",
+ "//aos/logging",
+ "//frc971:constants",
+ "//frc971/control_loops:control_loops_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+ ),
+ cc_test(
+ name = lib + "_test",
+ srcs = [lib + "_test.cc"],
+ deps = [
+ lib,
+ ":zeroing",
+ ":zeroing_test_lib",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:position_sensor_sim",
+ ],
+ ),
+ )
+ for lib in [
+ "absolute_and_absolute_encoder",
+ "absolute_encoder",
+ "continuous_absolute_encoder",
+ "hall_effect_and_position",
+ "pot_and_absolute_encoder",
+ "pot_and_index",
+ "pulse_index",
+ ]
+]
+
cc_library(
name = "wrap",
srcs = [
diff --git a/frc971/zeroing/absolute_encoder.h b/frc971/zeroing/absolute_encoder.h
index df40ec3..9d730f2 100644
--- a/frc971/zeroing/absolute_encoder.h
+++ b/frc971/zeroing/absolute_encoder.h
@@ -5,6 +5,7 @@
#include "flatbuffers/flatbuffers.h"
+#include "aos/containers/error_list.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
diff --git a/frc971/zeroing/continuous_absolute_encoder.cc b/frc971/zeroing/continuous_absolute_encoder.cc
new file mode 100644
index 0000000..a47a491
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder.cc
@@ -0,0 +1,169 @@
+#include "frc971/zeroing/continuous_absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "aos/containers/error_list.h"
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+ContinuousAbsoluteEncoderZeroingEstimator::
+ ContinuousAbsoluteEncoderZeroingEstimator(
+ const constants::ContinuousAbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void ContinuousAbsoluteEncoderZeroingEstimator::Reset() {
+ zeroed_ = false;
+ error_ = false;
+ first_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ position_ = 0.0;
+ nan_samples_ = 0;
+ relative_to_absolute_offset_samples_.clear();
+ move_detector_.Reset();
+}
+
+// The math here is a bit backwards, but I think it'll be less error prone that
+// way and more similar to the version with a pot as well.
+//
+// We start by unwrapping the absolute encoder using the relative encoder. This
+// puts us in a non-wrapping space and lets us average a bit easier. From
+// there, we can compute an offset and wrap ourselves back such that we stay
+// close to the middle value.
+//
+// To guard against the robot moving while updating estimates, buffer a number
+// of samples and check that the buffered samples are not different than the
+// zeroing threshold. At any point that the samples differ too much, do not
+// update estimates based on those samples.
+void ContinuousAbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const AbsolutePosition &info) {
+ // Check for Abs Encoder NaN value that would mess up the rest of the zeroing
+ // code below. NaN values are given when the Absolute Encoder is disconnected.
+ if (::std::isnan(info.absolute_encoder())) {
+ if (zeroed_) {
+ VLOG(1) << "NAN on absolute encoder.";
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
+ error_ = true;
+ } else {
+ ++nan_samples_;
+ VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
+ if (nan_samples_ >= constants_.average_filter_size) {
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
+ error_ = true;
+ zeroed_ = true;
+ }
+ }
+ // Throw some dummy values in for now.
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ position_ = offset_ + info.encoder();
+ return;
+ }
+
+ const bool moving = move_detector_.Update(info, constants_.moving_buffer_size,
+ constants_.zeroing_threshold);
+
+ if (!moving) {
+ const PositionStruct &sample = move_detector_.GetSample();
+
+ // adjusted_* numbers are nominally in the desired output frame.
+ const double adjusted_absolute_encoder =
+ sample.absolute_encoder - constants_.measured_absolute_position;
+
+ // Note: If are are near the breakpoint of the absolute encoder, this number
+ // will be jitter between numbers that are ~one_revolution_distance apart.
+ // For that reason, we rewrap it so that we are not near that boundary.
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - sample.encoder;
+
+ // To avoid the aforementioned jitter, choose a base value to use for
+ // wrapping. When we have no prior samples, just use the current offset.
+ // Otherwise, we use an arbitrary prior offset (the stored offsets will all
+ // already be wrapped).
+ const double relative_to_absolute_offset_wrap_base =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? relative_to_absolute_offset
+ : relative_to_absolute_offset_samples_[0];
+
+ const double relative_to_absolute_offset_wrapped =
+ UnWrap(relative_to_absolute_offset_wrap_base,
+ relative_to_absolute_offset, constants_.one_revolution_distance);
+
+ const size_t relative_to_absolute_offset_samples_size =
+ relative_to_absolute_offset_samples_.size();
+ if (relative_to_absolute_offset_samples_size <
+ constants_.average_filter_size) {
+ relative_to_absolute_offset_samples_.push_back(
+ relative_to_absolute_offset_wrapped);
+ } else {
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset_wrapped;
+ }
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ // Compute the average offset between the absolute encoder and relative
+ // encoder. Because we just pushed a value, the size() will never be zero.
+ offset_ =
+ ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(), 0.0) /
+ relative_to_absolute_offset_samples_.size();
+
+ // To provide a value that can be used to estimate the
+ // measured_absolute_position when zeroing, we just need to output the
+ // current absolute encoder value. We could make use of the averaging
+ // implicit in offset_ to reduce the noise on this slightly.
+ filtered_absolute_encoder_ = sample.absolute_encoder;
+
+ if (offset_ready()) {
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
+
+ if (::std::abs(first_offset_ - offset_) >
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance) {
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
+ errors_.Set(ZeroingError::OFFSET_MOVED_TOO_FAR);
+ error_ = true;
+ }
+
+ zeroed_ = true;
+ }
+ }
+
+ // Update the position. Wrap it to reflect the fact that we do not have
+ // sufficient information to disambiguate which revolution we are on (also,
+ // since this value is primarily meant for debugging, this makes it easier to
+ // see that the device is actually at zero without having to divide by 2 *
+ // pi).
+ position_ =
+ Wrap(0.0, offset_ + info.encoder(), constants_.one_revolution_distance);
+}
+
+flatbuffers::Offset<ContinuousAbsoluteEncoderZeroingEstimator::State>
+ContinuousAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<flatbuffers::Vector<ZeroingError>> errors_offset =
+ errors_.ToFlatbuffer(fbb);
+
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ builder.add_errors(errors_offset);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/continuous_absolute_encoder.h b/frc971/zeroing/continuous_absolute_encoder.h
new file mode 100644
index 0000000..e11d866
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder.h
@@ -0,0 +1,99 @@
+#ifndef FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
+#define FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
+
+#include <vector>
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "aos/containers/error_list.h"
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Estimates the position with an absolute encoder which spins continuously. The
+// absolute encoder must have a 1:1 ratio to the output.
+// The provided offset(), when added to incremental encoder, may return a value
+// outside of +/- pi. The user is responsible for handling wrapping.
+class ContinuousAbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<
+ AbsolutePosition,
+ constants::ContinuousAbsoluteEncoderZeroingConstants,
+ AbsoluteEncoderEstimatorState> {
+ public:
+ explicit ContinuousAbsoluteEncoderZeroingEstimator(
+ const constants::ContinuousAbsoluteEncoderZeroingConstants &constants);
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Updates the sensor values for the zeroing logic.
+ void UpdateEstimate(const AbsolutePosition &info) override;
+
+ void TriggerError() override { error_ = true; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const override {
+ return relative_to_absolute_offset_samples_.size() ==
+ constants_.average_filter_size;
+ }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ struct PositionStruct {
+ PositionStruct(const AbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double absolute_encoder;
+ double encoder;
+ };
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::ContinuousAbsoluteEncoderZeroingConstants constants_;
+
+ // True if the mechanism is zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+ // The first valid offset we recorded. This is only set after zeroed_ first
+ // changes to true.
+ double first_offset_;
+
+ // The filtered absolute encoder. This is used in the status for calibration.
+ double filtered_absolute_encoder_ = 0.0;
+
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+
+ MoveDetector<PositionStruct, AbsolutePosition> move_detector_;
+
+ // Estimated start position of the mechanism
+ double offset_ = 0;
+ // The next position in 'relative_to_absolute_offset_samples_' and
+ // 'encoder_samples_' to be used to store the next sample.
+ int samples_idx_ = 0;
+
+ // Number of NANs we've seen in a row.
+ size_t nan_samples_ = 0;
+
+ // The filtered position.
+ double position_ = 0.0;
+
+ // Marker to track what kind of error has occured.
+ aos::ErrorList<ZeroingError> errors_;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
diff --git a/frc971/zeroing/continuous_absolute_encoder_test.cc b/frc971/zeroing/continuous_absolute_encoder_test.cc
new file mode 100644
index 0000000..3869393
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder_test.cc
@@ -0,0 +1,198 @@
+#include "frc971/zeroing/continuous_absolute_encoder.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/wrap.h"
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::ContinuousAbsoluteEncoderZeroingConstants;
+
+class ContinuousAbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ ContinuousAbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ flatbuffers::FlatBufferBuilder fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+}
+
+// Makes sure that if the underlying mechanism moves by >1 revolution that the
+// continuous zeroing estimator handles it correctly.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ ContinuousEstimatorZeroesAcrossRevolution) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+
+ // Now, rotate by a full revolution, then stay still. We should stay zeroed.
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize; ++i) {
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+ }
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+}
+
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwards.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 10 * index_diff;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithNaN) {
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+ ASSERT_FALSE(estimator.error());
+
+ estimator.UpdateEstimate(*sensor_values);
+ ASSERT_TRUE(estimator.error());
+
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.Finish(estimator.GetEstimatorState(&fbb2));
+
+ const AbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteEncoderEstimatorState>(
+ fbb2.GetBufferPointer());
+
+ EXPECT_THAT(*state->errors(),
+ ::testing::ElementsAre(ZeroingError::LOST_ABSOLUTE_ENCODER));
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index f7b52a6..5d5b6eb 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -172,13 +172,4 @@
} // namespace zeroing
} // namespace frc971
-// TODO(Brian): Actually split these targets apart. Need to convert all the
-// reverse dependencies to #include what they actually need...
-#include "frc971/zeroing/absolute_and_absolute_encoder.h"
-#include "frc971/zeroing/absolute_encoder.h"
-#include "frc971/zeroing/hall_effect_and_position.h"
-#include "frc971/zeroing/pot_and_absolute_encoder.h"
-#include "frc971/zeroing/pot_and_index.h"
-#include "frc971/zeroing/pulse_index.h"
-
#endif // FRC971_ZEROING_ZEROING_H_
diff --git a/scouting/db/db.go b/scouting/db/db.go
index ac9a6e8..1a43634 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -199,6 +199,14 @@
return result.Error
}
+func (database *Database) DeleteFromActions(compLevel_ string, matchNumber_ int32, setNumber_ int32, teamNumber_ string) error {
+ var actions []Action
+ result := database.
+ Where("comp_level = ? AND match_number = ? AND set_number = ? AND team_number = ?", compLevel_, matchNumber_, setNumber_, teamNumber_).
+ Delete(&actions)
+ return result.Error
+}
+
func (database *Database) AddOrUpdateRankings(r Ranking) error {
result := database.Clauses(clause.OnConflict{
UpdateAll: true,
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 8a4c0bc..d49e649 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -505,6 +505,75 @@
}
}
+func TestDeleteFromActions(t *testing.T) {
+ fixture := createDatabase(t)
+ defer fixture.TearDown()
+
+ startingActions := []Action{
+ Action{
+ TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0000, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0321, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0222, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0110, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1239", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0004, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0005, CollectedBy: "",
+ },
+ }
+
+ correct := []Action{
+ Action{
+ TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0000, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0321, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0222, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0110, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0005, CollectedBy: "",
+ },
+ }
+
+ for _, action := range startingActions {
+ err := fixture.db.AddAction(action)
+ check(t, err, "Failed to add stat")
+ }
+
+ err := fixture.db.DeleteFromActions("quals", 94, 1, "1239")
+
+ got, err := fixture.db.ReturnActions()
+ check(t, err, "Failed ReturnActions()")
+
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+}
+
func TestQueryShiftDB(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index 52d838b..b1276d9 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -8,6 +8,10 @@
// (index 3) which resolves to team 3990 in quals match 1.
const QUALS_MATCH_1_TEAM_3990 = 0 * 6 + 3;
+// On the 2st row of matches (index 1) click on the fourth team
+// (index 3) which resolves to team 4481 in quals match 1.
+const QUALS_MATCH_2_TEAM_4481 = 1 * 6 + 3;
+
function disableAlerts() {
cy.get('#block_alerts').check({force: true}).should('be.checked');
}
@@ -66,6 +70,54 @@
return element;
}
+function submitDataScouting(
+ matchButtonKey = SEMI_FINAL_2_MATCH_3_TEAM_5254,
+ teamNumber = 5254
+) {
+ // Click on a random team in the Match list. The exact details here are not
+ // important, but we need to know what they are. This could as well be any
+ // other team from any other match.
+ cy.get('button.match-item').eq(matchButtonKey).click();
+
+ // Select Starting Position.
+ headerShouldBe(teamNumber + ' Init ');
+ cy.get('[type="radio"]').first().check();
+ clickButton('Start Match');
+
+ // Pick and Place Cone in Auto.
+ clickButton('CONE');
+ clickButton('HIGH');
+
+ // Pick and Place Cube in Teleop.
+ clickButton('Start Teleop');
+ clickButton('CUBE');
+ clickButton('LOW');
+
+ // Robot dead and revive.
+ clickButton('DEAD');
+ clickButton('Revive');
+
+ // Endgame.
+ clickButton('Endgame');
+ cy.contains(/Docked & Engaged/).click();
+
+ clickButton('End Match');
+ headerShouldBe(teamNumber + ' Review and Submit ');
+ cy.get('#review_data li')
+ .eq(0)
+ .should('have.text', ' Started match at position 1 ');
+ cy.get('#review_data li').eq(1).should('have.text', ' Picked up kCone ');
+ cy.get('#review_data li')
+ .last()
+ .should(
+ 'have.text',
+ ' Ended Match; docked: false, engaged: true, attempted to dock and engage: false '
+ );
+
+ clickButton('Submit');
+ headerShouldBe(teamNumber + ' Success ');
+}
+
before(() => {
cy.visit('/');
disableAlerts();
@@ -139,48 +191,7 @@
//TODO(FILIP): Verify last action when the last action header gets added.
it('should: be able to submit data scouting.', () => {
- // Click on a random team in the Match list. The exact details here are not
- // important, but we need to know what they are. This could as well be any
- // other team from any other match.
- cy.get('button.match-item').eq(SEMI_FINAL_2_MATCH_3_TEAM_5254).click();
-
- // Select Starting Position.
- headerShouldBe('5254 Init ');
- cy.get('[type="radio"]').first().check();
- clickButton('Start Match');
-
- // Pick and Place Cone in Auto.
- clickButton('CONE');
- clickButton('HIGH');
-
- // Pick and Place Cube in Teleop.
- clickButton('Start Teleop');
- clickButton('CUBE');
- clickButton('LOW');
-
- // Robot dead and revive.
- clickButton('DEAD');
- clickButton('Revive');
-
- // Endgame.
- clickButton('Endgame');
- cy.contains(/Docked & Engaged/).click();
-
- clickButton('End Match');
- headerShouldBe('5254 Review and Submit ');
- cy.get('#review_data li')
- .eq(0)
- .should('have.text', ' Started match at position 1 ');
- cy.get('#review_data li').eq(1).should('have.text', ' Picked up kCone ');
- cy.get('#review_data li')
- .last()
- .should(
- 'have.text',
- ' Ended Match; docked: false, engaged: true, attempted to dock and engage: false '
- );
-
- clickButton('Submit');
- headerShouldBe('5254 Success ');
+ submitDataScouting();
// Now that the data is submitted, the button should be disabled.
switchToTab('Match List');
@@ -189,6 +200,30 @@
.should('be.disabled');
});
+ it('should: be able to delete data scouting entry', () => {
+ // Submit data to delete.
+ submitDataScouting(QUALS_MATCH_2_TEAM_4481, 4481);
+
+ switchToTab('View');
+
+ cy.get('[data-bs-toggle="dropdown"]').click();
+ cy.get('[id="stats_source_dropdown"]').click();
+
+ // Check that table contains data.
+ cy.get('table.table tbody td').should('contain', '4481');
+
+ // Find and click the delete button for the row containing team 4481.
+ cy.get('table.table tbody td')
+ .contains('4481')
+ .parent()
+ .find('[id^="delete_button_"]')
+ .click();
+ cy.on('window:confirm', () => true);
+
+ // Check that deleted data is not in table.
+ cy.get('table.table tbody').should('not.contain', '4481');
+ });
+
it('should: be able to return to correct screen with undo for pick and place.', () => {
cy.get('button.match-item').eq(QUALS_MATCH_1_TEAM_3990).click();
diff --git a/scouting/webserver/requests/BUILD b/scouting/webserver/requests/BUILD
index 2ff8b85..795d0ee 100644
--- a/scouting/webserver/requests/BUILD
+++ b/scouting/webserver/requests/BUILD
@@ -8,6 +8,8 @@
visibility = ["//visibility:public"],
deps = [
"//scouting/db",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_go_fbs",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:error_response_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_go_fbs",
@@ -42,6 +44,7 @@
deps = [
"//scouting/db",
"//scouting/webserver/requests/debug",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_go_fbs",
diff --git a/scouting/webserver/requests/debug/BUILD b/scouting/webserver/requests/debug/BUILD
index d9bb030..ef14e5a 100644
--- a/scouting/webserver/requests/debug/BUILD
+++ b/scouting/webserver/requests/debug/BUILD
@@ -7,6 +7,7 @@
target_compatible_with = ["@platforms//cpu:x86_64"],
visibility = ["//visibility:public"],
deps = [
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:error_response_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_response_go_fbs",
diff --git a/scouting/webserver/requests/debug/debug.go b/scouting/webserver/requests/debug/debug.go
index acb9dd4..4837cfe 100644
--- a/scouting/webserver/requests/debug/debug.go
+++ b/scouting/webserver/requests/debug/debug.go
@@ -9,6 +9,7 @@
"log"
"net/http"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/error_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_driver_rankings_response"
@@ -164,3 +165,9 @@
server+"/requests/submit/submit_actions", requestBytes,
submit_actions_response.GetRootAsSubmitActionsResponse)
}
+
+func Delete2023DataScouting(server string, requestBytes []byte) (*delete_2023_data_scouting_response.Delete2023DataScoutingResponseT, error) {
+ return sendMessage[delete_2023_data_scouting_response.Delete2023DataScoutingResponseT](
+ server+"/requests/delete/delete_2023_data_scouting", requestBytes,
+ delete_2023_data_scouting_response.GetRootAsDelete2023DataScoutingResponse)
+}
diff --git a/scouting/webserver/requests/messages/BUILD b/scouting/webserver/requests/messages/BUILD
index c1cd999..db422ed 100644
--- a/scouting/webserver/requests/messages/BUILD
+++ b/scouting/webserver/requests/messages/BUILD
@@ -23,6 +23,8 @@
"submit_driver_ranking_response",
"submit_actions",
"submit_actions_response",
+ "delete_2023_data_scouting",
+ "delete_2023_data_scouting_response",
)
filegroup(
diff --git a/scouting/webserver/requests/messages/delete_2023_data_scouting.fbs b/scouting/webserver/requests/messages/delete_2023_data_scouting.fbs
new file mode 100644
index 0000000..a2a3ce6
--- /dev/null
+++ b/scouting/webserver/requests/messages/delete_2023_data_scouting.fbs
@@ -0,0 +1,10 @@
+namespace scouting.webserver.requests;
+
+table Delete2023DataScouting {
+ comp_level:string (id: 0);
+ match_number:int (id: 1);
+ set_number:int (id: 2);
+ team_number:string (id: 3);
+}
+
+root_type Delete2023DataScouting;
diff --git a/scouting/webserver/requests/messages/delete_2023_data_scouting_response.fbs b/scouting/webserver/requests/messages/delete_2023_data_scouting_response.fbs
new file mode 100644
index 0000000..fd07526
--- /dev/null
+++ b/scouting/webserver/requests/messages/delete_2023_data_scouting_response.fbs
@@ -0,0 +1,7 @@
+namespace scouting.webserver.requests;
+
+table Delete2023DataScoutingResponse {
+ // empty response
+}
+
+root_type Delete2023DataScoutingResponse;
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 37a6ff2..533959c 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -12,6 +12,8 @@
"strings"
"github.com/frc971/971-Robot-Code/scouting/db"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/error_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting_response"
@@ -58,6 +60,8 @@
type SubmitActions = submit_actions.SubmitActions
type Action = submit_actions.Action
type SubmitActionsResponseT = submit_actions_response.SubmitActionsResponseT
+type Delete2023DataScouting = delete_2023_data_scouting.Delete2023DataScouting
+type Delete2023DataScoutingResponseT = delete_2023_data_scouting_response.Delete2023DataScoutingResponseT
// The interface we expect the database abstraction to conform to.
// We use an interface here because it makes unit testing easier.
@@ -76,6 +80,8 @@
AddNotes(db.NotesData) error
AddDriverRanking(db.DriverRankingData) error
AddAction(db.Action) error
+ DeleteFromStats(string, int32, int32, string) error
+ DeleteFromActions(string, int32, int32, string) error
}
// Handles unknown requests. Just returns a 404.
@@ -871,6 +877,50 @@
w.Write(builder.FinishedBytes())
}
+type Delete2023DataScoutingHandler struct {
+ db Database
+}
+
+func (handler Delete2023DataScoutingHandler) ServeHTTP(w http.ResponseWriter, req *http.Request) {
+ requestBytes, err := io.ReadAll(req.Body)
+ if err != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprint("Failed to read request bytes:", err))
+ return
+ }
+
+ request, success := parseRequest(w, requestBytes, "Delete2023DataScouting", delete_2023_data_scouting.GetRootAsDelete2023DataScouting)
+ if !success {
+ return
+ }
+
+ err = handler.db.DeleteFromStats(
+ string(request.CompLevel()),
+ request.MatchNumber(),
+ request.SetNumber(),
+ string(request.TeamNumber()))
+
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprintf("Failed to delete from stats: %v", err))
+ return
+ }
+
+ err = handler.db.DeleteFromActions(
+ string(request.CompLevel()),
+ request.MatchNumber(),
+ request.SetNumber(),
+ string(request.TeamNumber()))
+
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprintf("Failed to delete from actions: %v", err))
+ return
+ }
+
+ var response Delete2023DataScoutingResponseT
+ builder := flatbuffers.NewBuilder(10)
+ builder.Finish((&response).Pack(builder))
+ w.Write(builder.FinishedBytes())
+}
+
func HandleRequests(db Database, scoutingServer server.ScoutingServer) {
scoutingServer.HandleFunc("/requests", unknown)
scoutingServer.Handle("/requests/request/all_matches", requestAllMatchesHandler{db})
@@ -883,4 +933,5 @@
scoutingServer.Handle("/requests/request/shift_schedule", requestShiftScheduleHandler{db})
scoutingServer.Handle("/requests/submit/submit_driver_ranking", SubmitDriverRankingHandler{db})
scoutingServer.Handle("/requests/submit/submit_actions", submitActionsHandler{db})
+ scoutingServer.Handle("/requests/delete/delete_2023_data_scouting", Delete2023DataScoutingHandler{db})
}
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index c1fe860..20a63ca 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -7,6 +7,7 @@
"github.com/frc971/971-Robot-Code/scouting/db"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/debug"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_driver_rankings"
@@ -907,6 +908,112 @@
}
}
+// Validates that we can delete stats.
+func TestDeleteFromStats(t *testing.T) {
+ database := MockDatabase{
+ stats2023: []db.Stats2023{
+ {
+ TeamNumber: "3634", MatchNumber: 1, SetNumber: 2,
+ CompLevel: "quals", StartingQuadrant: 3, LowCubesAuto: 10,
+ MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 0,
+ LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
+ ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
+ HighCubes: 2, CubesDropped: 1, LowCones: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: false,
+ BalanceAttemptAuto: false, Docked: false, Engaged: false,
+ BalanceAttempt: true, CollectedBy: "isaac",
+ },
+ {
+ TeamNumber: "2343", MatchNumber: 1, SetNumber: 2,
+ CompLevel: "quals", StartingQuadrant: 1, LowCubesAuto: 0,
+ MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 2,
+ LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
+ ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
+ HighCubes: 1, CubesDropped: 0, LowCones: 0,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 53, Mobility: false, DockedAuto: false, EngagedAuto: false,
+ BalanceAttemptAuto: true, Docked: false, Engaged: false,
+ BalanceAttempt: true, CollectedBy: "unknown",
+ },
+ },
+ actions: []db.Action{
+ {
+ PreScouting: true,
+ TeamNumber: "3634",
+ MatchNumber: 1,
+ SetNumber: 2,
+ CompLevel: "quals",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 2400,
+ },
+ {
+ PreScouting: true,
+ TeamNumber: "2343",
+ MatchNumber: 1,
+ SetNumber: 2,
+ CompLevel: "quals",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 1009,
+ },
+ },
+ }
+ scoutingServer := server.NewScoutingServer()
+ HandleRequests(&database, scoutingServer)
+ scoutingServer.Start(8080)
+ defer scoutingServer.Stop()
+
+ builder := flatbuffers.NewBuilder(1024)
+ builder.Finish((&delete_2023_data_scouting.Delete2023DataScoutingT{
+ CompLevel: "quals",
+ MatchNumber: 1,
+ SetNumber: 2,
+ TeamNumber: "2343",
+ }).Pack(builder))
+
+ _, err := debug.Delete2023DataScouting("http://localhost:8080", builder.FinishedBytes())
+ if err != nil {
+ t.Fatal("Failed to delete from data scouting ", err)
+ }
+
+ expectedActions := []db.Action{
+ {
+ PreScouting: true,
+ TeamNumber: "3634",
+ MatchNumber: 1,
+ SetNumber: 2,
+ CompLevel: "quals",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 2400,
+ },
+ }
+
+ expectedStats := []db.Stats2023{
+ {
+ TeamNumber: "3634", MatchNumber: 1, SetNumber: 2,
+ CompLevel: "quals", StartingQuadrant: 3, LowCubesAuto: 10,
+ MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 0,
+ LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
+ ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
+ HighCubes: 2, CubesDropped: 1, LowCones: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: false,
+ BalanceAttemptAuto: false, Docked: false, Engaged: false,
+ BalanceAttempt: true, CollectedBy: "isaac",
+ },
+ }
+
+ if !reflect.DeepEqual(expectedActions, database.actions) {
+ t.Fatal("Expected ", expectedActions, ", but got:", database.actions)
+ }
+ if !reflect.DeepEqual(expectedStats, database.stats2023) {
+ t.Fatal("Expected ", expectedStats, ", but got:", database.stats2023)
+ }
+}
+
// A mocked database we can use for testing. Add functionality to this as
// needed for your tests.
@@ -995,3 +1102,29 @@
func (database *MockDatabase) ReturnActions() ([]db.Action, error) {
return database.actions, nil
}
+
+func (database *MockDatabase) DeleteFromStats(compLevel_ string, matchNumber_ int32, setNumber_ int32, teamNumber_ string) error {
+ for i, stat := range database.stats2023 {
+ if stat.CompLevel == compLevel_ &&
+ stat.MatchNumber == matchNumber_ &&
+ stat.SetNumber == setNumber_ &&
+ stat.TeamNumber == teamNumber_ {
+ // Match found, remove the element from the array.
+ database.stats2023 = append(database.stats2023[:i], database.stats2023[i+1:]...)
+ }
+ }
+ return nil
+}
+
+func (database *MockDatabase) DeleteFromActions(compLevel_ string, matchNumber_ int32, setNumber_ int32, teamNumber_ string) error {
+ for i, action := range database.actions {
+ if action.CompLevel == compLevel_ &&
+ action.MatchNumber == matchNumber_ &&
+ action.SetNumber == setNumber_ &&
+ action.TeamNumber == teamNumber_ {
+ // Match found, remove the element from the array.
+ database.actions = append(database.actions[:i], database.actions[i+1:]...)
+ }
+ }
+ return nil
+}
diff --git a/scouting/www/view/BUILD b/scouting/www/view/BUILD
index d787e8f..67c7e3b 100644
--- a/scouting/www/view/BUILD
+++ b/scouting/www/view/BUILD
@@ -10,6 +10,8 @@
],
deps = [
":node_modules/@angular/forms",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_ts_fbs",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_ts_fbs",
"//scouting/webserver/requests/messages:error_response_ts_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_ts_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_ts_fbs",
diff --git a/scouting/www/view/view.component.ts b/scouting/www/view/view.component.ts
index 561ae08..c75f83b 100644
--- a/scouting/www/view/view.component.ts
+++ b/scouting/www/view/view.component.ts
@@ -1,4 +1,6 @@
import {Component, OnInit} from '@angular/core';
+import {Builder, ByteBuffer} from 'flatbuffers';
+import {ErrorResponse} from '../../webserver/requests/messages/error_response_generated';
import {
Ranking,
RequestAllDriverRankingsResponse,
@@ -11,6 +13,8 @@
Note,
RequestAllNotesResponse,
} from '../../webserver/requests/messages/request_all_notes_response_generated';
+import {Delete2023DataScouting} from '../../webserver/requests/messages/delete_2023_data_scouting_generated';
+import {Delete2023DataScoutingResponse} from '../../webserver/requests/messages/delete_2023_data_scouting_response_generated';
import {ViewDataRequestor} from '../rpc';
@@ -104,16 +108,83 @@
}
// TODO(Filip): Add delete functionality.
- // Gets called when a user clicks the delete icon.
- async deleteData() {
+ // Gets called when a user clicks the delete icon (note scouting).
+ async deleteNoteData() {
const block_alerts = document.getElementById(
'block_alerts'
) as HTMLInputElement;
- if (!block_alerts.checked) {
- if (!window.confirm('Actually delete data?')) {
- this.errorMessage = 'Deleting data has not been implemented yet.';
- return;
- }
+ if (block_alerts.checked || window.confirm('Actually delete data?')) {
+ this.errorMessage = 'Deleting data has not been implemented yet.';
+ return;
+ }
+ }
+
+ // TODO(Filip): Add delete functionality.
+ // Gets called when a user clicks the delete icon (driver ranking).
+ async deleteDriverRankingData() {
+ const block_alerts = document.getElementById(
+ 'block_alerts'
+ ) as HTMLInputElement;
+ if (block_alerts.checked || window.confirm('Actually delete data?')) {
+ this.errorMessage = 'Deleting data has not been implemented yet.';
+ return;
+ }
+ }
+
+ // Gets called when a user clicks the delete icon.
+ async deleteDataScouting(
+ compLevel: string,
+ matchNumber: number,
+ setNumber: number,
+ teamNumber: string
+ ) {
+ const block_alerts = document.getElementById(
+ 'block_alerts'
+ ) as HTMLInputElement;
+ if (block_alerts.checked || window.confirm('Actually delete data?')) {
+ await this.requestDeleteDataScouting(
+ compLevel,
+ matchNumber,
+ setNumber,
+ teamNumber
+ );
+ await this.fetchStats2023();
+ }
+ }
+
+ async requestDeleteDataScouting(
+ compLevel: string,
+ matchNumber: number,
+ setNumber: number,
+ teamNumber: string
+ ) {
+ this.progressMessage = 'Deleting data. Please be patient.';
+ const builder = new Builder();
+ const compLevelData = builder.createString(compLevel);
+ const teamNumberData = builder.createString(teamNumber);
+
+ builder.finish(
+ Delete2023DataScouting.createDelete2023DataScouting(
+ builder,
+ compLevelData,
+ matchNumber,
+ setNumber,
+ teamNumberData
+ )
+ );
+
+ const buffer = builder.asUint8Array();
+ const res = await fetch('/requests/delete/delete_2023_data_scouting', {
+ method: 'POST',
+ body: buffer,
+ });
+
+ if (!res.ok) {
+ const resBuffer = await res.arrayBuffer();
+ const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
+ const parsedResponse = ErrorResponse.getRootAsErrorResponse(fbBuffer);
+ const errorMessage = parsedResponse.errorMessage();
+ this.errorMessage = `Received ${res.status} ${res.statusText}: "${errorMessage}"`;
}
}
diff --git a/scouting/www/view/view.ng.html b/scouting/www/view/view.ng.html
index 667765f..ee2c62f 100644
--- a/scouting/www/view/view.ng.html
+++ b/scouting/www/view/view.ng.html
@@ -11,12 +11,22 @@
</button>
<ul class="dropdown-menu">
<li>
- <a class="dropdown-item" href="#" (click)="switchDataSource('Notes')">
+ <a
+ class="dropdown-item"
+ href="#"
+ (click)="switchDataSource('Notes')"
+ id="notes_source_dropdown"
+ >
Notes
</a>
</li>
<li>
- <a class="dropdown-item" href="#" (click)="switchDataSource('Stats2023')">
+ <a
+ class="dropdown-item"
+ href="#"
+ (click)="switchDataSource('Stats2023')"
+ id="stats_source_dropdown"
+ >
Stats
</a>
</li>
@@ -25,6 +35,7 @@
class="dropdown-item"
href="#"
(click)="switchDataSource('DriverRanking')"
+ id="driver_ranking_source_dropdown"
>
Driver Ranking
</a>
@@ -64,7 +75,7 @@
<td>{{parseKeywords(note)}}</td>
<!-- Delete Icon. -->
<td>
- <button class="btn btn-danger" (click)="deleteData()">
+ <button class="btn btn-danger" (click)="deleteNoteData()">
<i class="bi bi-trash"></i>
</button>
</td>
@@ -87,20 +98,24 @@
</div>
</th>
<th scope="col">Team</th>
- <th scope="col">Set</th>
<th scope="col">Comp Level</th>
+ <th scope="col">Scout</th>
<th scope="col"></th>
</tr>
</thead>
<tbody>
<tr *ngFor="let stat2023 of statList; index as i;">
- <th scope="row">{{stat2023.match()}}</th>
- <td>{{stat2023.team()}}</td>
- <td>{{stat2023.setNumber()}}</td>
+ <th scope="row">{{stat2023.matchNumber()}}</th>
+ <td>{{stat2023.teamNumber()}}</td>
<td>{{COMP_LEVEL_LABELS[stat2023.compLevel()]}}</td>
+ <td>{{stat2023.collectedBy()}}</td>
<!-- Delete Icon. -->
<td>
- <button class="btn btn-danger" (click)="deleteData()">
+ <button
+ class="btn btn-danger"
+ id="delete_button_{{i}}"
+ (click)="deleteDataScouting(stat2023.compLevel(), stat2023.matchNumber(), stat2023.setNumber(), stat2023.teamNumber())"
+ >
<i class="bi bi-trash"></i>
</button>
</td>
@@ -136,7 +151,7 @@
<td>{{ranking.rank3()}}</td>
<!-- Delete Icon. -->
<td>
- <button class="btn btn-danger" (click)="deleteData()">
+ <button class="btn btn-danger" (click)="deleteDriverRankingData()">
<i class="bi bi-trash"></i>
</button>
</td>
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 2d83482..6e1cc7f 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -71,6 +71,8 @@
"//frc971:constants",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/zeroing",
+ "//frc971/zeroing:hall_effect_and_position",
+ "//frc971/zeroing:pulse_index",
"//frc971/zeroing:wrap",
"//y2017:constants",
"//y2017/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 9acd190..b8a12f7 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -11,6 +11,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/profiled_subsystem.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/pulse_index.h"
#include "y2017/control_loops/superstructure/column/column_integral_plant.h"
#include "y2017/control_loops/superstructure/column/stuck_column_integral_plant.h"
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
index a7fe47b..c07a504 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.h
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
#include "frc971/constants.h"
+#include "frc971/zeroing/hall_effect_and_position.h"
#include "frc971/zeroing/zeroing.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 8f162a7..9edcc8e 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -43,6 +43,7 @@
deps = [
":hood_plants",
"//frc971/control_loops:profiled_subsystem",
+ "//frc971/zeroing:pulse_index",
"//y2017:constants",
"//y2017/control_loops/superstructure:superstructure_goal_fbs",
"//y2017/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 8284e6e..47ace6e 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/zeroing/pulse_index.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
index 550c241..00642b0 100644
--- a/y2017/control_loops/superstructure/intake/BUILD
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -43,6 +43,7 @@
deps = [
":intake_plants",
"//frc971/control_loops:profiled_subsystem",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2017:constants",
"//y2017/control_loops/superstructure:superstructure_goal_fbs",
],
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index a91d2f0..29899fb 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 9f969ff..ec18f9d 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -15,6 +15,7 @@
"//frc971/control_loops/double_jointed_arm:graph",
"//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
"//y2018/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 0b0a6a4..a9cf614 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/graph.h"
#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index fa4eb7e..50d9989 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -46,6 +46,7 @@
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_output_fbs",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 09a7e4d..bec5ff6 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -5,6 +5,7 @@
#include "aos/commonmath.h"
#include "frc971/control_loops/control_loop.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/wrap.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
diff --git a/y2019/BUILD b/y2019/BUILD
index 2fba6d6..d388819 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -41,6 +41,8 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/control_loops/drivetrain:camera",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
"//y2019/control_loops/superstructure/elevator:elevator_plants",
"//y2019/control_loops/superstructure/intake:intake_plants",
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 10582ed..380899a 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -12,6 +12,8 @@
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2019/control_loops/superstructure/elevator/integral_elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/integral_stilts_plant.h"
diff --git a/y2019/constants.h b/y2019/constants.h
index b6c1b55..a36e4b3 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -9,6 +9,8 @@
#include "frc971/control_loops/drivetrain/camera.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 9504c9f..f97c6f0 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -69,6 +69,8 @@
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2019:constants",
"//y2019:status_light_fbs",
],
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index f8ad0fd..4d59132 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -5,6 +5,8 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2020/BUILD b/y2020/BUILD
index 32d392a..7c04d99 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -77,6 +77,9 @@
"//frc971:constants",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
"//y2020/control_loops/superstructure/control_panel:control_panel_plants",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 4d9d066..6218f9b 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -12,6 +12,9 @@
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2020/control_loops/superstructure/control_panel/integral_control_panel_plant.h"
#include "y2020/control_loops/superstructure/hood/integral_hood_plant.h"
#include "y2020/control_loops/superstructure/intake/integral_intake_plant.h"
diff --git a/y2020/constants.h b/y2020/constants.h
index 83b2cec..7332b4f 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -8,6 +8,9 @@
#include "frc971/constants.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
#include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index d1f20a2..83af834 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -83,6 +83,9 @@
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2020:constants",
"//y2020/control_loops/superstructure/hood:hood_encoder_zeroing_estimator",
"//y2020/control_loops/superstructure/shooter",
diff --git a/y2020/control_loops/superstructure/hood/BUILD b/y2020/control_loops/superstructure/hood/BUILD
index 437c67a..7983e62 100644
--- a/y2020/control_loops/superstructure/hood/BUILD
+++ b/y2020/control_loops/superstructure/hood/BUILD
@@ -44,6 +44,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
"//frc971/zeroing",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
"//y2020:constants",
],
)
diff --git a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
index dee4461..249dd00 100644
--- a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
+++ b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
@@ -2,6 +2,8 @@
#include <cmath>
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+
namespace y2020::control_loops::superstructure::hood {
HoodEncoderZeroingEstimator::HoodEncoderZeroingEstimator(
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 5d371fc..8a00bf0 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -5,6 +5,9 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/input/joystick_state_generated.h"
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
diff --git a/y2022/BUILD b/y2022/BUILD
index f25925e..8d9d901 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -224,6 +224,7 @@
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
"//frc971/wpilib:wpilib_utils",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
"//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
diff --git a/y2022/constants.cc b/y2022/constants.cc
index ff1c738..8ba3367 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -13,6 +13,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "frc971/wpilib/wpilib_utils.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/control_loops/superstructure/catapult/integral_catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
diff --git a/y2022/constants.h b/y2022/constants.h
index 62b9b4a..f27b6cd 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/climber_plant.h"
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index e0db64c..e370f0e 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -86,6 +86,7 @@
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022:constants",
"//y2022/control_loops/superstructure/catapult",
"//y2022/control_loops/superstructure/turret:aiming",
diff --git a/y2022/control_loops/superstructure/catapult/BUILD b/y2022/control_loops/superstructure/catapult/BUILD
index a43925e..36e34bd 100644
--- a/y2022/control_loops/superstructure/catapult/BUILD
+++ b/y2022/control_loops/superstructure/catapult/BUILD
@@ -41,6 +41,7 @@
deps = [
":catapult_plants",
"//aos:realtime",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//third_party/osqp-cpp",
"//y2022:constants",
"//y2022/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index a4c82de..d30e8f5 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -5,6 +5,7 @@
#include "glog/logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "osqp++.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 5e3415c..36269e9 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/catapult/catapult.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
diff --git a/y2022_bot3/BUILD b/y2022_bot3/BUILD
index 29debe7..125ff44 100644
--- a/y2022_bot3/BUILD
+++ b/y2022_bot3/BUILD
@@ -116,6 +116,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022_bot3/control_loops/drivetrain:polydrivetrain_plants",
"//y2022_bot3/control_loops/superstructure/climber:climber_plants",
"//y2022_bot3/control_loops/superstructure/intake:intake_plants",
diff --git a/y2022_bot3/constants.cc b/y2022_bot3/constants.cc
index 7ec5187..82982aa 100644
--- a/y2022_bot3/constants.cc
+++ b/y2022_bot3/constants.cc
@@ -12,6 +12,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022_bot3/control_loops/superstructure/climber/integral_climber_plant.h"
#include "y2022_bot3/control_loops/superstructure/intake/integral_intake_plant.h"
diff --git a/y2022_bot3/constants.h b/y2022_bot3/constants.h
index 2ced38d..a87930f 100644
--- a/y2022_bot3/constants.h
+++ b/y2022_bot3/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2022_bot3/control_loops/superstructure/climber/climber_plant.h"
#include "y2022_bot3/control_loops/superstructure/intake/intake_plant.h"
diff --git a/y2022_bot3/control_loops/superstructure/BUILD b/y2022_bot3/control_loops/superstructure/BUILD
index 7d89a0a..5b44e2a 100644
--- a/y2022_bot3/control_loops/superstructure/BUILD
+++ b/y2022_bot3/control_loops/superstructure/BUILD
@@ -75,6 +75,7 @@
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022_bot3:constants",
],
)
diff --git a/y2022_bot3/control_loops/superstructure/superstructure.h b/y2022_bot3/control_loops/superstructure/superstructure.h
index 4f33c3c..13d5dec 100644
--- a/y2022_bot3/control_loops/superstructure/superstructure.h
+++ b/y2022_bot3/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022_bot3/constants.h"
#include "y2022_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022_bot3/control_loops/superstructure/superstructure_output_generated.h"
diff --git a/y2023/BUILD b/y2023/BUILD
index e200f87..d934927 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -254,6 +254,8 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023/control_loops/drivetrain:polydrivetrain_plants",
"//y2023/control_loops/superstructure/arm:arm_constants",
"//y2023/control_loops/superstructure/roll:roll_plants",
diff --git a/y2023/constants.cc b/y2023/constants.cc
index c17ffd0..847c7f1 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -84,6 +84,37 @@
break;
case kCompTeamNumber:
+ arm_proximal->zeroing.measured_absolute_position = 0.911747959388894;
+ arm_proximal->potentiometer_offset =
+ 10.5178592988554 + 0.0944609125285876 - 0.00826532984625095 +
+ 0.167359305216504 + 0.135144500925909 - 0.214909475332252 +
+ 0.0377032255050543;
+
+ arm_distal->zeroing.measured_absolute_position = 0.294291930885304;
+ arm_distal->potentiometer_offset =
+ 7.673132586937 - 0.0799284644472573 - 0.0323574039310657 +
+ 0.0143810684138064 + 0.00945555248207735 + 0.452446388633863 +
+ 0.0194863477007102 + 0.235993332670562 + 0.00138417783482921 -
+ 1.29562640607084 - 0.390356125757262 - 0.267002511437832 -
+ 0.611626839639182 + 2.55745730136924;
+
+ arm_distal->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kDistalEncoderRatio() *
+ (3.12725165289659 + 0.002) / 3.1485739705977704;
+
+ roll_joint->zeroing.measured_absolute_position = 1.82824749141201;
+ roll_joint->potentiometer_offset =
+ 0.624713611895747 + 3.10458504917251 - 0.0966407797407789 +
+ 0.0257708772364788 - 0.0395076737853459 - 6.87914956118006 -
+ 0.097581301615046 + 3.3424421683095 - 3.97605190912604 +
+ 0.709274294168941 - 0.0817908884966825;
+
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.892290340605656;
+
+ break;
+
+ case kPracticeTeamNumber:
arm_proximal->zeroing.measured_absolute_position = 0.146982006490838;
arm_proximal->potentiometer_offset =
0.931355973012855 + 8.6743197253382 - 0.101200335326309 -
@@ -118,36 +149,6 @@
break;
- case kPracticeTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.911194143585562;
- arm_proximal->potentiometer_offset =
- 10.5178592988554 + 0.0944609125285876 - 0.00826532984625095 +
- 0.167359305216504 + 0.135144500925909 - 0.214909475332252;
-
- arm_distal->zeroing.measured_absolute_position = 0.295329750530428;
- arm_distal->potentiometer_offset =
- 7.673132586937 - 0.0799284644472573 - 0.0323574039310657 +
- 0.0143810684138064 + 0.00945555248207735 + 0.452446388633863 +
- 0.0194863477007102 + 0.235993332670562 + 0.00138417783482921 -
- 1.29562640607084;
-
- arm_distal->zeroing.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kDistalEncoderRatio() *
- // 3.11964893168338 / 3.148;
- (3.12725165289659 + 0.002) / 3.1485739705977704;
-
- roll_joint->zeroing.measured_absolute_position = 1.79390317510529;
- roll_joint->potentiometer_offset =
- 0.624713611895747 + 3.10458504917251 - 0.0966407797407789 +
- 0.0257708772364788 - 0.0395076737853459 - 6.87914956118006 -
- 0.097581301615046 + 3.3424421683095 - 3.97605190912604 +
- 0.709274294168941;
-
- wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 2.97717660361257;
-
- break;
-
case kCodingRobotTeamNumber:
arm_proximal->zeroing.measured_absolute_position = 0.0;
arm_proximal->potentiometer_offset = 0.0;
diff --git a/y2023/constants.h b/y2023/constants.h
index 32ed317..84d6499 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -8,6 +8,8 @@
#include "frc971/constants.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/arm/arm_constants.h"
#include "y2023/control_loops/superstructure/roll/roll_plant.h"
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 6e7bc4d..fc21b74 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -1,16 +1,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-04-12.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-04-12.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-12.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-04-14.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json' %}
}
],
"robot": {
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index 31529b2..d687c9e 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -1,16 +1,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-1_cam-23-05_ext_2023-04-12.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-04-15.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-2_cam-23-06_ext_2023-04-12.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-04-15.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-3_cam-23-04_ext_2023-04-12.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-04-15.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-08_ext_2023-04-14.json' %}
}
],
"robot": {
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 861bbf8..3d35dae 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -106,6 +106,8 @@
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023:constants",
"//y2023/constants:constants_fbs",
"//y2023/constants:simulated_constants_sender",
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index a742c50..768a54e 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -16,6 +16,7 @@
"//frc971/control_loops/double_jointed_arm:graph",
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023:constants",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 1f97d80..6153400 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -5,6 +5,7 @@
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/arm/generated_graph.h"
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index fdfef4e..bcee3ea 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -7,6 +7,8 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023/constants.h"
#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 26b88f3..926846e 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -9,6 +9,7 @@
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/subsystem_simulator.h"
#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/absolute_encoder.h"
#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-04-12.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-04-12.json
deleted file mode 100644
index 55641ae..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-04-12.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi1", "team_number": 971, "intrinsics": [ 888.821655, 0.0, 616.580811, 0.0, 888.336731, 366.017395, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.487918, 0.221538, 0.844309, 0.190808, 0.866039, 0.00192, 0.499972, -0.218036, 0.109142, 0.97515, -0.192797, 0.544037, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.4493, 0.25223, 0.000471, -0.00003, -0.079545 ], "calibration_timestamp": 1358501902915096335, "camera_id": "23-05" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json
new file mode 100644
index 0000000..051897b
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi1", "team_number": 971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.231149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-04-12.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-04-12.json
deleted file mode 100644
index e334884..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-04-12.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 971, "intrinsics": [ 891.706848, 0.0, 640.791931, 0.0, 890.630981, 358.582306, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.858536, 0.224804, 0.460847, 0.198133, 0.473832, -0.00434901, -0.880604, -0.221657, -0.195959, 0.974394, -0.110253, 0.593406, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451473, 0.257519, 0.000337, -0.000298, -0.086102 ], "calibration_timestamp": 1358501526409252911, "camera_id": "23-06" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-03-08.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-03-08.json
new file mode 100644
index 0000000..37cb8d9
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-03-08.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.830467, 0.233602, 0.505721, 0.242226, 0.519225, 0.004294, -0.854627, -0.167586, -0.201814, 0.972323, -0.117727, 0.614872, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-04-15.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-04-15.json
new file mode 100644
index 0000000..f35b97a
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-04-15.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.84843, 0.211678, 0.485137, 0.158197, 0.495653, 0.00388008, -0.868512, -0.196222, -0.185728, 0.977332, -0.101626, 0.575463, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json
new file mode 100644
index 0000000..f353533
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.84372, 0.233067, 0.483545, 0.188786, 0.499721,-0.0121191, -0.866102, -0.244606, -0.196001, 0.972385, -0.126693, 0.600451, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-12.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-12.json
deleted file mode 100644
index b62d3f5..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-12.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 888.213257, 0.0, 622.755371, 0.0, 887.352905, 365.591736, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.482409, -0.181339, -0.85697, -0.115455, -0.871457, -0.000440151, -0.490471, -0.175596, 0.0885639, 0.983421, -0.158241, 0.61873, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.447815, 0.251622, 0.001888, -0.000286, -0.081473 ], "calibration_timestamp": 1358501982039874176, "camera_id": "23-04" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-03-05.json
new file mode 100644
index 0000000..13c9036
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.503726, -0.160611, -0.848802, 0.064621, -0.857988, 0.021383, -0.513224, -0.195154, 0.100579, 0.986786, -0.127032, 0.653821, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-04-15.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-04-15.json
new file mode 100644
index 0000000..3fe71de
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-04-15.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.468672, -0.174907, -0.865883, -0.106535, -0.875024, 0.0425189, -0.482209, -0.169986, 0.121159, 0.983667, -0.13312, 0.557647, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json
new file mode 100644
index 0000000..d7851fa
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.484157, -0.154776, -0.861182, -0.0935153, -0.872577, -0.0125032, -0.488317, -0.225918, 0.0648126, 0.987871, -0.141107, 0.623926, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-04-14.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-04-14.json
deleted file mode 100644
index bc5f065..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-04-14.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi4", "team_number": 971, "intrinsics": [ 890.962952, 0.0, 656.281555, 0.0, 890.707336, 353.938538, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.865915, -0.186983, -0.463928, -0.014873, -0.473362, 0.006652, 0.880843, -0.215738, -0.161617, 0.982341, -0.094271, 0.676433, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.450961, 0.256988, -0.000348, -0.00042, -0.083559 ], "calibration_timestamp": 1358501265150551614, "camera_id": "23-08" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-03-05.json
new file mode 100644
index 0000000..82073fc
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.845241, -0.169204, -0.506891, 0.008046, -0.511496, -0.018479, 0.859087, -0.243442, -0.154727, 0.985408, -0.070928, 0.69704, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-04-15.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-04-15.json
new file mode 100644
index 0000000..a1b4e9a
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-04-15.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.87256, -0.183584, -0.452697, -0.126273, -0.466236, 0.036353, 0.883913, -0.184333, -0.145815, 0.982332, -0.117314, 0.616782, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json
new file mode 100644
index 0000000..e12c22c
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.864114, -0.173035, -0.472616, -0.111428, -0.477471, -0.0150962, 0.878518, -0.22396, -0.159149, 0.9848, -0.0695749, 0.696406, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-05_ext_2023-04-12.json b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-05_ext_2023-04-12.json
new file mode 100644
index 0000000..521d12a
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-05_ext_2023-04-12.json
@@ -0,0 +1 @@
+{ "node_name": "pi1", "team_number": 9971, "intrinsics": [ 888.821655, 0.0, 616.580811, 0.0, 888.336731, 366.017395, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.487918, 0.221538, 0.844309, 0.190808, 0.866039, 0.00192, 0.499972, -0.218036, 0.109142, 0.97515, -0.192797, 0.544037, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.4493, 0.25223, 0.000471, -0.00003, -0.079545 ], "calibration_timestamp": 1358501902915096335, "camera_id": "23-05" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json
deleted file mode 100644
index c79f7d3..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi1", "team_number": 9971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.191149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-06_ext_2023-04-12.json b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-06_ext_2023-04-12.json
new file mode 100644
index 0000000..5c9cca8
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-06_ext_2023-04-12.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 9971, "intrinsics": [ 891.706848, 0.0, 640.791931, 0.0, 890.630981, 358.582306, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.858536, 0.224804, 0.460847, 0.198133, 0.473832, -0.00434901, -0.880604, -0.221657, -0.195959, 0.974394, -0.110253, 0.593406, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451473, 0.257519, 0.000337, -0.000298, -0.086102 ], "calibration_timestamp": 1358501526409252911, "camera_id": "23-06" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json
deleted file mode 100644
index 547d8d7..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 9971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.830467, 0.233602, 0.505721, 0.242226, 0.519225, 0.004294, -0.854627, -0.167586, -0.201814, 0.972323, -0.117727, 0.614872, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-04-15.json b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-04-15.json
deleted file mode 100644
index e1c0c36..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-04-15.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 9971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.84843, 0.211678, 0.485137, 0.158197, 0.495653, 0.00388008, -0.868512, -0.196222, -0.185728, 0.977332, -0.101626, 0.575463, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-04_ext_2023-04-12.json b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-04_ext_2023-04-12.json
new file mode 100644
index 0000000..11b7f85
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-04_ext_2023-04-12.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 9971, "intrinsics": [ 888.213257, 0.0, 622.755371, 0.0, 887.352905, 365.591736, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.482409, -0.181339, -0.85697, -0.115455, -0.871457, -0.000440151, -0.490471, -0.175596, 0.0885639, 0.983421, -0.158241, 0.61873, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.447815, 0.251622, 0.001888, -0.000286, -0.081473 ], "calibration_timestamp": 1358501982039874176, "camera_id": "23-04" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json
deleted file mode 100644
index d4b8bd2..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 9971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.503726, -0.160611, -0.848802, 0.064621, -0.857988, 0.021383, -0.513224, -0.195154, 0.100579, 0.986786, -0.127032, 0.653821, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-04-15.json b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-04-15.json
deleted file mode 100644
index 7505a3c..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-04-15.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 9971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.468672, -0.174907, -0.865883, -0.106535, -0.875024, 0.0425189, -0.482209, -0.169986, 0.121159, 0.983667, -0.13312, 0.557647, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-08_ext_2023-04-14.json b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-08_ext_2023-04-14.json
new file mode 100644
index 0000000..cc86664
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-08_ext_2023-04-14.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 9971, "intrinsics": [ 890.962952, 0.0, 656.281555, 0.0, 890.707336, 353.938538, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.865915, -0.186983, -0.463928, -0.014873, -0.473362, 0.006652, 0.880843, -0.215738, -0.161617, 0.982341, -0.094271, 0.676433, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.450961, 0.256988, -0.000348, -0.00042, -0.083559 ], "calibration_timestamp": 1358501265150551614, "camera_id": "23-08" }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json
deleted file mode 100644
index 7ea2f6d..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi4", "team_number": 9971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.845241, -0.169204, -0.506891, 0.008046, -0.511496, -0.018479, 0.859087, -0.243442, -0.154727, 0.985408, -0.070928, 0.69704, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-04-15.json b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-04-15.json
deleted file mode 100644
index 9591b79..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-04-15.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi4", "team_number": 9971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.87256, -0.183584, -0.452697, -0.126273, -0.466236, 0.036353, 0.883913, -0.184333, -0.145815, 0.982332, -0.117314, 0.616782, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
diff --git a/y2023_bot4/BUILD b/y2023_bot4/BUILD
new file mode 100644
index 0000000..1a4ed64
--- /dev/null
+++ b/y2023_bot4/BUILD
@@ -0,0 +1,234 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("//aos/util:config_validator_macro.bzl", "config_validator_test")
+
+config_validator_test(
+ name = "config_validator_test",
+ config = "//y2023_bot4:aos_config",
+)
+
+robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ "//aos/events:aos_timing_report_streamer",
+ ],
+ data = [
+ ":aos_config",
+ ":swerve_publisher_output_json",
+ "@ctre_phoenix6_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix6_tools_athena//:shared_libraries",
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ ":wpilib_interface",
+ ":swerve_publisher",
+ "//frc971/can_logger",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+ name = "pi_download",
+ binaries = [
+ "//aos/util:foxglove_websocket",
+ "//aos/events:aos_timing_report_streamer",
+ "//aos/events/logging:log_cat",
+ "//y2023/rockpi:imu_main",
+ "//frc971/image_streamer:image_streamer",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ "//aos/events/logging:logger_main",
+ ],
+ data = [
+ ":aos_config",
+ "//frc971/rockpi:rockpi_config.json",
+ ],
+ start_binaries = [
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+ target_type = "pi",
+)
+
+filegroup(
+ name = "swerve_publisher_output_json",
+ srcs = [
+ "swerve_drivetrain_output.json",
+ ],
+ visibility = ["//y2023_bot4:__subpackages__"],
+)
+
+cc_library(
+ name = "constants",
+ srcs = ["constants.cc"],
+ hdrs = [
+ "constants.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network:team_number",
+ "//frc971:constants",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_position_fbs",
+ srcs = ["drivetrain_position.fbs"],
+ gen_reflections = 1,
+ deps = ["//frc971/control_loops:control_loops_fbs"],
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_can_position_fbs",
+ srcs = ["drivetrain_can_position.fbs"],
+ gen_reflections = 1,
+ deps = ["//frc971/control_loops:can_falcon_fbs"],
+)
+
+cc_binary(
+ name = "swerve_publisher",
+ srcs = ["swerve_publisher_main.cc"],
+ deps = [
+ ":swerve_publisher_lib",
+ "//aos/events:shm_event_loop",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
+
+cc_library(
+ name = "swerve_publisher_lib",
+ srcs = ["swerve_publisher_lib.cc"],
+ hdrs = ["swerve_publisher_lib.h"],
+ deps = [
+ "//aos:init",
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_test(
+ name = "swerve_publisher_lib_test",
+ srcs = [
+ "swerve_publisher_lib_test.cc",
+ ],
+ data = [
+ ":aos_config",
+ ":swerve_publisher_output_json",
+ ],
+ deps = [
+ ":swerve_publisher_lib",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_binary(
+ name = "wpilib_interface",
+ srcs = ["wpilib_interface.cc"],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":constants",
+ ":drivetrain_can_position_fbs",
+ ":drivetrain_position_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/wpilib:can_sensor_reader",
+ "//frc971/wpilib:falcon",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//frc971/wpilib/swerve:swerve_drivetrain_writer",
+ ],
+)
+
+aos_config(
+ name = "aos_config",
+ src = "y2023_bot4.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/input:robot_state_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":config_imu",
+ ":config_logger",
+ ":config_roborio",
+ ],
+)
+
+aos_config(
+ name = "config_roborio",
+ src = "y2023_bot4_roborio.json",
+ flatbuffers = [
+ ":drivetrain_position_fbs",
+ ":drivetrain_can_position_fbs",
+ "//frc971:can_configuration_fbs",
+ "//aos/network:remote_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/can_logger:can_logging_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/autonomous:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ "//frc971/wpilib:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_imu",
+ src = "y2023_bot4_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:target_map_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_logger",
+ src = "y2023_bot4_logger.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ ],
+)
diff --git a/y2023_bot4/constants.cc b/y2023_bot4/constants.cc
new file mode 100644
index 0000000..aab1935
--- /dev/null
+++ b/y2023_bot4/constants.cc
@@ -0,0 +1,52 @@
+#include "y2023_bot4/constants.h"
+
+#include <cstdint>
+
+#include "glog/logging.h"
+
+#include "aos/network/team_number.h"
+
+namespace y2023_bot4 {
+namespace constants {
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+ Values r;
+ auto *const front_left_zeroing_constants = &r.front_left_zeroing_constants;
+ auto *const front_right_zeroing_constants = &r.front_right_zeroing_constants;
+ auto *const back_left_zeroing_constants = &r.back_left_zeroing_constants;
+ auto *const back_right_zeroing_constants = &r.back_right_zeroing_constants;
+
+ front_left_zeroing_constants->average_filter_size = 0;
+ front_left_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ front_left_zeroing_constants->measured_absolute_position = 0.76761395509829;
+ front_left_zeroing_constants->zeroing_threshold = 0.0;
+ front_left_zeroing_constants->moving_buffer_size = 0.0;
+ front_left_zeroing_constants->allowable_encoder_error = 0.0;
+
+ front_right_zeroing_constants->average_filter_size = 0;
+ front_right_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ front_right_zeroing_constants->measured_absolute_position = 0.779403958443922;
+ front_right_zeroing_constants->zeroing_threshold = 0.0;
+ front_right_zeroing_constants->moving_buffer_size = 0.0;
+ front_right_zeroing_constants->allowable_encoder_error = 0.0;
+
+ back_left_zeroing_constants->average_filter_size = 0;
+ back_left_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ back_left_zeroing_constants->measured_absolute_position = 0.053439698061417;
+ back_left_zeroing_constants->zeroing_threshold = 0.0;
+ back_left_zeroing_constants->moving_buffer_size = 0.0;
+ back_left_zeroing_constants->allowable_encoder_error = 0.0;
+
+ back_right_zeroing_constants->average_filter_size = 0;
+ back_right_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ back_right_zeroing_constants->measured_absolute_position = 0.719329333121509;
+ back_right_zeroing_constants->zeroing_threshold = 0.0;
+ back_right_zeroing_constants->moving_buffer_size = 0.0;
+ back_right_zeroing_constants->allowable_encoder_error = 0.0;
+
+ return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+} // namespace constants
+} // namespace y2023_bot4
diff --git a/y2023_bot4/constants.h b/y2023_bot4/constants.h
new file mode 100644
index 0000000..676ae06
--- /dev/null
+++ b/y2023_bot4/constants.h
@@ -0,0 +1,55 @@
+#ifndef Y2023_BOT4_CONSTANTS_H
+#define Y2023_BOT4_CONSTANTS_H
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#include <cstdint>
+
+#include "frc971/constants.h"
+
+namespace y2023_bot4 {
+namespace constants {
+struct Values {
+ static const int kZeroingSampleSize = 200;
+
+ static const int kDrivetrainWriterPriority = 35;
+ static const int kDrivetrainTxPriority = 36;
+ static const int kDrivetrainRxPriority = 36;
+
+ // TODO (maxwell): Make this the real value;
+ static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
+
+ static constexpr double kDrivetrainStatorCurrentLimit() { return 35.0; }
+ static constexpr double kDrivetrainSupplyCurrentLimit() { return 60.0; }
+
+ // TODO (maxwell): Make this the real value
+ static constexpr double kFollowerWheelCountsPerRevolution() { return 512.0; }
+ static constexpr double kFollowerWheelEncoderRatio() { return 1.0; }
+ static constexpr double kFollowerWheelRadius() { return 3.25 / 2 * 0.0254; }
+ static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+ return 2048.0;
+ }
+
+ static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+ return 1200000;
+ }
+
+ frc971::constants::ContinuousAbsoluteEncoderZeroingConstants
+ front_left_zeroing_constants,
+ front_right_zeroing_constants, back_left_zeroing_constants,
+ back_right_zeroing_constants;
+};
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+Values MakeValues();
+} // namespace constants
+} // namespace y2023_bot4
+
+#endif // Y2023_BOT4_CONSTANTS_H
diff --git a/y2023_bot4/drivetrain_can_position.fbs b/y2023_bot4/drivetrain_can_position.fbs
new file mode 100644
index 0000000..e8c1235
--- /dev/null
+++ b/y2023_bot4/drivetrain_can_position.fbs
@@ -0,0 +1,17 @@
+include "frc971/control_loops/can_falcon.fbs";
+namespace y2023_bot4;
+
+table SwerveModuleCANPosition {
+ rotation: frc971.control_loops.CANFalcon (id: 0);
+ translation: frc971.control_loops.CANFalcon (id: 1);
+}
+
+// CAN Readings from the CAN sensor reader loop for each swerve module
+table AbsoluteCANPosition {
+ front_left: SwerveModuleCANPosition (id: 0);
+ front_right: SwerveModuleCANPosition (id: 1);
+ back_left: SwerveModuleCANPosition (id: 2);
+ back_right: SwerveModuleCANPosition (id: 3);
+}
+
+root_type AbsoluteCANPosition;
diff --git a/y2023_bot4/drivetrain_position.fbs b/y2023_bot4/drivetrain_position.fbs
new file mode 100644
index 0000000..e5d0fc3
--- /dev/null
+++ b/y2023_bot4/drivetrain_position.fbs
@@ -0,0 +1,16 @@
+include "frc971/control_loops/control_loops.fbs";
+namespace y2023_bot4;
+
+table AbsoluteDrivetrainPosition {
+ // Position of the follower wheels from the encoders
+ follower_wheel_one_position:double (id: 0);
+ follower_wheel_two_position:double (id: 1);
+
+ // Position from the mag encoder on each module.
+ front_left_position: frc971.AbsolutePosition (id: 2);
+ front_right_position: frc971.AbsolutePosition (id: 3);
+ back_left_position: frc971.AbsolutePosition (id: 4);
+ back_right_position: frc971.AbsolutePosition (id: 5);
+}
+
+root_type AbsoluteDrivetrainPosition;
diff --git a/y2023_bot4/swerve_drivetrain_output.json b/y2023_bot4/swerve_drivetrain_output.json
new file mode 100644
index 0000000..bc152e2
--- /dev/null
+++ b/y2023_bot4/swerve_drivetrain_output.json
@@ -0,0 +1,18 @@
+{
+ "front_left_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ },
+ "front_right_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ },
+ "back_left_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ },
+ "back_right_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ }
+}
diff --git a/y2023_bot4/swerve_publisher_lib.cc b/y2023_bot4/swerve_publisher_lib.cc
new file mode 100644
index 0000000..78a778e
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_lib.cc
@@ -0,0 +1,51 @@
+#include "y2023_bot4/swerve_publisher_lib.h"
+
+y2023_bot4::SwervePublisher::SwervePublisher(aos::EventLoop *event_loop,
+ aos::ExitHandle *exit_handle,
+ const std::string &filename,
+ double duration)
+ : drivetrain_output_sender_(
+ event_loop->MakeSender<drivetrain::swerve::Output>("/drivetrain")) {
+ event_loop
+ ->AddTimer([this, filename]() {
+ auto output_builder = drivetrain_output_sender_.MakeBuilder();
+
+ auto drivetrain_output =
+ aos::JsonFileToFlatbuffer<drivetrain::swerve::Output>(filename);
+
+ auto copied_flatbuffer =
+ aos::CopyFlatBuffer<drivetrain::swerve::Output>(
+ drivetrain_output, output_builder.fbb());
+ CHECK(drivetrain_output.Verify());
+
+ output_builder.CheckOk(output_builder.Send(copied_flatbuffer));
+ })
+ ->Schedule(event_loop->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop
+ ->AddTimer([this, exit_handle]() {
+ auto builder = drivetrain_output_sender_.MakeBuilder();
+ drivetrain::swerve::SwerveModuleOutput::Builder swerve_module_builder =
+ builder.MakeBuilder<drivetrain::swerve::SwerveModuleOutput>();
+
+ swerve_module_builder.add_rotation_current(0.0);
+ swerve_module_builder.add_translation_current(0.0);
+
+ auto swerve_module_offset = swerve_module_builder.Finish();
+
+ drivetrain::swerve::Output::Builder drivetrain_output_builder =
+ builder.MakeBuilder<drivetrain::swerve::Output>();
+
+ drivetrain_output_builder.add_front_left_output(swerve_module_offset);
+ drivetrain_output_builder.add_front_right_output(swerve_module_offset);
+ drivetrain_output_builder.add_back_left_output(swerve_module_offset);
+ drivetrain_output_builder.add_back_right_output(swerve_module_offset);
+
+ builder.CheckOk(builder.Send(drivetrain_output_builder.Finish()));
+
+ exit_handle->Exit();
+ })
+ ->Schedule(event_loop->monotonic_now() +
+ std::chrono::milliseconds((int)duration));
+}
diff --git a/y2023_bot4/swerve_publisher_lib.h b/y2023_bot4/swerve_publisher_lib.h
new file mode 100644
index 0000000..5969b7b
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_lib.h
@@ -0,0 +1,29 @@
+#ifndef Y2023_BOT4_SWERVE_PUBLISHER_H_
+#define Y2023_BOT4_SWERVE_PUBLISHER_H_
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output_generated.h"
+
+namespace y2023_bot4 {
+
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+class SwervePublisher {
+ public:
+ SwervePublisher(aos::EventLoop *event_loop, aos::ExitHandle *exit_handle,
+ const std::string &filename, double duration);
+
+ private:
+ aos::Sender<frc971::control_loops::drivetrain::swerve::Output>
+ drivetrain_output_sender_;
+};
+
+} // namespace y2023_bot4
+
+#endif // Y2023_BOT4_SWERVE_PUBLISHER_H_
diff --git a/y2023_bot4/swerve_publisher_lib_test.cc b/y2023_bot4/swerve_publisher_lib_test.cc
new file mode 100644
index 0000000..817f295
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_lib_test.cc
@@ -0,0 +1,54 @@
+#include "y2023_bot4/swerve_publisher_lib.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+
+namespace y2023_bot4 {
+namespace testing {
+class SwervePublisherTest : public ::testing::Test {
+ public:
+ SwervePublisherTest()
+ : config_(aos::configuration::ReadConfig("y2023_bot4/aos_config.json")),
+ event_loop_factory_(&config_.message()),
+ roborio_(aos::configuration::GetNode(
+ event_loop_factory_.configuration(), "roborio")),
+ event_loop_(
+ event_loop_factory_.MakeEventLoop("swerve_publisher", roborio_)),
+ exit_handle_(event_loop_factory_.MakeExitHandle()),
+ drivetrain_swerve_output_fetcher_(
+ event_loop_->MakeFetcher<
+ frc971::control_loops::drivetrain::swerve::Output>(
+ "/drivetrain")),
+ swerve_publisher_(event_loop_.get(), exit_handle_.get(),
+ "y2023_bot4/swerve_drivetrain_output.json", 100) {}
+
+ void SendOutput() { event_loop_factory_.Run(); }
+
+ void CheckOutput() {
+ drivetrain_swerve_output_fetcher_.Fetch();
+
+ ASSERT_TRUE(drivetrain_swerve_output_fetcher_.get() != nullptr)
+ << ": No drivetrain output";
+ }
+
+ private:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_;
+
+ std::unique_ptr<aos::EventLoop> event_loop_;
+ std::unique_ptr<aos::ExitHandle> exit_handle_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::swerve::Output>
+ drivetrain_swerve_output_fetcher_;
+
+ y2023_bot4::SwervePublisher swerve_publisher_;
+};
+
+TEST_F(SwervePublisherTest, CheckSentFb) {
+ SendOutput();
+ CheckOutput();
+}
+} // namespace testing
+} // namespace y2023_bot4
diff --git a/y2023_bot4/swerve_publisher_main.cc b/y2023_bot4/swerve_publisher_main.cc
new file mode 100644
index 0000000..87470d7
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "y2023_bot4/swerve_publisher_lib.h"
+
+DEFINE_double(duration, 100.0, "Length of time in Ms to apply current for");
+DEFINE_string(drivetrain_position, "swerve_drivetrain_output.json",
+ "The path to the json drivetrain position to apply");
+DEFINE_string(config, "aos_config.json", "The path to aos_config.json");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
+
+ y2023_bot4::SwervePublisher publisher(&event_loop, exit_handle.get(),
+ FLAGS_drivetrain_position,
+ FLAGS_duration);
+
+ event_loop.Run();
+}
diff --git a/y2023_bot4/wpilib_interface.cc b/y2023_bot4/wpilib_interface.cc
new file mode 100644
index 0000000..a744ae0
--- /dev/null
+++ b/y2023_bot4/wpilib_interface.cc
@@ -0,0 +1,325 @@
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
+#include "ctre/phoenix6/TalonFX.hpp"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/wpilib/can_sensor_reader.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/swerve/swerve_drivetrain_writer.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2023_bot4/constants.h"
+#include "y2023_bot4/drivetrain_can_position_generated.h"
+#include "y2023_bot4/drivetrain_position_generated.h"
+
+DEFINE_bool(ctre_diag_server, false,
+ "If true, enable the diagnostics server for interacting with "
+ "devices on the CAN bus using Phoenix Tuner");
+
+using frc971::wpilib::CANSensorReader;
+using frc971::wpilib::Falcon;
+using frc971::wpilib::swerve::DrivetrainWriter;
+using frc971::wpilib::swerve::SwerveModule;
+
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+namespace y2023_bot4 {
+namespace wpilib {
+namespace {
+
+template <class T>
+T value_or_exit(std::optional<T> optional) {
+ CHECK(optional.has_value());
+ return optional.value();
+}
+
+flatbuffers::Offset<frc971::AbsolutePosition> module_offset(
+ frc971::AbsolutePosition::Builder builder,
+ frc971::wpilib::AbsoluteEncoder *module) {
+ builder.add_encoder(module->ReadRelativeEncoder());
+ builder.add_absolute_encoder(module->ReadAbsoluteEncoder());
+
+ return builder.Finish();
+}
+
+flatbuffers::Offset<SwerveModuleCANPosition> can_module_offset(
+ SwerveModuleCANPosition::Builder builder,
+ std::shared_ptr<SwerveModule> module) {
+ std::optional<flatbuffers::Offset<control_loops::CANFalcon>> rotation_offset =
+ module->rotation->TakeOffset();
+ std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
+ translation_offset = module->translation->TakeOffset();
+
+ CHECK(rotation_offset.has_value());
+ CHECK(translation_offset.has_value());
+
+ builder.add_rotation(rotation_offset.value());
+ builder.add_translation(translation_offset.value());
+
+ return builder.Finish();
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+ constants::Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+} // namespace
+
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+ SensorReader(aos::ShmEventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values)
+ : ::frc971::wpilib::SensorReader(event_loop),
+ values_(values),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<AbsoluteDrivetrainPosition>("/drivetrain")) {
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ }
+
+ void RunIteration() override {
+ {
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+
+ auto front_left_offset =
+ module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
+ &front_left_encoder_);
+ auto front_right_offset =
+ module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
+ &front_right_encoder_);
+ auto back_left_offset = module_offset(
+ builder.MakeBuilder<frc971::AbsolutePosition>(), &back_left_encoder_);
+ auto back_right_offset =
+ module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
+ &back_right_encoder_);
+
+ AbsoluteDrivetrainPosition::Builder drivetrain_position_builder =
+ builder.MakeBuilder<AbsoluteDrivetrainPosition>();
+
+ drivetrain_position_builder.add_follower_wheel_one_position(
+ follower_wheel_one_encoder_->GetRaw());
+ drivetrain_position_builder.add_follower_wheel_two_position(
+ follower_wheel_two_encoder_->GetRaw());
+
+ drivetrain_position_builder.add_front_left_position(front_left_offset);
+ drivetrain_position_builder.add_front_right_position(front_right_offset);
+ drivetrain_position_builder.add_back_left_position(back_left_offset);
+ drivetrain_position_builder.add_back_right_position(back_right_offset);
+
+ builder.CheckOk(builder.Send(drivetrain_position_builder.Finish()));
+ }
+ }
+
+ void set_follower_wheel_one_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ follower_wheel_one_encoder_ = std::move(encoder);
+ follower_wheel_one_encoder_->SetMaxPeriod(0.005);
+ }
+ void set_follower_wheel_two_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ follower_wheel_two_encoder_ = std::move(encoder);
+ follower_wheel_two_encoder_->SetMaxPeriod(0.005);
+ }
+
+ void set_front_left_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ front_left_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_front_left_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ front_left_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ void set_front_right_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ front_right_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_front_right_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ front_right_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ void set_back_left_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ back_left_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_back_left_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ back_left_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ void set_back_right_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ back_right_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_back_right_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ back_right_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ private:
+ std::shared_ptr<const constants::Values> values_;
+
+ aos::Sender<AbsoluteDrivetrainPosition> drivetrain_position_sender_;
+
+ std::unique_ptr<frc::Encoder> follower_wheel_one_encoder_,
+ follower_wheel_two_encoder_;
+
+ frc971::wpilib::AbsoluteEncoder front_left_encoder_, front_right_encoder_,
+ back_left_encoder_, back_right_encoder_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ return std::make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ frc::Encoder::k4X);
+ }
+ void Run() override {
+ std::shared_ptr<const constants::Values> values =
+ std::make_shared<const constants::Values>(constants::MakeValues());
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+ std::vector<std::shared_ptr<Falcon>> falcons;
+
+ // TODO(max): Change the CanBus names with TalonFX software.
+ std::shared_ptr<SwerveModule> front_left = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{6, false},
+ frc971::wpilib::FalconParams{5, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<SwerveModule> front_right = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{3, false},
+ frc971::wpilib::FalconParams{4, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<SwerveModule> back_left = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{8, false},
+ frc971::wpilib::FalconParams{7, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<SwerveModule> back_right = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{2, false},
+ frc971::wpilib::FalconParams{1, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+
+ // Thread 1
+ aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ can_sensor_reader_event_loop.set_name("CANSensorReader");
+
+ falcons.push_back(front_left->rotation);
+ falcons.push_back(front_left->translation);
+
+ falcons.push_back(front_right->rotation);
+ falcons.push_back(front_right->translation);
+
+ falcons.push_back(back_left->rotation);
+ falcons.push_back(back_left->translation);
+
+ falcons.push_back(back_right->rotation);
+ falcons.push_back(back_right->translation);
+
+ aos::Sender<AbsoluteCANPosition> can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<AbsoluteCANPosition>(
+ "/drivetrain");
+
+ CANSensorReader can_sensor_reader(
+ &can_sensor_reader_event_loop, std::move(signals_registry), falcons,
+ [this, falcons, front_left, front_right, back_left, back_right,
+ &can_position_sender](ctre::phoenix::StatusCode status) {
+ // TODO(max): use status properly in the flatbuffer.
+ (void)status;
+
+ auto builder = can_position_sender.MakeBuilder();
+
+ for (auto falcon : falcons) {
+ falcon->RefreshNontimesyncedSignals();
+ falcon->SerializePosition(builder.fbb(), 1.0);
+ }
+
+ auto front_left_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), front_left);
+ auto front_right_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), front_right);
+ auto back_left_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), back_left);
+ auto back_right_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), back_right);
+
+ AbsoluteCANPosition::Builder can_position_builder =
+ builder.MakeBuilder<AbsoluteCANPosition>();
+
+ can_position_builder.add_front_left(front_left_offset);
+ can_position_builder.add_front_right(front_right_offset);
+ can_position_builder.add_back_left(back_left_offset);
+ can_position_builder.add_back_right(back_right_offset);
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ });
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 2
+ // Setup CAN
+ if (!FLAGS_ctre_diag_server) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+ constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+ ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+ constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+ aos::ShmEventLoop drivetrain_writer_event_loop(&config.message());
+ drivetrain_writer_event_loop.set_name("DrivetrainWriter");
+
+ DrivetrainWriter drivetrain_writer(
+ &drivetrain_writer_event_loop,
+ constants::Values::kDrivetrainWriterPriority, 12);
+
+ drivetrain_writer.set_falcons(front_left, front_right, back_left,
+ back_right);
+
+ AddLoop(&drivetrain_writer_event_loop);
+
+ // Thread 3
+ aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+ sensor_reader_event_loop.set_name("SensorReader");
+ SensorReader sensor_reader(&sensor_reader_event_loop, values);
+
+ sensor_reader.set_follower_wheel_one_encoder(make_encoder(4));
+ sensor_reader.set_follower_wheel_two_encoder(make_encoder(5));
+
+ sensor_reader.set_front_left_encoder(make_encoder(1));
+ sensor_reader.set_front_left_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(1));
+
+ sensor_reader.set_front_right_encoder(make_encoder(0));
+ sensor_reader.set_front_right_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(0));
+
+ sensor_reader.set_back_left_encoder(make_encoder(2));
+ sensor_reader.set_back_left_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(2));
+
+ sensor_reader.set_back_right_encoder(make_encoder(3));
+ sensor_reader.set_back_right_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(3));
+
+ AddLoop(&sensor_reader_event_loop);
+
+ RunLoops();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2023_bot4
+
+AOS_ROBOT_CLASS(::y2023_bot4::wpilib::WPILibRobot)
diff --git a/y2023_bot4/y2023_bot4.json b/y2023_bot4/y2023_bot4.json
new file mode 100644
index 0000000..49db6fc
--- /dev/null
+++ b/y2023_bot4/y2023_bot4.json
@@ -0,0 +1,19 @@
+{
+ "channel_storage_duration": 10000000000,
+ "maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.RobotState"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "imports": [
+ "y2023_bot4_roborio.json",
+ "y2023_bot4_imu.json",
+ "y2023_bot4_logger.json"
+ ]
+}
diff --git a/y2023_bot4/y2023_bot4_imu.json b/y2023_bot4/y2023_bot4_imu.json
new file mode 100644
index 0000000..274b158
--- /dev/null
+++ b/y2023_bot4/y2023_bot4_imu.json
@@ -0,0 +1,212 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.timing.Report",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "imu",
+ "frequency": 200,
+ "num_senders": 20
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.Status",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "imu",
+ "frequency": 20,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio",
+ "logger"
+ ],
+ "max_size": 400,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 1600,
+ "num_senders": 2
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "imu",
+ "executable_name": "imu_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "imu",
+ "hostname": "pi6",
+ "hostnames": [
+ "pi-971-6",
+ "pi-7971-6",
+ "pi-8971-6",
+ "pi-9971-6"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "logger"
+ },
+ {
+ "name": "roborio"
+ }
+ ]
+}
diff --git a/y2023_bot4/y2023_bot4_logger.json b/y2023_bot4/y2023_bot4_logger.json
new file mode 100644
index 0000000..5cca31f
--- /dev/null
+++ b/y2023_bot4/y2023_bot4_logger.json
@@ -0,0 +1,190 @@
+{
+ "channels": [
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.timing.Report",
+ "source_node": "logger",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "logger",
+ "frequency": 400,
+ "num_senders": 20
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "logger",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "logger",
+ "frequency": 20,
+ "max_size": 2000,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "logger",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.Status",
+ "source_node": "logger",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2000
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "logger",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "logger",
+ "frequency": 15,
+ "num_senders": 2,
+ "max_size": 400,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu",
+ "roborio"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "roborio",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/roborio/logger/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/aos"
+ }
+ },
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/constants"
+ }
+ },
+ {
+ "match": {
+ "name": "/camera*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/camera"
+ }
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "user": "pi",
+ "nodes": [
+ "logger"
+ ]
+ }
+ ],
+ "nodes": [
+ {
+ "name": "logger",
+ "hostname": "pi5",
+ "hostnames": [
+ "pi-971-5",
+ "pi-9971-5",
+ "pi-7971-5"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ },
+ {
+ "name": "roborio"
+ }
+ ]
+}
diff --git a/y2023_bot4/y2023_bot4_roborio.json b/y2023_bot4/y2023_bot4_roborio.json
new file mode 100644
index 0000000..7ff175c
--- /dev/null
+++ b/y2023_bot4/y2023_bot4_roborio.json
@@ -0,0 +1,350 @@
+{
+ "channels": [
+ {
+ "name": "/roborio/aos",
+ "type": "aos.RobotState",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.timing.Report",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 8192
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "roborio",
+ "frequency": 500,
+ "max_size": 1000,
+ "num_senders": 20
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2000
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "frequency": 10,
+ "max_size": 400,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "roborio",
+ "frequency": 20,
+ "max_size": 2000,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "roborio",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/logger/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 300,
+ "source_node": "roborio"
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "roborio",
+ "frequency": 15,
+ "num_senders": 2,
+ "max_size": 512,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2023_bot4.AbsoluteDrivetrainPosition",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 1,
+ "max_size": 480
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2023_bot4.AbsoluteCANPosition",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 1,
+ "max_size": 480
+ },
+ {
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "roborio",
+ "frequency": 6000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.SplineGoal",
+ "source_node": "roborio",
+ "frequency": 10
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Goal",
+ "source_node": "roborio",
+ "max_size": 224,
+ "frequency": 250
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.swerve.Position",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 112,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.swerve.Output",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 200,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Status",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 1616,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.LocalizerControl",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 96,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 0
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 400,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/autonomous",
+ "type": "aos.common.actions.Status",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.Goal",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.AutonomousMode",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.PDPValues",
+ "source_node": "roborio",
+ "frequency": 55,
+ "max_size": 368
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.wpilib.PneumaticsToLog",
+ "source_node": "roborio",
+ "frequency": 50
+ },
+ {
+ "name": "/roborio",
+ "type": "frc971.CANConfiguration",
+ "source_node": "roborio",
+ "frequency": 2
+ }
+ ],
+ "applications": [
+ {
+ "name": "wpilib_interface",
+ "executable_name": "wpilib_interface",
+ "args": [
+ "--nodie_on_malloc",
+ "--ctre_diag_server"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "swerve_publisher",
+ "executable_name": "swerve_publisher",
+ "autostart": false,
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "args": [
+ "--rt_priority=16"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "logger",
+ "executable_name": "logger_main",
+ "args": [
+ "--snappy_compress",
+ "--logging_folder=/home/admin/logs/",
+ "--rotate_every",
+ "30.0"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "can_logger",
+ "executable_name": "can_logger",
+ "nodes": [
+ "roborio"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "roborio",
+ "hostname": "roborio",
+ "hostnames": [
+ "roboRIO-971-FRC",
+ "roboRIO-6971-FRC",
+ "roboRIO-7971-FRC",
+ "roboRIO-8971-FRC",
+ "roboRIO-9971-FRC"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ },
+ {
+ "name": "logger"
+ }
+ ]
+}