Merge "Registered signal handlers for joysticks and control loops"
diff --git a/aos/build/queues/output/q_file.rb b/aos/build/queues/output/q_file.rb
index 471bdcb..a2f438d 100644
--- a/aos/build/queues/output/q_file.rb
+++ b/aos/build/queues/output/q_file.rb
@@ -49,7 +49,11 @@
def initialize(path)
@path = path
end
+ def extern=(value)
+ @extern=value
+ end
def create(cpp_tree)
+ return if (@extern)
# inc = cpp_tree.header.add_include("\"#{@path}\"")
inc = cpp_tree.add_header_include("\"#{@path}\"")
cpp_tree.set(self,inc)
diff --git a/frc971/constants.h b/frc971/constants.h
index 0da9c96..fbf7b1c 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -4,7 +4,7 @@
namespace frc971 {
namespace constants {
-struct ZeroingConstants {
+struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
int average_filter_size;
// The difference in scaled units between two index pulses.
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 2075ca7..8ceeace 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -147,3 +147,20 @@
'//third_party/eigen',
],
)
+
+cc_library(
+ name = 'profiled_subsystem',
+ srcs = [
+ 'profiled_subsystem.cc',
+ ],
+ hdrs = [
+ 'profiled_subsystem.h',
+ ],
+ deps = [
+ ':simple_capped_state_feedback_loop',
+ ':state_feedback_loop',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:trapezoid_profile',
+ '//frc971/zeroing:zeroing',
+ ],
+)
diff --git a/frc971/control_loops/profiled_subsystem.cc b/frc971/control_loops/profiled_subsystem.cc
new file mode 100644
index 0000000..4a90bf5
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.cc
@@ -0,0 +1,17 @@
+#include "frc971/control_loops/profiled_subsystem.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace internal {
+
+double UseUnlessZero(double target_value, double default_value) {
+ if (target_value != 0.0) {
+ return target_value;
+ } else {
+ return default_value;
+ }
+}
+
+} // namespace internal
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
new file mode 100644
index 0000000..9444b66
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -0,0 +1,330 @@
+#ifndef FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
+#define FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
+
+#include <array>
+#include <memory>
+#include <utility>
+
+#include "Eigen/Dense"
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// TODO(Brian): Use a tuple instead of an array to support heterogeneous zeroing
+// styles.
+template <int number_of_states, int number_of_axes,
+ class ZeroingEstimator =
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+class ProfiledSubsystem {
+ public:
+ ProfiledSubsystem(
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>> loop,
+ ::std::array<ZeroingEstimator, number_of_axes> &&estimators)
+ : loop_(::std::move(loop)), estimators_(::std::move(estimators)) {
+ zeroed_.fill(false);
+ unprofiled_goal_.setZero();
+ }
+
+ // Returns whether an error has occured
+ bool error() const {
+ for (const auto &estimator : estimators_) {
+ if (estimator.error()) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ void Reset() {
+ zeroed_.fill(false);
+ for (auto &estimator : estimators_) {
+ estimator.Reset();
+ }
+ }
+
+ // Returns the controller.
+ const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes> &
+ controller() const {
+ return *loop_;
+ }
+
+ int controller_index() const { return loop_->controller_index(); }
+
+ // Returns whether the estimators have been initialized and zeroed.
+ bool initialized() const { return initialized_; }
+
+ bool zeroed() const {
+ for (int i = 0; i < number_of_axes; ++i) {
+ if (!zeroed_[i]) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool zeroed(int index) const { return zeroed_[index]; };
+
+ // Returns the filtered goal.
+ const Eigen::Matrix<double, number_of_states, 1> &goal() const {
+ return loop_->R();
+ }
+ double goal(int row, int col) const { return loop_->R(row, col); }
+
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
+ // Returns the current state estimate.
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return loop_->X_hat();
+ }
+ double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+
+ // Returns the current internal estimator state for logging.
+ ::frc971::EstimatorState EstimatorState(int index) {
+ ::frc971::EstimatorState estimator_state;
+ ::frc971::zeroing::PopulateEstimatorState(estimators_[index],
+ &estimator_state);
+
+ return estimator_state;
+ }
+
+ // Sets the maximum voltage that will be commanded by the loop.
+ void set_max_voltage(::std::array<double, number_of_axes> voltages) {
+ for (int i = 0; i < number_of_axes; ++i) {
+ loop_->set_max_voltage(i, voltages[i]);
+ }
+ }
+
+ protected:
+ void set_zeroed(int index, bool val) { zeroed_[index] = val; }
+
+ // TODO(austin): It's a bold assumption to assume that we will have the same
+ // number of sensors as axes. So far, that's been fine.
+ ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ number_of_states, number_of_axes, number_of_axes>> loop_;
+
+ // The goal that the profile tries to reach.
+ Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
+
+ bool initialized_ = false;
+
+ ::std::array<ZeroingEstimator, number_of_axes> estimators_;
+
+ private:
+ ::std::array<bool, number_of_axes> zeroed_;
+};
+
+template <class ZeroingEstimator =
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+class SingleDOFProfiledSubsystem
+ : public ::frc971::control_loops::ProfiledSubsystem<3, 1> {
+ public:
+ SingleDOFProfiledSubsystem(
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
+ const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_angular_velocity,
+ double default_angular_acceleration);
+
+ // Updates our estimator with the latest position.
+ void Correct(typename ZeroingEstimator::Position position);
+ // Runs the controller and profile generator for a cycle.
+ void Update(bool disabled);
+
+ // Forces the current goal to the provided goal, bypassing the profiler.
+ void ForceGoal(double goal);
+ // Sets the unprofiled goal. The profiler will generate a profile to go to
+ // this goal.
+ void set_unprofiled_goal(double unprofiled_goal);
+ // Limits our profiles to a max velocity and acceleration for proper motion.
+ void AdjustProfile(double max_angular_velocity,
+ double max_angular_acceleration);
+
+ // Returns true if we have exceeded any hard limits.
+ bool CheckHardLimits();
+
+ // Returns the requested intake voltage.
+ double intake_voltage() const { return loop_->U(0, 0); }
+
+ // Returns the current position.
+ double angle() const { return Y_(0, 0); }
+
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimators_[0].TriggerError(); }
+
+ private:
+ // Limits the provided goal to the soft limits. Prints "name" when it fails
+ // to aid debugging.
+ void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
+
+ void UpdateOffset(double offset);
+
+ aos::util::TrapezoidProfile profile_;
+
+ // Current measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // Current offset. Y_ = offset_ + raw_sensor;
+ Eigen::Matrix<double, 1, 1> offset_;
+
+ const ::frc971::constants::Range range_;
+
+ const double default_velocity_;
+ const double default_acceleration_;
+};
+
+namespace internal {
+
+double UseUnlessZero(double target_value, double default_value);
+
+} // namespace internal
+
+template <class ZeroingEstimator>
+SingleDOFProfiledSubsystem<ZeroingEstimator>::SingleDOFProfiledSubsystem(
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
+ const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_velocity,
+ double default_acceleration)
+ : ProfiledSubsystem(::std::move(loop), {{zeroing_constants}}),
+ profile_(::aos::controls::kLoopFrequency),
+ range_(range),
+ default_velocity_(default_velocity),
+ default_acceleration_(default_acceleration) {
+ Y_.setZero();
+ offset_.setZero();
+ AdjustProfile(0.0, 0.0);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateOffset(double offset) {
+ const double doffset = offset - offset_(0, 0);
+ LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
+
+ loop_->mutable_X_hat()(0, 0) += doffset;
+ Y_(0, 0) += doffset;
+ loop_->mutable_R(0, 0) += doffset;
+
+ profile_.MoveGoal(doffset);
+ offset_(0, 0) = offset;
+
+ CapGoal("R", &loop_->mutable_R());
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
+ typename ZeroingEstimator::Position position) {
+ estimators_[0].UpdateEstimate(position);
+
+ if (estimators_[0].error()) {
+ LOG(ERROR, "zeroing error with intake_estimator\n");
+ return;
+ }
+
+ if (!initialized_) {
+ if (estimators_[0].offset_ready()) {
+ UpdateOffset(estimators_[0].offset());
+ initialized_ = true;
+ }
+ }
+
+ if (!zeroed(0) && estimators_[0].zeroed()) {
+ UpdateOffset(estimators_[0].offset());
+ set_zeroed(0, true);
+ }
+
+ Y_ << position.encoder;
+ Y_ += offset_;
+ loop_->Correct(Y_);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
+ const char *name, Eigen::Matrix<double, 3, 1> *goal) {
+ // Limit the goal to min/max allowable angles.
+ if ((*goal)(0, 0) > range_.upper) {
+ LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ range_.upper);
+ (*goal)(0, 0) = range_.upper;
+ }
+ if ((*goal)(0, 0) < range_.lower) {
+ LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ range_.lower);
+ (*goal)(0, 0) = range_.lower;
+ }
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(double goal) {
+ set_unprofiled_goal(goal);
+ loop_->mutable_R() = unprofiled_goal_;
+ loop_->mutable_next_R() = loop_->R();
+
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::set_unprofiled_goal(
+ double unprofiled_goal) {
+ unprofiled_goal_(0, 0) = unprofiled_goal;
+ unprofiled_goal_(1, 0) = 0.0;
+ unprofiled_goal_(2, 0) = 0.0;
+ CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+ if (!disable) {
+ ::Eigen::Matrix<double, 2, 1> goal_state =
+ profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
+
+ loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+ loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+ loop_->mutable_next_R(2, 0) = 0.0;
+ CapGoal("next R", &loop_->mutable_next_R());
+ }
+
+ loop_->Update(disable);
+
+ if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+ profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ }
+}
+
+template <class ZeroingEstimator>
+bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
+ // Returns whether hard limits have been exceeded.
+
+ if (angle() > range_.upper_hard || angle() < range_.lower_hard) {
+ LOG(ERROR,
+ "SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
+ angle(), range_.lower_hard, range_.upper_hard);
+ return true;
+ }
+
+ return false;
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+ double max_angular_velocity, double max_angular_acceleration) {
+ profile_.set_maximum_velocity(
+ internal::UseUnlessZero(max_angular_velocity, default_velocity_));
+ profile_.set_maximum_acceleration(
+ internal::UseUnlessZero(max_angular_acceleration, default_acceleration_));
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 4ff6391..69c4de6 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -6,16 +6,17 @@
namespace frc971 {
namespace zeroing {
-void PopulateEstimatorState(const zeroing::ZeroingEstimator& estimator,
- EstimatorState* state) {
+void PopulateEstimatorState(
+ const zeroing::PotAndIndexPulseZeroingEstimator &estimator,
+ EstimatorState *state) {
state->error = estimator.error();
state->zeroed = estimator.zeroed();
state->position = estimator.position();
state->pot_position = estimator.filtered_position();
}
-ZeroingEstimator::ZeroingEstimator(
- const constants::ZeroingConstants& constants) {
+PotAndIndexPulseZeroingEstimator::PotAndIndexPulseZeroingEstimator(
+ const constants::PotAndIndexPulseZeroingConstants &constants) {
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
known_index_pos_ = constants.measured_index_position;
@@ -24,7 +25,7 @@
Reset();
}
-void ZeroingEstimator::Reset() {
+void PotAndIndexPulseZeroingEstimator::Reset() {
samples_idx_ = 0;
start_pos_ = 0;
start_pos_samples_.clear();
@@ -35,15 +36,15 @@
error_ = false;
}
-void ZeroingEstimator::TriggerError() {
+void PotAndIndexPulseZeroingEstimator::TriggerError() {
if (!error_) {
LOG(ERROR, "Manually triggered zeroing error.\n");
error_ = true;
}
}
-double ZeroingEstimator::CalculateStartPosition(double start_average,
- double latched_encoder) const {
+double PotAndIndexPulseZeroingEstimator::CalculateStartPosition(
+ double start_average, double latched_encoder) const {
// We calculate an aproximation of the value of the last index position.
// Also account for index pulses not lining up with integer multiples of the
// index_diff.
@@ -54,7 +55,8 @@
return accurate_index_pos - latched_encoder + known_index_pos_;
}
-void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
+void PotAndIndexPulseZeroingEstimator::UpdateEstimate(
+ const PotAndIndexPosition &info) {
// We want to make sure that we encounter at least one index pulse while
// zeroing. So we take the index pulse count from the first sample after
// reset and wait for that count to change before we consider ourselves
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index b5e2c14..1872ad0 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -19,11 +19,15 @@
namespace frc971 {
namespace zeroing {
-// Estimates the position with encoder,
-// the pot and the indices.
-class ZeroingEstimator {
+// Estimates the position with an incremental encoder with an index pulse and a
+// potentiometer.
+class PotAndIndexPulseZeroingEstimator {
public:
- ZeroingEstimator(const constants::ZeroingConstants &constants);
+ using Position = PotAndIndexPosition;
+ using ZeroingConstants = constants::PotAndIndexPulseZeroingConstants;
+
+ PotAndIndexPulseZeroingEstimator(
+ const constants::PotAndIndexPulseZeroingConstants &constants);
// Update the internal logic with the next sensor values.
void UpdateEstimate(const PotAndIndexPosition &info);
@@ -123,7 +127,7 @@
// Populates an EstimatorState struct with information from the zeroing
// estimator.
-void PopulateEstimatorState(const ZeroingEstimator &estimator,
+void PopulateEstimatorState(const PotAndIndexPulseZeroingEstimator &estimator,
EstimatorState *state);
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 1134259..2f562aa 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -16,7 +16,7 @@
namespace zeroing {
using control_loops::PositionSensorSimulator;
-using constants::ZeroingConstants;
+using constants::PotAndIndexPulseZeroingConstants;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -26,7 +26,8 @@
protected:
void SetUp() override { aos::SetDieTestMode(true); }
- void MoveTo(PositionSensorSimulator* simulator, ZeroingEstimator* estimator,
+ void MoveTo(PositionSensorSimulator *simulator,
+ PotAndIndexPulseZeroingEstimator *estimator,
double new_position) {
PotAndIndexPosition sensor_values_;
simulator->MoveTo(new_position);
@@ -41,7 +42,7 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
@@ -65,7 +66,7 @@
double position = 3.6 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(position, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure that the zeroing code does not consider itself zeroed until we
@@ -84,7 +85,7 @@
double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
@@ -117,7 +118,7 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// The zeroing code is supposed to perform some filtering on the difference
@@ -151,7 +152,7 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
for (unsigned int i = 0; i < kSampleSize / 2; i++) {
@@ -171,7 +172,7 @@
double index_diff = 0.89;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
MoveTo(&sim, &estimator, 3.1 * index_diff);
@@ -187,7 +188,7 @@
double index_diff = 0.6;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
@@ -222,7 +223,7 @@
const double known_index_pos = 3.5 * index_diff;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.3 * index_diff, index_diff / 3.0, known_index_pos);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
// Make sure to fill up the averaging filter with samples.
@@ -247,7 +248,7 @@
TEST_F(ZeroingTest, BasicErrorAPITest) {
const double index_diff = 1.0;
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
kSampleSize, index_diff, 0.0, kIndexErrorFraction});
PositionSensorSimulator sim(index_diff);
sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
@@ -283,7 +284,7 @@
int sample_size = 30;
PositionSensorSimulator sim(index_diff);
sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
- ZeroingEstimator estimator(ZeroingConstants{
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
sample_size, index_diff, known_index_pos, kIndexErrorFraction});
for (int i = 0; i < sample_size; i++) {
diff --git a/y2014/constants.h b/y2014/constants.h
index 4ad06bc..7fb2507 100644
--- a/y2014/constants.h
+++ b/y2014/constants.h
@@ -43,7 +43,7 @@
double drivetrain_max_speed;
- struct ZeroingConstants {
+ struct PotAndIndexPulseZeroingConstants {
// The number of samples in the moving average filter.
int average_filter_size;
// The difference in scaled units between two index pulses.
@@ -126,7 +126,7 @@
// should be larger than claw_shooting_separation so that we can shoot
// promptly.
double claw_separation_goal;
- };
+ };
ShooterAction shooter_action;
};
diff --git a/y2015/constants.h b/y2015/constants.h
index 8cefe7e..d60efac 100644
--- a/y2015/constants.h
+++ b/y2015/constants.h
@@ -69,7 +69,7 @@
struct Claw {
Range wrist;
- ::frc971::constants::ZeroingConstants zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants zeroing;
// The value to add to potentiometer readings after they have been converted
// to radians so that the resulting value is 0 when the claw is at absolute
// 0 (horizontal straight out the front).
@@ -87,10 +87,10 @@
Range elevator;
Range arm;
- ::frc971::constants::ZeroingConstants left_elev_zeroing;
- ::frc971::constants::ZeroingConstants right_elev_zeroing;
- ::frc971::constants::ZeroingConstants left_arm_zeroing;
- ::frc971::constants::ZeroingConstants right_arm_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants left_elev_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants right_elev_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants left_arm_zeroing;
+ ::frc971::constants::PotAndIndexPulseZeroingConstants right_arm_zeroing;
// Values to add to scaled potentiometer readings so 0 lines up with the
// physical absolute 0.
diff --git a/y2015/control_loops/claw/claw.h b/y2015/control_loops/claw/claw.h
index 324f9f3..e7523eb 100644
--- a/y2015/control_loops/claw/claw.h
+++ b/y2015/control_loops/claw/claw.h
@@ -95,7 +95,7 @@
// Latest position from queue.
control_loops::ClawQueue::Position current_position_;
// Zeroing estimator for claw.
- ::frc971::zeroing::ZeroingEstimator claw_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator claw_estimator_;
// The goal for the claw.
double claw_goal_ = 0.0;
diff --git a/y2015/control_loops/fridge/fridge.h b/y2015/control_loops/fridge/fridge.h
index ee64554..6152caf 100644
--- a/y2015/control_loops/fridge/fridge.h
+++ b/y2015/control_loops/fridge/fridge.h
@@ -22,7 +22,7 @@
class FridgeTest_SafeArmZeroing_Test;
} // namespace testing
-template<int S>
+template <int S>
class CappedStateFeedbackLoop : public StateFeedbackLoop<S, 2, 2> {
public:
CappedStateFeedbackLoop(StateFeedbackLoop<S, 2, 2> &&loop)
@@ -42,8 +42,7 @@
double max_voltage_;
};
-class Fridge
- : public aos::controls::ControlLoop<FridgeQueue> {
+class Fridge : public aos::controls::ControlLoop<FridgeQueue> {
public:
explicit Fridge(FridgeQueue *fridge_queue =
&::y2015::control_loops::fridge::fridge_queue);
@@ -128,10 +127,10 @@
::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
- ::frc971::zeroing::ZeroingEstimator left_arm_estimator_;
- ::frc971::zeroing::ZeroingEstimator right_arm_estimator_;
- ::frc971::zeroing::ZeroingEstimator left_elevator_estimator_;
- ::frc971::zeroing::ZeroingEstimator right_elevator_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator left_arm_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator right_arm_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator left_elevator_estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator right_elevator_estimator_;
// Offsets from the encoder position to the absolute position. Add these to
// the encoder position to get the absolute position.
@@ -170,5 +169,4 @@
} // namespace control_loops
} // namespace y2015
-#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
-
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
diff --git a/y2016/constants.h b/y2016/constants.h
index a83456c..3cbb895 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -11,7 +11,7 @@
namespace constants {
using ::frc971::constants::ShifterHallEffect;
-using ::frc971::constants::ZeroingConstants;
+using ::frc971::constants::PotAndIndexPulseZeroingConstants;
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
@@ -83,19 +83,19 @@
struct Intake {
double pot_offset;
- ZeroingConstants zeroing;
+ PotAndIndexPulseZeroingConstants zeroing;
};
Intake intake;
struct Shoulder {
double pot_offset;
- ZeroingConstants zeroing;
+ PotAndIndexPulseZeroingConstants zeroing;
};
Shoulder shoulder;
struct Wrist {
double pot_offset;
- ZeroingConstants zeroing;
+ PotAndIndexPulseZeroingConstants zeroing;
};
Wrist wrist;
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a10b61..af02d14 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,10 +78,11 @@
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
'//aos/common:math',
- '//y2016/queues:ball_detector',
- '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:profiled_subsystem',
'//frc971/control_loops:simple_capped_state_feedback_loop',
+ '//frc971/control_loops:state_feedback_loop',
'//frc971/zeroing',
+ '//y2016/queues:ball_detector',
'//y2016:constants',
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 0c1ac6a..cb09b42 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -648,11 +648,11 @@
}
}
arm_.set_max_voltage(
- kill_shoulder_ ? 0.0 : max_voltage,
- kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
- : kShooterHangingVoltage)
- : max_voltage);
- intake_.set_max_voltage(max_voltage);
+ {{kill_shoulder_ ? 0.0 : max_voltage,
+ kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
+ : kShooterHangingVoltage)
+ : max_voltage}});
+ intake_.set_max_voltage({{max_voltage}});
if (IsRunning() && !kill_shoulder_) {
// We don't want lots of negative voltage when we are near the bellypan on
@@ -743,7 +743,7 @@
status->shoulder.voltage_error = arm_.X_hat(4, 0);
status->shoulder.calculated_velocity =
(arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
+ status->shoulder.estimator_state = arm_.EstimatorState(0);
status->wrist.angle = arm_.X_hat(2, 0);
status->wrist.angular_velocity = arm_.X_hat(3, 0);
@@ -754,7 +754,7 @@
status->wrist.voltage_error = arm_.X_hat(5, 0);
status->wrist.calculated_velocity =
(arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.WristEstimatorState();
+ status->wrist.estimator_state = arm_.EstimatorState(1);
status->intake.angle = intake_.X_hat(0, 0);
status->intake.angular_velocity = intake_.X_hat(1, 0);
@@ -766,7 +766,7 @@
status->intake.calculated_velocity =
(intake_.angle() - last_intake_angle_) / 0.005;
status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.IntakeEstimatorState();
+ status->intake.estimator_state = intake_.EstimatorState(0);
status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
status->shoulder_controller_index = arm_.controller_index();
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7d4dea5..ce334bf 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -13,7 +13,6 @@
namespace superstructure {
using ::frc971::PotAndIndexPosition;
-using ::frc971::EstimatorState;
namespace {
double UseUnlessZero(double target_value, double default_value) {
@@ -30,148 +29,23 @@
// Intake
Intake::Intake()
- : ProfiledSubsystem(
+ : ::frc971::control_loops::SingleDOFProfiledSubsystem<>(
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
3, 1, 1>(::y2016::control_loops::superstructure::
- MakeIntegralIntakeLoop()))),
- estimator_(constants::GetValues().intake.zeroing),
- profile_(::aos::controls::kLoopFrequency) {
- Y_.setZero();
- offset_.setZero();
- AdjustProfile(0.0, 0.0);
-}
-
-void Intake::UpdateIntakeOffset(double offset) {
- const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting Intake offset from %f to %f\n", offset_(0, 0), offset);
-
- loop_->mutable_X_hat()(0, 0) += doffset;
- Y_(0, 0) += doffset;
- loop_->mutable_R(0, 0) += doffset;
-
- profile_.MoveGoal(doffset);
- offset_(0, 0) = offset;
-
- CapGoal("R", &loop_->mutable_R());
-}
-
-void Intake::Correct(PotAndIndexPosition position) {
- estimator_.UpdateEstimate(position);
-
- if (estimator_.error()) {
- LOG(ERROR, "zeroing error with intake_estimator\n");
- return;
- }
-
- if (!initialized_) {
- if (estimator_.offset_ready()) {
- UpdateIntakeOffset(estimator_.offset());
- initialized_ = true;
- }
- }
-
- if (!zeroed(0) && estimator_.zeroed()) {
- UpdateIntakeOffset(estimator_.offset());
- set_zeroed(0, true);
- }
-
- Y_ << position.encoder;
- Y_ += offset_;
- loop_->Correct(Y_);
-}
-
-void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- // Limit the goal to min/max allowable angles.
- if ((*goal)(0, 0) > constants::Values::kIntakeRange.upper) {
- LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- constants::Values::kIntakeRange.upper);
- (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
- }
- if ((*goal)(0, 0) < constants::Values::kIntakeRange.lower) {
- LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
- constants::Values::kIntakeRange.lower);
- (*goal)(0, 0) = constants::Values::kIntakeRange.lower;
- }
-}
-
-void Intake::ForceGoal(double goal) {
- set_unprofiled_goal(goal);
- loop_->mutable_R() = unprofiled_goal_;
- loop_->mutable_next_R() = loop_->R();
-
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
-}
-
-void Intake::set_unprofiled_goal(double unprofiled_goal) {
- unprofiled_goal_(0, 0) = unprofiled_goal;
- unprofiled_goal_(1, 0) = 0.0;
- unprofiled_goal_(2, 0) = 0.0;
- CapGoal("unprofiled R", &unprofiled_goal_);
-}
-
-void Intake::Update(bool disable) {
- if (!disable) {
- ::Eigen::Matrix<double, 2, 1> goal_state =
- profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
-
- loop_->mutable_next_R(0, 0) = goal_state(0, 0);
- loop_->mutable_next_R(1, 0) = goal_state(1, 0);
- loop_->mutable_next_R(2, 0) = 0.0;
- CapGoal("next R", &loop_->mutable_next_R());
- }
-
- loop_->Update(disable);
-
- if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
- }
-}
-
-bool Intake::CheckHardLimits() {
- // Returns whether hard limits have been exceeded.
-
- if (angle() > constants::Values::kIntakeRange.upper_hard ||
- angle() < constants::Values::kIntakeRange.lower_hard) {
- LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
- constants::Values::kIntakeRange.lower_hard,
- constants::Values::kIntakeRange.upper_hard);
- return true;
- }
-
- return false;
-}
-
-void Intake::set_max_voltage(double voltage) {
- loop_->set_max_voltage(0, voltage);
-}
-
-void Intake::AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration) {
- profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
- profile_.set_maximum_acceleration(
- UseUnlessZero(max_angular_acceleration, 10.0));
-}
-
-void Intake::DoReset() {
- estimator_.Reset();
-}
-
-EstimatorState Intake::IntakeEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
-
- return estimator_state;
-}
+ MakeIntegralIntakeLoop())),
+ constants::GetValues().intake.zeroing,
+ constants::Values::kIntakeRange, 10.0, 10.0) {}
Arm::Arm()
- : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
- ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
+ : ProfiledSubsystem(
+ ::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+ ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+ {{constants::GetValues().shoulder.zeroing,
+ constants::GetValues().wrist.zeroing}}),
shoulder_profile_(::aos::controls::kLoopFrequency),
- wrist_profile_(::aos::controls::kLoopFrequency),
- shoulder_estimator_(constants::GetValues().shoulder.zeroing),
- wrist_estimator_(constants::GetValues().wrist.zeroing) {
+ wrist_profile_(::aos::controls::kLoopFrequency) {
Y_.setZero();
offset_.setZero();
AdjustProfile(0.0, 0.0, 0.0, 0.0);
@@ -220,33 +94,34 @@
void Arm::Correct(PotAndIndexPosition position_shoulder,
PotAndIndexPosition position_wrist) {
- shoulder_estimator_.UpdateEstimate(position_shoulder);
- wrist_estimator_.UpdateEstimate(position_wrist);
+ estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(position_wrist);
// Handle zeroing errors
- if (shoulder_estimator_.error()) {
+ if (estimators_[kShoulderIndex].error()) {
LOG(ERROR, "zeroing error with shoulder_estimator\n");
return;
}
- if (wrist_estimator_.error()) {
+ if (estimators_[kWristIndex].error()) {
LOG(ERROR, "zeroing error with wrist_estimator\n");
return;
}
if (!initialized_) {
- if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
- UpdateWristOffset(wrist_estimator_.offset());
+ if (estimators_[kShoulderIndex].offset_ready() &&
+ estimators_[kWristIndex].offset_ready()) {
+ UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
+ UpdateWristOffset(estimators_[kWristIndex].offset());
initialized_ = true;
}
}
- if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
- UpdateShoulderOffset(shoulder_estimator_.offset());
+ if (!zeroed(kShoulderIndex) && estimators_[kShoulderIndex].zeroed()) {
+ UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
set_zeroed(kShoulderIndex, true);
}
- if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
- UpdateWristOffset(wrist_estimator_.offset());
+ if (!zeroed(kWristIndex) && estimators_[kWristIndex].zeroed()) {
+ UpdateWristOffset(estimators_[kWristIndex].offset());
set_zeroed(kWristIndex, true);
}
@@ -370,32 +245,6 @@
}
}
-void Arm::set_max_voltage(double shoulder_max_voltage,
- double wrist_max_voltage) {
- loop_->set_max_voltage(0, shoulder_max_voltage);
- loop_->set_max_voltage(1, wrist_max_voltage);
-}
-
-void Arm::DoReset() {
- shoulder_estimator_.Reset();
- wrist_estimator_.Reset();
-}
-
-EstimatorState Arm::ShoulderEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(shoulder_estimator_,
- &estimator_state);
-
- return estimator_state;
-}
-
-EstimatorState Arm::WristEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(wrist_estimator_, &estimator_state);
-
- return estimator_state;
-}
-
} // namespace superstructure
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index adb40e0..8f3a59e 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -4,13 +4,14 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
namespace y2016 {
namespace control_loops {
@@ -66,7 +67,8 @@
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) * overage_amount;
+ (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
@@ -96,149 +98,15 @@
}
};
-template <int number_of_states, int number_of_axes>
-class ProfiledSubsystem {
- public:
- ProfiledSubsystem(
- ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>>
- loop)
- : loop_(::std::move(loop)) {
- zeroed_.fill(false);
- unprofiled_goal_.setZero();
- }
-
- void Reset() {
- zeroed_.fill(false);
- DoReset();
- }
-
- // Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes>
- &controller() const {
- return *loop_;
- }
-
- int controller_index() const { return loop_->controller_index(); }
-
- // Returns whether the estimators have been initialized and zeroed.
- bool initialized() const { return initialized_; }
-
- bool zeroed() const {
- for (int i = 0; i < number_of_axes; ++i) {
- if (!zeroed_[i]) {
- return false;
- }
- }
- return true;
- }
-
- bool zeroed(int index) const { return zeroed_[index]; };
-
- // Returns the filtered goal.
- const Eigen::Matrix<double, number_of_states, 1> &goal() const {
- return loop_->R();
- }
- double goal(int row, int col) const { return loop_->R(row, col); }
-
- // Returns the unprofiled goal.
- const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
- return unprofiled_goal_;
- }
- double unprofiled_goal(int row, int col) const {
- return unprofiled_goal_(row, col);
- }
-
- // Returns the current state estimate.
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
- return loop_->X_hat();
- }
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
- protected:
- void set_zeroed(int index, bool val) { zeroed_[index] = val; }
-
- // TODO(austin): It's a bold assumption to assume that we will have the same
- // number of sensors as axes. So far, that's been fine.
- ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
- number_of_states, number_of_axes, number_of_axes>>
- loop_;
-
- // The goal that the profile tries to reach.
- Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
-
- bool initialized_ = false;
-
- private:
- ::std::array<bool, number_of_axes> zeroed_;
-
- virtual void DoReset() = 0;
-};
-
-class Intake : public ProfiledSubsystem<3, 1> {
+class Intake : public ::frc971::control_loops::SingleDOFProfiledSubsystem<> {
public:
Intake();
- // Returns whether an error has occured
- bool error() const { return estimator_.error(); }
-
- // Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position);
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
- // Sets the maximum voltage that will be commanded by the loop.
- void set_max_voltage(double voltage);
-
- // Forces the current goal to the provided goal, bypassing the profiler.
- void ForceGoal(double goal);
- // Sets the unprofiled goal. The profiler will generate a profile to go to
- // this goal.
- void set_unprofiled_goal(double unprofiled_goal);
- // Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(double max_angular_velocity,
- double max_angular_acceleration);
-
- // Returns true if we have exceeded any hard limits.
- bool CheckHardLimits();
-
- // Returns the current internal estimator state for logging.
- ::frc971::EstimatorState IntakeEstimatorState();
-
- // Returns the requested intake voltage.
- double intake_voltage() const { return loop_->U(0, 0); }
-
- // Returns the current position.
- double angle() const { return Y_(0, 0); }
-
- // For testing:
- // Triggers an estimator error.
- void TriggerEstimatorError() { estimator_.TriggerError(); }
-
- private:
- // Limits the provided goal to the soft limits. Prints "name" when it fails
- // to aid debugging.
- void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
-
- // Resets the internal state.
- void DoReset() override;
-
- void UpdateIntakeOffset(double offset);
-
- ::frc971::zeroing::ZeroingEstimator estimator_;
- aos::util::TrapezoidProfile profile_;
-
- // Current measurement.
- Eigen::Matrix<double, 1, 1> Y_;
- // Current offset. Y_ = offset_ + raw_sensor;
- Eigen::Matrix<double, 1, 1> offset_;
};
-class Arm : public ProfiledSubsystem<6, 2> {
+
+class Arm : public ::frc971::control_loops::ProfiledSubsystem<6, 2> {
public:
Arm();
- // Returns whether an error has occured
- bool error() const {
- return shoulder_estimator_.error() || wrist_estimator_.error();
- }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position_shoulder,
@@ -259,7 +127,6 @@
double max_angular_acceleration_shoulder,
double max_angular_velocity_wrist,
double max_angular_acceleration_wrist);
- void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
void set_shoulder_asymetric_limits(double shoulder_min_voltage,
double shoulder_max_voltage) {
@@ -269,10 +136,6 @@
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // Returns the current internal estimator state for logging.
- ::frc971::EstimatorState ShoulderEstimatorState();
- ::frc971::EstimatorState WristEstimatorState();
-
// Returns the requested shoulder and wrist voltages.
double shoulder_voltage() const { return loop_->U(0, 0); }
double wrist_voltage() const { return loop_->U(1, 0); }
@@ -283,12 +146,9 @@
// For testing:
// Triggers an estimator error.
- void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
+ void TriggerEstimatorError() { estimators_[0].TriggerError(); }
private:
- // Resets the internal state.
- void DoReset() override;
-
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
@@ -300,7 +160,6 @@
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
- ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
// Current measurement.
Eigen::Matrix<double, 2, 1> Y_;
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index b50ccca..6fee1d2 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -11,7 +11,8 @@
namespace y2016_bot3 {
namespace constants {
constexpr double IntakeZero::pot_offset;
-constexpr ::frc971::constants::ZeroingConstants IntakeZero::zeroing;
+constexpr ::frc971::constants::PotAndIndexPulseZeroingConstants
+ IntakeZero::zeroing;
} // namespace constants
namespace control_loops {
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index fdde74b..c654591 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -39,9 +39,9 @@
struct IntakeZero {
static constexpr double pot_offset = 5.462409 + 0.333162;
- static constexpr ::frc971::constants::ZeroingConstants zeroing{
- kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.148604 - 0.291240,
- 0.3};
+ static constexpr ::frc971::constants::PotAndIndexPulseZeroingConstants
+ zeroing{kZeroingSampleSize, kIntakeEncoderIndexDifference,
+ 0.148604 - 0.291240, 0.3};
};
} // namespace constants
namespace control_loops {
diff --git a/y2016_bot3/control_loops/intake/intake_controls.h b/y2016_bot3/control_loops/intake/intake_controls.h
index 0b2daa0..320d803 100644
--- a/y2016_bot3/control_loops/intake/intake_controls.h
+++ b/y2016_bot3/control_loops/intake/intake_controls.h
@@ -90,7 +90,7 @@
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>> loop_;
- ::frc971::zeroing::ZeroingEstimator estimator_;
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator estimator_;
aos::util::TrapezoidProfile profile_;
// Current measurement.