Began shooter integration in superstructure class.
This adds the shooter control loops to the superstructure class.
Change-Id: Ia54550a8c9598349347df7df8e07d81becd1673e
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index c6dce6c..6c8291d 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -28,6 +28,7 @@
'//y2017/control_loops/superstructure/hood',
'//y2017/control_loops/superstructure/turret',
'//y2017/control_loops/superstructure/intake',
+ '//y2017/control_loops/superstructure/shooter',
'//y2017:constants',
],
)
@@ -40,15 +41,16 @@
deps = [
':superstructure_queue',
':superstructure_lib',
- '//aos/testing:googletest',
- '//aos/common:queues',
'//aos/common/controls:control_loop_test',
'//aos/common:math',
+ '//aos/common:queues',
'//aos/common:time',
+ '//aos/testing:googletest',
'//frc971/control_loops:position_sensor_sim',
'//frc971/control_loops:team_number_test_environment',
'//y2017/control_loops/superstructure/hood:hood_plants',
- '//y2017/control_loops/superstructure/turret:turret_plants',
'//y2017/control_loops/superstructure/intake:intake_plants',
+ '//y2017/control_loops/superstructure/shooter:shooter_plants',
+ '//y2017/control_loops/superstructure/turret:turret_plants',
],
)
diff --git a/y2017/control_loops/superstructure/shooter/BUILD b/y2017/control_loops/superstructure/shooter/BUILD
index cb9fcf4..e689237 100644
--- a/y2017/control_loops/superstructure/shooter/BUILD
+++ b/y2017/control_loops/superstructure/shooter/BUILD
@@ -1,3 +1,7 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
genrule(
name = 'genrule_shooter',
cmd = '$(location //y2017/control_loops/python:shooter) $(OUTS)',
@@ -27,3 +31,19 @@
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'shooter',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'shooter.cc',
+ ],
+ hdrs = [
+ 'shooter.h',
+ ],
+ deps = [
+ ':shooter_plants',
+ '//aos/common/controls:control_loop',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
new file mode 100644
index 0000000..c73a008
--- /dev/null
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -0,0 +1,147 @@
+#include "y2017/control_loops/superstructure/shooter/shooter.h"
+
+#include <chrono>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2017/control_loops/superstructure/shooter/shooter.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+namespace {
+constexpr double kTolerance = 10.0;
+} // namespace
+
+// TODO(austin): Pseudo current limit?
+
+ShooterController::ShooterController()
+ : loop_(new StateFeedbackLoop<3, 1, 1>(
+ superstructure::shooter::MakeIntegralShooterLoop())) {
+ history_.fill(0);
+ Y_.setZero();
+}
+
+void ShooterController::set_goal(double angular_velocity_goal) {
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+}
+
+void ShooterController::set_position(double current_position) {
+ // Update position in the model.
+ Y_ << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ dt_velocity_ = (current_position - last_position_) / 0.005;
+ last_position_ = current_position;
+}
+
+double ShooterController::voltage() const { return loop_->U(0, 0); }
+
+void ShooterController::Reset() { reset_ = true; }
+
+void ShooterController::Update(bool disabled) {
+ loop_->mutable_R() = loop_->next_R();
+ if (::std::abs(loop_->R(1, 0)) < 1.0) {
+ // Kill power at low angular velocities.
+ disabled = true;
+ }
+
+ loop_->Correct(Y_);
+
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ // Compute the distance moved over that time period.
+ average_angular_velocity_ =
+ (history_[oldest_history_position] - history_[history_position_]) /
+ (chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count() *
+ static_cast<double>(kHistoryLength - 1));
+
+ // Ready if average angular velocity is close to the goal.
+ error_ = average_angular_velocity_ - loop_->next_R(1, 0);
+
+ ready_ = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
+
+ if (last_ready_ && !ready_ && loop_->next_R(1, 0) > 1.0 && error_ < 0.0) {
+ needs_reset_ = true;
+ min_velocity_ = loop_->X_hat(1, 0);
+ }
+ if (needs_reset_) {
+ min_velocity_ = ::std::min(min_velocity_, loop_->X_hat(1, 0));
+ if (loop_->X_hat(1, 0) > min_velocity_ + 5.0) {
+ reset_ = true;
+ needs_reset_ = false;
+ }
+ }
+ if (reset_) {
+ loop_->mutable_X_hat(2, 0) = 0.0;
+ loop_->mutable_X_hat(1, 0) = dt_velocity_;
+ loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ reset_ = false;
+ }
+ last_ready_ = ready_;
+
+ X_hat_current_ = loop_->X_hat();
+ position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
+
+ loop_->Update(disabled);
+}
+
+void ShooterController::SetStatus(ShooterStatus *status) {
+ status->avg_angular_velocity = average_angular_velocity_;
+
+ status->angular_velocity = X_hat_current_(1, 0);
+ status->ready = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
+
+ status->voltage_error = X_hat_current_(2, 0);
+ status->position_error = position_error_;
+ status->instantaneous_velocity = dt_velocity_;
+}
+
+void Shooter::Reset() { wheel_.Reset(); }
+
+void Shooter::Iterate(const control_loops::ShooterGoal *goal,
+ const double *position, double *output,
+ control_loops::ShooterStatus *status) {
+ if (goal) {
+ // Update position/goal for our wheel.
+ wheel_.set_goal(goal->angular_velocity);
+ }
+
+ wheel_.set_position(*position);
+
+ wheel_.Update(output == nullptr);
+
+ wheel_.SetStatus(status);
+
+ if (last_ready_ && !status->ready) {
+ min_ = wheel_.dt_velocity();
+ } else if (!status->ready) {
+ min_ = ::std::min(min_, wheel_.dt_velocity());
+ } else if (!last_ready_ && status->ready) {
+ LOG(INFO, "Shot min was [%f]\n", min_);
+ }
+
+ if (output) {
+ *output = wheel_.voltage();
+ }
+ last_ready_ = status->ready;
+}
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
new file mode 100644
index 0000000..7f11267
--- /dev/null
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -0,0 +1,101 @@
+#ifndef Y2017_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+#define Y2017_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2017/control_loops/superstructure/shooter/shooter_integral_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+class ShooterController {
+ public:
+ ShooterController();
+
+ // 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);
+
+ // Populates the status structure.
+ void SetStatus(control_loops::ShooterStatus *status);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
+ double dt_velocity() const { return dt_velocity_; }
+
+ double error() const { return error_; }
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ // Resets the kalman filter and any other internal state.
+ void Reset();
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 5;
+ ::std::array<double, kHistoryLength> history_;
+ ptrdiff_t history_position_ = 0;
+
+ double error_ = 0.0;
+ double dt_velocity_ = 0.0;
+ double last_position_ = 0.0;
+ double average_angular_velocity_ = 0.0;
+ double min_velocity_ = 0.0;
+ double position_error_ = 0.0;
+
+ Eigen::Matrix<double, 3, 1> X_hat_current_;
+
+ bool ready_ = false;
+ bool needs_reset_ = false;
+ bool reset_ = false;
+
+ bool last_ready_ = false;
+ DISALLOW_COPY_AND_ASSIGN(ShooterController);
+};
+
+class Shooter {
+ public:
+ Shooter() {}
+
+ // Iterates the shooter control loop one cycle. position and status must
+ // 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);
+
+ // Sets the shooter up to reset the kalman filter next time Iterate is called.
+ void Reset();
+
+ private:
+ ShooterController wheel_;
+
+ bool last_ready_ = false;
+ double min_ = 0.0;
+
+ DISALLOW_COPY_AND_ASSIGN(Shooter);
+};
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index f579292..99de502 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -26,13 +26,13 @@
hood_.Reset();
turret_.Reset();
intake_.Reset();
+ shooter_.Reset();
}
hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
&(position->hood),
output != nullptr ? &(output->voltage_hood) : nullptr,
&(status->hood));
-
turret_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
&(position->turret),
output != nullptr ? &(output->voltage_turret) : nullptr,
@@ -42,6 +42,10 @@
&(position->intake),
output != nullptr ? &(output->voltage_intake) : nullptr,
&(status->intake));
+ shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
+ &(position->theta_shooter),
+ output != nullptr ? &(output->voltage_shooter) : nullptr,
+ &(status->shooter));
}
} // namespace superstructure
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 4811ab1..4dc99a5 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -9,6 +9,7 @@
#include "y2017/control_loops/superstructure/turret/turret.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/shooter/shooter.h"
namespace y2017 {
namespace control_loops {
@@ -36,6 +37,7 @@
hood::Hood hood_;
turret::Turret turret_;
intake::Intake intake_;
+ shooter::Shooter shooter_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 43e6004..4a5edd8 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -81,6 +81,15 @@
// If true, we have aborted.
bool estopped;
+
+ // The estimated voltage error from the kalman filter in volts.
+ double voltage_error;
+
+ // The current velocity measured as delta x / delta t in radians/sec.
+ double instantaneous_velocity;
+
+ // The error between our measurement and expected measurement in radians.
+ double position_error;
};
queue_group SuperstructureQueue {
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 25de1a3..6f96066 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,8 +12,9 @@
#include "gtest/gtest.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/hood/hood_plant.h"
-#include "y2017/control_loops/superstructure/turret/turret_plant.h"
#include "y2017/control_loops/superstructure/intake/intake_plant.h"
+#include "y2017/control_loops/superstructure/shooter/shooter_plant.h"
+#include "y2017/control_loops/superstructure/turret/turret_plant.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -28,6 +29,25 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+class ShooterPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU() override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
class HoodPlant : public StateFeedbackPlant<2, 1, 1> {
public:
explicit HoodPlant(StateFeedbackPlant<2, 1, 1> &&other)
@@ -102,6 +122,9 @@
::y2017::control_loops::superstructure::intake::MakeIntakePlant())),
intake_pot_encoder_(constants::Values::kIntakeEncoderIndexDifference),
+ shooter_plant_(new ShooterPlant(::y2017::control_loops::superstructure::
+ shooter::MakeShooterPlant())),
+
superstructure_queue_(".y2017.control_loops.superstructure", 0xdeadbeef,
".y2017.control_loops.superstructure.goal",
".y2017.control_loops.superstructure.position",
@@ -158,6 +181,7 @@
hood_pot_encoder_.GetSensorValues(&position->hood);
turret_pot_encoder_.GetSensorValues(&position->turret);
intake_pot_encoder_.GetSensorValues(&position->intake);
+ position->theta_shooter = shooter_plant_->Y(0, 0);
position.Send();
}
@@ -170,6 +194,8 @@
double intake_position() const { return intake_plant_->X(0, 0); }
double intake_velocity() const { return intake_plant_->X(1, 0); }
+ double shooter_velocity() const { return shooter_plant_->X(1, 0); }
+
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
void set_hood_power_error(double power_error) {
@@ -184,6 +210,10 @@
intake_plant_->set_voltage_offset(power_error);
}
+ void set_shooter_voltage_offset(double power_error) {
+ shooter_plant_->set_voltage_offset(power_error);
+ }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
@@ -228,9 +258,14 @@
intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
intake_plant_->voltage_offset();
+ shooter_plant_->mutable_U()
+ << superstructure_queue_.output->voltage_shooter +
+ shooter_plant_->voltage_offset();
+
hood_plant_->Update();
turret_plant_->Update();
intake_plant_->Update();
+ shooter_plant_->Update();
const double angle_hood = hood_plant_->Y(0, 0);
const double angle_turret = turret_plant_->Y(0, 0);
@@ -258,6 +293,8 @@
::std::unique_ptr<IntakePlant> intake_plant_;
PositionSensorSimulator intake_pot_encoder_;
+ ::std::unique_ptr<ShooterPlant> shooter_plant_;
+
SuperstructureQueue superstructure_queue_;
};
@@ -294,6 +331,14 @@
superstructure_queue_.status->intake.position, 0.001);
EXPECT_NEAR(superstructure_queue_.goal->intake.distance,
superstructure_plant_.intake_position(), 0.001);
+
+ EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
+ superstructure_queue_.status->shooter.angular_velocity, 0.1);
+ EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
+ superstructure_queue_.status->shooter.avg_angular_velocity,
+ 0.1);
+ EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
+ superstructure_plant_.shooter_velocity(), 0.1);
}
// Runs one iteration of the whole simulation.
@@ -611,6 +656,60 @@
// TODO(austin): Test saturation
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(SuperstructureTest, SpinUpAndDown) {
+ // Spin up.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
+ goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->shooter.angular_velocity = 300.0;
+ ASSERT_TRUE(goal.Send());
+ }
+
+
+ RunForTime(chrono::seconds(5));
+ VerifyNearGoal();
+ EXPECT_TRUE(superstructure_queue_.status->shooter.ready);
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
+ goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->shooter.angular_velocity = 0.0;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Make sure we don't apply voltage on spin-down.
+ RunIteration();
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_EQ(0.0, superstructure_queue_.output->voltage_shooter);
+ // Continue to stop.
+ RunForTime(chrono::seconds(5));
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_EQ(0.0, superstructure_queue_.output->voltage_shooter);
+}
+
+// Tests that the shooter can spin up nicely after being disabled for a while.
+TEST_F(SuperstructureTest, Disabled) {
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
+ goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
+ goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
+ goal->shooter.angular_velocity = 200.0;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(5), false);
+ EXPECT_EQ(nullptr, superstructure_queue_.output.get());
+
+ RunForTime(chrono::seconds(5));
+
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops