Add a sensor message for relative encoder + index pulse
This is needed on the turret this year. Austin says there's no room
for more than this. The good news is that we can still zero with this
information.
Change-Id: I46f8a9bf0a864b717dfbf6dddfb03e848a8a7d30
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 05a660b..f13d82b 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -1,5 +1,20 @@
package frc971;
+// Represents all of the data for a single indexed encoder. In other words,
+// just a relative encoder with an index pulse.
+// The units on all of the positions are the same.
+// All encoder values are relative to where the encoder was at some arbitrary
+// point in time. All potentiometer values are relative to some arbitrary 0
+// position which varies with each robot.
+struct IndexPosition {
+ // Current position read from the encoder.
+ double encoder;
+ // Position from the encoder latched at the last index pulse.
+ double latched_encoder;
+ // How many index pulses we've seen since startup. Starts at 0.
+ uint32_t index_pulses;
+};
+
// Represents all of the data for a single potentiometer and indexed encoder
// pair.
// The units on all of the positions are the same.
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 6075155..44fda1f 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -79,6 +79,22 @@
cur_pos_ = new_pos;
}
+void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
+ values->encoder = cur_pos_ - start_position_;
+
+ if (index_count_ == 0) {
+ values->latched_encoder = 0.0;
+ } else {
+ // Determine the position of the index pulse relative to absolute zero.
+ double index_pulse_position = cur_index_ * index_diff_ + known_index_pos_;
+
+ // Populate the latched encoder samples.
+ values->latched_encoder = index_pulse_position - start_position_;
+ }
+
+ values->index_pulses = index_count_;
+}
+
void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
values->encoder = cur_pos_ - start_position_;
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 3ce3056..947e963 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -46,6 +46,12 @@
// 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(IndexPosition* values);
+
+ // 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:
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 1ca5033..a32ab72 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -23,37 +23,46 @@
// 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;
+ IndexPosition index_position;
+ PotAndIndexPosition pot_and_index_position;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * 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(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_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(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_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(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_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(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ ASSERT_EQ(0u, index_position.index_pulses);
}
}
@@ -63,43 +72,58 @@
// 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;
+ IndexPosition index_position;
+ PotAndIndexPosition pot_and_index_position;
PositionSensorSimulator sim(index_diff);
sim.Initialize(4.6 * index_diff, 0);
// Make sure that we get an index pulse on every transition.
- sim.GetSensorValues(&position);
- ASSERT_EQ(0u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_EQ(0u, index_position.index_pulses);
+ ASSERT_EQ(0u, pot_and_index_position.index_pulses);
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
- ASSERT_EQ(1u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(1u, index_position.index_pulses);
+ ASSERT_EQ(1u, pot_and_index_position.index_pulses);
sim.MoveTo(4.5 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
- ASSERT_EQ(2u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(2u, index_position.index_pulses);
+ ASSERT_EQ(2u, pot_and_index_position.index_pulses);
sim.MoveTo(5.9 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(5.0 * index_diff, position.latched_pot);
- ASSERT_EQ(3u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(3u, index_position.index_pulses);
+ ASSERT_EQ(3u, pot_and_index_position.index_pulses);
sim.MoveTo(6.1 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(6.0 * index_diff, position.latched_pot);
- ASSERT_EQ(4u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(4u, index_position.index_pulses);
+ ASSERT_EQ(4u, pot_and_index_position.index_pulses);
sim.MoveTo(8.7 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
- ASSERT_EQ(5u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(5u, index_position.index_pulses);
+ ASSERT_EQ(5u, pot_and_index_position.index_pulses);
sim.MoveTo(7.3 * index_diff);
- sim.GetSensorValues(&position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
- ASSERT_EQ(6u, position.index_pulses);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
+ ASSERT_EQ(6u, index_position.index_pulses);
+ ASSERT_EQ(6u, pot_and_index_position.index_pulses);
}
// Tests that the simulator handles non-zero specified index pulse locations
@@ -108,46 +132,65 @@
const double index_diff = 0.5;
PositionSensorSimulator sim(index_diff);
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 0.5);
- PotAndIndexPosition position;
+ IndexPosition index_position;
+ PotAndIndexPosition pot_and_index_position;
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(2u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(2u, index_position.index_pulses);
+ EXPECT_EQ(2u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
// Try it with our known index pulse not being our first one.
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(1u, index_position.index_pulses);
+ EXPECT_EQ(1u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(2u, position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, position.latched_encoder);
+ sim.GetSensorValues(&index_position);
+ sim.GetSensorValues(&pot_and_index_position);
+ EXPECT_EQ(2u, index_position.index_pulses);
+ EXPECT_EQ(2u, pot_and_index_position.index_pulses);
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
}
// Tests that the latched values update correctly.