Added turret and intake code and tests.
Tests are from Adam.
Change-Id: I5a89700cfe2e9983771b4523facc302243b5dc50
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_