Added turret and intake code and tests.
Tests are from Adam.
Change-Id: I5a89700cfe2e9983771b4523facc302243b5dc50
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 78a9a76..9a19201 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -97,8 +97,8 @@
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;
+ typename ZeroingEstimator::State EstimatorState(int index) {
+ typename ZeroingEstimator::State estimator_state;
::frc971::zeroing::PopulateEstimatorState(estimators_[index],
&estimator_state);
@@ -131,10 +131,10 @@
::std::array<bool, number_of_axes> zeroed_;
};
-template <class ZeroingEstimator =
+template <typename ZeroingEstimator =
::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
class SingleDOFProfiledSubsystem
- : public ::frc971::control_loops::ProfiledSubsystem<3, 1> {
+ : public ::frc971::control_loops::ProfiledSubsystem<3, 1, ZeroingEstimator> {
public:
SingleDOFProfiledSubsystem(
::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
@@ -148,7 +148,8 @@
void Update(bool disabled);
// Fills out the ProfiledJointStatus structure with the current state.
- void PopulateStatus(ProfiledJointStatus *status);
+ template <class StatusType>
+ void PopulateStatus(StatusType *status);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
@@ -164,14 +165,14 @@
bool CheckHardLimits();
// Returns the requested voltage.
- double voltage() const { return loop_->U(0, 0); }
+ double voltage() const { return this->loop_->U(0, 0); }
// Returns the current position.
- double position() const { return Y_(0, 0); }
+ double position() const { return this->Y_(0, 0); }
// For testing:
// Triggers an estimator error.
- void TriggerEstimatorError() { estimators_[0].TriggerError(); }
+ void TriggerEstimatorError() { this->estimators_[0].TriggerError(); }
const ::frc971::constants::Range &range() const { return range_; }
@@ -209,7 +210,8 @@
const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
const ::frc971::constants::Range &range, double default_velocity,
double default_acceleration)
- : ProfiledSubsystem(::std::move(loop), {{zeroing_constants}}),
+ : ProfiledSubsystem<3, 1, ZeroingEstimator>(::std::move(loop),
+ {{zeroing_constants}}),
profile_(::aos::controls::kLoopFrequency),
range_(range),
default_velocity_(default_velocity),
@@ -224,71 +226,72 @@
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;
+ this->loop_->mutable_X_hat()(0, 0) += doffset;
+ this->Y_(0, 0) += doffset;
last_position_ += doffset;
- loop_->mutable_R(0, 0) += doffset;
+ this->loop_->mutable_R(0, 0) += doffset;
profile_.MoveGoal(doffset);
offset_(0, 0) = offset;
- CapGoal("R", &loop_->mutable_R());
+ CapGoal("R", &this->loop_->mutable_R());
}
template <class ZeroingEstimator>
+template <class StatusType>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
- ProfiledJointStatus *status) {
- status->zeroed = zeroed();
+ StatusType *status) {
+ status->zeroed = this->zeroed();
status->state = -1;
// We don't know, so default to the bad case.
status->estopped = true;
- status->position = X_hat(0, 0);
- status->velocity = X_hat(1, 0);
- status->goal_position = goal(0, 0);
- status->goal_velocity = goal(1, 0);
- status->unprofiled_goal_position = unprofiled_goal(0, 0);
- status->unprofiled_goal_velocity = unprofiled_goal(1, 0);
- status->voltage_error = X_hat(2, 0);
+ status->position = this->X_hat(0, 0);
+ status->velocity = this->X_hat(1, 0);
+ status->goal_position = this->goal(0, 0);
+ status->goal_velocity = this->goal(1, 0);
+ status->unprofiled_goal_position = this->unprofiled_goal(0, 0);
+ status->unprofiled_goal_velocity = this->unprofiled_goal(1, 0);
+ status->voltage_error = this->X_hat(2, 0);
status->calculated_velocity =
(position() - last_position_) /
::std::chrono::duration_cast<::std::chrono::duration<double>>(
::aos::controls::kLoopFrequency)
.count();
- status->estimator_state = EstimatorState(0);
+ status->estimator_state = this->EstimatorState(0);
- Eigen::Matrix<double, 3, 1> error = controller().error();
- status->position_power = controller().K(0, 0) * error(0, 0);
- status->velocity_power = controller().K(0, 1) * error(1, 0);
+ Eigen::Matrix<double, 3, 1> error = this->controller().error();
+ status->position_power = this->controller().K(0, 0) * error(0, 0);
+ status->velocity_power = this->controller().K(0, 1) * error(1, 0);
}
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
typename ZeroingEstimator::Position new_position) {
- estimators_[0].UpdateEstimate(new_position);
+ this->estimators_[0].UpdateEstimate(new_position);
- if (estimators_[0].error()) {
+ if (this->estimators_[0].error()) {
LOG(ERROR, "zeroing error\n");
return;
}
- if (!initialized_) {
- if (estimators_[0].offset_ready()) {
- UpdateOffset(estimators_[0].offset());
- initialized_ = true;
+ if (!this->initialized_) {
+ if (this->estimators_[0].offset_ready()) {
+ UpdateOffset(this->estimators_[0].offset());
+ this->initialized_ = true;
}
}
- if (!zeroed(0) && estimators_[0].zeroed()) {
- UpdateOffset(estimators_[0].offset());
- set_zeroed(0, true);
+ if (!this->zeroed(0) && this->estimators_[0].zeroed()) {
+ UpdateOffset(this->estimators_[0].offset());
+ this->set_zeroed(0, true);
}
last_position_ = position();
- Y_ << new_position.encoder;
- Y_ += offset_;
- loop_->Correct(Y_);
+ this->Y_ << new_position.encoder;
+ this->Y_ += this->offset_;
+ this->loop_->Correct(Y_);
}
template <class ZeroingEstimator>
@@ -310,37 +313,39 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(double goal) {
set_unprofiled_goal(goal);
- loop_->mutable_R() = unprofiled_goal_;
- loop_->mutable_next_R() = loop_->R();
+ this->loop_->mutable_R() = this->unprofiled_goal_;
+ this->loop_->mutable_next_R() = this->loop_->R();
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ const ::Eigen::Matrix<double, 3, 1> &R = this->loop_->R();
+ this->profile_.MoveCurrentState(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_);
+ this->unprofiled_goal_(0, 0) = unprofiled_goal;
+ this->unprofiled_goal_(1, 0) = 0.0;
+ this->unprofiled_goal_(2, 0) = 0.0;
+ CapGoal("unprofiled R", &this->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));
+ ::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
+ this->unprofiled_goal_(0, 0), this->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());
+ this->loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+ this->loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+ this->loop_->mutable_next_R(2, 0) = 0.0;
+ CapGoal("next R", &this->loop_->mutable_next_R());
}
- loop_->Update(disable);
+ this->loop_->Update(disable);
- if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
- profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
+ if (!disable && this->loop_->U(0, 0) != this->loop_->U_uncapped(0, 0)) {
+ const ::Eigen::Matrix<double, 3, 1> &R = this->loop_->R();
+ profile_.MoveCurrentState(R.block<2, 1>(0, 0));
}
}
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 11d641d..2bc1879 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -3,7 +3,7 @@
import "frc971/control_loops/control_loops.q";
struct ProfiledJointStatus {
- // Is the turret zeroed?
+ // Is the subsystem zeroed?
bool zeroed;
// The state of the subsystem, if applicable. -1 otherwise.
@@ -39,3 +39,41 @@
// State of the estimator.
.frc971.EstimatorState estimator_state;
};
+
+struct AbsoluteProfiledJointStatus {
+ // Is the subsystem zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ .frc971.AbsoluteEstimatorState estimator_state;
+};
diff --git a/y2017/analysis/intake_test b/y2017/analysis/intake_test
new file mode 100644
index 0000000..4e11705
--- /dev/null
+++ b/y2017/analysis/intake_test
@@ -0,0 +1,7 @@
+superstructure_lib_test output voltage_intake
+
+superstructure_lib_test status intake estimator_state position
+superstructure_lib_test status intake state
+superstructure_lib_test goal intake distance
+superstructure_lib_test position intake pot
+superstructure_lib_test position intake encoder
diff --git a/y2017/analysis/turret_test b/y2017/analysis/turret_test
new file mode 100644
index 0000000..e471b93
--- /dev/null
+++ b/y2017/analysis/turret_test
@@ -0,0 +1,7 @@
+superstructure_lib_test output voltage_turret
+
+superstructure_lib_test status turret estimator_state position
+superstructure_lib_test status turret state
+superstructure_lib_test goal turret angle
+superstructure_lib_test position turret pot
+superstructure_lib_test position turret encoder
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 295a800..dee04dd 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -70,14 +70,14 @@
r->drivetrain_max_speed = 5;
intake->zeroing.average_filter_size = Values::kZeroingSampleSize;
- intake->zeroing.index_difference = Values::kIntakeEncoderIndexDifference;
- intake->zeroing.measured_index_position = 0;
- intake->zeroing.allowable_encoder_error = 0.3;
+ intake->zeroing.one_revolution_distance = Values::kIntakeEncoderIndexDifference;
+ intake->zeroing.measured_absolute_position = 0;
+ intake->zeroing.zeroing_threshold = 0.3;
turret->zeroing.average_filter_size = Values::kZeroingSampleSize;
- turret->zeroing.index_difference = Values::kTurretEncoderIndexDifference;
- turret->zeroing.measured_index_position = 0;
- turret->zeroing.allowable_encoder_error = 0.3;
+ turret->zeroing.one_revolution_distance = Values::kTurretEncoderIndexDifference;
+ turret->zeroing.measured_absolute_position = 0;
+ turret->zeroing.zeroing_threshold = 0.3;
hood->zeroing.average_filter_size = Values::kZeroingSampleSize;
hood->zeroing.index_difference = Values::kHoodEncoderIndexDifference;
diff --git a/y2017/constants.h b/y2017/constants.h
index 2c8916f..1ad582b 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -29,12 +29,12 @@
struct Values {
struct Intake {
double pot_offset;
- ::frc971::constants::PotAndIndexPulseZeroingConstants zeroing;
+ ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
};
struct Turret {
double pot_offset;
- ::frc971::constants::PotAndIndexPulseZeroingConstants zeroing;
+ ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
};
struct Hood {
@@ -86,8 +86,8 @@
control_loops::superstructure::hood::kFreeSpeed * control_loops::superstructure::hood::kOutputRatio /
constants::Values::kHoodEncoderRatio * kHoodEncoderCountsPerRevolution;
static constexpr ::frc971::constants::Range kHoodRange{
- -0.39 * M_PI / 2.0, 37.11 * M_PI / 2.0, (-0.39 + 1.0) * M_PI / 2.0,
- (37.11 - 1.0) * M_PI / 2.0};
+ -0.39 * M_PI / 180.0, 37.11 * M_PI / 180.0, (-0.39 + 1.0) * M_PI / 180.0,
+ (37.11 - 1.0) * M_PI / 180.0};
static constexpr double kTurretEncoderCountsPerRevolution = 1024 * 4;
static constexpr double kTurretEncoderRatio = 16.0 / 92.0;
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 00f8a2c..c6dce6c 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -26,6 +26,8 @@
':superstructure_queue',
'//aos/common/controls:control_loop',
'//y2017/control_loops/superstructure/hood',
+ '//y2017/control_loops/superstructure/turret',
+ '//y2017/control_loops/superstructure/intake',
'//y2017:constants',
],
)
@@ -45,5 +47,8 @@
'//aos/common:time',
'//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',
],
)
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 8617587..9760eac 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -30,6 +30,7 @@
cc_library(
name = 'hood',
+ visibility = ['//visibility:public'],
srcs = [
'hood.cc',
],
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
index 68faeba..164a63b 100644
--- a/y2017/control_loops/superstructure/intake/BUILD
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -27,3 +27,20 @@
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'intake',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'intake.cc',
+ ],
+ hdrs = [
+ 'intake.h',
+ ],
+ deps = [
+ ':intake_plants',
+ '//frc971/control_loops:profiled_subsystem',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
new file mode 100644
index 0000000..c66e493
--- /dev/null
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -0,0 +1,125 @@
+#include "y2017/control_loops/superstructure/intake/intake.h"
+
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/intake/intake_integral_plant.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+constexpr double Intake::kZeroingVoltage;
+constexpr double Intake::kOperatingVoltage;
+
+Intake::Intake()
+ : profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(MakeIntegralIntakeLoop())),
+ constants::GetValues().intake.zeroing,
+ constants::Values::kIntakeRange, 0.3, 5.0) {}
+
+void Intake::Reset() {
+ state_ = State::UNINITIALIZED;
+ profiled_subsystem_.Reset();
+}
+
+void Intake::Iterate(
+ const control_loops::IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position, double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ bool disable = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the intake is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake\n");
+ if (profiled_subsystem_.initialized()) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ disable = true;
+ break;
+
+ case State::DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled.
+ if (disable) {
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ }
+ } else {
+ state_ = State::ZEROING;
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(0.10, 1);
+
+ // We are not ready to start doing anything yet.
+ disable = true;
+ break;
+
+ case State::ZEROING:
+ // Now, zero by actively holding still.
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ } else if (disable) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ break;
+
+ case State::RUNNING: {
+ if (disable) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ }
+
+ if (unsafe_goal) {
+ profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
+ profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance);
+ }
+
+ // ESTOP if we hit the hard limits.
+ if (profiled_subsystem_.CheckHardLimits()) {
+ state_ = State::ESTOP;
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage =
+ (state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disable);
+
+ // Write out all the voltages.
+ if (output) {
+ *output = profiled_subsystem_.voltage();
+ }
+
+ // Save debug/internal state.
+ // TODO(austin): Save more.
+ status->zeroed = profiled_subsystem_.zeroed();
+
+ profiled_subsystem_.PopulateStatus(status);
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+}
+
+} // namespace intake
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
new file mode 100644
index 0000000..def4d96
--- /dev/null
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -0,0 +1,53 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+class Intake {
+ public:
+ Intake();
+ double goal(int row, int col) const {
+ return profiled_subsystem_.goal(row, col);
+ }
+
+ // The zeroing and operating voltages.
+ static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ void Iterate(const control_loops::IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position,
+ double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+
+ void Reset();
+
+ enum class State : int32_t{
+ UNINITIALIZED,
+ DISABLED_INITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ State state_;
+
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator>
+ profiled_subsystem_;
+};
+
+} // namespace intake
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 424af9a..f579292 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -4,6 +4,8 @@
#include "aos/common/logging/logging.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
+#include "y2017/control_loops/superstructure/turret/turret.h"
+#include "y2017/control_loops/superstructure/intake/intake.h"
namespace y2017 {
namespace control_loops {
@@ -22,12 +24,24 @@
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
hood_.Reset();
+ turret_.Reset();
+ intake_.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,
+ &(status->turret));
+
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake),
+ output != nullptr ? &(output->voltage_intake) : nullptr,
+ &(status->intake));
}
} // namespace superstructure
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index c6a2742..4811ab1 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -6,6 +6,8 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
+#include "y2017/control_loops/superstructure/turret/turret.h"
+#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
@@ -19,34 +21,9 @@
control_loops::SuperstructureQueue *my_superstructure =
&control_loops::superstructure_queue);
- enum State {
- // Wait for all the filters to be ready before starting the initialization
- // process.
- UNINITIALIZED = 0,
-
- // We now are ready to decide how to zero. Decide what to do once we are
- // enabled.
- DISABLED_INITIALIZED = 1,
-
- ZEROING = 2,
- // Run with full power.
- RUNNING = 3,
- // Internal error caused the superstructure to abort.
- ESTOP = 4,
- };
-
const hood::Hood &hood() const { return hood_; }
-
- bool IsRunning() const { return state_ == RUNNING; }
-
- // Returns the value to move the joint to such that it will stay below
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance);
- // Returns the value to move the joint to such that it will stay above
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance);
+ const turret::Turret &turret() const { return turret_; }
+ const intake::Intake &intake() const { return intake_; }
protected:
virtual void RunIteration(
@@ -57,8 +34,8 @@
private:
hood::Hood hood_;
-
- State state_ = UNINITIALIZED;
+ turret::Turret turret_;
+ intake::Intake intake_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index be5b824..43e6004 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -102,8 +102,8 @@
bool estopped;
// Each subsystems status.
- .frc971.control_loops.ProfiledJointStatus intake;
- .frc971.control_loops.ProfiledJointStatus turret;
+ .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.AbsoluteProfiledJointStatus turret;
.frc971.control_loops.ProfiledJointStatus hood;
IndexerStatus indexer;
ShooterStatus shooter;
@@ -115,14 +115,14 @@
// Position of the intake, zero when the intake is in, positive when it is
// out.
- .frc971.PotAndIndexPosition intake;
+ .frc971.PotAndAbsolutePosition intake;
// Indexer angle in radians.
double theta_indexer;
// The sensor readings for the turret. The units and sign are defined the
// same as what's in the Goal message.
- .frc971.PotAndIndexPosition turret;
+ .frc971.PotAndAbsolutePosition turret;
// The sensor readings for the hood. The units and sign are defined the
// same as what's in the Goal message.
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 1b2cf23..25de1a3 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,8 @@
#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"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -19,11 +21,13 @@
namespace control_loops {
namespace superstructure {
namespace testing {
+namespace {
+constexpr double kNoiseScalar = 0.01;
+} // namespace
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-// TODO(Adam): Check that the dimensions are correct.
class HoodPlant : public StateFeedbackPlant<2, 1, 1> {
public:
explicit HoodPlant(StateFeedbackPlant<2, 1, 1> &&other)
@@ -43,16 +47,61 @@
double voltage_offset_ = 0.0;
};
-// Class which simulates the hood and sends out queue messages with the
-// position.
+class TurretPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit TurretPlant(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 IntakePlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit IntakePlant(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 which simulates the superstructure and sends out queue messages with
+// the position.
class SuperstructureSimulation {
public:
- // Constructs a hood simulation.
- static constexpr double kNoiseScalar = 0.01;
SuperstructureSimulation()
: hood_plant_(new HoodPlant(
::y2017::control_loops::superstructure::hood::MakeHoodPlant())),
hood_pot_encoder_(constants::Values::kHoodEncoderIndexDifference),
+
+ turret_plant_(new TurretPlant(
+ ::y2017::control_loops::superstructure::turret::MakeTurretPlant())),
+ turret_pot_encoder_(constants::Values::kTurretEncoderIndexDifference),
+
+ intake_plant_(new IntakePlant(
+ ::y2017::control_loops::superstructure::intake::MakeIntakePlant())),
+ intake_pot_encoder_(constants::Values::kIntakeEncoderIndexDifference),
+
superstructure_queue_(".y2017.control_loops.superstructure", 0xdeadbeef,
".y2017.control_loops.superstructure.goal",
".y2017.control_loops.superstructure.position",
@@ -62,6 +111,16 @@
InitializeHoodPosition((constants::Values::kHoodRange.lower +
constants::Values::kHoodRange.upper) /
2.0);
+
+ // Start the turret out in the middle by default.
+ InitializeTurretPosition((constants::Values::kTurretRange.lower +
+ constants::Values::kTurretRange.upper) /
+ 2.0);
+
+ // Start the intake out in the middle by default.
+ InitializeIntakePosition((constants::Values::kIntakeRange.lower +
+ constants::Values::kIntakeRange.upper) /
+ 2.0);
}
void InitializeHoodPosition(double start_pos) {
@@ -73,20 +132,43 @@
constants::GetValues().hood.zeroing.measured_index_position);
}
- // Sends a queue message with the position of the hood.
+ void InitializeTurretPosition(double start_pos) {
+ turret_plant_->mutable_X(0, 0) = start_pos;
+ turret_plant_->mutable_X(1, 0) = 0.0;
+
+ turret_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues().turret.zeroing.measured_absolute_position);
+ }
+
+ void InitializeIntakePosition(double start_pos) {
+ intake_plant_->mutable_X(0, 0) = start_pos;
+ intake_plant_->mutable_X(1, 0) = 0.0;
+
+ intake_pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ constants::GetValues().intake.zeroing.measured_absolute_position);
+ }
+
+ // Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
superstructure_queue_.position.MakeMessage();
hood_pot_encoder_.GetSensorValues(&position->hood);
+ turret_pot_encoder_.GetSensorValues(&position->turret);
+ intake_pot_encoder_.GetSensorValues(&position->intake);
position.Send();
}
double hood_position() const { return hood_plant_->X(0, 0); }
+ double hood_angular_velocity() const { return hood_plant_->X(1, 0); }
- double hood_angular_velocity() const {
- return hood_plant_->X(1, 0);
- }
+ double turret_position() const { return turret_plant_->X(0, 0); }
+ double turret_angular_velocity() const { return turret_plant_->X(1, 0); }
+
+ double intake_position() const { return intake_plant_->X(0, 0); }
+ double intake_velocity() const { return intake_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
@@ -94,33 +176,88 @@
hood_plant_->set_voltage_offset(power_error);
}
- // Simulates hood for a single timestep.
+ void set_turret_power_error(double power_error) {
+ turret_plant_->set_voltage_offset(power_error);
+ }
+
+ void set_intake_power_error(double power_error) {
+ intake_plant_->set_voltage_offset(power_error);
+ }
+
+ // Simulates the superstructure for a single timestep.
void Simulate() {
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
- const double voltage_check =
- (superstructure_queue_.status->hood.state == Superstructure::RUNNING)
+ const double voltage_check_hood =
+ (static_cast<hood::Hood::State>(
+ superstructure_queue_.status->hood.state) ==
+ hood::Hood::State::RUNNING)
? superstructure::hood::Hood::kOperatingVoltage
: superstructure::hood::Hood::kZeroingVoltage;
+ const double voltage_check_turret =
+ (static_cast<turret::Turret::State>(
+ superstructure_queue_.status->turret.state) ==
+ turret::Turret::State::RUNNING)
+ ? superstructure::turret::Turret::kOperatingVoltage
+ : superstructure::turret::Turret::kZeroingVoltage;
+
+ const double voltage_check_intake =
+ (static_cast<intake::Intake::State>(
+ superstructure_queue_.status->intake.state) ==
+ intake::Intake::State::RUNNING)
+ ? superstructure::intake::Intake::kOperatingVoltage
+ : superstructure::intake::Intake::kZeroingVoltage;
+
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_hood),
- voltage_check);
- hood_plant_->mutable_U()
- << superstructure_queue_.output->voltage_hood +
- hood_plant_->voltage_offset();
+ voltage_check_hood);
+
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_turret),
+ voltage_check_turret);
+
+ CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ voltage_check_intake);
+
+ hood_plant_->mutable_U() << superstructure_queue_.output->voltage_hood +
+ hood_plant_->voltage_offset();
+
+ turret_plant_->mutable_U() << superstructure_queue_.output->voltage_turret +
+ turret_plant_->voltage_offset();
+
+ intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
+ intake_plant_->voltage_offset();
hood_plant_->Update();
+ turret_plant_->Update();
+ intake_plant_->Update();
- const double angle = hood_plant_->Y(0, 0);
- hood_pot_encoder_.MoveTo(angle);
- EXPECT_GE(angle, constants::Values::kHoodRange.lower_hard);
- EXPECT_LE(angle, constants::Values::kHoodRange.upper_hard);
+ const double angle_hood = hood_plant_->Y(0, 0);
+ const double angle_turret = turret_plant_->Y(0, 0);
+ const double position_intake = intake_plant_->Y(0, 0);
+
+ hood_pot_encoder_.MoveTo(angle_hood);
+ turret_pot_encoder_.MoveTo(angle_turret);
+ intake_pot_encoder_.MoveTo(position_intake);
+
+ EXPECT_GE(angle_hood, constants::Values::kHoodRange.lower_hard);
+ EXPECT_LE(angle_hood, constants::Values::kHoodRange.upper_hard);
+ EXPECT_GE(angle_turret, constants::Values::kTurretRange.lower_hard);
+ EXPECT_LE(angle_turret, constants::Values::kTurretRange.upper_hard);
+ EXPECT_GE(position_intake, constants::Values::kIntakeRange.lower_hard);
+ EXPECT_LE(position_intake, constants::Values::kIntakeRange.upper_hard);
}
private:
::std::unique_ptr<HoodPlant> hood_plant_;
PositionSensorSimulator hood_pot_encoder_;
+
+ ::std::unique_ptr<TurretPlant> turret_plant_;
+ PositionSensorSimulator turret_pot_encoder_;
+
+ ::std::unique_ptr<IntakePlant> intake_plant_;
+ PositionSensorSimulator intake_pot_encoder_;
+
SuperstructureQueue superstructure_queue_;
};
@@ -147,6 +284,16 @@
superstructure_queue_.status->hood.position, 0.001);
EXPECT_NEAR(superstructure_queue_.goal->hood.angle,
superstructure_plant_.hood_position(), 0.001);
+
+ EXPECT_NEAR(superstructure_queue_.goal->turret.angle,
+ superstructure_queue_.status->turret.position, 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->turret.angle,
+ superstructure_plant_.turret_position(), 0.001);
+
+ EXPECT_NEAR(superstructure_queue_.goal->intake.distance,
+ superstructure_queue_.status->intake.position, 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->intake.distance,
+ superstructure_plant_.intake_position(), 0.001);
}
// Runs one iteration of the whole simulation.
@@ -169,9 +316,6 @@
}
}
- void set_peak_acceleration(double value) { peak_acceleration_ = value; }
- void set_peak_velocity(double value) { peak_velocity_ = value; }
-
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory
// that is no longer valid.
@@ -180,17 +324,15 @@
// Create a control loop and simulation.
Superstructure superstructure_;
SuperstructureSimulation superstructure_plant_;
-
- private:
- double peak_velocity_ = 1e10;
- double peak_acceleration_ = 1e10;
};
-// Tests that the hood does nothing when the goal is zero.
+// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->hood.angle = 0.0;
+ goal->hood.angle = 0.2;
+ goal->turret.angle = 0.0;
+ goal->intake.distance = 0.05;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(5));
@@ -200,7 +342,7 @@
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
}
-// Tests that the loop can reach a goal.
+// Tests that the hood, turret and intake loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
// Set a reasonable goal.
{
@@ -208,6 +350,15 @@
goal->hood.angle = 0.1;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
+
+ goal->turret.angle = 0.1;
+ goal->turret.profile_params.max_velocity = 1;
+ goal->turret.profile_params.max_acceleration = 0.5;
+
+ goal->intake.distance = 0.1;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
ASSERT_TRUE(goal.Send());
}
@@ -217,8 +368,8 @@
VerifyNearGoal();
}
-// Tests that the loop doesn't try and go beyond the physical range of the
-// mechanisms.
+// Tests that the hood, turret and intake loops doesn't try and go beyond the
+// physical range of the mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
// Set some ridiculous goals to test upper limits.
{
@@ -226,6 +377,15 @@
goal->hood.angle = 100.0;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
+
+ goal->turret.angle = 100.0;
+ goal->turret.profile_params.max_velocity = 1;
+ goal->turret.profile_params.max_acceleration = 0.5;
+
+ goal->intake.distance = 100.0;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(10));
@@ -235,12 +395,27 @@
EXPECT_NEAR(constants::Values::kHoodRange.upper,
superstructure_queue_.status->hood.position, 0.001);
+ EXPECT_NEAR(constants::Values::kTurretRange.upper,
+ superstructure_queue_.status->turret.position, 0.001);
+
+ EXPECT_NEAR(constants::Values::kIntakeRange.upper,
+ superstructure_queue_.status->intake.position, 0.001);
+
// Set some ridiculous goals to test lower limits.
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = -100.0;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
+
+ goal->turret.angle = -100.0;
+ goal->turret.profile_params.max_velocity = 1;
+ goal->turret.profile_params.max_acceleration = 0.5;
+
+ goal->intake.distance = -100.0;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
+
ASSERT_TRUE(goal.Send());
}
@@ -250,15 +425,29 @@
superstructure_queue_.status.FetchLatest();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
superstructure_queue_.status->hood.position, 0.001);
+
+ EXPECT_NEAR(constants::Values::kTurretRange.lower,
+ superstructure_queue_.status->turret.position, 0.001);
+
+ EXPECT_NEAR(constants::Values::kIntakeRange.lower,
+ superstructure_queue_.status->intake.position, 0.001);
}
-// Tests that the loop zeroes when run for a while.
+// Tests that the hood, turret and intake loops zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
+
+ goal->turret.angle = constants::Values::kTurretRange.lower;
+ goal->turret.profile_params.max_velocity = 1;
+ goal->turret.profile_params.max_acceleration = 0.5;
+
+ goal->intake.distance = constants::Values::kIntakeRange.lower;
+ goal->intake.profile_params.max_velocity = 1;
+ goal->intake.profile_params.max_acceleration = 0.5;
ASSERT_TRUE(goal.Send());
}
@@ -272,14 +461,24 @@
RunForTime(chrono::seconds(5));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
+ EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
+ EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
}
TEST_F(SuperstructureTest, LowerHardstopStartup) {
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.lower_hard);
+
+ superstructure_plant_.InitializeTurretPosition(
+ constants::Values::kTurretRange.lower_hard);
+
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.lower_hard);
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
+ goal->turret.angle = constants::Values::kTurretRange.lower;
+ goal->intake.distance = constants::Values::kIntakeRange.lower;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(5));
@@ -291,9 +490,17 @@
TEST_F(SuperstructureTest, UpperHardstopStartup) {
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.upper_hard);
+
+ superstructure_plant_.InitializeTurretPosition(
+ constants::Values::kTurretRange.upper_hard);
+
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.upper_hard);
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.upper;
+ goal->turret.angle = constants::Values::kTurretRange.upper;
+ goal->intake.distance = constants::Values::kIntakeRange.upper;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(5));
@@ -306,20 +513,39 @@
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.upper);
+ superstructure_plant_.InitializeTurretPosition(
+ constants::Values::kTurretRange.upper);
+
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.upper);
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.upper - 0.1;
+ goal->turret.angle = constants::Values::kTurretRange.upper - 0.1;
+ goal->intake.distance = constants::Values::kIntakeRange.upper - 0.1;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(10));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
+ EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
+ EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+
VerifyNearGoal();
SimulateSensorReset();
RunForTime(chrono::milliseconds(100));
+
EXPECT_EQ(hood::Hood::State::UNINITIALIZED, superstructure_.hood().state());
+ EXPECT_EQ(turret::Turret::State::UNINITIALIZED,
+ superstructure_.turret().state());
+ EXPECT_EQ(intake::Intake::State::UNINITIALIZED,
+ superstructure_.intake().state());
+
RunForTime(chrono::milliseconds(5000));
+
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
+ EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
+ EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
VerifyNearGoal();
}
@@ -329,15 +555,21 @@
{
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;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::milliseconds(100), false);
EXPECT_EQ(0.0, superstructure_.hood().goal(0, 0));
+ EXPECT_EQ(0.0, superstructure_.turret().goal(0, 0));
+ EXPECT_EQ(0.0, superstructure_.intake().goal(0, 0));
// Now make sure they move correctly
- RunForTime(chrono::milliseconds(4000), true);
+ RunForTime(chrono::seconds(4), true);
EXPECT_NE(0.0, superstructure_.hood().goal(0, 0));
+ EXPECT_NE(0.0, superstructure_.turret().goal(0, 0));
+ EXPECT_NE(0.0, superstructure_.intake().goal(0, 0));
}
// Tests that zeroing while disabled works. Starts the superstructure near a
@@ -350,6 +582,8 @@
{
auto goal = superstructure_queue_.goal.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
+ goal->turret.angle = constants::Values::kTurretRange.lower;
+ goal->intake.distance = constants::Values::kIntakeRange.lower;
ASSERT_TRUE(goal.Send());
}
@@ -357,12 +591,19 @@
RunForTime(chrono::seconds(2), false);
EXPECT_EQ(hood::Hood::State::DISABLED_INITIALIZED,
superstructure_.hood().state());
+ EXPECT_EQ(turret::Turret::State::RUNNING,
+ superstructure_.turret().state());
+ EXPECT_EQ(intake::Intake::State::RUNNING,
+ superstructure_.intake().state());
superstructure_plant_.set_hood_power_error(1.0);
RunForTime(chrono::seconds(1), false);
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
+ EXPECT_EQ(turret::Turret::State::RUNNING, superstructure_.turret().state());
+ EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
+
RunForTime(chrono::seconds(2), true);
VerifyNearGoal();
diff --git a/y2017/control_loops/superstructure/turret/BUILD b/y2017/control_loops/superstructure/turret/BUILD
index 70b9508..8969693 100644
--- a/y2017/control_loops/superstructure/turret/BUILD
+++ b/y2017/control_loops/superstructure/turret/BUILD
@@ -27,3 +27,20 @@
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'turret',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'turret.cc',
+ ],
+ hdrs = [
+ 'turret.h',
+ ],
+ deps = [
+ ':turret_plants',
+ '//frc971/control_loops:profiled_subsystem',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/turret/turret.cc b/y2017/control_loops/superstructure/turret/turret.cc
new file mode 100644
index 0000000..9616dff
--- /dev/null
+++ b/y2017/control_loops/superstructure/turret/turret.cc
@@ -0,0 +1,125 @@
+#include "y2017/control_loops/superstructure/turret/turret.h"
+
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/turret/turret_integral_plant.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+constexpr double Turret::kZeroingVoltage;
+constexpr double Turret::kOperatingVoltage;
+
+Turret::Turret()
+ : profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(MakeIntegralTurretLoop())),
+ constants::GetValues().turret.zeroing,
+ constants::Values::kTurretRange, 7.0, 50.0) {}
+
+void Turret::Reset() {
+ state_ = State::UNINITIALIZED;
+ profiled_subsystem_.Reset();
+}
+
+void Turret::Iterate(
+ const control_loops::TurretGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position, double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ bool disable = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the turret is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for turret\n");
+ if (profiled_subsystem_.initialized()) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ disable = true;
+ break;
+
+ case State::DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled.
+ if (disable) {
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ }
+ } else {
+ state_ = State::ZEROING;
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(0.10, 1);
+
+ // We are not ready to start doing anything yet.
+ disable = true;
+ break;
+
+ case State::ZEROING:
+ // Now, zero by actively holding still.
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ } else if (disable) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ break;
+
+ case State::RUNNING: {
+ if (disable) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ }
+
+ if (unsafe_goal) {
+ profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
+ profiled_subsystem_.set_unprofiled_goal(unsafe_goal->angle);
+ }
+
+ // ESTOP if we hit the hard limits.
+ if (profiled_subsystem_.CheckHardLimits()) {
+ state_ = State::ESTOP;
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage =
+ (state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disable);
+
+ // Write out all the voltages.
+ if (output) {
+ *output = profiled_subsystem_.voltage();
+ }
+
+ // Save debug/internal state.
+ // TODO(austin): Save more.
+ status->zeroed = profiled_subsystem_.zeroed();
+
+ profiled_subsystem_.PopulateStatus(status);
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+}
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/turret/turret.h b/y2017/control_loops/superstructure/turret/turret.h
new file mode 100644
index 0000000..5499d92
--- /dev/null
+++ b/y2017/control_loops/superstructure/turret/turret.h
@@ -0,0 +1,53 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
+
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+class Turret {
+ public:
+ Turret();
+ double goal(int row, int col) const {
+ return profiled_subsystem_.goal(row, col);
+ }
+
+ // The zeroing and operating voltages.
+ static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ void Iterate(const control_loops::TurretGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position,
+ double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+
+ void Reset();
+
+ enum class State : int32_t{
+ UNINITIALIZED,
+ DISABLED_INITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ State state_;
+
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator>
+ profiled_subsystem_;
+};
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_