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/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_