Recalibrated all our loops.
Shooter works much much better now. There's still a bit of
oscillation, but it's a lot better.
Change-Id: Ie3b8e09125d62e5b2011c6874fab97641c0b6526
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index cffc7ad..7c119e0 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -626,6 +626,7 @@
profiled_subsystem_.PopulateTurretStatus(turret_status);
turret_status->estopped = (state_ == State::ESTOP);
turret_status->state = static_cast<int32_t>(state_);
+ turret_status->turret_encoder_angle = profiled_subsystem_.turret_position();
if (vision_time_adjuster_.valid()) {
turret_status->vision_angle = vision_time_adjuster_.goal();
turret_status->raw_vision_angle =
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index f864696..6df4b04 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -77,6 +77,8 @@
ready_ = std::abs(error_) < kTolerance && loop_->next_R(2, 0) > 1.0;
+ // If we are no longer ready, but were, and are spinning, then we shot a ball.
+ // Reset the KF.
if (last_ready_ && !ready_ && loop_->next_R(2, 0) > 1.0 && error_ < 0.0) {
needs_reset_ = true;
min_velocity_ = velocity();
@@ -89,11 +91,9 @@
}
}
if (reset_) {
- loop_->mutable_X_hat(0, 0) = Y_(0, 0);
- // TODO(austin): This should be 0 for the WPILib reset since dt_velocity_
- // will be rubbish.
- loop_->mutable_X_hat(1, 0) = dt_velocity_;
- loop_->mutable_X_hat(2, 0) = 0.0;
+ // TODO(austin): I'd rather not be incrementing X_hat each time. Sort out
+ // something better.
+ loop_->mutable_X_hat(3, 0) += 1.5;
reset_ = false;
}
last_ready_ = ready_;
@@ -110,6 +110,7 @@
void ShooterController::SetStatus(ShooterStatus *status) {
status->avg_angular_velocity = average_angular_velocity_;
+ status->filtered_velocity = X_hat_current_(1, 0);
status->angular_velocity = X_hat_current_(2, 0);
status->ready = ready_;
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index c7283b2..a771f2e 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -112,6 +112,7 @@
// The current velocity measured as delta x / delta t in radians/sec.
double instantaneous_velocity;
+ double filtered_velocity;
double fixed_instantaneous_velocity;
// The error between our measurement and expected measurement in radians.
@@ -174,6 +175,8 @@
double raw_vision_angle;
double vision_angle;
bool vision_tracking;
+
+ double turret_encoder_angle;
};
queue_group SuperstructureQueue {