Added detection of bad index zeroing pulse.
Change-Id: I1607ce0d37b336136a4a278afc8a0f2897d1c597
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 636735e..13775b9 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -120,8 +120,7 @@
{-0.05, M_PI / 2.0 + 0.1, 0.0, M_PI / 2.0},
// Zeroing constants for wrist.
- {kZeroingSampleSize, kClawEncoderIndexDifference,
- 0.9},
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.9, 0.3},
6.308141,
kClawPistonSwitchTime,
@@ -139,11 +138,11 @@
{-M_PI / 2 - 0.05, M_PI / 2 + 0.05, -M_PI / 2, M_PI / 2},
// Elevator zeroing constants: left, right.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0, 0.3},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0, 0.3},
// Arm zeroing constants: left, right.
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0, 0.3},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0, 0.3},
0.7,
-0.08,
-3.5 - 0.01 - -0.02,
@@ -188,8 +187,8 @@
{-0.05, M_PI / 2.0 + 0.1, 0.0, M_PI / 2.0},
// Zeroing constants for wrist.
- {kZeroingSampleSize, kClawEncoderIndexDifference,
- 0.9104180000000001},
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.9104180000000001,
+ 0.3},
6.308141,
kClawPistonSwitchTime,
@@ -208,11 +207,11 @@
// Elevator zeroing constants: left, right.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.066380},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.051888},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.066380, 0.3},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.051888, 0.3},
// Arm zeroing constants: left, right.
- {kZeroingSampleSize, kArmEncoderIndexDifference, -0.324437},
- {kZeroingSampleSize, kArmEncoderIndexDifference, -0.064683},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.324437, 0.3},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.064683, 0.3},
0.722230 - -0.000594,
-0.081354 - -0.000374,
-3.509611 - 0.007415 - -0.019081,
@@ -258,7 +257,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kClawEncoderIndexDifference, 0.973311},
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.973311, 0.3},
6.1663463999999992,
kClawPistonSwitchTime,
@@ -277,13 +276,12 @@
// Elevator zeroing constants: left, right.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kElevatorEncoderIndexDifference,
- 0.016041 + 0.001290},
+ 0.016041 + 0.001290, 0.3},
{kZeroingSampleSize, kElevatorEncoderIndexDifference,
- 0.011367 + 0.003216},
+ 0.011367 + 0.003216, 0.3},
// Arm zeroing constants: left, right.
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.060592},
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.210155},
-
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.060592, 0.3},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, 0.210155, 0.3},
0.72069366666666679 - 0.026008,
-0.078959636363636357 - 0.024646,
-3.509611 - 0.007415 - -0.019081 - 0.029393 - -0.013585,
diff --git a/frc971/constants.h b/frc971/constants.h
index 18e85ae..d66853b 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -63,6 +63,9 @@
double index_difference;
// The absolute position in scaled units of one of the index pulses.
double measured_index_position;
+ // Value between 0 and 1 which determines a fraction of the index_diff
+ // you want to use.
+ double allowable_encoder_error;
};
// Defines a range of motion for a subsystem.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 0c12d27..004ea7a 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -52,8 +52,7 @@
}
void Claw::SetClawOffset(double offset) {
- LOG(INFO, "Changing claw offset from %f to %f.\n",
- claw_offset_, offset);
+ LOG(INFO, "Changing claw offset from %f to %f.\n", claw_offset_, offset);
const double doffset = offset - claw_offset_;
// Adjust the height. The derivative should not need to be updated since the
@@ -109,11 +108,10 @@
return claw_zeroing_velocity_;
}
-void Claw::RunIteration(
- const control_loops::ClawQueue::Goal *unsafe_goal,
- const control_loops::ClawQueue::Position *position,
- control_loops::ClawQueue::Output *output,
- control_loops::ClawQueue::Status *status) {
+void Claw::RunIteration(const control_loops::ClawQueue::Goal *unsafe_goal,
+ const control_loops::ClawQueue::Position *position,
+ control_loops::ClawQueue::Output *output,
+ control_loops::ClawQueue::Status *status) {
const auto &values = constants::GetValues();
if (WasReset()) {
@@ -168,8 +166,8 @@
SetClawOffset(claw_estimator_.offset());
} else if (!disable) {
claw_goal_velocity = claw_zeroing_velocity();
- claw_goal_ += claw_goal_velocity *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ claw_goal_ +=
+ claw_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
}
break;
@@ -226,8 +224,7 @@
double deltaR = claw_loop_->UnsaturateOutputGoalChange();
// Move the claw goal by the amount observed.
- LOG(WARNING, "Moving claw goal by %f to handle saturation.\n",
- deltaR);
+ LOG(WARNING, "Moving claw goal by %f to handle saturation.\n", deltaR);
claw_goal_ += deltaR;
}
}
@@ -260,9 +257,11 @@
status->goal_angle = claw_goal_;
if (output) {
- status->rollers_open = !output->rollers_closed &&
+ status->rollers_open =
+ !output->rollers_closed &&
(Time::Now() - last_piston_edge_ >= values.claw.piston_switch_time);
- status->rollers_closed = output->rollers_closed &&
+ status->rollers_closed =
+ output->rollers_closed &&
(Time::Now() - last_piston_edge_ >= values.claw.piston_switch_time);
} else {
status->rollers_open = false;
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 6abc082..381bd15 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -37,8 +37,7 @@
double max_voltage_;
};
-class Claw
- : public aos::controls::ControlLoop<control_loops::ClawQueue> {
+class Claw : public aos::controls::ControlLoop<control_loops::ClawQueue> {
public:
enum State {
// Waiting to receive data before doing anything.
@@ -59,11 +58,10 @@
control_loops::ClawQueue *claw_queue = &control_loops::claw_queue);
protected:
- virtual void RunIteration(
- const control_loops::ClawQueue::Goal *goal,
- const control_loops::ClawQueue::Position *position,
- control_loops::ClawQueue::Output *output,
- control_loops::ClawQueue::Status *status);
+ virtual void RunIteration(const control_loops::ClawQueue::Goal *goal,
+ const control_loops::ClawQueue::Position *position,
+ control_loops::ClawQueue::Output *output,
+ control_loops::ClawQueue::Status *status);
private:
friend class testing::ClawTest_DisabledGoal_Test;
@@ -112,5 +110,4 @@
} // namespace control_loops
} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_CLAW_H_
-
+#endif // FRC971_CONTROL_LOOPS_CLAW_H_
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 46d4f28..a4aa2e7 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -23,7 +23,6 @@
constexpr double kArmZeroingVelocity = 0.20;
} // namespace
-
template <int S>
void CappedStateFeedbackLoop<S>::CapU() {
VoltageCap(max_voltage_, this->U(0, 0), this->U(1, 0), &this->mutable_U(0, 0),
@@ -67,10 +66,9 @@
right_arm_estimator_.offset_ratio_ready() < 1.0) {
state_ = INITIALIZING;
} else if (!left_elevator_estimator_.zeroed() ||
- !right_elevator_estimator_.zeroed()) {
+ !right_elevator_estimator_.zeroed()) {
state_ = ZEROING_ELEVATOR;
- } else if (!left_arm_estimator_.zeroed() ||
- !right_arm_estimator_.zeroed()) {
+ } else if (!left_arm_estimator_.zeroed() || !right_arm_estimator_.zeroed()) {
state_ = ZEROING_ARM;
} else {
state_ = RUNNING;
@@ -330,7 +328,7 @@
} else if (!disable) {
elevator_goal_velocity = elevator_zeroing_velocity();
elevator_goal_ += elevator_goal_velocity *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ ::aos::controls::kLoopFrequency.ToSeconds();
}
break;
@@ -346,8 +344,8 @@
LOG(DEBUG, "Zeroed the arm!\n");
} else if (!disable) {
arm_goal_velocity = arm_zeroing_velocity();
- arm_goal_ += arm_goal_velocity *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ arm_goal_ +=
+ arm_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
}
break;
@@ -530,14 +528,13 @@
status->grabbers.bottom_front = false;
status->grabbers.bottom_back = false;
}
- zeroing::PopulateEstimatorState(left_arm_estimator_,
- &status->left_arm_state);
+ zeroing::PopulateEstimatorState(left_arm_estimator_, &status->left_arm_state);
zeroing::PopulateEstimatorState(right_arm_estimator_,
&status->right_arm_state);
zeroing::PopulateEstimatorState(left_elevator_estimator_,
- &status->left_elevator_state);
+ &status->left_elevator_state);
zeroing::PopulateEstimatorState(right_elevator_estimator_,
- &status->right_elevator_state);
+ &status->right_elevator_state);
status->estopped = (state_ == ESTOP);
status->state = state_;
last_state_ = state_;
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 9b2347d..d8c77e8 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -6,8 +6,8 @@
namespace frc971 {
namespace zeroing {
-void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
- EstimatorState *state) {
+void PopulateEstimatorState(const zeroing::ZeroingEstimator& estimator,
+ EstimatorState* state) {
state->error = estimator.error();
state->zeroed = estimator.zeroed();
state->position = estimator.position();
@@ -18,9 +18,8 @@
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
known_index_pos_ = constants.measured_index_position;
-
+ allowable_encoder_error_ = constants.allowable_encoder_error;
start_pos_samples_.reserve(max_sample_count_);
-
Reset();
}
@@ -31,6 +30,7 @@
zeroed_ = false;
wait_for_index_pulse_ = true;
last_used_index_pulse_count_ = 0;
+ first_start_pos_ = 0.0;
error_ = false;
}
@@ -93,10 +93,28 @@
// resilient to corrupted intermediate data.
start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
last_used_index_pulse_count_ = info.index_pulses;
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = start_pos_;
+ LOG(INFO, "latching start position %f\n", first_start_pos_);
+ }
// Now that we have an accurate starting position we can consider ourselves
// zeroed.
zeroed_ = true;
+ // Throw an error if first_start_pos is bigger/smaller than
+ // allowable_encoder_error_ * index_diff +
+ // start_pos.
+ if (::std::abs(first_start_pos_ - start_pos_) >
+ allowable_encoder_error_ * index_diff_) {
+ if (!error_) {
+ LOG(ERROR,
+ "Encoder ticks out of range since last index pulse. first start "
+ "position: %f recent starting position: %f\n",
+ first_start_pos_, start_pos_);
+ error_ = true;
+ }
+ }
}
pos_ = start_pos_ + info.encoder;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 7fa2262..08911ac 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -102,6 +102,12 @@
// Marker to track whether an error has occurred. This gets reset to false
// whenever Reset() is called.
bool error_;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+ // Value between 0 and 1 which determines a fraction of the index_diff
+ // you want to use.
+ double allowable_encoder_error_;
};
// Populates an EstimatorState struct with information from the zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 2ef6865..404b0e0 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -20,6 +20,7 @@
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
+static const double kIndexErrorFraction = 0.3;
class ZeroingTest : public ::testing::Test {
protected:
@@ -40,8 +41,8 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
@@ -64,8 +65,8 @@
double position = 3.6 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(position, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure that the zeroing code does not consider itself zeroed until we
// collect a good amount of samples. In this case we're waiting until the
@@ -83,8 +84,8 @@
double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
@@ -116,8 +117,8 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
@@ -150,8 +151,8 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
for (unsigned int i = 0; i < kSampleSize / 2; i++) {
MoveTo(&sim, &estimator, 3.5 * index_diff);
@@ -163,8 +164,8 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
MoveTo(&sim, &estimator, 3.1 * index_diff);
@@ -179,8 +180,8 @@
double index_diff = 0.6;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
for (unsigned int i = 0; i < kSampleSize; i++) {
@@ -214,8 +215,8 @@
const double known_index_pos = 3.5 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.3 * index_diff, index_diff / 3.0, known_index_pos);
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, known_index_pos});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
for (unsigned int i = 0; i < kSampleSize; i++) {
@@ -239,8 +240,8 @@
TEST_F(ZeroingTest, BasicErrorAPITest) {
const double index_diff = 1.0;
- ZeroingEstimator estimator(
- Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
PositionSensorSimulator sim(index_diff);
sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
@@ -260,5 +261,37 @@
ASSERT_FALSE(estimator.error());
}
+// I want to test that the the zeroing class can
+// detect an error when the starting position
+// changes too much. I do so by creating the
+// simulator at an 'X' positon, making sure
+// that the estimator is zeroed, and then
+// initializing the simulator at another
+// position. After making sure it's zeroed,
+// if the error() function returns true,
+// then, it works.
+TEST_F(ZeroingTest, TestOffsetError) {
+ const double index_diff = 0.8;
+ const double known_index_pos = 2 * index_diff;
+ int sample_size = 30;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
+ ZeroingEstimator estimator(Values::ZeroingConstants{
+ sample_size, index_diff, known_index_pos, kIndexErrorFraction});
+
+ for (int i = 0; i < sample_size; i++) {
+ MoveTo(&sim, &estimator, 13 * index_diff);
+ }
+ MoveTo(&sim, &estimator, 8 * index_diff);
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+ sim.Initialize(9.0 * index_diff + 0.31 * index_diff, index_diff / 3.0,
+ known_index_pos);
+ MoveTo(&sim, &estimator, 9 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.error());
+}
+
} // namespace zeroing
} // namespace frc971