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_;