Fixed missing zeroed transition on intake and formatted.
Change-Id: I051c29f1ba5803766f4a6cd50c47bf97a47b2b1c
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 2095d6c..f0c85b6 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -63,7 +63,6 @@
CapGoal("R", &loop_->mutable_R());
}
-
void Intake::Correct(PotAndIndexPosition position) {
estimator_.UpdateEstimate(position);
@@ -74,6 +73,11 @@
}
}
+ if (!zeroed_ && estimator_.zeroed()) {
+ UpdateIntakeOffset(estimator_.offset());
+ zeroed_ = true;
+ }
+
Y_ << position.encoder;
Y_ += offset_;
loop_->Correct(Y_);
@@ -142,12 +146,15 @@
return false;
}
-void Intake::set_max_voltage(double voltage) { loop_->set_max_voltage(voltage); }
+void Intake::set_max_voltage(double voltage) {
+ loop_->set_max_voltage(voltage);
+}
void Intake::AdjustProfile(double max_angular_velocity,
double max_angular_acceleration) {
profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
- profile_.set_maximum_acceleration(UseUnlessZero(max_angular_acceleration, 10.0));
+ profile_.set_maximum_acceleration(
+ UseUnlessZero(max_angular_acceleration, 10.0));
}
void Intake::Reset() {
@@ -195,8 +202,7 @@
void Arm::UpdateShoulderOffset(double offset) {
const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0),
- offset);
+ LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0), offset);
loop_->mutable_X_hat()(0, 0) += doffset;
loop_->mutable_X_hat()(2, 0) += doffset;