Merge "Update drivetrain gearing and time step."
diff --git a/aos/common/controls/control_loop_test.cc b/aos/common/controls/control_loop_test.cc
index 899b96b..40357b2 100644
--- a/aos/common/controls/control_loop_test.cc
+++ b/aos/common/controls/control_loop_test.cc
@@ -32,7 +32,7 @@
new_state->enabled = enabled;
new_state->autonomous = false;
- new_state->team_id = 971;
+ new_state->team_id = team_id_;
new_state.Send();
}
diff --git a/aos/common/controls/control_loop_test.h b/aos/common/controls/control_loop_test.h
index 13c4f0d..a1165a9 100644
--- a/aos/common/controls/control_loop_test.h
+++ b/aos/common/controls/control_loop_test.h
@@ -19,6 +19,9 @@
ControlLoopTest();
virtual ~ControlLoopTest();
+ void set_team_id(uint16_t team_id) { team_id_ = team_id; }
+ uint16_t team_id() const { return team_id_; }
+
// Sends out all of the required queue messages.
void SendMessages(bool enabled);
// Ticks time for a single control loop cycle.
@@ -37,6 +40,8 @@
static constexpr ::aos::time::Time kDSPacketTime =
::aos::time::Time::InMS(20);
+ uint16_t team_id_ = 971;
+
::aos::time::Time last_ds_time_ = ::aos::time::Time::InSeconds(0);
::aos::time::Time current_time_ = ::aos::time::Time::InSeconds(0);
diff --git a/aos/common/network/team_number.cc b/aos/common/network/team_number.cc
index 38b169a..626d1e6 100644
--- a/aos/common/network/team_number.cc
+++ b/aos/common/network/team_number.cc
@@ -11,7 +11,7 @@
namespace network {
namespace {
-uint16_t override_team = 0;
+uint16_t override_team;
uint16_t *DoGetTeamNumber() {
if (override_team != 0) return &override_team;
diff --git a/aos/common/network/team_number.h b/aos/common/network/team_number.h
index 08103ed..520b9ba 100644
--- a/aos/common/network/team_number.h
+++ b/aos/common/network/team_number.h
@@ -15,6 +15,7 @@
// before GetTeamNumber() is ever called.
// Overriding to team 0 won't work.
// Intended only for tests.
+// Guaranteed to be safe to call during static initialization time.
void OverrideTeamNumber(uint16_t team);
} // namespace network
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index e8e3276..31a6933 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -48,7 +48,9 @@
'claw_lib',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
],
},
{
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 8d0ac78..b7404ff 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -1,14 +1,17 @@
+#include <math.h>
#include <unistd.h>
#include <memory>
#include "gtest/gtest.h"
#include "aos/common/queue.h"
+#include "aos/common/time.h"
#include "aos/common/controls/control_loop_test.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/team_number_test_environment.h"
using ::aos::time::Time;
@@ -24,8 +27,9 @@
ClawSimulation()
: claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeClawPlant())),
pot_and_encoder_(
- 0.0, constants::GetValues().claw_zeroing_constants.index_difference,
- 0.3),
+ constants::GetValues().claw.wrist.lower_limit,
+ constants::GetValues().claw_zeroing_constants.index_difference,
+ constants::GetValues().claw_zeroing_constants.index_difference / 3.0),
claw_queue_(".frc971.control_loops.claw_queue", 0x9d7452fb,
".frc971.control_loops.claw_queue.goal",
".frc971.control_loops.claw_queue.position",
@@ -33,8 +37,8 @@
".frc971.control_loops.claw_queue.status") {}
// Do specific initialization for the sensors.
- void SetSensors(double start_value, double pot_noise_stddev) {
- pot_and_encoder_.OverrideParams(start_value, pot_noise_stddev);
+ void SetSensors(double start_pos, double pot_noise_stddev) {
+ pot_and_encoder_.OverrideParams(start_pos, pot_noise_stddev);
}
// Sends a queue message with the position.
@@ -76,7 +80,9 @@
".frc971.control_loops.claw_queue.output",
".frc971.control_loops.claw_queue.status"),
claw_(&claw_queue_),
- claw_plant_() {}
+ claw_plant_() {
+ set_team_id(kTeamNumber);
+ }
void VerifyNearGoal() {
claw_queue_.goal.FetchLatest();
@@ -86,6 +92,25 @@
10.0);
}
+ // Runs one iteration of the whole simulation.
+ void RunIteration() {
+ SendMessages(true);
+
+ claw_plant_.SendPositionMessage();
+ claw_.Iterate();
+ claw_plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration();
+ }
+ }
+
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointed to
// shared memory that is no longer valid.
@@ -96,17 +121,56 @@
ClawSimulation claw_plant_;
};
-// Tests that the loop does nothing when the goal is zero.
+// Tests that the loop does nothing when the goal is our lower limit.
TEST_F(ClawTest, DoesNothing) {
- claw_queue_.goal.MakeWithBuilder().angle(0.0).Send();
- SendMessages(true);
- claw_plant_.SendPositionMessage();
- claw_.Iterate();
- claw_plant_.Simulate();
- TickTime();
+ const auto values = constants::GetValues();
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(values.claw.wrist.lower_limit)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ // We should not have moved.
VerifyNearGoal();
- claw_queue_.output.FetchLatest();
- EXPECT_EQ(claw_queue_.output->voltage, 0.0);
+}
+
+// Tests that we can reach a reasonable goal.
+TEST_F(ClawTest, ReachesGoal) {
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ VerifyNearGoal();
+}
+
+// Tests that it doesn't try to move past the physical range of the mechanism.
+TEST_F(ClawTest, RespectsRange) {
+ const auto values = constants::GetValues();
+ // Upper limit
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(values.claw.wrist.upper_hard_limit + 5.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ claw_queue_.status.FetchLatest();
+ EXPECT_NEAR(values.claw.wrist.upper_limit,
+ claw_queue_.status->angle,
+ 2.0 * M_PI / 180.0);
+
+ // Lower limit.
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
+ .angle(values.claw.wrist.lower_hard_limit + 5.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ claw_queue_.status.FetchLatest();
+ EXPECT_NEAR(values.claw.wrist.lower_limit,
+ claw_queue_.status->angle,
+ 2.0 * M_PI / 180.0);
}
} // namespace testing
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 6e882e1..7d0d395 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -1,6 +1,20 @@
{
'targets': [
{
+ 'target_name': 'team_number_test_environment',
+ 'type': 'static_library',
+ 'sources': [
+ 'team_number_test_environment.cc'
+ ],
+ 'dependencies': [
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(EXTERNALS):gtest',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):gtest',
+ ],
+ },
+ {
'target_name': 'state_feedback_loop_test',
'type': 'executable',
'sources': [
@@ -36,6 +50,18 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'position_sensor_sim_test',
+ 'type': 'executable',
+ 'sources': [
+ 'position_sensor_sim_test.cc',
+ ],
+ 'dependencies': [
+ 'queues',
+ 'position_sensor_sim',
+ '<(EXTERNALS):gtest',
+ ],
+ },
+ {
'target_name': 'position_sensor_sim',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/fridge/fridge.gyp b/frc971/control_loops/fridge/fridge.gyp
index ade2d0a..4a39901 100644
--- a/frc971/control_loops/fridge/fridge.gyp
+++ b/frc971/control_loops/fridge/fridge.gyp
@@ -49,7 +49,9 @@
'fridge_lib',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
],
},
{
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 98deb8e..03f1d1b 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -1,21 +1,23 @@
+#include <math.h>
#include <unistd.h>
#include <memory>
#include "gtest/gtest.h"
#include "aos/common/queue.h"
+#include "aos/common/time.h"
#include "aos/common/controls/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/control_loops/fridge/fridge.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/team_number_test_environment.h"
using ::aos::time::Time;
namespace frc971 {
namespace control_loops {
namespace testing {
-
// Class which simulates the fridge and sends out queue messages with the
// position.
class FridgeSimulation {
@@ -25,21 +27,27 @@
: arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
elev_plant_(new StateFeedbackPlant<4, 2, 2>(MakeElevatorPlant())),
left_arm_pot_encoder_(
- 0.0,
+ constants::GetValues().fridge.arm.lower_limit,
constants::GetValues().left_arm_zeroing_constants.index_difference,
- 0.3),
+ constants::GetValues().left_arm_zeroing_constants.index_difference
+ / 3.0),
right_arm_pot_encoder_(
- 0.0,
+ constants::GetValues().fridge.arm.lower_limit,
constants::GetValues().right_arm_zeroing_constants.index_difference,
- 0.3),
+ constants::GetValues().right_arm_zeroing_constants.index_difference
+ / 3.0),
left_elev_pot_encoder_(
- 0.0, constants::GetValues()
- .left_elevator_zeroing_constants.index_difference,
- 0.3),
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues()
+ .left_elevator_zeroing_constants.index_difference,
+ constants::GetValues()
+ .left_elevator_zeroing_constants.index_difference / 3.0),
right_elev_pot_encoder_(
- 0.0, constants::GetValues()
- .right_elevator_zeroing_constants.index_difference,
- 0.3),
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues()
+ .right_elevator_zeroing_constants.index_difference,
+ constants::GetValues()
+ .right_elevator_zeroing_constants.index_difference / 3.0),
fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
".frc971.control_loops.fridge_queue.goal",
".frc971.control_loops.fridge_queue.position",
@@ -47,20 +55,20 @@
".frc971.control_loops.fridge_queue.status") {}
// Do specific initialization for all the sensors.
- void SetLeftElevatorSensors(double start_value, double pot_noise_stddev) {
- left_elev_pot_encoder_.OverrideParams(start_value, pot_noise_stddev);
+ void SetLeftElevatorSensors(double start_pos, double pot_noise_stddev) {
+ left_elev_pot_encoder_.OverrideParams(start_pos, pot_noise_stddev);
}
- void SetRightElevatorSensors(double start_value, double pot_noise_stddev) {
- right_elev_pot_encoder_.OverrideParams(start_value, pot_noise_stddev);
+ void SetRightElevatorSensors(double start_pos, double pot_noise_stddev) {
+ right_elev_pot_encoder_.OverrideParams(start_pos, pot_noise_stddev);
}
- void SetLeftArmSensors(double start_value, double pot_noise_stddev) {
- left_arm_pot_encoder_.OverrideParams(start_value, pot_noise_stddev);
+ void SetLeftArmSensors(double start_pos, double pot_noise_stddev) {
+ left_arm_pot_encoder_.OverrideParams(start_pos, pot_noise_stddev);
}
- void SetRightArmSensors(double start_value, double pot_noise_stddev) {
- right_arm_pot_encoder_.OverrideParams(start_value, pot_noise_stddev);
+ void SetRightArmSensors(double start_pos, double pot_noise_stddev) {
+ right_arm_pot_encoder_.OverrideParams(start_pos, pot_noise_stddev);
}
// Sends a queue message with the position.
@@ -110,6 +118,18 @@
right_elev_pot_encoder_.MoveTo(right_elev_height);
}
+ void VerifySeparation() {
+ // TODO(danielp): Make sure that we are getting the correct values from the
+ // plant.
+ const double left_arm_angle = arm_plant_->Y(0, 0);
+ const double right_arm_angle = arm_plant_->Y(1, 0);
+ const double left_elev_height = elev_plant_->Y(0, 0);
+ const double right_elev_height = elev_plant_->Y(1, 0);
+
+ EXPECT_NEAR(left_arm_angle, right_arm_angle, 5.0 * M_PI / 180.0);
+ EXPECT_NEAR(left_elev_height, right_elev_height, 0.05);
+ }
+
private:
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> arm_plant_;
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> elev_plant_;
@@ -131,15 +151,40 @@
".frc971.control_loops.fridge_queue.output",
".frc971.control_loops.fridge_queue.status"),
fridge_(&fridge_queue_),
- fridge_plant_() {}
+ fridge_plant_() {
+ set_team_id(kTeamNumber);
+ }
void VerifyNearGoal() {
fridge_queue_.goal.FetchLatest();
fridge_queue_.status.FetchLatest();
+ // TODO(danielp): I think those tens need to change...
EXPECT_NEAR(fridge_queue_.goal->angle, fridge_queue_.status->angle, 10.0);
EXPECT_NEAR(fridge_queue_.goal->height, fridge_queue_.status->height, 10.0);
}
+ // Runs one iteration of the whole simulation and checks that separation
+ // remains reasonable.
+ void RunIteration() {
+ SendMessages(true);
+
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+
+ TickTime();
+
+ fridge_plant_.VerifySeparation();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration();
+ }
+ }
+
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointed to
// shared memory that is no longer valid.
@@ -152,41 +197,70 @@
// Tests that the loop does nothing when the goal is zero.
TEST_F(FridgeTest, DoesNothing) {
- // Set the initial positions/angles etc explicitly to zero.
- // TODO(danielp): Correct values for this.
- static constexpr double kStartValue = 0.0;
- const constants::Values values = constants::GetValues();
- fridge_plant_.SetLeftElevatorSensors(
- kStartValue, values.left_elevator_zeroing_constants.index_difference / 3);
- fridge_plant_.SetRightElevatorSensors(
- kStartValue,
- values.right_elevator_zeroing_constants.index_difference / 3);
- fridge_plant_.SetLeftArmSensors(
- kStartValue, values.left_arm_zeroing_constants.index_difference / 3);
- fridge_plant_.SetRightArmSensors(
- kStartValue, values.right_arm_zeroing_constants.index_difference / 3);
-
// Set the goals to the starting values. This should theoretically guarantee
// that the controller does nothing.
- fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.0).Send();
+ const constants::Values values = constants::GetValues();
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(0.0)
+ .height(values.fridge.elevator.lower_limit)
+ .Send());
- for (int i = 0; i < 10; ++i) {
- // Run one iteration of the simulation.
- SendMessages(true);
- fridge_plant_.SendPositionMessage();
- fridge_.Iterate();
- fridge_plant_.Simulate();
- TickTime();
- }
+ // Run a few iterations.
+ RunForTime(Time::InMS(50));
VerifyNearGoal();
+}
- // Make sure that all outputs are turned off.
- fridge_queue_.output.FetchLatest();
- EXPECT_EQ(fridge_queue_.output->left_arm, 0.0);
- EXPECT_EQ(fridge_queue_.output->right_arm, 0.0);
- EXPECT_EQ(fridge_queue_.output->left_elevator, 0.0);
- EXPECT_EQ(fridge_queue_.output->right_elevator, 0.0);
+// Tests that the loop can reach a goal.
+TEST_F(FridgeTest, ReachesGoal) {
+ // Set a reasonable goal.
+ const auto values = constants::GetValues();
+ const double soft_limit = values.fridge.elevator.lower_limit;
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 4.0)
+ .height(soft_limit + 0.5)
+ .Send());
+
+ // Give it a lot of time to get there.
+ RunForTime(Time::InMS(4000));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop doesn't try and go beyond the physical range of the
+// mechanisms.
+TEST_F(FridgeTest, RespectsRange) {
+ // Put the arm up to get it out of the way.
+ // We're going to send the elevator to zero, which should be significantly too
+ // low.
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(M_PI / 2.0)
+ .height(0.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ // Check that we are near our soft limit.
+ fridge_queue_.status.FetchLatest();
+ const auto values = constants::GetValues();
+ EXPECT_NEAR(values.fridge.elevator.lower_limit,
+ fridge_queue_.status->height,
+ 0.05);
+
+ // Put the arm down to get it out of the way.
+ // We're going to give the elevator some ridiculously high goal.
+ ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
+ .angle(-M_PI / 2.0)
+ .height(50.0)
+ .Send());
+
+ RunForTime(Time::InMS(4000));
+
+ // Check that we are near our soft limit.
+ fridge_queue_.status.FetchLatest();
+ EXPECT_NEAR(values.fridge.elevator.upper_limit,
+ fridge_queue_.status->height,
+ 0.05);
}
} // namespace testing
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index 4f112a9..3ae5117 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -1,55 +1,91 @@
#include "frc971/control_loops/position_sensor_sim.h"
+#include <cmath>
+
namespace frc971 {
namespace control_loops {
-PositionSensorSimulator::PositionSensorSimulator(double offset,
+/* Index pulse/segment Explanation:
+ *
+ * The index segments are labelled starting at zero and go up. Each index
+ * segment is the space between the two bordering index pulses. The length of
+ * each index segment is determined by the `index_diff` variable in the
+ * constructor below.
+ *
+ * The index pulses are encountered when the mechanism moves from one index
+ * segment to another. Currently the first index pulse is limited to occur at
+ * absolute zero.
+ *
+ * index segment
+ * |
+ * V
+ *
+ * +--- 0---+--- 1---+--- 2---+--- 3---+--- 4---+--- 5---+--- 6---+
+ *
+ * | | | | | | | |
+ * 0 1 2 3 4 5 6 7
+ *
+ * A
+ * |
+ * index pulse
+ */
+
+PositionSensorSimulator::PositionSensorSimulator(double start_position,
double index_diff,
double pot_noise_stddev)
- : index_diff_(index_diff),
- pot_noise_(0, pot_noise_stddev) {
- OverrideParams(offset, pot_noise_stddev);
+ : index_diff_(index_diff), pot_noise_(0, pot_noise_stddev) {
+ OverrideParams(start_position, pot_noise_stddev);
}
-void PositionSensorSimulator::OverrideParams(double offset,
+void PositionSensorSimulator::OverrideParams(double start_position,
double pot_noise_stddev) {
- cur_index_segment_ = 0;
+ cur_index_segment_ = floor(start_position / index_diff_);
cur_index_ = 0;
index_count_ = 0;
- cur_pos_ = 0;
- offset_ = offset;
+ cur_pos_ = start_position;
+ start_position_ = start_position;
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_);
+ // Compute which index segment we're in. In other words, compute between
+ // which two index pulses we are.
+ const int new_index_segment = floor(new_pos / index_diff_);
- if (new_index < cur_index_segment_) {
- // Position is decreasing.
- cur_index_ = new_index + 1;
+ if (new_index_segment < cur_index_segment_) {
+ // We've crossed an index pulse in the negative direction. That means the
+ // index pulse we just crossed is the higher end of the current index
+ // segment. For example, if the mechanism moved from index segment four to
+ // index segment three, then we just crossed index pulse 4.
+ cur_index_ = new_index_segment + 1;
index_count_++;
- } else if (new_index > cur_index_segment_) {
- // Position is increasing.
- cur_index_ = new_index;
+ } else if (new_index_segment > cur_index_segment_) {
+ // We've crossed an index pulse in the positive direction. That means the
+ // index pulse we just crossed is the lower end of the index segment. For
+ // example, if the mechanism moved from index segment seven to index
+ // segment eight, then we just crossed index pulse eight.
+ cur_index_ = new_index_segment;
index_count_++;
}
- cur_index_segment_ = new_index;
+ cur_index_segment_ = new_index_segment;
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_;
+ values->pot = pot_noise_.AddNoiseToSample(cur_pos_);
+ values->encoder = cur_pos_ - start_position_;
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_;
+ // Determine the position of the index pulse relative to absolute zero.
+ double index_pulse_position = cur_index_ * index_diff_;
+
+ // Populate the latched pot/encoder samples.
+ values->latched_pot = pot_noise_.AddNoiseToSample(index_pulse_position);
+ values->latched_encoder = index_pulse_position - start_position_;
}
values->index_pulses = index_count_;
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index dc080de..9975a78 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -12,43 +12,56 @@
class PositionSensorSimulator {
public:
- // offset: The difference between zero on the pot and an index pulse.
- // index_diff: The interval between index pulses.
+ // start_position: The position relative to absolute zero where the simulated
+ // structure starts. For example, to simulate the elevator
+ // starting at 40cm above absolute zero, set this to 0.4.
+ // index_diff: The interval between index pulses. This is measured in SI
+ // units. For example, if an index pulse hits every 5cm on the
+ // elevator, set this to 0.05.
// 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,
+ PositionSensorSimulator(double start_position, 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);
+ void OverrideParams(double start_position, 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);
+ // Simulate the structure moving to a new position. The new value is measured
+ // relative to absolute zero. This will update the simulated sensors with new
+ // readings.
+ // new_position: The new position relative to absolute zero.
+ void MoveTo(double new_position);
- // Get the current values of the sensors.
- // values: These will be filled in.
+ // 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:
- // Which index pulse we are currently on relative to the one we started on.
+ // The absolute segment between two index pulses the simulation is on. For
+ // example, when the current position is betwen index pulse zero and one,
+ // the current index segment is considered to be zero. Index segment one is
+ // between index pulses 1 and 2, etc.
int cur_index_segment_;
- // Index pulse to use for calculating latched sensor values, relative to the
- // one we started on.
+ // Index pulse to use for calculating latched sensor values, relative to
+ // absolute zero. In other words this always holds the index pulse that was
+ // encountered most recently.
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.
+ // Current position of the mechanism relative to absolute zero.
double cur_pos_;
- // Difference between zero on the pot and zero on the encoder.
- double offset_;
+ // Starting position of the mechanism relative to absolute zero. See the
+ // `starting_position` parameter in the constructor for more info.
+ double start_position_;
// Gaussian noise to add to pot readings.
GaussianNoise pot_noise_;
};
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
new file mode 100644
index 0000000..7374e2d
--- /dev/null
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -0,0 +1,104 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include <random>
+
+#include "gtest/gtest.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "aos/common/die.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class PositionSensorSimTest : public ::testing::Test {
+ protected:
+ PositionSensorSimTest() {}
+};
+
+TEST_F(PositionSensorSimTest, NoIndices) {
+ // We'll simulate a potentiometer with no noise so that we can accurately
+ // verify where the mechanism currently is. Overall though, the purpose of
+ // 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;
+ PositionSensorSimulator sim(3.6 * index_diff, 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(static_cast<unsigned int>(0), 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(static_cast<unsigned int>(0), 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(static_cast<unsigned int>(0), 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(static_cast<unsigned int>(0), position.index_pulses);
+ }
+}
+
+TEST_F(PositionSensorSimTest, CountIndices) {
+ // The purpose of this test is to verify that the simulator latches the
+ // correct index pulse when transitioning from one segment to another. We
+ // 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;
+ PositionSensorSimulator sim(4.6 * index_diff, index_diff, 0);
+
+ // Make sure that we get an index pulse on every transition.
+ sim.GetSensorValues(&position);
+ ASSERT_EQ(static_cast<unsigned int>(0), position.index_pulses);
+
+ sim.MoveTo(3.6 * index_diff);
+ sim.GetSensorValues(&position);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
+ ASSERT_EQ(static_cast<unsigned int>(1), position.index_pulses);
+
+ sim.MoveTo(4.5 * index_diff);
+ sim.GetSensorValues(&position);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, position.latched_pot);
+ ASSERT_EQ(static_cast<unsigned int>(2), position.index_pulses);
+
+ sim.MoveTo(5.9 * index_diff);
+ sim.GetSensorValues(&position);
+ ASSERT_DOUBLE_EQ(5.0 * index_diff, position.latched_pot);
+ ASSERT_EQ(static_cast<unsigned int>(3), position.index_pulses);
+
+ sim.MoveTo(6.1 * index_diff);
+ sim.GetSensorValues(&position);
+ ASSERT_DOUBLE_EQ(6.0 * index_diff, position.latched_pot);
+ ASSERT_EQ(static_cast<unsigned int>(4), position.index_pulses);
+
+ sim.MoveTo(8.7 * index_diff);
+ sim.GetSensorValues(&position);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
+ ASSERT_EQ(static_cast<unsigned int>(5), position.index_pulses);
+
+ sim.MoveTo(7.3 * index_diff);
+ sim.GetSensorValues(&position);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, position.latched_pot);
+ ASSERT_EQ(static_cast<unsigned int>(6), position.index_pulses);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/team_number_test_environment.cc b/frc971/control_loops/team_number_test_environment.cc
new file mode 100644
index 0000000..624a119
--- /dev/null
+++ b/frc971/control_loops/team_number_test_environment.cc
@@ -0,0 +1,15 @@
+#include "frc971/control_loops/team_number_test_environment.h"
+
+#include "aos/common/network/team_number.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+void TeamNumberEnvironment::SetUp() {
+ ::aos::network::OverrideTeamNumber(kTeamNumber);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/team_number_test_environment.h b/frc971/control_loops/team_number_test_environment.h
new file mode 100644
index 0000000..f0c1a16
--- /dev/null
+++ b/frc971/control_loops/team_number_test_environment.h
@@ -0,0 +1,30 @@
+#ifndef FRC971_CONTROL_LOOPS_TEAM_NUMBER_TEST_ENVIRONMENT_H_
+#define FRC971_CONTROL_LOOPS_TEAM_NUMBER_TEST_ENVIRONMENT_H_
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// The team number we use for tests.
+static const int kTeamNumber = 1;
+
+// Overrides the team number to kTeamNumber before any test consructors run.
+// This is important for tests which retrieve constants values during
+// construction.
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ void SetUp() override;
+};
+
+// The static variable in a header is intentional. Kind of a hack, undefined
+// order, but that works OK here.
+static ::testing::Environment* const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment());
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_TEAM_NUMBER_TEST_ENVIRONMENT_H_
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 67ec175..661a434 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_test',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
'../control_loops/fridge/fridge.gyp:fridge',