Correct hood zeroing calculations
Used trig to find screw length based on hood angle, preventing incorrect
estops. Calculations based on
https://slack-files.com/T2752T5SA-F02JJ39RY03-f83f22b78d
Change-Id: I8a143d6dfe6476a4a57b702f9bd93cad5c6b9262
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
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..126502b 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;
@@ -99,16 +100,18 @@
State state_ = State::UNINITIALIZED;
double min_position_, max_position_;
- 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 +125,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) {
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/constants.cc b/y2020/constants.cc
index 1ef9073..80448e8 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}},
@@ -80,6 +81,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/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.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_; }