Add single mag encoder zeroing method
This assumes the mag encoder doesn't move more than 1 revolution.
Change-Id: I932f4ea0e6457a4ac3430c35b7bfda7e6eb1b5c4
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index bebecd0..44eb262 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -51,6 +51,16 @@
double pot;
};
+// Represents all of the data for an absolute and relative encoder pair.
+// The units on all of the positions are the same.
+// The relative encoder values are relative to where the encoder was at some
+// arbitrary point in time.
+struct AbsolutePosition {
+ // Current position read from each encoder.
+ double encoder;
+ double absolute_encoder;
+};
+
// The internal state of a zeroing estimator.
struct EstimatorState {
// If true, there has been a fatal error for the estimator.
@@ -82,6 +92,20 @@
};
// The internal state of a zeroing estimator.
+struct AbsoluteEncoderEstimatorState {
+ // 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 absolute position of the encoder. This is filtered, so it
+ // can be easily used when zeroing.
+ double absolute_position;
+};
+
+// The internal state of a zeroing estimator.
struct IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
bool error;
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 97a96d5..c875a0b 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -172,6 +172,18 @@
}
}
+void PositionSensorSimulator::GetSensorValues(AbsolutePosition *values) {
+ values->encoder = current_position_ - 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 = ::std::remainder(
+ current_position_ + known_absolute_encoder_, distance_per_revolution_);
+ if (values->absolute_encoder < 0) {
+ values->absolute_encoder += distance_per_revolution_;
+ }
+}
+
void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
values->current = lower_index_edge_.current_index_segment() !=
upper_index_edge_.current_index_segment();
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index b8a214b..2880429 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -60,6 +60,7 @@
// can be given in radians, meters, etc.
void GetSensorValues(IndexPosition *values);
void GetSensorValues(PotAndIndexPosition *values);
+ void GetSensorValues(AbsolutePosition *values);
void GetSensorValues(PotAndAbsolutePosition *values);
void GetSensorValues(HallEffectAndPosition *values);