Switched shooter to use the hybrid KF.
Change-Id: I4a25cffd9fa4877c3d0e79e74d75318045698aee
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 8991658..18fc795 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -42,10 +42,7 @@
history_[history_position_] = current_position;
history_position_ = (history_position_ + 1) % kHistoryLength;
- dt_velocity_ = (current_position - last_position_) /
- chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ dt_position_ = current_position - last_position_;
last_position_ = current_position;
}
@@ -53,7 +50,7 @@
void ShooterController::Reset() { reset_ = true; }
-void ShooterController::Update(bool disabled) {
+void ShooterController::Update(bool disabled, chrono::nanoseconds dt) {
loop_->mutable_R() = loop_->next_R();
if (::std::abs(loop_->R(1, 0)) < 1.0) {
// Kill power at low angular velocities.
@@ -102,8 +99,11 @@
X_hat_current_ = loop_->X_hat();
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
+ dt_velocity_ = dt_position_ /
+ chrono::duration_cast<chrono::duration<double>>(dt).count();
+ fixed_dt_velocity_ = dt_position_ / 0.005;
- loop_->Update(disabled);
+ loop_->Update(disabled, dt);
}
void ShooterController::SetStatus(ShooterStatus *status) {
@@ -115,13 +115,15 @@
status->voltage_error = X_hat_current_(2, 0);
status->position_error = position_error_;
status->instantaneous_velocity = dt_velocity_;
+ status->fixed_instantaneous_velocity = fixed_dt_velocity_;
}
void Shooter::Reset() { wheel_.Reset(); }
void Shooter::Iterate(const control_loops::ShooterGoal *goal,
- const double *position, double *output,
- control_loops::ShooterStatus *status) {
+ const double *position,
+ ::aos::monotonic_clock::time_point position_time,
+ double *output, control_loops::ShooterStatus *status) {
if (goal) {
// Update position/goal for our wheel.
wheel_.set_goal(goal->angular_velocity);
@@ -129,7 +131,13 @@
wheel_.set_position(*position);
- wheel_.Update(output == nullptr);
+ chrono::nanoseconds dt = ::aos::controls::kLoopFrequency;
+ if (last_time_ != ::aos::monotonic_clock::min_time) {
+ dt = position_time - last_time_;
+ }
+ last_time_ = position_time;
+
+ wheel_.Update(output == nullptr, dt);
wheel_.SetStatus(status);
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index 7dc44bf..d9b6f45 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -40,7 +40,7 @@
double error() const { return error_; }
// Executes the control loop for a cycle.
- void Update(bool disabled);
+ void Update(bool disabled, ::std::chrono::nanoseconds dt);
// Resets the kalman filter and any other internal state.
void Reset();
@@ -59,7 +59,9 @@
ptrdiff_t history_position_ = 0;
double error_ = 0.0;
+ double dt_position_ = 0.0;
double dt_velocity_ = 0.0;
+ double fixed_dt_velocity_ = 0.0;
double last_position_ = 0.0;
double average_angular_velocity_ = 0.0;
double min_velocity_ = 0.0;
@@ -83,7 +85,8 @@
// never be nullptr. goal can be nullptr if no goal exists, and output should
// be nullptr if disabled.
void Iterate(const control_loops::ShooterGoal *goal, const double *position,
- double *output, control_loops::ShooterStatus *status);
+ ::aos::monotonic_clock::time_point position_time, double *output,
+ control_loops::ShooterStatus *status);
// Sets the shooter up to reset the kalman filter next time Iterate is called.
void Reset();
@@ -93,6 +96,8 @@
bool last_ready_ = false;
double min_ = 0.0;
+ ::aos::monotonic_clock::time_point last_time_ =
+ ::aos::monotonic_clock::min_time;
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 0b5f2d9..35de4dc 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -50,7 +50,7 @@
output != nullptr ? &(output->voltage_intake) : nullptr,
&(status->intake));
shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
- &(position->theta_shooter),
+ &(position->theta_shooter), position->sent_time,
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));
indexer_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 5a50a06..0f707dc 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -102,6 +102,7 @@
// The current velocity measured as delta x / delta t in radians/sec.
double instantaneous_velocity;
+ double fixed_instantaneous_velocity;
// The error between our measurement and expected measurement in radians.
double position_error;