Fix position_sensor_sim's latched_pot value
I noticed it was changing every cycle, which is inaccurate and was
bothering me.
Change-Id: I288ce04ba43d4907a82f9e6c235357d7467e4dde
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 5eeed1d..9dee424 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -59,6 +59,7 @@
cc_library(
name = 'position_sensor_sim',
+ testonly = True,
srcs = [
'position_sensor_sim.cc',
],
@@ -69,6 +70,7 @@
':queues',
':gaussian_noise',
'//debian:libm',
+ '//aos/testing:random_seed',
],
)
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index caa383e..6075155 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -70,6 +70,11 @@
index_count_++;
}
+ if (new_index_segment != cur_index_segment_) {
+ latched_pot_ = pot_noise_.AddNoiseToSample(cur_index_ * index_diff_ +
+ known_index_pos_);
+ }
+
cur_index_segment_ = new_index_segment;
cur_pos_ = new_pos;
}
@@ -86,7 +91,7 @@
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);
+ values->latched_pot = latched_pot_;
values->latched_encoder = index_pulse_position - start_position_;
}
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 6c1884d..3ce3056 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,6 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#include "aos/testing/random_seed.h"
+
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/gaussian_noise.h"
@@ -18,7 +20,8 @@
// 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 = 0);
+ PositionSensorSimulator(double index_diff, 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.
@@ -57,6 +60,8 @@
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_;
// Distance between index pulses on the mechanism.
double index_diff_;
// Absolute position of a known index pulse.
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 0c9d3a9..1ca5033 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -150,5 +150,50 @@
EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
}
+// Tests that the latched values update correctly.
+TEST_F(PositionSensorSimTest, LatchedValues) {
+ const double index_diff = 0.5;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(0, 0.25);
+ PotAndIndexPosition position;
+
+ sim.MoveTo(0.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(0u, position.index_pulses);
+
+ sim.MoveTo(1.75 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+ const double first_latched_pot = position.latched_pot;
+
+ sim.MoveTo(1.95 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(1u, position.index_pulses);
+ EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(first_latched_pot, position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+
+ sim.MoveTo(2.05 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(2u, position.index_pulses);
+ EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+
+ sim.MoveTo(1.95 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(3u, position.index_pulses);
+ EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+
+ sim.MoveTo(0.95 * index_diff);
+ sim.GetSensorValues(&position);
+ EXPECT_EQ(4u, position.index_pulses);
+ EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
+ EXPECT_GT(::std::abs(first_latched_pot - position.latched_pot), 0.005);
+ EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+}
+
} // namespace control_loops
} // namespace frc971