Added Zeroing Estimator for the column
Change-Id: I0c9dc557d91ed62f48d8ab6a9a20d2508e362b82
diff --git a/frc971/constants.h b/frc971/constants.h
index 7b4cb47..0171e47 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -6,6 +6,21 @@
namespace frc971 {
namespace constants {
+struct HallEffectZeroingConstants {
+ // The absolute position of the lower edge of the hall effect sensor.
+ double lower_hall_position;
+ // The absolute position of the upper edge of the hall effect sensor.
+ double upper_hall_position;
+ // The difference in scaled units between two hall effect edges. This is the
+ // number of units/cycle.
+ double index_difference;
+ // Number of cycles we need to see the hall effect high.
+ size_t hall_trigger_zeroing_length;
+ // Direction the system must be moving in order to zero. True is positive,
+ // False is negative direction.
+ bool zeroing_move_direction;
+};
+
struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 757e257..0f248f1 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -98,6 +98,20 @@
int32_t index_pulses_seen;
};
+struct HallEffectAndPositionEstimatorState {
+ // If error.
+ bool error;
+ // If we've found a positive edge while moving backwards and is zeroed.
+ bool zeroed;
+ // Encoder angle relative to where we started.
+ double encoder;
+ // The positions of the extreme posedges we've seen.
+ // If we've gotten enough samples where the hall effect is high before can be
+ // certain it is not a false positive.
+ bool high_long_enough;
+ double offset;
+};
+
// A left/right pair of PotAndIndexPositions.
struct PotAndIndexPair {
PotAndIndexPosition left;
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index b8addc1..0f09f7f 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -370,6 +370,18 @@
EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
EXPECT_EQ(4, position.negedge_count);
EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+
+ for (int i = 0; i < 10; ++i) {
+ // Now, go over the lower edge, falling.
+ sim.MoveTo(-0.25 - i * 1.0e-6);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_NEAR(-i * 1.0e-6, position.position, 1e-8);
+ EXPECT_EQ(4, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
+ EXPECT_EQ(4, position.negedge_count);
+ EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ }
}
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 947a15c..4fe0236 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -142,6 +142,118 @@
return r;
}
+HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants)
+ : constants_(constants) {
+ Reset();
+}
+
+void HallEffectAndPositionZeroingEstimator::Reset() {
+ offset_ = 0.0;
+ min_low_position_ = ::std::numeric_limits<double>::max();
+ max_low_position_ = ::std::numeric_limits<double>::lowest();
+ zeroed_ = false;
+ initialized_ = false;
+ last_used_posedge_count_ = 0;
+ cycles_high_ = 0;
+ high_long_enough_ = false;
+ first_start_pos_ = 0.0;
+ error_ = false;
+ current_ = 0.0;
+ first_start_pos_ = 0.0;
+}
+
+void HallEffectAndPositionZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
+}
+
+void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
+ const HallEffectAndPosition &info) {
+ // If we have a new posedge.
+ if (!info.current) {
+ if (last_hall_) {
+ min_low_position_ = max_low_position_ = info.position;
+ } else {
+ min_low_position_ = ::std::min(min_low_position_, info.position);
+ max_low_position_ = ::std::max(max_low_position_, info.position);
+ }
+ }
+ last_hall_ = info.current;
+}
+
+void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
+ const HallEffectAndPosition &info) {
+ // We want to make sure that we encounter at least one posedge while zeroing.
+ // So we take the posedge count from the first sample after reset and wait for
+ // that count to change and for the hall effect to stay high before we
+ // consider ourselves zeroed.
+ if (!initialized_) {
+ last_used_posedge_count_ = info.posedge_count;
+ initialized_ = true;
+ last_hall_ = info.current;
+ }
+
+ StoreEncoderMaxAndMin(info);
+
+ if (info.current) {
+ cycles_high_++;
+ } else {
+ cycles_high_ = 0;
+ last_used_posedge_count_ = info.posedge_count;
+ }
+
+ high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
+
+ bool moving_backward = false;
+ if (constants_.zeroing_move_direction) {
+ moving_backward = info.position > min_low_position_;
+ } else {
+ moving_backward = info.position < max_low_position_;
+ }
+
+ // If there are no posedges to use or we don't have enough samples yet to
+ // have a well-filtered starting position then we use the filtered value as
+ // our best guess.
+ if (last_used_posedge_count_ != info.posedge_count && high_long_enough_ &&
+ moving_backward) {
+ // Note the offset and the current posedge count so that we only run this
+ // logic once per posedge. That should be more resilient to corrupted
+ // intermediate data.
+ offset_ = -info.posedge_value;
+ if (constants_.zeroing_move_direction) {
+ offset_ += constants_.lower_hall_position;
+ } else {
+ offset_ += constants_.upper_hall_position;
+ }
+ last_used_posedge_count_ = info.posedge_count;
+
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = offset_;
+ LOG(INFO, "latching start position %f\n", first_start_pos_);
+ }
+
+ // Now that we have an accurate starting position we can consider ourselves
+ // zeroed.
+ zeroed_ = true;
+ }
+
+ position_ = info.position - offset_;
+}
+
+HallEffectAndPositionZeroingEstimator::State
+HallEffectAndPositionZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.encoder = position_;
+ r.high_long_enough = high_long_enough_;
+ r.offset = offset_;
+ return r;
+}
PotAndAbsEncoderZeroingEstimator::PotAndAbsEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 7b77fdd..91d5b8c 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -121,6 +121,80 @@
double first_start_pos_;
};
+// Estimates the position with an incremental encoder with an index pulse and a
+// potentiometer.
+class HallEffectAndPositionZeroingEstimator : public ZeroingEstimator {
+ public:
+ using Position = HallEffectAndPosition;
+ using ZeroingConstants = constants::HallEffectZeroingConstants;
+ using State = HallEffectAndPositionEstimatorState;
+
+ explicit HallEffectAndPositionZeroingEstimator(const ZeroingConstants &constants);
+
+ // Update the internal logic with the next sensor values.
+ void UpdateEstimate(const Position &info);
+
+ // Reset the internal logic so it needs to be re-zeroed.
+ void Reset();
+
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError();
+
+ bool error() const override { return error_; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
+ private:
+ // Sets the minimum and maximum posedge position values.
+ void StoreEncoderMaxAndMin(const HallEffectAndPosition &info);
+
+ // The zeroing constants used to describe the configuration of the system.
+ const ZeroingConstants constants_;
+
+ // The estimated state of the hall effect.
+ double current_ = 0.0;
+ // The estimated position.
+ double position_ = 0.0;
+ // The smallest and largest positions of the last set of encoder positions
+ // while the hall effect was low.
+ double min_low_position_;
+ double max_low_position_;
+ // If we've seen the hall effect high for enough times without going low, then
+ // we can be sure it isn't a false positive.
+ bool high_long_enough_;
+ size_t cycles_high_;
+
+ bool last_hall_ = false;
+
+ // The estimated starting position of the mechanism. We also call this the
+ // 'offset' in some contexts.
+ double offset_;
+ // Flag for triggering logic that takes note of the current posedge count
+ // after a reset. See `last_used_posedge_count_'.
+ bool initialized_;
+ // After a reset we keep track of the posedge count with this. Only after the
+ // posedge count changes (i.e. increments at least once or wraps around) will
+ // we consider the mechanism zeroed. We also use this to store the most recent
+ // `HallEffectAndPosition::posedge_count' value when the start position
+ // was calculated. It helps us calculate the start position only on posedges
+ // to reject corrupted intermediate data.
+ int32_t last_used_posedge_count_;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_ = false;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+};
+
// Estimates the position with an absolute encoder which also reports
// incremental counts, and a potentiometer.
class PotAndAbsEncoderZeroingEstimator : public ZeroingEstimator {
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 32ca049..a4ba3d7 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -55,6 +55,15 @@
estimator->UpdateEstimate(sensor_values_);
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ HallEffectAndPositionZeroingEstimator *estimator,
+ double new_position) {
+ HallEffectAndPosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
+
::aos::testing::TestSharedMemory my_shm_;
};
@@ -438,5 +447,88 @@
estimator.GetEstimatorState().position);
}
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(ZeroingTest, TestHallEffectZeroing) {
+ constants::HallEffectZeroingConstants constants;
+ constants.lower_hall_position = 0.25;
+ constants.upper_hall_position = 0.75;
+ constants.index_difference = 1.0;
+ constants.hall_trigger_zeroing_length = 2;
+ constants.zeroing_move_direction = false;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos = 1.0;
+
+ sim.InitializeHallEffectAndPosition(start_pos, constants.lower_hall_position,
+ constants.upper_hall_position);
+
+ HallEffectAndPositionZeroingEstimator estimator(constants);
+
+ // Should not be zeroed when we stand still.
+ for (int i = 0; i < 300; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, 0.9);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move to where the hall effect is triggered and make sure it becomes zeroed.
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ ASSERT_TRUE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly.
+ EXPECT_DOUBLE_EQ(-0.25, estimator.offset());
+
+ // Make sure triggering errors works.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Ensure resetting resets the state of the estimator.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+
+ // Make sure we don't become zeroed if the hall effect doesn't trigger for
+ // long enough.
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Make sure we can zero moving in the opposite direction as before and stay
+ // zeroed once the hall effect is no longer triggered.
+
+ MoveTo(&sim, &estimator, 0.0);
+ ASSERT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.4);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.6);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly.
+ EXPECT_DOUBLE_EQ(-0.75, estimator.offset());
+
+ // Make sure we don't zero if we start in the hall effect's range, before we
+ // reset, we also check that there were no errors.
+ MoveTo(&sim, &estimator, 0.5);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+ estimator.Reset();
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+}
+
+
} // namespace zeroing
} // namespace frc971
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 008e08b..b9361c7 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -62,6 +62,7 @@
Values *const r = new Values();
Values::Intake *const intake = &r->intake;
Values::Hood *const hood = &r->hood;
+ Values::Column *const column = &r->column;
r->drivetrain_max_speed = 5;
@@ -71,6 +72,9 @@
intake->zeroing.moving_buffer_size = 20;
intake->zeroing.allowable_encoder_error = 0.3;
+ column->indexer_zeroing.index_difference = 2.0 * M_PI;
+ column->turret_zeroing.index_difference = 2.0 * M_PI;
+
hood->zeroing.index_pulse_count = 2;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
hood->zeroing.known_index_pulse = 0;
@@ -81,6 +85,12 @@
intake->pot_offset = 0;
intake->zeroing.measured_absolute_position = 0;
+ column->indexer_zeroing.lower_hall_position = 2.0;
+ column->indexer_zeroing.upper_hall_position = 2.1;
+
+ column->turret_zeroing.lower_hall_position = 0;
+ column->turret_zeroing.upper_hall_position = 0.1;
+
hood->pot_offset = 0.1;
hood->zeroing.measured_index_position = 0.05;
@@ -92,6 +102,12 @@
intake->pot_offset = 0.26712;
intake->zeroing.measured_absolute_position = 0.008913;
+ column->indexer_zeroing.lower_hall_position = 2.0;
+ column->indexer_zeroing.upper_hall_position = 2.1;
+
+ column->turret_zeroing.lower_hall_position = 0;
+ column->turret_zeroing.upper_hall_position = 0.1;
+
hood->zeroing.measured_index_position = 0.652898 - 0.488117;
r->down_error = 0;
@@ -102,6 +118,12 @@
intake->pot_offset = 0.2921 + 0.00039 + 0.012236 - 0.023602;
intake->zeroing.measured_absolute_position = 0.031437;
+ column->indexer_zeroing.lower_hall_position = 2.0;
+ column->indexer_zeroing.upper_hall_position = 2.1;
+
+ column->turret_zeroing.lower_hall_position = 0;
+ column->turret_zeroing.upper_hall_position = 0.1;
+
hood->zeroing.measured_index_position = 0.655432 - 0.460505;
r->down_error = 0;
diff --git a/y2017/constants.h b/y2017/constants.h
index ec8de5d..e8faae1 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -32,16 +32,16 @@
::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
};
- struct Turret {
- double pot_offset;
- ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
- };
-
struct Hood {
double pot_offset;
::frc971::constants::EncoderPlusIndexZeroingConstants zeroing;
};
+ struct Column {
+ ::frc971::constants::HallEffectZeroingConstants indexer_zeroing;
+ ::frc971::constants::HallEffectZeroingConstants turret_zeroing;
+ };
+
static const int kZeroingSampleSize = 200;
static constexpr double kDrivetrainCyclesPerRevolution = 256;
@@ -111,6 +111,8 @@
Intake intake;
+ Column column;
+
Hood hood;
double down_error;
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
new file mode 100644
index 0000000..5a79dbc
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -0,0 +1,64 @@
+genrule(
+ name = 'genrule_column',
+ cmd = '$(location //y2017/control_loops/python:column) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:column',
+ ],
+ outs = [
+ 'column_plant.h',
+ 'column_plant.cc',
+ 'column_integral_plant.h',
+ 'column_integral_plant.cc',
+ 'stuck_column_integral_plant.cc',
+ 'stuck_column_integral_plant.h',
+ ],
+)
+
+cc_library(
+ name = 'column_plants',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'column_plant.cc',
+ 'column_integral_plant.cc',
+ ],
+ hdrs = [
+ 'column_plant.h',
+ 'column_integral_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'column_zeroing',
+ srcs = [
+ 'column_zeroing.cc',
+ ],
+ hdrs = [
+ 'column_zeroing.h',
+ ],
+ deps = [
+ '//frc971/control_loops:queues',
+ '//frc971/zeroing:wrap',
+ '//frc971/zeroing:zeroing',
+ '//frc971:constants',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
+
+cc_test(
+ name = 'column_zeroing_test',
+ srcs = [
+ 'column_zeroing_test.cc',
+ ],
+ deps = [
+ ':column_zeroing',
+ '//aos/testing:test_shm',
+ '//frc971/control_loops:position_sensor_sim',
+ '//frc971/control_loops:team_number_test_environment',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.cc b/y2017/control_loops/superstructure/column/column_zeroing.cc
new file mode 100644
index 0000000..e26d33e
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column_zeroing.cc
@@ -0,0 +1,81 @@
+#include "y2017/control_loops/superstructure/column/column_zeroing.h"
+
+#include "frc971/zeroing/wrap.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+ColumnZeroingEstimator::ColumnZeroingEstimator(
+ const ZeroingConstants &column_constants)
+ : indexer_(column_constants.indexer_zeroing),
+ turret_(column_constants.turret_zeroing),
+ turret_zeroed_distance_(column_constants.turret_zeroed_distance) {
+ Reset();
+}
+
+void ColumnZeroingEstimator::Reset() {
+ zeroed_ = false;
+ error_ = false;
+ offset_ready_ = false;
+ indexer_.Reset();
+ turret_.Reset();
+}
+
+void ColumnZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
+}
+
+void ColumnZeroingEstimator::UpdateEstimate(const ColumnPosition &position) {
+ indexer_.UpdateEstimate(position.indexer);
+ turret_.UpdateEstimate(position.turret);
+
+ if (indexer_.zeroed() && turret_.zeroed()) {
+ indexer_offset_ = indexer_.offset();
+
+ // Compute the current turret position.
+ const double current_turret = indexer_offset_ + position.indexer.position +
+ turret_.offset() + position.turret.position;
+
+ // Now, we can compute the turret position which is closest to 0 radians
+ // (within +- M_PI).
+ const double adjusted_turret =
+ ::frc971::zeroing::Wrap(0.0, current_turret, M_PI * 2.0);
+
+ // Now, compute the actual turret offset.
+ turret_offset_ = adjusted_turret - position.turret.position -
+ (indexer_offset_ + position.indexer.position);
+ offset_ready_ = true;
+
+ // If we are close enough to 0, we are zeroed. Otherwise, we don't know
+ // which revolution we are on and need more info. We will always report the
+ // turret position as within +- M_PI from 0 with the provided offset.
+ if (::std::abs(position.indexer.position + position.turret.position +
+ indexer_offset_ + turret_offset_) <
+ turret_zeroed_distance_) {
+ zeroed_ = true;
+ }
+
+ // TODO(austin): Latch the offset when we get zeroed for the first time and
+ // see if we get conflicting information further on.
+ }
+}
+
+ColumnZeroingEstimator::State ColumnZeroingEstimator::GetEstimatorState()
+ const {
+ State r;
+ r.error = error();
+ r.zeroed = zeroed();
+ r.indexer = indexer_.GetEstimatorState();
+ r.turret = turret_.GetEstimatorState();
+ return r;
+}
+
+} // column
+} // superstructure
+} // control_loops
+} // y2017
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
new file mode 100644
index 0000000..f2ac94f
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -0,0 +1,74 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+class ColumnZeroingEstimator {
+ public:
+ using ZeroingConstants = ::y2017::constants::Values::Column;
+ using SubZeroingConstants = ::frc971::constants::HallEffectZeroingConstants;
+ using State = ColumnEstimatorState;
+ using SubEstimator = ::frc971::zeroing::HallEffectAndPositionZeroingEstimator;
+
+ ColumnZeroingEstimator(const ZeroingConstants &column_constants);
+
+ void UpdateEstimate(const ColumnPosition &position);
+
+ void Reset();
+
+ void TriggerError();
+
+ bool offset_ready() const { return offset_ready_; }
+
+ bool error() const {
+ return error_ || indexer_.error() || turret_.error();
+ }
+
+ bool zeroed() const {
+ return zeroed_ && indexer_.zeroed() && turret_.zeroed();
+ }
+
+ double indexer_offset() const { return indexer_offset_; }
+ double turret_offset() const { return turret_offset_; }
+
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
+ private:
+ // We are ensuring that two subsystems are zeroed, so here they are!
+ SubEstimator indexer_, turret_;
+ // The offset in positions between the zero indexer and zero turret.
+ double indexer_offset_ = 0.0;
+ double turret_offset_ = 0.0;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_ = false;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_ = false;
+
+ // True if we have seen both edges the first time, but have not seen the
+ // region close enough to zero to be convinced which ambiguous start position
+ // we started in.
+ bool offset_ready_ = false;
+
+ // The max absolute value of the turret angle that we need to get to to be
+ // classified as zeroed. Otherwise, we may be ambiguous on which wrap we
+ // are on.
+ const double turret_zeroed_distance_;
+};
+
+} // column
+} // superstructure
+} // control_loops
+} // y2017
+
+#endif // y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
diff --git a/y2017/control_loops/superstructure/column/column_zeroing_test.cc b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
new file mode 100644
index 0000000..e6739fc
--- /dev/null
+++ b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
@@ -0,0 +1,125 @@
+#include <unistd.h>
+#include <memory>
+#include <random>
+
+#include "aos/common/die.h"
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/zeroing.h"
+#include "gtest/gtest.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/column/column_zeroing.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace column {
+
+using ::frc971::HallEffectAndPosition;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::zeroing::HallEffectAndPositionZeroingEstimator;
+
+class ZeroingTest : public ::testing::Test {
+ public:
+ explicit ZeroingTest()
+ : indexer_sensor_(2.0 * M_PI),
+ turret_sensor_(2.0 * M_PI),
+ column_zeroing_estimator_(constants::GetValues().column) {}
+
+ protected:
+ void SetUp() override { aos::SetDieTestMode(true); }
+ // Initializes logging and provides a tmp shmem.
+ ::aos::testing::TestSharedMemory my_shm_;
+
+ PositionSensorSimulator indexer_sensor_;
+ PositionSensorSimulator turret_sensor_;
+ ColumnZeroingEstimator column_zeroing_estimator_;
+
+ void InitializeHallEffectAndPosition(double indexer_start_position,
+ double turret_start_position) {
+ indexer_sensor_.InitializeHallEffectAndPosition(
+ indexer_start_position,
+ constants::GetValues().column.indexer_zeroing.lower_hall_position,
+ constants::GetValues().column.indexer_zeroing.upper_hall_position);
+ turret_sensor_.InitializeHallEffectAndPosition(
+ turret_start_position - indexer_start_position,
+ constants::GetValues().column.turret_zeroing.lower_hall_position,
+ constants::GetValues().column.turret_zeroing.upper_hall_position);
+ }
+
+ void MoveTo(double indexer, double turret) {
+ ColumnPosition column_position;
+ indexer_sensor_.MoveTo(indexer);
+ turret_sensor_.MoveTo(turret - indexer);
+
+ indexer_sensor_.GetSensorValues(&column_position.indexer);
+ turret_sensor_.GetSensorValues(&column_position.turret);
+
+ column_zeroing_estimator_.UpdateEstimate(column_position);
+ }
+};
+
+// Tests that starting at 0 and spinning around the indexer in a full circle
+// zeroes it (skipping the only offset_ready state).
+TEST_F(ZeroingTest, TestZeroStartingPoint) {
+ InitializeHallEffectAndPosition(0.0, 0.0);
+
+ for (double i = 0; i < 2.0 * M_PI; i += M_PI / 100) {
+ MoveTo(i, 0.0);
+ EXPECT_EQ(column_zeroing_estimator_.zeroed(),
+ column_zeroing_estimator_.offset_ready());
+ }
+ EXPECT_NEAR(column_zeroing_estimator_.indexer_offset(), 0.0, 1e-6);
+ EXPECT_NEAR(column_zeroing_estimator_.turret_offset(), 0.0, 1e-6);
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+}
+
+// Tests that starting the turret at M_PI + a bit stays offset_ready until we
+// move the turret closer to 0.0
+TEST_F(ZeroingTest, TestPiStartingPoint) {
+ constexpr double kStartingPosition = M_PI + 0.1;
+ InitializeHallEffectAndPosition(0.0, kStartingPosition);
+
+ for (double i = 0; i < 2.0 * M_PI; i += M_PI / 100) {
+ MoveTo(i, kStartingPosition);
+ EXPECT_FALSE(column_zeroing_estimator_.zeroed());
+ }
+ for (double i = kStartingPosition; i > 0.0; i -= M_PI / 100) {
+ MoveTo(2.0 * M_PI, i);
+ }
+ EXPECT_TRUE(column_zeroing_estimator_.offset_ready());
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+
+ EXPECT_NEAR(column_zeroing_estimator_.indexer_offset(), 0.0, 1e-6);
+ EXPECT_NEAR(column_zeroing_estimator_.turret_offset(), kStartingPosition,
+ 1e-6);
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+}
+
+// Tests that starting the turret at -M_PI - a bit stays offset_ready until we
+// move the turret closer to 0.0
+TEST_F(ZeroingTest, TestMinusPiStartingPoint) {
+ constexpr double kStartingPosition = -M_PI - 0.1;
+ InitializeHallEffectAndPosition(0.0, kStartingPosition);
+
+ for (double i = 0; i < 2.0 * M_PI; i += M_PI / 100) {
+ MoveTo(i, kStartingPosition);
+ EXPECT_FALSE(column_zeroing_estimator_.zeroed());
+ }
+ for (double i = kStartingPosition; i < 0.0; i += M_PI / 100) {
+ MoveTo(2.0 * M_PI, i);
+ }
+ EXPECT_TRUE(column_zeroing_estimator_.offset_ready());
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+
+ EXPECT_NEAR(column_zeroing_estimator_.indexer_offset(), 0.0, 1e-6);
+ EXPECT_NEAR(column_zeroing_estimator_.turret_offset(), kStartingPosition,
+ 1e-6);
+ EXPECT_TRUE(column_zeroing_estimator_.zeroed());
+}
+
+} // namespace column
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 35de4dc..14beff5 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -44,7 +44,6 @@
&(position->turret),
output != nullptr ? &(output->voltage_turret) : nullptr,
&(status->turret));
-
intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
&(position->intake),
output != nullptr ? &(output->voltage_intake) : nullptr,
@@ -54,9 +53,9 @@
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));
indexer_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
- &(position->theta_indexer),
- output != nullptr ? &(output->voltage_indexer) : nullptr,
- &(status->indexer));
+ &(position->theta_indexer),
+ output != nullptr ? &(output->voltage_indexer) : nullptr,
+ &(status->indexer));
if (output && unsafe_goal) {
output->voltage_intake_rollers =
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 0f707dc..99989b3 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -117,6 +117,51 @@
.frc971.HallEffectAndPosition turret;
};
+struct ColumnEstimatorState {
+ bool error;
+ bool zeroed;
+ .frc971.HallEffectAndPositionEstimatorState intake;
+ .frc971.HallEffectAndPositionEstimatorState turret;
+};
+
+struct TurretProfiledSubsystemStatus {
+ // Is the subsystem zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ ColumnEstimatorState estimator_state;
+};
+
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -137,10 +182,11 @@
// Each subsystems status.
.frc971.control_loops.AbsoluteProfiledJointStatus intake;
- .frc971.control_loops.AbsoluteProfiledJointStatus turret;
.frc971.control_loops.IndexProfiledJointStatus hood;
- IndexerStatus indexer;
ShooterStatus shooter;
+
+ TurretProfiledSubsystemStatus turret;
+ IndexerStatus indexer;
};
message Position {