Added Zeroing Estimator for the column
Change-Id: I0c9dc557d91ed62f48d8ab6a9a20d2508e362b82
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 {