Added detection of bad index zeroing pulse.
Change-Id: I1607ce0d37b336136a4a278afc8a0f2897d1c597
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_;