Added shooter and tests.
Added the shooter into the main superstructure and added tests.
Change-Id: I6c9afe3c74a08251854805050c40fafdca90fba8
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 0f920a4..77fb769 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -13,7 +13,8 @@
FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
: loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
- history_.fill(0);
+ history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
+ 0, ::aos::monotonic_clock::epoch()));
Y_.setZero();
}
@@ -22,12 +23,16 @@
last_goal_ = angular_velocity_goal;
}
-void FlywheelController::set_position(double current_position) {
+void FlywheelController::set_position(
+ double current_position,
+ const aos::monotonic_clock::time_point position_timestamp) {
// Update position in the model.
Y_ << current_position;
// Add the position to the history.
- history_[history_position_] = current_position;
+ history_[history_position_] =
+ std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
+ position_timestamp);
history_position_ = (history_position_ + 1) % kHistoryLength;
}
@@ -50,15 +55,20 @@
const int oldest_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_[oldest_history_position]));
+
+ const double distance_traveled =
+ std::get<0>(history_[history_position_]) -
+ std::get<0>(history_[oldest_history_position]);
+
// Compute the distance moved over that time period.
- const double avg_angular_velocity =
- (history_[oldest_history_position] - history_[history_position_]) /
- (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
- static_cast<double>(kHistoryLength - 1));
+ avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
FlywheelControllerStatusBuilder builder(*fbb);
- builder.add_avg_angular_velocity(avg_angular_velocity);
+ builder.add_avg_angular_velocity(avg_angular_velocity_);
builder.add_angular_velocity(loop_->X_hat(1, 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 20cd681..5853097 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -23,7 +23,8 @@
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
// Sets the current encoder position in radians
- void set_position(double current_position);
+ void set_position(double current_position,
+ const aos::monotonic_clock::time_point position_timestamp);
// Populates the status structure.
flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
@@ -38,6 +39,8 @@
// Executes the control loop for a cycle.
void Update(bool disabled);
+ double avg_angular_velocity() { return avg_angular_velocity_; }
+
private:
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -46,8 +49,14 @@
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
- ::std::array<double, kHistoryLength> history_;
+ ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
+ kHistoryLength>
+ history_;
ptrdiff_t history_position_ = 0;
+
+ // Average velocity logging.
+ double avg_angular_velocity_;
+
double last_goal_ = 0;
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 6a1d201..25f6a6a 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -11,29 +11,59 @@
namespace superstructure {
namespace shooter {
+namespace {
+const double kVelocityTolerance = 0.01;
+} // namespace
+
Shooter::Shooter()
: finisher_(finisher::MakeIntegralFinisherLoop()),
accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
+bool Shooter::UpToSpeed(const ShooterGoal *goal) {
+ return (
+ std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_right_.avg_angular_velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_finisher() - finisher_.velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
+ kVelocityTolerance);
+}
+
flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
const ShooterGoal *goal, const ShooterPosition *position,
- flatbuffers::FlatBufferBuilder *fbb, OutputT *output) {
- if (goal) {
- // Update position/goal for our two shooter sides.
- finisher_.set_goal(goal->velocity_finisher());
- accelerator_left_.set_goal(goal->velocity_accelerator());
- accelerator_right_.set_goal(goal->velocity_accelerator());
- }
-
- finisher_.set_position(position->theta_finisher());
- accelerator_left_.set_position(position->theta_accelerator_left());
- accelerator_right_.set_position(position->theta_accelerator_right());
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+ const aos::monotonic_clock::time_point position_timestamp) {
+ // Update position, output, and status for our two shooter sides.
+ finisher_.set_position(position->theta_finisher(), position_timestamp);
+ accelerator_left_.set_position(position->theta_accelerator_left(),
+ position_timestamp);
+ 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());
+
+ if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
+ goal->velocity_accelerator() > kVelocityTolerance) {
+ ready_ = true;
+ } else {
+ ready_ = false;
+ }
+ }
+
flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
finisher_.SetStatus(fbb);
flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 88bcb3b..f72eeeb 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -21,11 +21,17 @@
flatbuffers::Offset<ShooterStatus> RunIteration(
const ShooterGoal *goal, const ShooterPosition *position,
- flatbuffers::FlatBufferBuilder *fbb, OutputT *output);
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+ const aos::monotonic_clock::time_point position_timestamp);
+
+ bool ready() { return ready_; }
private:
FlywheelController finisher_, accelerator_left_, accelerator_right_;
+ bool UpToSpeed(const ShooterGoal *goal);
+ bool ready_ = false;
+
DISALLOW_COPY_AND_ASSIGN(Shooter);
};