Added HallEffectAndPosition
Added the queue message and taught PositionSensorSim how to
populate it.
Change-Id: Ic6be75cebf3f51ff6b3a4162cb3f0800f1aa94e7
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 4d87587..757e257 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -113,6 +113,21 @@
double negedge_value;
};
+// Records the hall effect sensor and encoder values.
+struct HallEffectAndPosition {
+ // The current hall effect state.
+ bool current;
+ // The current encoder position.
+ double position;
+ // The number of positive and negative edges we've seen on the hall effect
+ // sensor.
+ int32_t posedge_count;
+ int32_t negedge_count;
+ // The values corresponding to the last hall effect sensor reading.
+ double posedge_value;
+ double negedge_value;
+};
+
// Records the positions for a mechanism with edge-capturing sensors on it.
struct HallEffectPositions {
double current;
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 9412ea3..12e72cb 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -56,105 +56,132 @@
* remainder of the graph.
*/
-PositionSensorSimulator::PositionSensorSimulator(double index_diff,
+PositionSensorSimulator::PositionSensorSimulator(double index_difference,
unsigned int noise_seed)
- : index_diff_(index_diff), pot_noise_(noise_seed, 0.0) {
+ : lower_index_edge_(index_difference, noise_seed),
+ upper_index_edge_(index_difference, noise_seed),
+ index_difference_(index_difference) {
Initialize(0.0, 0.0);
}
void PositionSensorSimulator::Initialize(
double start_position, double pot_noise_stddev,
- double known_index_pos /* = 0*/,
+ double known_index_position /* = 0*/,
double known_absolute_encoder_pos /* = 0*/) {
- // We're going to make the index pulse we know "segment zero".
- cur_index_segment_ = floor((start_position - known_index_pos) / index_diff_);
- known_index_pos_ = known_index_pos;
+ InitializeHallEffectAndPosition(start_position, known_index_position,
+ known_index_position);
+
known_absolute_encoder_ = known_absolute_encoder_pos;
- cur_index_ = 0;
- index_count_ = 0;
- cur_pos_ = start_position;
- start_position_ = start_position;
- pot_noise_.set_standard_deviation(pot_noise_stddev);
+
+ lower_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
+ upper_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
}
-void PositionSensorSimulator::MoveTo(double new_pos) {
- // Compute which index segment we're in. In other words, compute between
- // which two index pulses we are.
- const int new_index_segment =
- floor((new_pos - known_index_pos_) / index_diff_);
+void PositionSensorSimulator::InitializeHallEffectAndPosition(
+ double start_position, double known_index_lower, double known_index_upper) {
+ current_position_ = start_position;
+ start_position_ = start_position;
- if (new_index_segment < cur_index_segment_) {
- // We've crossed an index pulse in the negative direction. That means the
- // index pulse we just crossed is the higher end of the current index
- // segment. For example, if the mechanism moved from index segment four to
- // index segment three, then we just crossed index pulse 4.
- cur_index_ = new_index_segment + 1;
- index_count_++;
- } else if (new_index_segment > cur_index_segment_) {
- // We've crossed an index pulse in the positive direction. That means the
- // index pulse we just crossed is the lower end of the index segment. For
- // example, if the mechanism moved from index segment seven to index
- // segment eight, then we just crossed index pulse eight.
- cur_index_ = new_index_segment;
- index_count_++;
+ lower_index_edge_.Initialize(start_position, known_index_lower);
+ upper_index_edge_.Initialize(start_position, known_index_upper);
+
+ posedge_count_ = 0;
+ negedge_count_ = 0;
+ posedge_value_ = start_position;
+ negedge_value_ = start_position;
+}
+
+void PositionSensorSimulator::MoveTo(double new_position) {
+ {
+ const int lower_start_segment = lower_index_edge_.current_index_segment();
+ lower_index_edge_.MoveTo(new_position);
+ const int lower_end_segment = lower_index_edge_.current_index_segment();
+ if (lower_end_segment > lower_start_segment) {
+ // Moving up past the lower edge.
+ ++posedge_count_;
+ posedge_value_ = lower_index_edge_.IndexPulsePosition();
+ }
+ if (lower_end_segment < lower_start_segment) {
+ // Moved down.
+ ++negedge_count_;
+ negedge_value_ = lower_index_edge_.IndexPulsePosition();
+ }
}
- if (new_index_segment != cur_index_segment_) {
- latched_pot_ = pot_noise_.AddNoiseToSample(cur_index_ * index_diff_ +
- known_index_pos_);
+ {
+ const int upper_start_segment = upper_index_edge_.current_index_segment();
+ upper_index_edge_.MoveTo(new_position);
+ const int upper_end_segment = upper_index_edge_.current_index_segment();
+ if (upper_end_segment > upper_start_segment) {
+ // Moving up past the upper edge.
+ ++negedge_count_;
+ negedge_value_ = upper_index_edge_.IndexPulsePosition();
+ }
+ if (upper_end_segment < upper_start_segment) {
+ // Moved down.
+ ++posedge_count_;
+ posedge_value_ = upper_index_edge_.IndexPulsePosition();
+ }
}
- cur_index_segment_ = new_index_segment;
- cur_pos_ = new_pos;
+ current_position_ = new_position;
}
void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
- values->encoder = cur_pos_ - start_position_;
+ values->encoder = current_position_ - start_position_;
- if (index_count_ == 0) {
+ values->index_pulses = lower_index_edge_.index_count();
+ if (values->index_pulses == 0) {
values->latched_encoder = 0.0;
} else {
- // Determine the position of the index pulse relative to absolute zero.
- double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
-
// Populate the latched encoder samples.
- values->latched_encoder = index_pulse_position - start_position_;
+ values->latched_encoder =
+ lower_index_edge_.IndexPulsePosition() - start_position_;
}
-
- values->index_pulses = index_count_;
}
void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
- values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
- values->encoder = cur_pos_ - start_position_;
+ values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_);
+ values->encoder = current_position_ - start_position_;
- if (index_count_ == 0) {
+ if (lower_index_edge_.index_count() == 0) {
values->latched_pot = 0.0;
values->latched_encoder = 0.0;
} else {
- // Determine the position of the index pulse relative to absolute zero.
- double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
-
// Populate the latched pot/encoder samples.
- values->latched_pot = latched_pot_;
- values->latched_encoder = index_pulse_position - start_position_;
+ values->latched_pot = lower_index_edge_.latched_pot();
+ values->latched_encoder =
+ lower_index_edge_.IndexPulsePosition() - start_position_;
}
- values->index_pulses = index_count_;
+ values->index_pulses = lower_index_edge_.index_count();
}
void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
- values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
- values->encoder = cur_pos_ - start_position_;
+ values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_);
+ 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(cur_pos_ + known_absolute_encoder_, index_diff_);
+ values->absolute_encoder = ::std::remainder(
+ current_position_ + known_absolute_encoder_, index_difference_);
if (values->absolute_encoder < 0) {
- values->absolute_encoder += index_diff_;
+ values->absolute_encoder += index_difference_;
}
}
+void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
+ values->current = lower_index_edge_.current_index_segment() !=
+ upper_index_edge_.current_index_segment();
+ values->position = current_position_ - start_position_;
+
+ values->posedge_count = posedge_count_;
+ values->negedge_count = negedge_count_;
+ values->posedge_value = posedge_value_ - start_position_;
+ values->negedge_value = negedge_value_ - start_position_;
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 3de96c1..6e4f017 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -22,9 +22,9 @@
// interval between when the absolute encoder reads 0.
// noise_seed: The seed to feed into the random number generator for the
// potentiometer values.
- // TODO(danielp): Allow for starting with a non-zero encoder value.
- PositionSensorSimulator(double index_diff, unsigned int noise_seed =
- ::aos::testing::RandomSeed());
+ PositionSensorSimulator(
+ double index_difference,
+ unsigned int noise_seed = ::aos::testing::RandomSeed());
// Set new parameters for the sensors. This is useful for unit tests to change
// the simulated sensors' behavior on the fly.
@@ -35,11 +35,17 @@
// This specifies the standard deviation of that
// distribution.
// known_index_pos: The absolute position of an index pulse.
- void Initialize(double start_position,
- double pot_noise_stddev,
+ void Initialize(double start_position, double pot_noise_stddev,
double known_index_pos = 0.0,
double known_absolute_encoder_pos = 0.0);
+ // Initializes a sensor simulation which is pretending to be a hall effect +
+ // encoder setup. This is assuming that the hall effect sensor triggers once
+ // per cycle, and a cycle is index_difference_ long;
+ void InitializeHallEffectAndPosition(double start_position,
+ double known_index_lower,
+ double known_index_upper);
+
// 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.
@@ -50,51 +56,112 @@
// values: The target structure will be populated with simulated sensor
// readings. The readings will be in SI units. For example the units
// can be given in radians, meters, etc.
- void GetSensorValues(IndexPosition* values);
-
- // Get the current values of the simulated sensors.
- // values: The target structure will be populated with simulated sensor
- // readings. The readings will be in SI units. For example the units
- // can be given in radians, meters, etc.
- void GetSensorValues(PotAndIndexPosition* values);
-
- // Get the current values of the simulated sensors.
- // values: The target structure will be populated with simulated sensor
- // readings. The readings will be in SI units. For example the units
- // can be given in radians, meters, etc.
- void GetSensorValues(PotAndAbsolutePosition* values);
+ void GetSensorValues(IndexPosition *values);
+ void GetSensorValues(PotAndIndexPosition *values);
+ void GetSensorValues(PotAndAbsolutePosition *values);
+ void GetSensorValues(HallEffectAndPosition *values);
private:
- // The absolute segment between two index pulses the simulation is on. For
- // example, when the current position is betwen index pulse zero and one,
- // the current index segment is considered to be zero. Index segment one is
- // between index pulses 1 and 2, etc.
- int cur_index_segment_;
- // Index pulse to use for calculating latched sensor values, relative to
- // absolute zero. In other words this always holds the index pulse that was
- // encountered most recently.
- int cur_index_;
- // How many index pulses we've seen.
- int index_count_;
- // The pot position at the most recent index pulse with noise added.
- double latched_pot_;
+ // It turns out that we can re-use a lot of the same logic to count the index
+ // pulses as we can use to count the hall effect edges. The trick is to add 2
+ // "index pulses" spaced apart by the width of the hall effect sensor, and
+ // call the sensor "high" when the index segments disagree for the two
+ // IndexEdge classes disagree.
+ class IndexEdge {
+ public:
+ explicit IndexEdge(double index_difference, unsigned int noise_seed)
+ : index_difference_(index_difference), pot_noise_(noise_seed, 0.0) {}
+
+ void Initialize(double start_position, double segment_position) {
+ current_index_segment_ =
+ ::std::floor((start_position - segment_position) / index_difference_);
+ known_index_position_ = segment_position;
+ last_index_ = 0;
+ index_count_ = 0;
+ }
+
+ double index_count() const { return index_count_; }
+ double latched_pot() const { return latched_pot_; }
+ int current_index_segment() const { return current_index_segment_; }
+
+ double IndexPulsePosition() const {
+ return last_index_ * index_difference_ + known_index_position_;
+ }
+
+ void MoveTo(double new_position) {
+ const int new_index_segment = ::std::floor(
+ (new_position - known_index_position_) / index_difference_);
+
+ if (new_index_segment < current_index_segment_) {
+ // We've crossed an index pulse in the negative direction. That means
+ // the index pulse we just crossed is the higher end of the current
+ // index segment. For example, if the mechanism moved from index segment
+ // four to index segment three, then we just crossed index pulse 4.
+ last_index_ = new_index_segment + 1;
+ index_count_++;
+ } else if (new_index_segment > current_index_segment_) {
+ // We've crossed an index pulse in the positive direction. That means
+ // the index pulse we just crossed is the lower end of the index
+ // segment. For example, if the mechanism moved from index segment seven
+ // to index segment eight, then we just crossed index pulse eight.
+ last_index_ = new_index_segment;
+ index_count_++;
+ }
+
+ if (new_index_segment != current_index_segment_) {
+ latched_pot_ = pot_noise_.AddNoiseToSample(
+ last_index_ * index_difference_ + known_index_position_);
+ }
+
+ current_index_segment_ = new_index_segment;
+ }
+
+ GaussianNoise *mutable_pot_noise() { return &pot_noise_; }
+
+ private:
+ // The absolute segment between two index pulses the simulation is on. For
+ // example, when the current position is betwen index pulse zero and one,
+ // the current index segment is considered to be zero. Index segment one is
+ // between index pulses 1 and 2, etc.
+ int current_index_segment_;
+ // Index pulse to use for calculating latched sensor values, relative to
+ // absolute zero. In other words this always holds the index pulse that was
+ // encountered most recently.
+ int last_index_;
+ // How many index pulses we've seen.
+ int index_count_;
+
+ // Absolute position of a known index pulse.
+ double known_index_position_;
+ // Distance between index pulses on the mechanism.
+ double index_difference_;
+
+ // The pot position at the most recent index pulse with noise added.
+ double latched_pot_;
+
+ // Gaussian noise to add to pot readings.
+ GaussianNoise pot_noise_;
+ };
+
+ IndexEdge lower_index_edge_;
+ IndexEdge upper_index_edge_;
+
// Distance between index pulses on the mechanism.
- double index_diff_;
- // Absolute position of a known index pulse.
- // OR
- // Absolute position of the absolute encoder's reading stored in
- // known_absolute_encoder_.
- double known_index_pos_;
+ double index_difference_;
+
// The readout of the absolute encoder when the robot's mechanism is at
- // known_index_pos_.
+ // zero.
double known_absolute_encoder_;
// Current position of the mechanism relative to absolute zero.
- double cur_pos_;
+ double current_position_;
// Starting position of the mechanism relative to absolute zero. See the
// `starting_position` parameter in the constructor for more info.
double start_position_;
- // Gaussian noise to add to pot readings.
- GaussianNoise pot_noise_;
+
+ int posedge_count_;
+ int negedge_count_;
+ double posedge_value_;
+ double negedge_value_;
};
} // namespace control_loops
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 4a3ac1e..b8addc1 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -303,5 +303,75 @@
// TODO(philipp): Test negative values.
}
+// Tests that we get the right number of edges with the HallEffectAndPosition
+// sensor value.
+TEST_F(PositionSensorSimTest, HallEffectAndPosition) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+ sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
+ HallEffectAndPosition position;
+
+ // Go over only the lower edge rising.
+ sim.MoveTo(0.25);
+ sim.GetSensorValues(&position);
+ EXPECT_TRUE(position.current);
+ EXPECT_DOUBLE_EQ(0.50, position.position);
+ EXPECT_EQ(1, position.posedge_count);
+ EXPECT_EQ(0.25, position.posedge_value);
+ EXPECT_EQ(0, position.negedge_count);
+ EXPECT_EQ(0, position.negedge_value);
+
+ // Now, go over the upper edge, falling.
+ sim.MoveTo(0.75);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(1.0, position.position);
+ EXPECT_EQ(1, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.25, position.posedge_value);
+ EXPECT_EQ(1, position.negedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.negedge_value);
+
+ // Now, jump a whole cycle.
+ sim.MoveTo(1.75);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(2.0, position.position);
+ EXPECT_EQ(2, position.posedge_count);
+ EXPECT_DOUBLE_EQ(1.25, position.posedge_value);
+ EXPECT_EQ(2, position.negedge_count);
+ EXPECT_DOUBLE_EQ(1.75, position.negedge_value);
+
+ // Now, jump a whole cycle backwards.
+ sim.MoveTo(0.75);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(1.0, position.position);
+ EXPECT_EQ(3, position.posedge_count);
+ EXPECT_DOUBLE_EQ(1.75, position.posedge_value);
+ EXPECT_EQ(3, position.negedge_count);
+ EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+
+ // Now, go over the upper edge, rising.
+ sim.MoveTo(0.25);
+ sim.GetSensorValues(&position);
+ EXPECT_TRUE(position.current);
+ EXPECT_DOUBLE_EQ(0.5, position.position);
+ EXPECT_EQ(4, position.posedge_count);
+ EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
+ EXPECT_EQ(3, position.negedge_count);
+ EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+
+ // Now, go over the lower edge, falling.
+ sim.MoveTo(-0.25);
+ sim.GetSensorValues(&position);
+ EXPECT_FALSE(position.current);
+ EXPECT_DOUBLE_EQ(0.0, position.position);
+ 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);
+}
+
+
} // namespace control_loops
} // namespace frc971