Tuned intake.
Change-Id: I028f6cbb8df55d281734ca39abb4f62b7fd27793
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
index 7212fd3..080720a 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -71,7 +71,7 @@
goal_velocity = 0.0;
} else {
goal_velocity = ::aos::Clip(
- ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 6.0), -10.0, 10.0);
+ ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 12.0), -16.0, 16.0);
}
// Computes the goal.
loop_->mutable_R() << 0.0, goal_velocity, 0.0, goal_velocity,
@@ -84,8 +84,8 @@
const double *unsafe_goal) {
status->goal_position = goal_angle(unsafe_goal);
status->goal_velocity = loop_->R(1, 0);
- status->spring_position = loop_->X_hat(0);
- status->spring_velocity = loop_->X_hat(1);
+ status->spring_position = loop_->X_hat(0) - loop_->X_hat(2);
+ status->spring_velocity = loop_->X_hat(1) - loop_->X_hat(3);
status->motor_position = loop_->X_hat(2);
status->motor_velocity = loop_->X_hat(3);
status->delayed_voltage = loop_->X_hat(4);
@@ -102,7 +102,6 @@
const control_loops::IntakeElasticSensors *position,
control_loops::IntakeVoltage *output,
control_loops::IntakeSideStatus *status) {
- double intake_last_position_ = status->estimator_state.position;
zeroing_estimator_.UpdateEstimate(position->motor_position);
switch (state_) {
@@ -164,6 +163,7 @@
status->zeroed = zeroing_estimator_.zeroed();
status->estopped = (state_ == State::ESTOP);
status->state = static_cast<int32_t>(state_);
+ intake_last_position_ = status->estimator_state.position;
}
} // namespace intake