Make sensor sim able to not have index pulse at 0.
There is a argument to the constructor which allows you to tell it
an absolute position where you want an index pulse.
Change-Id: I32a1c23d4c25257f545cca4de8bb8e4b3cff0aae
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 421d6c6..5141327 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -13,8 +13,7 @@
* constructor below.
*
* The index pulses are encountered when the mechanism moves from one index
- * segment to another. Currently the first index pulse is limited to occur at
- * absolute zero.
+ * segment to another.
*
* index segment
* |
@@ -31,13 +30,17 @@
*/
PositionSensorSimulator::PositionSensorSimulator(double index_diff)
- : index_diff_(index_diff), pot_noise_(0, 0.0) {
+ : index_diff_(index_diff),
+ pot_noise_(0, 0.0) {
Initialize(0.0, 0.0);
}
void PositionSensorSimulator::Initialize(double start_position,
- double pot_noise_stddev) {
- cur_index_segment_ = floor(start_position / index_diff_);
+ double pot_noise_stddev,
+ double known_index_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;
cur_index_ = 0;
index_count_ = 0;
cur_pos_ = start_position;
@@ -48,7 +51,8 @@
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 / index_diff_);
+ const int new_index_segment = floor((new_pos - known_index_pos_)
+ / index_diff_);
if (new_index_segment < cur_index_segment_) {
// We've crossed an index pulse in the negative direction. That means the
@@ -79,7 +83,7 @@
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_;
+ double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
// Populate the latched pot/encoder samples.
values->latched_pot = pot_noise_.AddNoiseToSample(index_pulse_position);
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 94db806..19a9112 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -16,8 +16,6 @@
// units. For example, if an index pulse hits every 5cm on the
// elevator, set this to 0.05.
// TODO(danielp): Allow for starting with a non-zero encoder value.
- // TODO(danielp): Allow for the first index pulse to be at a non-zero
- // position.
PositionSensorSimulator(double index_diff);
// Set new parameters for the sensors. This is useful for unit tests to change
@@ -28,7 +26,10 @@
// pot_noise_stddev: The pot noise is sampled from a gaussian distribution.
// This specifies the standard deviation of that
// distribution.
- void Initialize(double start_position, double pot_noise_stddev);
+ // known_index_pos: The absolute position of an index pulse.
+ void Initialize(double start_position,
+ double pot_noise_stddev,
+ double known_index_pos = 0.0);
// 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
@@ -56,6 +57,8 @@
int index_count_;
// Distance between index pulses on the mechanism.
double index_diff_;
+ // Absolute position of a known index pulse.
+ double known_index_pos_;
// Current position of the mechanism relative to absolute zero.
double cur_pos_;
// Starting position of the mechanism relative to absolute zero. See the
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 37f38f4..0c9d3a9 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -32,28 +32,28 @@
sim.MoveTo(3.6 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(3.6 * index_diff, position.pot);
- ASSERT_EQ(static_cast<unsigned int>(0), position.index_pulses);
+ ASSERT_EQ(0u, position.index_pulses);
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(3.0 * index_diff, position.pot);
- ASSERT_EQ(static_cast<unsigned int>(0), position.index_pulses);
+ ASSERT_EQ(0u, position.index_pulses);
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.99 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(3.99 * index_diff, position.pot);
- ASSERT_EQ(static_cast<unsigned int>(0), position.index_pulses);
+ ASSERT_EQ(0u, position.index_pulses);
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(3.0 * index_diff, position.pot);
- ASSERT_EQ(static_cast<unsigned int>(0), position.index_pulses);
+ ASSERT_EQ(0u, position.index_pulses);
}
}
@@ -69,37 +69,85 @@
// Make sure that we get an index pulse on every transition.
sim.GetSensorValues(&position);
- ASSERT_EQ(static_cast<unsigned int>(0), position.index_pulses);
+ ASSERT_EQ(0u, position.index_pulses);
sim.MoveTo(3.6 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
- ASSERT_EQ(static_cast<unsigned int>(1), position.index_pulses);
+ ASSERT_EQ(1u, position.index_pulses);
sim.MoveTo(4.5 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
- ASSERT_EQ(static_cast<unsigned int>(2), position.index_pulses);
+ ASSERT_EQ(2u, position.index_pulses);
sim.MoveTo(5.9 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(5.0 * index_diff, position.latched_pot);
- ASSERT_EQ(static_cast<unsigned int>(3), position.index_pulses);
+ ASSERT_EQ(3u, position.index_pulses);
sim.MoveTo(6.1 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(6.0 * index_diff, position.latched_pot);
- ASSERT_EQ(static_cast<unsigned int>(4), position.index_pulses);
+ ASSERT_EQ(4u, position.index_pulses);
sim.MoveTo(8.7 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
- ASSERT_EQ(static_cast<unsigned int>(5), position.index_pulses);
+ ASSERT_EQ(5u, position.index_pulses);
sim.MoveTo(7.3 * index_diff);
sim.GetSensorValues(&position);
ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
- ASSERT_EQ(static_cast<unsigned int>(6), position.index_pulses);
+ ASSERT_EQ(6u, position.index_pulses);
+}
+
+// Tests that the simulator handles non-zero specified index pulse locations
+// correctly.
+TEST_F(PositionSensorSimTest, NonZeroIndexLocation) {
+ const double index_diff = 0.5;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(index_diff * 0.25, 0.0, index_diff * 0.5);
+ PotAndIndexPosition position;
+
+ sim.MoveTo(0.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+
+ sim.MoveTo(index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+
+ sim.MoveTo(1.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(2u, position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
+
+ // Try it with our known index pulse not being our first one.
+ sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
+
+ sim.MoveTo(0.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+
+ sim.MoveTo(index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+
+ sim.MoveTo(1.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(2u, position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
}
} // namespace control_loops