Add AbsoluteAndAbsolutePosition and simulate it
Change-Id: I0b84981c48bc88db1a9cc586aec9596ea9abf531
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index a95d407..4377541 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -61,6 +61,23 @@
absolute_encoder:double;
}
+// Represents all of the data for an absolute and relative encoder pair,
+// along with an absolute encoder.
+// They operate similarly to a pot and absolute encoder, but another absolute
+// encoder is used in place of the potentiometer.
+// The units on all of the positions are the same.
+// The relative encoder values are relative to where the encoder was at some
+// arbitrary point in time.
+table AbsoluteAndAbsolutePosition {
+ // Current position read from each encoder.
+ encoder:double;
+ absolute_encoder:double;
+
+ // Current position read from the single turn absolute encoder.
+ // This can not turn more than one rotation.
+ single_turn_absolute_encoder:double;
+}
+
// Represents all of the data for a single encoder.
// The relative encoder values are relative to where the encoder was at some
// arbitrary point in time.
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index b605196..222af5d 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -56,22 +56,26 @@
* remainder of the graph.
*/
-PositionSensorSimulator::PositionSensorSimulator(double distance_per_revolution,
- unsigned int noise_seed)
+PositionSensorSimulator::PositionSensorSimulator(
+ double distance_per_revolution, double single_turn_distance_per_revolution,
+ unsigned int noise_seed)
: lower_index_edge_(distance_per_revolution, noise_seed),
upper_index_edge_(distance_per_revolution, noise_seed),
- distance_per_revolution_(distance_per_revolution) {
+ distance_per_revolution_(distance_per_revolution),
+ single_turn_distance_per_revolution_(
+ single_turn_distance_per_revolution) {
Initialize(0.0, 0.0);
}
void PositionSensorSimulator::Initialize(
- double start_position, double pot_noise_stddev,
- double known_index_position /* = 0*/,
- double known_absolute_encoder_pos /* = 0*/) {
+ double start_position, double pot_noise_stddev, double known_index_position,
+ double known_absolute_encoder_pos,
+ double single_turn_known_absolute_encoder_pos) {
InitializeHallEffectAndPosition(start_position, known_index_position,
known_index_position);
known_absolute_encoder_ = known_absolute_encoder_pos;
+ single_turn_known_absolute_encoder_ = single_turn_known_absolute_encoder_pos;
lower_index_edge_.mutable_pot_noise()->set_standard_deviation(
pot_noise_stddev);
@@ -180,15 +184,7 @@
builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
current_position_));
builder->add_encoder(current_position_ - start_position_);
- // TODO(phil): Create some lag here since this is a PWM signal it won't be
- // instantaneous like the other signals. Better yet, its lag varies
- // randomly with the distribution varying depending on the reading.
- double absolute_encoder = ::std::remainder(
- current_position_ + known_absolute_encoder_, distance_per_revolution_);
- if (absolute_encoder < 0) {
- absolute_encoder += distance_per_revolution_;
- }
- builder->add_absolute_encoder(absolute_encoder);
+ builder->add_absolute_encoder(WrapAbsoluteEncoder());
return builder->Finish();
}
@@ -197,15 +193,24 @@
PositionSensorSimulator::GetSensorValues<AbsolutePositionBuilder>(
AbsolutePositionBuilder *builder) {
builder->add_encoder(current_position_ - start_position_);
- // TODO(phil): Create some lag here since this is a PWM signal it won't be
- // instantaneous like the other signals. Better yet, its lag varies
- // randomly with the distribution varying depending on the reading.
- double absolute_encoder = ::std::remainder(
- current_position_ + known_absolute_encoder_, distance_per_revolution_);
- if (absolute_encoder < 0) {
- absolute_encoder += distance_per_revolution_;
+ builder->add_absolute_encoder(WrapAbsoluteEncoder());
+ return builder->Finish();
+}
+
+template <>
+flatbuffers::Offset<AbsoluteAndAbsolutePosition>
+PositionSensorSimulator::GetSensorValues<AbsoluteAndAbsolutePositionBuilder>(
+ AbsoluteAndAbsolutePositionBuilder *builder) {
+ builder->add_encoder(current_position_ - start_position_);
+ builder->add_absolute_encoder(WrapAbsoluteEncoder());
+
+ double single_turn_absolute_encoder =
+ ::std::remainder(current_position_ + single_turn_known_absolute_encoder_,
+ single_turn_distance_per_revolution_);
+ if (single_turn_absolute_encoder < 0) {
+ single_turn_absolute_encoder += single_turn_distance_per_revolution_;
}
- builder->add_absolute_encoder(absolute_encoder);
+ builder->add_single_turn_absolute_encoder(single_turn_absolute_encoder);
return builder->Finish();
}
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index faa5dd5..77e2d5f 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -21,11 +21,18 @@
// NOTE: When retrieving the sensor values for a PotAndAbsolutePosition
// message this field represents the interval between when the absolute
// encoder reads 0.
+ // single_turn_distance_per_revolution:
+ // This is only used when retrieving
+ // sensor values for an AbsoluteAndAbsolutePosition.
+ // The interval between when the single turn abosolute encoder reads 0.
+ // This will be the whole range of the single turn absolute encoder
+ // as it cannot turn more than once.
// noise_seed:
// The seed to feed into the random number generator for the potentiometer
// values.
PositionSensorSimulator(
double distance_per_revolution,
+ double single_turn_distance_per_revolution = 0.0,
unsigned int noise_seed = ::aos::testing::RandomSeed());
// Set new parameters for the sensors. This is useful for unit tests to change
@@ -37,9 +44,15 @@
// This specifies the standard deviation of that
// distribution.
// known_index_pos: The absolute position of an index pulse.
+ // known_absolute_encoder_pos: The readout of the absolute encoder when the
+ // robot's mechanism is at zero.
+ // single_turn_known_absolute_encoder_pos: The readout of the single turn
+ // absolute encoder when the robot's
+ // mechanism is at zero.
void Initialize(double start_position, double pot_noise_stddev,
double known_index_pos = 0.0,
- double known_absolute_encoder_pos = 0.0);
+ double known_absolute_encoder_pos = 0.0,
+ double single_turn_known_absolute_encoder_pos = 0.0);
// Initializes a sensor simulation which is pretending to be a hall effect +
// encoder setup. This is assuming that the hall effect sensor triggers once
@@ -158,15 +171,33 @@
GaussianNoise pot_noise_;
};
+ double WrapAbsoluteEncoder() {
+ // TODO(phil): Create some lag here since this is a PWM signal it won't be
+ // instantaneous like the other signals. Better yet, its lag varies
+ // randomly with the distribution varying depending on the reading.
+ double absolute_encoder = ::std::remainder(
+ current_position_ + known_absolute_encoder_, distance_per_revolution_);
+ if (absolute_encoder < 0) {
+ absolute_encoder += distance_per_revolution_;
+ }
+ return absolute_encoder;
+ }
+
IndexEdge lower_index_edge_;
IndexEdge upper_index_edge_;
// Distance between index pulses on the mechanism.
double distance_per_revolution_;
+ // The distance that the mechanism travels for one revolution of the single
+ // turn absolue encoder
+ double single_turn_distance_per_revolution_;
+
// The readout of the absolute encoder when the robot's mechanism is at
// zero.
double known_absolute_encoder_;
+ // The other absolute encoder's value when the mechanism is at zero.
+ double single_turn_known_absolute_encoder_;
// Current position of the mechanism relative to absolute zero.
double current_position_;
// 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 424a3af..e8beabf 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -409,5 +409,56 @@
}
}
+TEST_F(PositionSensorSimTest, AbsoluteAndAbsoluteEncoderTest) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, 0 /* Pot noise N/A */, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ flatbuffers::FlatBufferBuilder fbb;
+
+ sim.MoveTo(0.0);
+ const AbsoluteAndAbsolutePosition *position =
+ sim.FillSensorValues<AbsoluteAndAbsolutePosition>(&fbb);
+
+ EXPECT_NEAR(position->encoder(), -start_pos, 1e-10);
+ EXPECT_NEAR(position->absolute_encoder(), measured_absolute_position, 1e-10);
+ EXPECT_NEAR(position->single_turn_absolute_encoder(),
+ single_turn_measured_absolute_position, 1e-10);
+
+ sim.MoveTo(1.0);
+ position = sim.FillSensorValues<AbsoluteAndAbsolutePosition>(&fbb);
+
+ EXPECT_NEAR(position->encoder(), 1.0 - start_pos, 1e-10);
+
+ // because it has moved to exactly distance_per_revolution, it will wrap,
+ // and come back to measured_absolute_position.
+ EXPECT_NEAR(position->absolute_encoder(), measured_absolute_position, 1e-10);
+ EXPECT_NEAR(position->single_turn_absolute_encoder(), 2.6, 1e-10);
+
+ sim.MoveTo(2.5);
+ position = sim.FillSensorValues<AbsoluteAndAbsolutePosition>(&fbb);
+
+ EXPECT_NEAR(position->encoder(), 2.5 - start_pos, 1e-10);
+
+ // (position + measured_absolute_position) % distance_per_revolution,
+ EXPECT_NEAR(position->absolute_encoder(), 0.8, 1e-10);
+ // (position + single_turn_measured_absolute_position) %
+ // single_turn_distance_per_revolution,
+ EXPECT_NEAR(position->single_turn_absolute_encoder(), 0.1, 1e-10);
+}
+
} // namespace control_loops
} // namespace frc971