Merge changes I28b2b1c2,Ifce86fec
* changes:
Merge commit 'b30f56638cca48769a52c046fb79a83d2c72dd86' as 'third_party/snappy'
Squashed 'third_party/snappy/' content from commit fe18b4632
diff --git a/aos/network/message_bridge_client_lib.h b/aos/network/message_bridge_client_lib.h
index 0552f79..23c982d 100644
--- a/aos/network/message_bridge_client_lib.h
+++ b/aos/network/message_bridge_client_lib.h
@@ -31,6 +31,7 @@
// transmit messages.
class SctpClientConnection {
public:
+ static constexpr std::chrono::seconds kReconnectTimeout{3};
SctpClientConnection(aos::ShmEventLoop *const event_loop,
std::string_view remote_name, const Node *my_node,
std::string_view local_host,
@@ -58,7 +59,7 @@
// from the server for a while we'll try sending it again.
void ScheduleConnectTimeout() {
connect_timer_->Setup(event_loop_->context().monotonic_event_time +
- std::chrono::seconds(1));
+ kReconnectTimeout);
}
// Event loop to register the server on.
diff --git a/aos/network/message_bridge_server_status.h b/aos/network/message_bridge_server_status.h
index 04b30de..e9a3322 100644
--- a/aos/network/message_bridge_server_status.h
+++ b/aos/network/message_bridge_server_status.h
@@ -22,7 +22,7 @@
// the filter.
static constexpr std::chrono::seconds kClientStatisticsStaleTimeout{1};
// Time after which we consider the timestamp stale, and reset the filter.
- static constexpr std::chrono::milliseconds kTimestampStaleTimeout{250};
+ static constexpr std::chrono::milliseconds kTimestampStaleTimeout{1000};
MessageBridgeServerStatus(aos::EventLoop *event_loop,
std::function<void(const Context &)> send_data =
diff --git a/aos/network/message_bridge_test.cc b/aos/network/message_bridge_test.cc
index 74d22c2..2a43170 100644
--- a/aos/network/message_bridge_test.cc
+++ b/aos/network/message_bridge_test.cc
@@ -773,7 +773,8 @@
StopPi2Client();
}
- std::this_thread::sleep_for(std::chrono::seconds(2));
+ std::this_thread::sleep_for(SctpClientConnection::kReconnectTimeout +
+ std::chrono::seconds(1));
{
// Now confirm we are un-synchronized.
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 02f8e3a..d50c1f9 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -3,6 +3,7 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "cpu_select")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
@@ -48,6 +49,13 @@
includes = ["//frc971/control_loops:control_loops_fbs_includes"],
)
+cc_static_flatbuffer(
+ name = "trajectory_schema",
+ function = "frc971::control_loops::drivetrain::fb::TrajectorySchema",
+ target = ":trajectory_fbs_reflection_out",
+ visibility = ["//visibility:public"],
+)
+
flatbuffer_ts_library(
name = "drivetrain_status_ts_fbs",
srcs = ["drivetrain_status.fbs"],
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 6780e3b..86e608c 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -192,6 +192,9 @@
const ::frc971::constants::Range &range() const { return range_; }
+ double default_velocity() const { return default_velocity_; }
+ double default_acceleration() const { return default_acceleration_; }
+
protected:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 948ed5d..dd64111 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -39,11 +39,12 @@
// Class for controlling and motion profiling a single degree of freedom
// subsystem with a zeroing strategy of not moving.
-template <typename TZeroingEstimator, typename TProfiledJointStatus>
+template <typename TZeroingEstimator, typename TProfiledJointStatus,
+ typename TSubsystemParams = TZeroingEstimator>
class StaticZeroingSingleDOFProfiledSubsystem {
public:
StaticZeroingSingleDOFProfiledSubsystem(
- const StaticZeroingSingleDOFProfiledSubsystemParams<TZeroingEstimator>
+ const StaticZeroingSingleDOFProfiledSubsystemParams<TSubsystemParams>
¶ms);
using ZeroingEstimator = TZeroingEstimator;
@@ -63,6 +64,16 @@
void set_max_position(double max_position) { max_position_ = max_position; }
+ // Sets a temporary acceleration limit. No accelerations faster than this may
+ // be commanded.
+ void set_max_acceleration(double max_acceleration) {
+ max_acceleration_ = max_acceleration;
+ }
+ // Clears the acceleration limit.
+ void clear_max_acceleration() {
+ max_acceleration_ = std::numeric_limits<float>::infinity();
+ }
+
// Resets constrained min/max position
void clear_min_position() { min_position_ = params_.range.lower_hard; }
@@ -98,17 +109,20 @@
private:
State state_ = State::UNINITIALIZED;
double min_position_, max_position_;
+ float max_acceleration_ = std::numeric_limits<float>::infinity();
- const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator> params_;
+ const StaticZeroingSingleDOFProfiledSubsystemParams<TSubsystemParams> params_;
::frc971::control_loops::SingleDOFProfiledSubsystem<ZeroingEstimator>
profiled_subsystem_;
};
-template <typename ZeroingEstimator, typename ProfiledJointStatus>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::
StaticZeroingSingleDOFProfiledSubsystem(
- const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
+ const StaticZeroingSingleDOFProfiledSubsystemParams<SubsystemParams>
¶ms)
: params_(params),
profiled_subsystem_(
@@ -122,18 +136,21 @@
Reset();
};
-template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
- ProfiledJointStatus>::Reset() {
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
profiled_subsystem_.Reset();
}
-template <typename ZeroingEstimator, typename ProfiledJointStatus>
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
flatbuffers::Offset<ProfiledJointStatus>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::
Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
flatbuffers::FlatBufferBuilder *status_fbb) {
@@ -192,7 +209,17 @@
}
if (goal) {
- profiled_subsystem_.AdjustProfile(goal->profile_params());
+ if (goal->profile_params()) {
+ profiled_subsystem_.AdjustProfile(
+ goal->profile_params()->max_velocity(),
+ std::min(goal->profile_params()->max_acceleration(),
+ max_acceleration_));
+ } else {
+ profiled_subsystem_.AdjustProfile(
+ profiled_subsystem_.default_velocity(),
+ std::min(profiled_subsystem_.default_acceleration(),
+ static_cast<double>(max_acceleration_)));
+ }
double safe_goal = goal->unsafe_goal();
if (safe_goal < min_position_) {
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.cc b/frc971/zeroing/absolute_and_absolute_encoder.cc
index 0645d43..40b0519 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder.cc
@@ -35,6 +35,15 @@
error_ = false;
}
+double
+AbsoluteAndAbsoluteEncoderZeroingEstimator::AdjustedSingleTurnAbsoluteEncoder(
+ const PositionStruct &sample) const {
+ return UnWrap(constants_.single_turn_middle_position,
+ sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position,
+ constants_.single_turn_one_revolution_distance);
+}
+
// So, this needs to be a multistep process. We need to first estimate the
// offset between the absolute encoder and the relative encoder. That process
// should get us an absolute number which is off by integer multiples of the
@@ -137,10 +146,7 @@
}
const double adjusted_single_turn_absolute_encoder =
- UnWrap(constants_.single_turn_middle_position,
- sample.single_turn_absolute_encoder -
- constants_.single_turn_measured_absolute_position,
- constants_.single_turn_one_revolution_distance);
+ AdjustedSingleTurnAbsoluteEncoder(sample);
// Now compute the offset between the pot and relative encoder.
if (offset_samples_.size() < constants_.average_filter_size) {
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.h b/frc971/zeroing/absolute_and_absolute_encoder.h
index dd2190d..499f7d1 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder.h
+++ b/frc971/zeroing/absolute_and_absolute_encoder.h
@@ -48,7 +48,7 @@
virtual flatbuffers::Offset<State> GetEstimatorState(
flatbuffers::FlatBufferBuilder *fbb) const override;
- private:
+ protected:
struct PositionStruct {
PositionStruct(const AbsoluteAndAbsolutePosition &position_buffer)
: single_turn_absolute_encoder(
@@ -60,6 +60,12 @@
double encoder;
};
+ // Returns an adjusted single turn absolute encoder reading.
+ // Filled in by default but can be overriden.
+ virtual double AdjustedSingleTurnAbsoluteEncoder(
+ const PositionStruct &sample) const;
+
+ private:
// The zeroing constants used to describe the configuration of the system.
const constants::AbsoluteAndAbsoluteEncoderZeroingConstants constants_;
diff --git a/y2020/BUILD b/y2020/BUILD
index 195e2f4..61987c9 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -162,7 +162,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":config_laptop",
+ ":config_logger",
":config_pi1",
":config_pi2",
":config_pi3",
@@ -204,8 +204,8 @@
]
aos_config(
- name = "config_laptop",
- src = "y2020_laptop.json",
+ name = "config_logger",
+ src = "y2020_logger.json",
flatbuffers = [
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 49eea13..e418831 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -28,9 +28,10 @@
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
*const hood = &r->hood;
+ Values::HoodGeometry *const hood_geometry = &r->hood_geometry;
- // We found that the finisher velocity does not change ball velocity much, so
- // keep it constant.
+ // We found that the finisher velocity does not change ball velocity much,
+ // so keep it constant.
constexpr double kVelocityFinisher = 350.0;
r->shot_interpolation_table =
InterpolationTable<Values::ShotParams>({{1.0, {0.01, 10.7}},
@@ -81,6 +82,14 @@
hood->zeroing_constants.measured_absolute_position = 0;
hood->zeroing_constants.single_turn_measured_absolute_position = 0;
+ constexpr double kDegToRad = M_PI / 180.0;
+ constexpr double kMmToM = 1.0 / 1000.0;
+ hood_geometry->theta_0 = 22.98004 * kDegToRad;
+ hood_geometry->screw_length_0 = 110.33888 * kMmToM;
+ hood_geometry->radius = 269.6262 * kMmToM;
+ hood_geometry->diagonal_length = 288.4353 * kMmToM;
+ hood_geometry->back_plate_diagonal_length = 22.86 * kMmToM;
+
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
&r->intake;
diff --git a/y2020/constants.h b/y2020/constants.h
index 9906ffd..9dfed44 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -44,16 +44,18 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kHoodEncoderRatio() {
- // TODO: This math is not quite right
- // 10.211 in of travel gets you 1 radian on the output
- const double radians_per_in_travel = 1.0 / 10.211;
+ // TODO: This math is not quite right
+ // 10.211 in of travel gets you 1 radian on the output
+ static constexpr double kHoodEncoderRadiansPerInTravel() {
+ return 1.0 / 10.211;
+ }
+ static constexpr double kHoodEncoderRatio() {
// one turn on the leadscrew gets you 0.5 in travel
const double in_travel_per_radian = 0.5 / (2.0 * M_PI);
// units reduce; radians on the encoder * this number = radians on the hood
- return in_travel_per_radian * radians_per_in_travel;
+ return in_travel_per_radian * kHoodEncoderRadiansPerInTravel();
}
static constexpr double kHoodSingleTurnEncoderRatio() { return 8.0 / 72.0; }
@@ -73,10 +75,25 @@
};
}
+ struct HoodGeometry {
+ // Measurements for hood zeroing calculations (all lengths in meters and
+ // angles in radians)
+
+ // Measurements when hood is at 0
+ double theta_0;
+ double screw_length_0;
+
+ double radius;
+ double diagonal_length;
+ double back_plate_diagonal_length;
+ };
+
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
hood;
+ HoodGeometry hood_geometry;
+
// Intake
static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index b12cdc3..17a2959 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -186,6 +186,7 @@
"//aos/events/logging:log_writer",
"//aos/testing:googletest",
"//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:trajectory_schema",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
],
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index def3cd3..297cf5b 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -11,13 +11,14 @@
#include "gtest/gtest.h"
#include "aos/configuration.h"
-#include "aos/events/logging/log_writer.h"
#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/trajectory_schema.h"
#include "gflags/gflags.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
@@ -36,9 +37,7 @@
class DrivetrainReplayTest : public ::testing::Test {
public:
DrivetrainReplayTest()
- : config_(aos::configuration::ReadConfig(FLAGS_config)),
- reader_(aos::logger::SortParts(aos::logger::FindLogs(FLAGS_logfile)),
- &config_.message()) {
+ : reader_(aos::logger::SortParts(aos::logger::FindLogs(FLAGS_logfile))) {
aos::network::OverrideTeamNumber(971);
// TODO(james): Actually enforce not sending on the same buses as the
@@ -47,12 +46,38 @@
"frc971.control_loops.drivetrain.Status");
reader_.RemapLoggedChannel("/drivetrain",
"frc971.control_loops.drivetrain.Output");
- reader_.Register();
+
+ // Patch in any new channels.
+ updated_config_ = aos::configuration::MergeWithConfig(
+ reader_.configuration(),
+ aos::configuration::AddSchema(
+ R"channel({
+ "channels": [
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+ "source_node": "roborio",
+ "max_size": 600000,
+ "frequency": 4,
+ "num_senders": 2,
+ "read_method": "PIN",
+ "num_readers": 10,
+ "logger": "NOT_LOGGED"
+ }
+ ]
+})channel",
+ {aos::FlatbufferVector<
+ reflection::Schema>(aos::FlatbufferSpan<reflection::Schema>(
+ frc971::control_loops::drivetrain::fb::TrajectorySchema()))}));
+
+ factory_ = std::make_unique<aos::SimulatedEventLoopFactory>(
+ &updated_config_.message());
+
+ reader_.Register(factory_.get());
roborio_ = aos::configuration::GetNode(reader_.configuration(), "roborio");
- drivetrain_event_loop_ =
- reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
+ drivetrain_event_loop_ = factory_->MakeEventLoop("drivetrain", roborio_);
drivetrain_event_loop_->SkipTimingReport();
frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
@@ -65,17 +90,25 @@
std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
config, drivetrain_event_loop_.get(), localizer_.get());
- test_event_loop_ =
- reader_.event_loop_factory()->MakeEventLoop("drivetrain_test", roborio_);
+ test_event_loop_ = factory_->MakeEventLoop("drivetrain_test", roborio_);
- status_fetcher_ = test_event_loop_->MakeFetcher<
- frc971::control_loops::drivetrain::Status>("/drivetrain");
+ status_fetcher_ =
+ test_event_loop_
+ ->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain");
}
- const aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ ~DrivetrainReplayTest() {
+ reader_.Deregister();
+ }
+
aos::logger::LogReader reader_;
const aos::Node *roborio_;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> updated_config_ =
+ aos::FlatbufferDetachedBuffer<aos::Configuration>::Empty();
+ std::unique_ptr<aos::SimulatedEventLoopFactory> factory_;
+
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
std::unique_ptr<frc971::control_loops::drivetrain::DeadReckonEkf> localizer_;
std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 100d90c..375e4b0 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -85,6 +85,7 @@
"//y2020:constants",
"//y2020/control_loops/superstructure/shooter",
"//y2020/control_loops/superstructure/turret:aiming",
+ "//y2020/control_loops/superstructure/hood:hood_encoder_zeroing_estimator",
],
)
diff --git a/y2020/control_loops/superstructure/hood/BUILD b/y2020/control_loops/superstructure/hood/BUILD
index d0e5953..3035a55 100644
--- a/y2020/control_loops/superstructure/hood/BUILD
+++ b/y2020/control_loops/superstructure/hood/BUILD
@@ -32,3 +32,18 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "hood_encoder_zeroing_estimator",
+ srcs = [
+ "hood_encoder_zeroing_estimator.cc",
+ ],
+ hdrs = [
+ "hood_encoder_zeroing_estimator.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971/zeroing:zeroing",
+ "//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
new file mode 100644
index 0000000..eb9351b
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
@@ -0,0 +1,42 @@
+#include <cmath>
+
+#include "y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h"
+
+namespace y2020::control_loops::superstructure::hood {
+
+HoodEncoderZeroingEstimator::HoodEncoderZeroingEstimator(
+ const frc971::constants::AbsoluteAndAbsoluteEncoderZeroingConstants
+ &constants)
+ : AbsoluteAndAbsoluteEncoderZeroingEstimator(constants),
+ hood_geometry_(constants::GetValues().hood_geometry) {
+ constants::InitValues();
+}
+
+double HoodEncoderZeroingEstimator::AdjustedSingleTurnAbsoluteEncoder(
+ const AbsoluteAndAbsoluteEncoderZeroingEstimator::PositionStruct &sample)
+ const {
+ // Using equation derived in
+ // https://slack-files.com/T2752T5SA-F02JJ39RY03-f83f22b78d
+ // In that equation, theta = theta, l = screw_length, b = diagonal_length,
+ // a = back_plate_diagonal_length, and r = radius.
+ const double theta =
+ frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator::
+ AdjustedSingleTurnAbsoluteEncoder(sample) +
+ hood_geometry_.theta_0;
+
+ const double screw_length =
+ std::sqrt(std::pow(hood_geometry_.radius, 2) +
+ std::pow(hood_geometry_.diagonal_length, 2) -
+ (2 * hood_geometry_.radius * hood_geometry_.diagonal_length *
+ std::cos(theta)) -
+ std::pow(hood_geometry_.back_plate_diagonal_length, 2)) -
+ hood_geometry_.screw_length_0;
+ constexpr double kMToIn = 39.3701;
+ const double adjusted_single_turn_absolute_encoder =
+ (screw_length * kMToIn) *
+ constants::Values::kHoodEncoderRadiansPerInTravel();
+
+ return adjusted_single_turn_absolute_encoder;
+}
+
+} // namespace y2020::control_loops::superstructure::hood
diff --git a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h
new file mode 100644
index 0000000..238154b
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h
@@ -0,0 +1,24 @@
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "y2020/constants.h"
+
+namespace y2020::control_loops::superstructure::hood {
+
+using AbsoluteAndAbsoluteEncoderZeroingEstimator =
+ ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator;
+
+class HoodEncoderZeroingEstimator
+ : public AbsoluteAndAbsoluteEncoderZeroingEstimator {
+ public:
+ HoodEncoderZeroingEstimator(
+ const frc971::constants::AbsoluteAndAbsoluteEncoderZeroingConstants
+ &constants);
+
+ private:
+ double AdjustedSingleTurnAbsoluteEncoder(
+ const AbsoluteAndAbsoluteEncoderZeroingEstimator::PositionStruct &sample)
+ const override;
+
+ const constants::Values::HoodGeometry hood_geometry_;
+};
+
+} // namespace y2020::control_loops::superstructure::hood
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 4c5f235..d3c2464 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -113,6 +113,7 @@
}
if (unsafe_goal->shooting()) {
+ intake_joint_.set_max_acceleration(30.0);
constexpr std::chrono::milliseconds kPeriod =
std::chrono::milliseconds(250);
if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
@@ -122,6 +123,7 @@
intake_joint_.set_min_position(-0.75);
}
} else {
+ intake_joint_.clear_max_acceleration();
intake_joint_.clear_min_position();
}
@@ -234,14 +236,19 @@
output_struct.washing_machine_spinner_voltage = 0.0;
output_struct.feeder_voltage = 0.0;
output_struct.intake_roller_voltage = 0.0;
+ output_struct.climber_voltage = 0.0;
if (unsafe_goal) {
- output_struct.climber_voltage =
- std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
+ if (unsafe_goal->has_turret()) {
+ output_struct.climber_voltage =
+ std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
- // Make sure the turret is relatively close to the goal before turning the
- // climber on.
- if (std::abs(turret_.goal(0) - turret_.position()) > 0.1) {
- output_struct.climber_voltage = 0;
+ // Make sure the turret is relatively close to the goal before turning
+ // the climber on.
+ CHECK(unsafe_goal->has_turret());
+ if (std::abs(unsafe_goal->turret()->unsafe_goal() -
+ turret_.position()) > 0.1) {
+ output_struct.climber_voltage = 0;
+ }
}
if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 8f3eb0a..3eb6cb2 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/input/joystick_state_generated.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
@@ -43,9 +44,10 @@
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
using AbsoluteAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
- ::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator,
+ hood::HoodEncoderZeroingEstimator,
::frc971::control_loops::
- AbsoluteAndAbsoluteEncoderProfiledJointStatus>;
+ AbsoluteAndAbsoluteEncoderProfiledJointStatus,
+ frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>;
const AbsoluteAndAbsoluteEncoderSubsystem &hood() const { return hood_; }
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index b34273c..2e6ec08 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -942,25 +942,39 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ // Since there is a turret lockout, we need to set a turret goal...
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), M_PI / 2.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_climber_voltage(-10.0);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- // Give it time to stabilize.
- RunFor(chrono::seconds(1));
+ // The turret needs to move out of the way first. This takes some time.
+ RunFor(chrono::milliseconds(100));
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), 0.0);
- // Can go backwards.
+ // Now, we should be far enough that it should work.
+ RunFor(chrono::seconds(10));
EXPECT_EQ(superstructure_plant_.climber_voltage(), -10.0);
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ // Since there is a turret lockout, we need to set a turret goal...
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), M_PI / 2.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_climber_voltage(10.0);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -1139,10 +1153,8 @@
class SuperstructureReplayTest : public ::testing::Test {
public:
SuperstructureReplayTest()
- : config_(aos::configuration::ReadConfig(FLAGS_config)),
- reader_(
- aos::logger::SortParts(aos::logger::FindLogs(FLAGS_replay_logfile)),
- &config_.message()) {
+ : reader_(aos::logger::SortParts(
+ aos::logger::FindLogs(FLAGS_replay_logfile))) {
aos::network::OverrideTeamNumber(971);
reader_.RemapLoggedChannel("/superstructure",
@@ -1172,7 +1184,6 @@
}
}
- const aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
aos::logger::LogReader reader_;
const aos::Node *roborio_;
diff --git a/y2020/vision/rootfs/change_hostname.sh b/y2020/vision/rootfs/change_hostname.sh
index f36c439..c9b0b35 100755
--- a/y2020/vision/rootfs/change_hostname.sh
+++ b/y2020/vision/rootfs/change_hostname.sh
@@ -38,9 +38,9 @@
echo -e "${IP_BASE}.2\troborio" >> /etc/hosts
fi
-if grep '^10\.[0-9]*\.[0-9]*\.13\s*laptop$' /etc/hosts >/dev/null;
+if grep '^10\.[0-9]*\.[0-9]*\.13\s*logger$' /etc/hosts >/dev/null;
then
- sed -i "s/^10\.[0-9]*\.[0-9]*\(\.13\s*laptop\)$/${IP_BASE}\1/" /etc/hosts
+ sed -i "s/^10\.[0-9]*\.[0-9]*\(\.13\s*logger\)$/${IP_BASE}\1/" /etc/hosts
else
- echo -e "${IP_BASE}.13\tlaptop" >> /etc/hosts
+ echo -e "${IP_BASE}.13\tlogger" >> /etc/hosts
fi
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 70e51b0..5e736ba 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -18,6 +18,6 @@
"y2020_pi3.json",
"y2020_pi4.json",
"y2020_pi5.json",
- "y2020_laptop.json"
+ "y2020_logger.json"
]
}
diff --git a/y2020/y2020_laptop.json b/y2020/y2020_logger.json
similarity index 78%
rename from y2020/y2020_laptop.json
rename to y2020/y2020_logger.json
index 59aa47c..670cf99 100644
--- a/y2020/y2020_laptop.json
+++ b/y2020/y2020_logger.json
@@ -7,11 +7,11 @@
"source_node": "roborio",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 2,
"timestamp_logger": "LOCAL_LOGGER",
"time_to_live": 10000000
@@ -25,11 +25,11 @@
"source_node": "roborio",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000,
"timestamp_logger" : "LOCAL_AND_REMOTE_LOGGER",
@@ -44,11 +44,11 @@
"source_node": "roborio",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 2,
"time_to_live": 50000000
}
@@ -61,11 +61,11 @@
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -77,11 +77,11 @@
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -93,11 +93,11 @@
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -109,11 +109,11 @@
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -125,51 +125,66 @@
"source_node": "pi5",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
]
},
{
- "name": "/laptop/aos",
+ "name": "/logger/aos",
"type": "aos.timing.Report",
- "source_node": "laptop",
+ "source_node": "logger",
"frequency": 50,
"num_senders": 20,
"max_size": 4096
},
{
- "name": "/laptop/aos",
+ "name": "/logger/aos",
"type": "aos.logging.LogMessageFbs",
- "source_node": "laptop",
+ "source_node": "logger",
"frequency": 400,
"num_senders": 20
},
{
- "name": "/laptop/aos",
+ "name": "/logger/aos",
"type": "aos.message_bridge.ServerStatistics",
- "source_node": "laptop",
+ "source_node": "logger",
"frequency": 10,
"num_senders": 2
},
{
- "name": "/laptop/aos",
+ "name": "/logger/aos",
"type": "aos.message_bridge.ClientStatistics",
- "source_node": "laptop",
+ "source_node": "logger",
"frequency": 10,
"num_senders": 2
},
{
- "name": "/laptop/aos",
+ "name": "/logger/aos",
+ "type": "aos.starter.Status",
+ "source_node": "logger",
+ "frequency": 50,
+ "num_senders": 20
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "logger",
+ "frequency": 10,
+ "max_size": 72,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
"type": "aos.message_bridge.Timestamp",
"logger_nodes": ["roborio"],
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "source_node": "laptop",
+ "source_node": "logger",
"frequency": 15,
"num_senders": 2,
"max_size": 400,
@@ -180,7 +195,7 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "laptop"
+ "logger"
]
},
{
@@ -189,7 +204,7 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "laptop"
+ "logger"
]
},
{
@@ -198,7 +213,7 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "laptop"
+ "logger"
]
},
{
@@ -207,7 +222,7 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "laptop"
+ "logger"
]
},
{
@@ -216,7 +231,7 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "laptop"
+ "logger"
]
},
{
@@ -225,60 +240,60 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "laptop"
+ "logger"
]
}
]
},
{
- "name": "/laptop/aos/remote_timestamps/roborio/laptop/aos/aos-message_bridge-Timestamp",
+ "name": "/logger/aos/remote_timestamps/roborio/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "laptop",
+ "source_node": "logger",
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi1/laptop/aos/aos-message_bridge-Timestamp",
+ "name": "/logger/aos/remote_timestamps/pi1/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "laptop",
+ "source_node": "logger",
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi2/laptop/aos/aos-message_bridge-Timestamp",
+ "name": "/logger/aos/remote_timestamps/pi2/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "laptop",
+ "source_node": "logger",
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi3/laptop/aos/aos-message_bridge-Timestamp",
+ "name": "/logger/aos/remote_timestamps/pi3/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "laptop",
+ "source_node": "logger",
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi4/laptop/aos/aos-message_bridge-Timestamp",
+ "name": "/logger/aos/remote_timestamps/pi4/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "laptop",
+ "source_node": "logger",
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi5/laptop/aos/aos-message_bridge-Timestamp",
+ "name": "/logger/aos/remote_timestamps/pi5/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "laptop",
+ "source_node": "logger",
"logger": "NOT_LOGGED",
"frequency": 20,
"num_senders": 2,
@@ -290,11 +305,11 @@
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -306,11 +321,11 @@
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -322,11 +337,11 @@
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -338,11 +353,11 @@
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -354,11 +369,11 @@
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -370,11 +385,11 @@
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -386,11 +401,11 @@
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -402,11 +417,11 @@
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -418,11 +433,11 @@
"source_node": "pi5",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -434,11 +449,11 @@
"source_node": "pi5",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "laptop"
+ "logger"
],
"destination_nodes": [
{
- "name": "laptop",
+ "name": "logger",
"priority": 1,
"time_to_live": 5000000
}
@@ -449,18 +464,35 @@
{
"match": {
"name": "/aos*",
- "source_node": "laptop"
+ "source_node": "logger"
},
"rename": {
- "name": "/laptop/aos"
+ "name": "/logger/aos"
}
}
],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "nodes": [
+ "logger"
+ ]
+ }
+ ],
"nodes": [
{
- "name": "laptop",
- "hostname": "laptop",
+ "name": "logger",
+ "hostname": "logger",
"hostnames": [
+ "pi-971-6",
"ASchuh-T480s",
"aschuh-3950x"
],
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index e7c54f2..a9c056e 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -59,7 +59,7 @@
"num_senders": 2
},
{
- "name": "/roborio/aos/remote_timestamps/laptop/roborio/aos/aos-message_bridge-Timestamp",
+ "name": "/roborio/aos/remote_timestamps/logger/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",