Add position_sensor_sim class.
This currently contains a class capable of simulating an
indexed encoder and a potentiometer with gaussian noise.
This is mostly Philipp's code, with some modifications by me.
Change-Id: I77925f51916934de02eed24f0de8d6ab51626ba2
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 1976850..6e882e1 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -36,6 +36,24 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'position_sensor_sim',
+ 'type': 'static_library',
+ 'sources': [
+ 'position_sensor_sim.cc',
+ ],
+ 'dependencies': [
+ 'queues',
+ 'gaussian_noise',
+ ],
+ },
+ {
+ 'target_name': 'gaussian_noise',
+ 'type': 'static_library',
+ 'sources': [
+ 'gaussian_noise.cc',
+ ],
+ },
+ {
'target_name': 'coerce_goal',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/gaussian_noise.cc b/frc971/control_loops/gaussian_noise.cc
new file mode 100644
index 0000000..c1c8701
--- /dev/null
+++ b/frc971/control_loops/gaussian_noise.cc
@@ -0,0 +1,18 @@
+#include "frc971/control_loops/gaussian_noise.h"
+
+namespace frc971 {
+namespace control_loops {
+
+GaussianNoise::GaussianNoise(unsigned int seed, double stddev)
+ : stddev_(stddev),
+ generator_(seed),
+ distribution_(0.0, 1.0) {
+ // Everything is initialized now.
+}
+
+double GaussianNoise::AddNoiseToSample(double sample) {
+ return sample + (distribution_(generator_) * stddev_);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/gaussian_noise.h b/frc971/control_loops/gaussian_noise.h
new file mode 100644
index 0000000..8adf840
--- /dev/null
+++ b/frc971/control_loops/gaussian_noise.h
@@ -0,0 +1,36 @@
+#ifndef FRC971_CONTROL_LOOPS_GAUSSIAN_NOISE_H_
+#define FRC971_CONTROL_LOOPS_GAUSSIAN_NOISE_H_
+
+#include <unistd.h>
+
+#include <memory>
+#include <random>
+
+namespace frc971 {
+namespace control_loops {
+
+class GaussianNoise {
+ public:
+ // seed: The seed for the random number generator.
+ // stddev: The standard deviation of the distribution.
+ GaussianNoise(unsigned int seed, double stddev);
+
+ // Returns a version of the sample with gaussian noise added in.
+ double AddNoiseToSample(double sample);
+
+ // Sets the standard deviation of the gaussian noise.
+ inline void set_standard_deviation(double stddev) {
+ stddev_ = stddev;
+ }
+
+ private:
+ double stddev_;
+
+ std::default_random_engine generator_;
+ std::normal_distribution<double> distribution_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_GAUSSIAN_NOISE_H_
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
new file mode 100644
index 0000000..4f112a9
--- /dev/null
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -0,0 +1,59 @@
+#include "frc971/control_loops/position_sensor_sim.h"
+
+namespace frc971 {
+namespace control_loops {
+
+PositionSensorSimulator::PositionSensorSimulator(double offset,
+ double index_diff,
+ double pot_noise_stddev)
+ : index_diff_(index_diff),
+ pot_noise_(0, pot_noise_stddev) {
+ OverrideParams(offset, pot_noise_stddev);
+}
+
+void PositionSensorSimulator::OverrideParams(double offset,
+ double pot_noise_stddev) {
+ cur_index_segment_ = 0;
+ cur_index_ = 0;
+ index_count_ = 0;
+ cur_pos_ = 0;
+ offset_ = offset;
+ 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_);
+
+ if (new_index < cur_index_segment_) {
+ // Position is decreasing.
+ cur_index_ = new_index + 1;
+ index_count_++;
+ } else if (new_index > cur_index_segment_) {
+ // Position is increasing.
+ cur_index_ = new_index;
+ index_count_++;
+ }
+
+ cur_index_segment_ = new_index;
+ 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_;
+
+ 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_;
+ }
+
+ values->index_pulses = index_count_;
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
new file mode 100644
index 0000000..dc080de
--- /dev/null
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -0,0 +1,59 @@
+#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/gaussian_noise.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// NOTE: All potentiometer and encoder values in this class are assumed to be in
+// translated SI units.
+
+class PositionSensorSimulator {
+ public:
+ // offset: The difference between zero on the pot and an index pulse.
+ // index_diff: The interval between index pulses.
+ // 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,
+ 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);
+
+ // Change sensors to a new position.
+ // new_pos: The new position. (This is an absolute position.)
+ void MoveTo(double new_pos);
+
+ // Get the current values of the sensors.
+ // values: These will be filled in.
+ void GetSensorValues(PotAndIndexPosition* values);
+
+ private:
+ // Which index pulse we are currently on relative to the one we started on.
+ int cur_index_segment_;
+ // Index pulse to use for calculating latched sensor values, relative to the
+ // one we started on.
+ 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.
+ double cur_pos_;
+ // Difference between zero on the pot and zero on the encoder.
+ double offset_;
+ // Gaussian noise to add to pot readings.
+ GaussianNoise pot_noise_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif /* FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_ */
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index b6c5ceb..3519826 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -7,6 +7,7 @@
'<(AOS)/build/aos_all.gyp:Prime',
'../control_loops/control_loops.gyp:state_feedback_loop_test',
+ '../control_loops/control_loops.gyp:position_sensor_sim',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
'../control_loops/fridge/fridge.gyp:fridge',