Tune flywheels
Switch to hybrid kalman filter, plot voltage error
Change-Id: I9ade9a741aae58bdb3818c23bfe91b18786235f3
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 77fb769..07c3d99 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -11,8 +11,12 @@
namespace superstructure {
namespace shooter {
-FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
- : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
+FlywheelController::FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop)
+ : loop_(new StateFeedbackLoop<3, 1, 1, double,
+ StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(std::move(loop))) {
history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
0, ::aos::monotonic_clock::epoch()));
Y_.setZero();
@@ -26,6 +30,18 @@
void FlywheelController::set_position(
double current_position,
const aos::monotonic_clock::time_point position_timestamp) {
+ // Project time forwards.
+ const int newest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ if (!first_) {
+ loop_->UpdateObserver(
+ loop_->U(),
+ position_timestamp - std::get<1>(history_[newest_history_position]));
+ } else {
+ first_ = false;
+ }
+
// Update position in the model.
Y_ << current_position;
@@ -34,6 +50,8 @@
std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
position_timestamp);
history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ loop_->Correct(Y_);
}
double FlywheelController::voltage() const { return loop_->U(0, 0); }
@@ -45,22 +63,22 @@
disabled = true;
}
- loop_->Correct(Y_);
- loop_->Update(disabled);
+ loop_->UpdateController(disabled);
}
flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
flatbuffers::FlatBufferBuilder *fbb) {
// Compute the oldest point in the history.
- const int oldest_history_position =
+ const int oldest_history_position = history_position_;
+ const int newest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
const double total_loop_time = ::aos::time::DurationInSeconds(
- std::get<1>(history_[history_position_]) -
+ std::get<1>(history_[newest_history_position]) -
std::get<1>(history_[oldest_history_position]));
const double distance_traveled =
- std::get<0>(history_[history_position_]) -
+ std::get<0>(history_[newest_history_position]) -
std::get<0>(history_[oldest_history_position]);
// Compute the distance moved over that time period.
@@ -70,6 +88,7 @@
builder.add_avg_angular_velocity(avg_angular_velocity_);
builder.add_angular_velocity(loop_->X_hat(1, 0));
+ builder.add_voltage_error(loop_->X_hat(2, 0));
builder.add_angular_velocity_goal(last_goal_);
return builder.Finish();
}
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index a3e9bdb..e130389 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -18,7 +18,9 @@
// Handles the velocity control of each flywheel.
class FlywheelController {
public:
- FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop);
+ FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop);
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
@@ -45,7 +47,10 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
@@ -59,6 +64,8 @@
double last_goal_ = 0;
+ bool first_ = true;
+
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
};
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 25f6a6a..a9f1c4c 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -12,7 +12,7 @@
namespace shooter {
namespace {
-const double kVelocityTolerance = 0.01;
+const double kVelocityTolerance = 20.0;
} // namespace
Shooter::Shooter()
@@ -46,16 +46,18 @@
accelerator_right_.set_position(position->theta_accelerator_right(),
position_timestamp);
- finisher_.Update(output == nullptr);
- accelerator_left_.Update(output == nullptr);
- accelerator_right_.Update(output == nullptr);
-
// Update goal.
if (goal) {
finisher_.set_goal(goal->velocity_finisher());
accelerator_left_.set_goal(goal->velocity_accelerator());
accelerator_right_.set_goal(goal->velocity_accelerator());
+ }
+ finisher_.Update(output == nullptr);
+ accelerator_left_.Update(output == nullptr);
+ accelerator_right_.Update(output == nullptr);
+
+ if (goal) {
if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
goal->velocity_accelerator() > kVelocityTolerance) {
ready_ = true;