sensor_sim: Add documentation and clear up some issues.
The sensor simulator turned somewhat into a mess, largely because of
two reasons:
- Many people disagreeing on what parameters should mean etc.
- No tests to verify the absence of bugs.
This commit aims to fix that by cleaning up some of the code, adding
a bunch of documentation and most importantly by adding a test for
the class.
The `position_sensor_sim.cc' file now contains an explanation along
with an ASCII diagram of what problem the class is meant to solve.
Change-Id: I34d203976ceefead50096ddc0d0fcfef12af6d87
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 6e882e1..785f1be 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -36,6 +36,18 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'position_sensor_sim_test',
+ 'type': 'executable',
+ 'sources': [
+ 'position_sensor_sim_test.cc',
+ ],
+ 'dependencies': [
+ 'queues',
+ 'position_sensor_sim',
+ '<(EXTERNALS):gtest',
+ ],
+ },
+ {
'target_name': 'position_sensor_sim',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 4f112a9..3ae5117 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -1,55 +1,91 @@
#include "frc971/control_loops/position_sensor_sim.h"
+#include <cmath>
+
namespace frc971 {
namespace control_loops {
-PositionSensorSimulator::PositionSensorSimulator(double offset,
+/* Index pulse/segment Explanation:
+ *
+ * The index segments are labelled starting at zero and go up. Each index
+ * segment is the space between the two bordering index pulses. The length of
+ * each index segment is determined by the `index_diff` variable in the
+ * 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.
+ *
+ * index segment
+ * |
+ * V
+ *
+ * +--- 0---+--- 1---+--- 2---+--- 3---+--- 4---+--- 5---+--- 6---+
+ *
+ * | | | | | | | |
+ * 0 1 2 3 4 5 6 7
+ *
+ * A
+ * |
+ * index pulse
+ */
+
+PositionSensorSimulator::PositionSensorSimulator(double start_position,
double index_diff,
double pot_noise_stddev)
- : index_diff_(index_diff),
- pot_noise_(0, pot_noise_stddev) {
- OverrideParams(offset, pot_noise_stddev);
+ : index_diff_(index_diff), pot_noise_(0, pot_noise_stddev) {
+ OverrideParams(start_position, pot_noise_stddev);
}
-void PositionSensorSimulator::OverrideParams(double offset,
+void PositionSensorSimulator::OverrideParams(double start_position,
double pot_noise_stddev) {
- cur_index_segment_ = 0;
+ cur_index_segment_ = floor(start_position / index_diff_);
cur_index_ = 0;
index_count_ = 0;
- cur_pos_ = 0;
- offset_ = offset;
+ cur_pos_ = start_position;
+ start_position_ = start_position;
pot_noise_.set_standard_deviation(pot_noise_stddev);
}
void PositionSensorSimulator::MoveTo(double new_pos) {
- // Which index pulse we're on.
- const int new_index = static_cast<int>((new_pos + offset_) / index_diff_);
+ // 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_);
- if (new_index < cur_index_segment_) {
- // Position is decreasing.
- cur_index_ = new_index + 1;
+ 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 > cur_index_segment_) {
- // Position is increasing.
- cur_index_ = new_index;
+ } 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_++;
}
- cur_index_segment_ = new_index;
+ cur_index_segment_ = new_index_segment;
cur_pos_ = new_pos;
}
void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
- values->pot = cur_pos_ + offset_;
- values->pot = pot_noise_.AddNoiseToSample(values->pot);
- values->encoder = cur_pos_;
+ values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
+ values->encoder = cur_pos_ - start_position_;
if (index_count_ == 0) {
values->latched_pot = 0.0;
values->latched_encoder = 0.0;
} else {
- values->latched_pot = cur_index_ * index_diff_;
- values->latched_encoder = cur_index_ * index_diff_;
+ // Determine the position of the index pulse relative to absolute zero.
+ double index_pulse_position = cur_index_ * index_diff_;
+
+ // Populate the latched pot/encoder samples.
+ values->latched_pot = pot_noise_.AddNoiseToSample(index_pulse_position);
+ values->latched_encoder = index_pulse_position - start_position_;
}
values->index_pulses = index_count_;
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index dc080de..9975a78 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -12,43 +12,56 @@
class PositionSensorSimulator {
public:
- // offset: The difference between zero on the pot and an index pulse.
- // index_diff: The interval between index pulses.
+ // start_position: The position relative to absolute zero where the simulated
+ // structure starts. For example, to simulate the elevator
+ // starting at 40cm above absolute zero, set this to 0.4.
+ // index_diff: The interval between index pulses. This is measured in SI
+ // units. For example, if an index pulse hits every 5cm on the
+ // elevator, set this to 0.05.
// pot_noise_stddev: The pot noise is sampled from a gaussian distribution.
// This specifies the standard deviation of that
// distribution.
// 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 offset, double index_diff,
+ PositionSensorSimulator(double start_position, double index_diff,
double pot_noise_stddev);
// Set new parameters for the sensors. This is useful for unit tests to change
// the simulated sensors' behavior on the fly.
- void OverrideParams(double start_pos, double pot_noise_stddev);
+ void OverrideParams(double start_position, double pot_noise_stddev);
- // Change sensors to a new position.
- // new_pos: The new position. (This is an absolute position.)
- void MoveTo(double new_pos);
+ // 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.
+ // new_position: The new position relative to absolute zero.
+ void MoveTo(double new_position);
- // Get the current values of the sensors.
- // values: These will be filled in.
+ // 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);
private:
- // Which index pulse we are currently on relative to the one we started on.
+ // 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 the
- // one we started on.
+ // 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_;
// Distance between index pulses on the mechanism.
double index_diff_;
- // Current encoder value.
+ // Current position of the mechanism relative to absolute zero.
double cur_pos_;
- // Difference between zero on the pot and zero on the encoder.
- double offset_;
+ // 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_;
};
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
new file mode 100644
index 0000000..7374e2d
--- /dev/null
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -0,0 +1,104 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include <random>
+
+#include "gtest/gtest.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "aos/common/die.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class PositionSensorSimTest : public ::testing::Test {
+ protected:
+ PositionSensorSimTest() {}
+};
+
+TEST_F(PositionSensorSimTest, NoIndices) {
+ // We'll simulate a potentiometer with no noise so that we can accurately
+ // verify where the mechanism currently is. Overall though, the purpose of
+ // this test is to verify that no false index pulses are generated while the
+ // mechanism stays between two index pulses.
+ const double index_diff = 0.5;
+ PotAndIndexPosition position;
+ PositionSensorSimulator sim(3.6 * index_diff, index_diff, 0);
+
+ // Make sure that we don't accidentally hit an index pulse.
+ for (int i = 0; i < 30; i++) {
+ 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);
+ }
+
+ 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);
+ }
+
+ 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);
+ }
+
+ 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);
+ }
+}
+
+TEST_F(PositionSensorSimTest, CountIndices) {
+ // The purpose of this test is to verify that the simulator latches the
+ // correct index pulse when transitioning from one segment to another. We
+ // again simulate zero noise on the potentiometer to accurately verify the
+ // mechanism's position during the index pulses.
+ const double index_diff = 0.8;
+ PotAndIndexPosition position;
+ PositionSensorSimulator sim(4.6 * index_diff, index_diff, 0);
+
+ // Make sure that we get an index pulse on every transition.
+ sim.GetSensorValues(&position);
+ ASSERT_EQ(static_cast<unsigned int>(0), 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);
+
+ 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);
+
+ 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);
+
+ 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);
+
+ 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);
+
+ 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);
+}
+
+} // namespace control_loops
+} // namespace frc971