Pot + absolute encoder zeroing class
This class takes a pot, absolute encoder, and relative encoder and uses
them to compute the offset. It doesn't yet detect errors or wait until
stopped.
Change-Id: I74bc2031132974d9f3e2e6ddf93b954384a6ce2c
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index f13d82b..f3d224b 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -44,7 +44,7 @@
// arbitrary 0 position which varies with each robot.
struct PotAndAbsolutePosition {
// Current position read from each encoder.
- double relative_encoder;
+ double encoder;
double absolute_encoder;
// Current position read from the potentiometer.
@@ -64,6 +64,19 @@
double pot_position;
};
+// The internal state of a zeroing estimator.
+struct AbsoluteEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ bool error;
+ // If the joint has seen an index pulse and is zeroed.
+ bool zeroed;
+ // The estimated position of the joint.
+ double position;
+
+ // The estimated position not using the index pulse.
+ double pot_position;
+};
+
// A left/right pair of PotAndIndexPositions.
struct PotAndIndexPair {
PotAndIndexPosition left;
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 1d06974..9412ea3 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -145,12 +145,15 @@
void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
- values->relative_encoder = cur_pos_ - start_position_;
+ values->encoder = cur_pos_ - start_position_;
// TODO(phil): Create some lag here since this is a PWM signal it won't be
// instantaneous like the other signals. Better yet, its lag varies
// randomly with the distribution varying depending on the reading.
values->absolute_encoder =
- fmod(cur_pos_ - known_index_pos_ + known_absolute_encoder_, index_diff_);
+ ::std::remainder(cur_pos_ + known_absolute_encoder_, index_diff_);
+ if (values->absolute_encoder < 0) {
+ values->absolute_encoder += index_diff_;
+ }
}
} // namespace control_loops
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 0c7fa0e..4a3ac1e 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -267,37 +267,37 @@
sim.MoveTo(0.20);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.00, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(0.00, position.encoder);
EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
sim.MoveTo(0.30);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.10, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(0.10, position.encoder);
EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
sim.MoveTo(0.40);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.20, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(0.20, position.encoder);
EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
sim.MoveTo(0.34);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.14, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(0.14, position.encoder);
EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
sim.MoveTo(0.24);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.04, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(0.04, position.encoder);
EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
sim.MoveTo(0.23);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.03, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(0.03, position.encoder);
EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
sim.MoveTo(0.13);
sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(-0.07, position.relative_encoder);
+ EXPECT_DOUBLE_EQ(-0.07, position.encoder);
EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
// TODO(philipp): Test negative values.