Add relative encoder zeroing strategy
Change-Id: I07b5461a9766aa25c9c858b7771af26ed08ad4ff
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index 4f1e888..a95d407 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -61,6 +61,14 @@
absolute_encoder:double;
}
+// Represents all of the data for a single encoder.
+// The relative encoder values are relative to where the encoder was at some
+// arbitrary point in time.
+table RelativePosition {
+ // Current position read from the encoder.
+ encoder:double;
+}
+
// The internal state of a zeroing estimator.
table EstimatorState {
// If true, there has been a fatal error for the estimator.
@@ -105,6 +113,14 @@
absolute_position:double;
}
+table RelativeEncoderEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ error:bool;
+
+ // The estimated position of the joint.
+ position:double;
+}
+
// The internal state of a zeroing estimator.
table IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 612801e..b874b1d 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -91,6 +91,10 @@
negedge_value_ = start_position;
}
+void PositionSensorSimulator::InitializeRelativeEncoder() {
+ current_position_ = 0.0;
+}
+
void PositionSensorSimulator::MoveTo(double new_position) {
{
const int lower_start_segment = lower_index_edge_.current_index_segment();
@@ -218,5 +222,14 @@
return builder->Finish();
}
+template <>
+flatbuffers::Offset<RelativePosition>
+PositionSensorSimulator::GetSensorValues<RelativePositionBuilder>(
+ RelativePositionBuilder *builder) {
+ builder->add_encoder(current_position_);
+ return builder->Finish();
+}
+
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 6be6bbc..faa5dd5 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,10 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
-#include "flatbuffers/flatbuffers.h"
-
#include "aos/testing/random_seed.h"
-
+#include "flatbuffers/flatbuffers.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/gaussian_noise.h"
@@ -50,6 +48,11 @@
double known_index_lower,
double known_index_upper);
+ // Initializes a sensor simulation with a single relative enoder, providing
+ // position information relative to a starting position of 0. For use with
+ // RelativePosition sensor values.
+ void InitializeRelativeEncoder();
+
// Simulate the structure moving to a new position. The new value is measured
// relative to absolute zero. This will update the simulated sensors with new
// readings.
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index 46bc9fc..e4b2f25 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -197,6 +197,42 @@
estimator_state:frc971.AbsoluteEncoderEstimatorState;
}
+table RelativeEncoderProfiledJointStatus {
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.RelativeEncoderEstimatorState;
+}
+
table StaticZeroingSingleDOFProfiledSubsystemGoal {
unsafe_goal:double;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 1c6bb77..fe1b461 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -6,10 +6,9 @@
#include <cstdint>
#include <vector>
-#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/constants.h"
-
#include "flatbuffers/flatbuffers.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/control_loops_generated.h"
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
// away from the last one (i.e. got extra counts from noise, etc..)
@@ -29,7 +28,7 @@
using Position = TPosition;
using ZeroingConstants = TZeroingConstants;
using State = TState;
- virtual ~ZeroingEstimator(){}
+ virtual ~ZeroingEstimator() {}
// Returns true if the logic considers the corresponding mechanism to be
// zeroed.
@@ -61,9 +60,10 @@
// Estimates the position with an incremental encoder with an index pulse and a
// potentiometer.
-class PotAndIndexPulseZeroingEstimator : public ZeroingEstimator<PotAndIndexPosition,
- constants::PotAndIndexPulseZeroingConstants,
- EstimatorState> {
+class PotAndIndexPulseZeroingEstimator
+ : public ZeroingEstimator<PotAndIndexPosition,
+ constants::PotAndIndexPulseZeroingConstants,
+ EstimatorState> {
public:
explicit PotAndIndexPulseZeroingEstimator(
const constants::PotAndIndexPulseZeroingConstants &constants);
@@ -148,10 +148,11 @@
// Estimates the position with an incremental encoder and a hall effect sensor.
class HallEffectAndPositionZeroingEstimator
: public ZeroingEstimator<HallEffectAndPosition,
- constants::HallEffectZeroingConstants,
- HallEffectAndPositionEstimatorState> {
+ constants::HallEffectZeroingConstants,
+ HallEffectAndPositionEstimatorState> {
public:
- explicit HallEffectAndPositionZeroingEstimator(const ZeroingConstants &constants);
+ explicit HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants);
// Update the internal logic with the next sensor values.
void UpdateEstimate(const Position &info) override;
@@ -372,9 +373,10 @@
// Zeros by seeing all the index pulses in the range of motion of the mechanism
// and using that to figure out which index pulse is which.
-class PulseIndexZeroingEstimator : public ZeroingEstimator<IndexPosition,
- constants::EncoderPlusIndexZeroingConstants,
- IndexEstimatorState> {
+class PulseIndexZeroingEstimator
+ : public ZeroingEstimator<IndexPosition,
+ constants::EncoderPlusIndexZeroingConstants,
+ IndexEstimatorState> {
public:
explicit PulseIndexZeroingEstimator(const ZeroingConstants &constants)
: constants_(constants) {
@@ -516,6 +518,47 @@
double position_ = 0.0;
};
+class RelativeEncoderZeroingEstimator
+ : public ZeroingEstimator<RelativePosition, void,
+ RelativeEncoderEstimatorState> {
+ public:
+ explicit RelativeEncoderZeroingEstimator() {}
+
+ // Update position with new position from encoder
+ void UpdateEstimate(const RelativePosition &position) override {
+ position_ = position.encoder();
+ }
+
+ // We alre always zeroed
+ bool zeroed() const override { return true; }
+
+ // Starting position of the joint
+ double offset() const override { return 0; }
+
+ // Has an error occured? Note: Only triggered by TriggerError()
+ bool error() const override { return error_; }
+
+ // Offset is always ready, since we are always zeroed.
+ bool offset_ready() const override { return true; }
+
+ void TriggerError() override { error_ = true; }
+
+ void Reset() override { error_ = false; }
+
+ flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_position(position_);
+ return builder.Finish();
+ }
+
+ private:
+ // Position from encoder relative to start
+ double position_ = 0;
+ bool error_ = false;
+};
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 61d2d0c..2b715ea 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -1,14 +1,14 @@
+#include "frc971/zeroing/zeroing.h"
+
#include <unistd.h>
#include <memory>
-
#include <random>
-#include "gtest/gtest.h"
-#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/zeroing/zeroing.h"
#include "aos/die.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
+#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
@@ -72,6 +72,14 @@
*simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ RelativeEncoderZeroingEstimator *estimator, double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<RelativePosition>(&fbb));
+ }
+
template <typename T>
double GetEstimatorPosition(T *estimator) {
FBB fbb;
@@ -789,5 +797,21 @@
ASSERT_TRUE(estimator.error());
}
+TEST_F(ZeroingTest, TestRelativeEncoderZeroingWithoutMovement) {
+ PositionSensorSimulator sim(1.0);
+ RelativeEncoderZeroingEstimator estimator;
+
+ sim.InitializeRelativeEncoder();
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.offset_ready());
+ EXPECT_DOUBLE_EQ(estimator.offset(), 0.0);
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.0);
+
+ MoveTo(&sim, &estimator, 0.1);
+
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.1);
+}
+
} // namespace zeroing
} // namespace frc971