Merge "Move Actions To Common:"
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 42702c0..bf2eea4 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -11,22 +11,21 @@
// TODO(aschuh): Tests.
-template <class T, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kStaleLogInterval;
-template <class T, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kPwmDisableTime;
+template <class T>
+constexpr ::aos::time::Time ControlLoop<T>::kStaleLogInterval;
+template <class T>
+constexpr ::aos::time::Time ControlLoop<T>::kPwmDisableTime;
-template <class T, bool fail_no_goal>
-void
-ControlLoop<T, fail_no_goal>::ZeroOutputs() {
+template <class T>
+void ControlLoop<T>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool fail_no_goal>
-void ControlLoop<T, fail_no_goal>::Iterate() {
+template <class T>
+void ControlLoop<T>::Iterate() {
no_goal_.Print();
driver_station_old_.Print();
no_sensor_state_.Print();
@@ -49,10 +48,6 @@
LOG_STRUCT(DEBUG, "goal", *goal);
} else {
LOG_INTERVAL(no_goal_);
- if (fail_no_goal) {
- ZeroOutputs();
- return;
- }
}
::aos::robot_state.FetchLatest();
@@ -130,8 +125,8 @@
status.Send();
}
-template <class T, bool fail_no_goal>
-void ControlLoop<T, fail_no_goal>::Run() {
+template <class T>
+void ControlLoop<T>::Run() {
while (true) {
Iterate();
}
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index a176fb6..d48f017 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -44,7 +44,7 @@
// that has a goal, position, status, and output queue.
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
-template <class T, bool fail_no_goal = true>
+template <class T>
class ControlLoop : public SerializableControlLoop {
public:
// Create some convenient typedefs to reference the Goal, Position, Status,
@@ -116,11 +116,12 @@
protected:
// Runs an iteration of the control loop.
- // goal is the last goal that was sent. It might be any number of cycles old.
- // position is the current position, or NULL if we didn't get a position this
- // cycle.
- // output is the values to be sent to the motors. This is NULL if the output
- // is going to be ignored and set to 0.
+ // goal is the last goal that was sent. It might be any number of cycles old
+ // or nullptr if we haven't ever received a goal.
+ // position is the current position, or nullptr if we didn't get a position
+ // this cycle.
+ // output is the values to be sent to the motors. This is nullptr if the
+ // output is going to be ignored and set to 0.
// status is the status of the control loop.
// Both output and status should be filled in by the implementation.
virtual void RunIteration(const GoalType *goal,
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 4f86697..49205d9 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -174,6 +174,17 @@
2.0 * M_PI / 180.0);*/
}
+// Tests that not having a goal doesn't break anything.
+TEST_F(ClawTest, NoGoal) {
+ for (int i = 0; i < 10; ++i) {
+ claw_plant_.SendPositionMessage();
+ claw_.Iterate();
+ claw_plant_.Simulate();
+
+ TickTime();
+ }
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 395f8b9..bcfc6a1 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -689,19 +689,28 @@
}
no_position_.Print();
- double wheel = goal->steering;
- double throttle = goal->throttle;
- bool quickturn = goal->quickturn;
+ bool control_loop_driving = false;
+ if (goal) {
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
#if HAVE_SHIFTERS
- bool highgear = goal->highgear;
+ bool highgear = goal->highgear;
#endif
- bool control_loop_driving = goal->control_loop_driving;
- double left_goal = goal->left_goal;
- double right_goal = goal->right_goal;
+ control_loop_driving = goal->control_loop_driving;
+ double left_goal = goal->left_goal;
+ double right_goal = goal->right_goal;
- dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
- goal->right_velocity_goal);
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+ goal->right_velocity_goal);
+#if HAVE_SHIFTERS
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+#else
+ dt_openloop.SetGoal(wheel, throttle, quickturn, false);
+#endif
+ }
+
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
@@ -714,11 +723,6 @@
}
}
dt_openloop.SetPosition(position);
-#if HAVE_SHIFTERS
- dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
-#else
- dt_openloop.SetGoal(wheel, throttle, quickturn, false);
-#endif
dt_openloop.Update();
if (control_loop_driving) {
@@ -732,7 +736,7 @@
}
dt_closedloop.Update(output == NULL, false);
}
-
+
// set the output status of the control loop state
if (status) {
bool done = false;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 96583da..7e14cf0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -170,6 +170,26 @@
VerifyNearGoal();
}
+// Tests that never having a goal doesn't break.
+TEST_F(DrivetrainTest, NoGoalStart) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ }
+}
+
+// Tests that never having a goal, but having driver's station messages, doesn't
+// break.
+TEST_F(DrivetrainTest, NoGoalWithRobotState) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+}
+
::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 9533ddf..65ee72e 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -56,7 +56,7 @@
}
Fridge::Fridge(control_loops::FridgeQueue *fridge)
- : aos::controls::ControlLoop<control_loops::FridgeQueue, false>(fridge),
+ : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
arm_loop_(new CappedStateFeedbackLoop(
StateFeedbackLoop<4, 2, 2>(MakeArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop(
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index eecfe1b..87dbfa4 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -40,7 +40,7 @@
};
class Fridge
- : public aos::controls::ControlLoop<control_loops::FridgeQueue, false> {
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
public:
explicit Fridge(
control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index d2c5253..e306553 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -43,7 +43,7 @@
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
bool last_auto_running = auto_running_;
auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.IsPressed(ControlBit::kEnabled);
+ data.GetControlBit(ControlBit::kEnabled);
if (auto_running_ != last_auto_running) {
if (auto_running_) {
StartAuto();
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 9228331..92f2875 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -10,7 +10,6 @@
const constants::Values::ZeroingConstants& constants) {
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
- index_pulse_count_after_reset_ = 0;
known_index_pos_ = constants.measured_index_position;
start_pos_samples_.reserve(max_sample_count_);
@@ -24,6 +23,19 @@
start_pos_samples_.clear();
zeroed_ = false;
wait_for_index_pulse_ = true;
+ last_used_index_pulse_count_ = 0;
+}
+
+double ZeroingEstimator::CalculateStartPosition(double start_average,
+ double latched_encoder) const {
+ // We calculate an aproximation of the value of the last index position.
+ // Also account for index pulses not lining up with integer multiples of the
+ // index_diff.
+ double index_pos = start_average + latched_encoder - known_index_pos_;
+ // We round index_pos to the closest valid value of the index.
+ double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
+ // Now we reverse the first calculation to get the accurate start position.
+ return accurate_index_pos - latched_encoder + known_index_pos_;
}
void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
@@ -32,7 +44,7 @@
// reset and wait for that count to change before we consider ourselves
// zeroed.
if (wait_for_index_pulse_) {
- index_pulse_count_after_reset_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses;
wait_for_index_pulse_ = false;
}
@@ -57,18 +69,15 @@
// If there are no index pulses to use or we don't have enough samples yet to
// have a well-filtered starting position then we use the filtered value as
// our best guess.
- if (info.index_pulses == index_pulse_count_after_reset_ ||
- offset_ratio_ready() < 1.0) {
+ if (!zeroed_ && (info.index_pulses == last_used_index_pulse_count_ ||
+ offset_ratio_ready() < 1.0)) {
start_pos_ = start_average;
- } else {
- // We calculate an aproximation of the value of the last index position.
- // Also account for index pulses not lining up with integer multiples of
- // the index_diff.
- double index_pos = start_average + info.latched_encoder - known_index_pos_;
- // We round index_pos to the closest valid value of the index.
- double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
- // Now we reverse the first calculation to get the accurate start position.
- start_pos_ = accurate_index_pos - info.latched_encoder + known_index_pos_;
+ } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses) {
+ // Note the accurate start position and the current index pulse count so
+ // that we only run this logic once per index pulse. That should be more
+ // resilient to corrupted intermediate data.
+ start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
+ last_used_index_pulse_count_ = info.index_pulses;
// Now that we have an accurate starting position we can consider ourselves
// zeroed.
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 5566533..9fa335a 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -58,6 +58,11 @@
}
private:
+ // This function calculates the start position given the internal state and
+ // the provided `latched_encoder' value.
+ double CalculateStartPosition(double start_average,
+ double latched_encoder) const;
+
// The estimated position.
double pos_;
// The distance between two consecutive index positions.
@@ -77,12 +82,15 @@
// account for the various ways the encoders get mounted into the robot.
double known_index_pos_;
// Flag for triggering logic that takes note of the current index pulse count
- // after a reset. See `index_pulse_count_after_reset_'.
+ // after a reset. See `last_used_index_pulse_count_'.
bool wait_for_index_pulse_;
// After a reset we keep track of the index pulse count with this. Only after
// the index pulse count changes (i.e. increments at least once or wraps
- // around) will we consider the mechanism zeroed.
- uint32_t index_pulse_count_after_reset_;
+ // around) will we consider the mechanism zeroed. We also use this to store
+ // the most recent `PotAndIndexPosition::index_pulses' value when the start
+ // position was calculated. It helps us calculate the start position only on
+ // index pulses to reject corrupted intermediate data.
+ uint32_t last_used_index_pulse_count_;
// Marker to track whether we're fully zeroed yet or not.
bool zeroed_;
};