Merge "Add shooter class and python."
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index b220581..e115b0e 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -137,6 +137,8 @@
Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
flatbuffers::FlatBufferBuilder *status_fbb) {
+ CHECK_NOTNULL(position);
+ CHECK_NOTNULL(status_fbb);
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/y2020/BUILD b/y2020/BUILD
index 9f7795e..215c760 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -35,6 +35,7 @@
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
+ "//y2020/control_loops/superstructure/turret:turret_plants",
"@com_google_absl//absl/base",
],
)
@@ -134,8 +135,8 @@
data = [
":config.json",
"//aos/network:web_proxy_main",
- "//y2020/www:main_bundle",
"//y2020/www:files",
"//y2020/www:flatbuffers",
+ "//y2020/www:main_bundle",
],
)
diff --git a/y2020/constants.cc b/y2020/constants.cc
index aee797f..6f79b6b 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -12,9 +12,9 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
-#include "y2020/control_loops/superstructure/intake/integral_intake_plant.h"
-
#include "y2020/control_loops/superstructure/hood/integral_hood_plant.h"
+#include "y2020/control_loops/superstructure/intake/integral_intake_plant.h"
+#include "y2020/control_loops/superstructure/turret/integral_turret_plant.h"
namespace y2020 {
namespace constants {
@@ -69,6 +69,28 @@
intake->zeroing_constants.allowable_encoder_error = 0.9;
intake->zeroing_constants.middle_position = Values::kIntakeRange().middle();
+ Values::PotAndAbsEncoderConstants *const turret = &r->turret;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const turret_params = &turret->subsystem_params;
+
+ //Turret Constants
+ turret_params->zeroing_voltage = 4.0;
+ turret_params->operating_voltage = 12.0;
+ // TODO(austin): Tune these.
+ turret_params->zeroing_profile_params = {0.5, 2.0};
+ turret_params->default_profile_params = {15.0, 40.0};
+ turret_params->range = Values::kTurretRange();
+ turret_params->make_integral_loop =
+ &control_loops::superstructure::turret::MakeIntegralTurretLoop;
+ turret_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ turret_params->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kTurretEncoderRatio();
+ turret_params->zeroing_constants.zeroing_threshold = 0.0005;
+ turret_params->zeroing_constants.moving_buffer_size = 20;
+ turret_params->zeroing_constants.allowable_encoder_error = 0.9;
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -76,17 +98,29 @@
case kCompTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+
intake->zeroing_constants.measured_absolute_position = 0.0;
+
+ turret->potentiometer_offset = 0.0;
+ turret_params->zeroing_constants.measured_absolute_position = 0.0;
break;
case kPracticeTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+
intake->zeroing_constants.measured_absolute_position = 0.0;
+
+ turret->potentiometer_offset = 0.0;
+ turret_params->zeroing_constants.measured_absolute_position = 0.0;
break;
case kCodingRobotTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+
intake->zeroing_constants.measured_absolute_position = 0.0;
+
+ turret->potentiometer_offset = 0.0;
+ turret_params->zeroing_constants.measured_absolute_position = 0.0;
break;
default:
diff --git a/y2020/constants.h b/y2020/constants.h
index 69e7f28..d7b9b4b 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -12,6 +12,7 @@
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
+#include "y2020/control_loops/superstructure/turret/turret_plant.h"
namespace y2020 {
namespace constants {
@@ -53,6 +54,10 @@
};
}
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ hood;
+
// Intake
static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
@@ -77,10 +82,43 @@
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
- hood;
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
intake;
+
+ // Turret
+ static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kTurretEncoderRatio() {
+ return 1.0; // TODO (Kai): Get Gear Ratios when ready
+ }
+
+ static constexpr double kMaxTurretEncoderPulsesPerSecond() {
+ return control_loops::superstructure::turret::kFreeSpeed *
+ control_loops::superstructure::turret::kOutputRatio /
+ kTurretEncoderRatio() / (2.0 * M_PI) *
+ kTurretEncoderCountsPerRevolution();
+ }
+
+ // TODO(austin): Figure out the actual constant here.
+ static constexpr double kTurretPotRatio() { return 1.0; }
+
+ static constexpr ::frc971::constants::Range kTurretRange() {
+ return ::frc971::constants::Range{
+ // TODO (Kai): Placeholders right now.
+ -3.2, // Back Hard
+ 3.2, // Front Hard
+ -3.14, // Back Soft
+ 3.14 // Front Soft
+ };
+ }
+
+ struct PotAndAbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+
+ PotAndAbsEncoderConstants turret;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index b9bbcf0..87ee689 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -55,6 +55,7 @@
"superstructure.h",
],
deps = [
+ ":climber",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -103,3 +104,20 @@
"//y2020/control_loops/superstructure/intake:intake_plants",
],
)
+
+cc_library(
+ name = "climber",
+ srcs = [
+ "climber.cc",
+ ],
+ hdrs = [
+ "climber.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/climber.cc b/y2020/control_loops/superstructure/climber.cc
new file mode 100644
index 0000000..bb449da
--- /dev/null
+++ b/y2020/control_loops/superstructure/climber.cc
@@ -0,0 +1,23 @@
+#include "y2020/control_loops/superstructure/climber.h"
+
+#include <algorithm>
+
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+
+void Climber::Iterate(const Goal *unsafe_goal, OutputT *output) {
+ if (unsafe_goal && output) {
+ // Pass through the voltage request from the user. Backwards isn't
+ // supported, so prevent that.
+ output->climber_voltage =
+ std::clamp(unsafe_goal->climber_voltage(), 0.0f, 12.0f);
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/climber.h b/y2020/control_loops/superstructure/climber.h
new file mode 100644
index 0000000..29b7e51
--- /dev/null
+++ b/y2020/control_loops/superstructure/climber.h
@@ -0,0 +1,21 @@
+#ifndef Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
+#define Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
+
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+
+// Class to encapsulate the climber logic.
+class Climber {
+ public:
+ void Iterate(const Goal *unsafe_goal, OutputT *output);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+
+#endif // Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index cfa6d94..c2512db 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -14,7 +14,8 @@
: aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
hood_(constants::GetValues().hood),
- intake_joint_(constants::GetValues().intake) {
+ intake_joint_(constants::GetValues().intake),
+ turret_(constants::GetValues().turret.subsystem_params) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -26,6 +27,7 @@
AOS_LOG(ERROR, "WPILib reset, restarting\n");
hood_.Reset();
intake_joint_.Reset();
+ turret_.Reset();
}
OutputT output_struct;
@@ -43,18 +45,32 @@
output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
status->fbb());
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ turret_status_offset = turret_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
+ position->turret(),
+ output != nullptr ? &(output_struct.turret_voltage) : nullptr,
+ status->fbb());
+
+ climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
+
bool zeroed;
bool estopped;
{
- AbsoluteEncoderProfiledJointStatus *hood_status =
+ const AbsoluteEncoderProfiledJointStatus *const hood_status =
GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
- AbsoluteEncoderProfiledJointStatus *intake_status =
+ const AbsoluteEncoderProfiledJointStatus *const intake_status =
GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
- zeroed = hood_status->zeroed() && intake_status->zeroed();
- estopped = hood_status->estopped() || intake_status->estopped();
+ const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
+ GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
+
+ zeroed = hood_status->zeroed() && intake_status->zeroed() &&
+ turret_status->zeroed();
+ estopped = hood_status->estopped() || intake_status->estopped() ||
+ turret_status->estopped();
}
Status::Builder status_builder = status->MakeBuilder<Status>();
@@ -64,6 +80,7 @@
status_builder.add_hood(hood_status_offset);
status_builder.add_intake(intake_status_offset);
+ status_builder.add_turret(turret_status_offset);
status->Send(status_builder.Finish());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 680b447..2bc0cda 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -8,6 +8,7 @@
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/control_loops/superstructure/climber.h"
namespace y2020 {
namespace control_loops {
@@ -19,6 +20,10 @@
explicit Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name = "/superstructure");
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
using AbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
@@ -26,6 +31,7 @@
const AbsoluteEncoderSubsystem &hood() const { return hood_; }
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
+ const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
@@ -35,7 +41,9 @@
private:
AbsoluteEncoderSubsystem hood_;
AbsoluteEncoderSubsystem intake_joint_;
+ PotAndAbsoluteEncoderSubsystem turret_;
+ Climber climber_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index f26b076..f066ab0 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -24,7 +24,7 @@
// Positive = forward
intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
- //Positive is rollers intaking to Washing Machine.
+ // Positive is rollers intaking to Washing Machine.
roller_voltage:float;
// 0 = facing the front of the robot. Positive rotates counterclockwise.
@@ -49,7 +49,7 @@
shooter_tracking:bool;
// Positive is deploying climber and to climb; cannot run in reverse
- climber_winch_voltage:double;
+ climber_voltage:float;
}
root_type Goal;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 39fb975..458a771 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -31,6 +31,8 @@
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
// Class which simulates the superstructure and sends out queue messages with
// the position.
@@ -51,9 +53,14 @@
.hood.zeroing_constants.one_revolution_distance),
intake_plant_(new CappedTestPlant(intake::MakeIntakePlant())),
intake_encoder_(constants::GetValues()
- .intake.zeroing_constants.one_revolution_distance) {
+ .intake.zeroing_constants.one_revolution_distance),
+ turret_plant_(new CappedTestPlant(turret::MakeTurretPlant())),
+ turret_encoder_(constants::GetValues()
+ .turret.subsystem_params.zeroing_constants
+ .one_revolution_distance) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
InitializeIntakePosition(constants::Values::kIntakeRange().upper);
+ InitializeTurretPosition(constants::Values::kTurretRange().middle());
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
@@ -87,6 +94,16 @@
.intake.zeroing_constants.measured_absolute_position);
}
+ void InitializeTurretPosition(double start_pos) {
+ turret_plant_->mutable_X(0, 0) = start_pos;
+ turret_plant_->mutable_X(1, 0) = 0.0;
+
+ turret_encoder_.Initialize(start_pos, kNoiseScalar, 0.0,
+ constants::GetValues()
+ .turret.subsystem_params.zeroing_constants
+ .measured_absolute_position);
+ }
+
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
::aos::Sender<Position>::Builder builder =
@@ -102,10 +119,16 @@
flatbuffers::Offset<frc971::AbsolutePosition> intake_offset =
intake_encoder_.GetSensorValues(&intake_builder);
+ frc971::PotAndAbsolutePosition::Builder turret_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+ turret_encoder_.GetSensorValues(&turret_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_hood(hood_offset);
position_builder.add_intake_joint(intake_offset);
+ position_builder.add_turret(turret_offset);
builder.Send(position_builder.Finish());
}
@@ -116,10 +139,14 @@
double intake_position() const { return intake_plant_->X(0, 0); }
double intake_velocity() const { return intake_plant_->X(1, 0); }
+ double turret_position() const { return turret_plant_->X(0, 0); }
+ double turret_velocity() const { return turret_plant_->X(1, 0); }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
const double last_hood_velocity = hood_velocity();
const double last_intake_velocity = intake_velocity();
+ const double last_turret_velocity = turret_velocity();
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -144,6 +171,16 @@
EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage(), 0.0,
voltage_check_intake);
+ const double voltage_check_turret =
+ (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+ superstructure_status_fetcher_->turret()->state()) ==
+ PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+ ? constants::GetValues().turret.subsystem_params.operating_voltage
+ : constants::GetValues().turret.subsystem_params.zeroing_voltage;
+
+ EXPECT_NEAR(superstructure_output_fetcher_->turret_voltage(), 0.0,
+ voltage_check_turret);
+
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_output_fetcher_->hood_voltage() +
hood_plant_->voltage_offset();
@@ -152,14 +189,21 @@
intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
intake_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> turret_U;
+ turret_U << superstructure_output_fetcher_->turret_voltage() +
+ turret_plant_->voltage_offset();
+
hood_plant_->Update(hood_U);
intake_plant_->Update(intake_U);
+ turret_plant_->Update(turret_U);
const double position_hood = hood_plant_->Y(0, 0);
const double position_intake = intake_plant_->Y(0, 0);
+ const double position_turret = turret_plant_->Y(0, 0);
hood_encoder_.MoveTo(position_hood);
intake_encoder_.MoveTo(position_intake);
+ turret_encoder_.MoveTo(position_turret);
EXPECT_GE(position_hood, constants::Values::kHoodRange().lower_hard);
EXPECT_LE(position_hood, constants::Values::kHoodRange().upper_hard);
@@ -167,6 +211,9 @@
EXPECT_GE(position_intake, constants::Values::kIntakeRange().lower_hard);
EXPECT_LE(position_intake, constants::Values::kIntakeRange().upper_hard);
+ EXPECT_GE(position_turret, constants::Values::kTurretRange().lower_hard);
+ EXPECT_LE(position_turret, constants::Values::kTurretRange().upper_hard);
+
const double loop_time = ::aos::time::DurationInSeconds(dt_);
const double hood_acceleration =
@@ -175,6 +222,9 @@
const double intake_acceleration =
(intake_velocity() - last_intake_velocity) / loop_time;
+ const double turret_acceleration =
+ (turret_velocity() - last_turret_velocity) / loop_time;
+
EXPECT_GE(peak_hood_acceleration_, hood_acceleration);
EXPECT_LE(-peak_hood_acceleration_, hood_acceleration);
EXPECT_GE(peak_hood_velocity_, hood_velocity());
@@ -184,8 +234,17 @@
EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
EXPECT_GE(peak_intake_velocity_, intake_velocity());
EXPECT_LE(-peak_intake_velocity_, intake_velocity());
+
+ EXPECT_GE(peak_turret_acceleration_, turret_acceleration);
+ EXPECT_LE(-peak_turret_acceleration_, turret_acceleration);
+ EXPECT_GE(peak_turret_velocity_, turret_velocity());
+ EXPECT_LE(-peak_turret_velocity_, turret_velocity());
+
+ climber_voltage_ = superstructure_output_fetcher_->climber_voltage();
}
+ float climber_voltage() const { return climber_voltage_; }
+
void set_peak_hood_acceleration(double value) {
peak_hood_acceleration_ = value;
}
@@ -196,6 +255,11 @@
}
void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+ void set_peak_turret_acceleration(double value) {
+ peak_turret_acceleration_ = value;
+ }
+ void set_peak_turret_velocity(double value) { peak_turret_velocity_ = value; }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -213,13 +277,20 @@
::std::unique_ptr<CappedTestPlant> intake_plant_;
PositionSensorSimulator intake_encoder_;
+ ::std::unique_ptr<CappedTestPlant> turret_plant_;
+ PositionSensorSimulator turret_encoder_;
+
// The acceleration limits to check for while moving.
double peak_hood_acceleration_ = 1e10;
double peak_intake_acceleration_ = 1e10;
+ double peak_turret_acceleration_ = 1e10;
// The velocity limits to check for while moving.
double peak_hood_velocity_ = 1e10;
double peak_intake_velocity_ = 1e10;
+ double peak_turret_velocity_ = 1e10;
+
+ float climber_voltage_ = 0.0f;
};
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
@@ -250,11 +321,21 @@
superstructure_goal_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(superstructure_goal_fetcher_->hood()->unsafe_goal(),
- superstructure_status_fetcher_->hood()->position(), 0.001);
+ // Only check the goal if there is one.
+ if (superstructure_goal_fetcher_->has_hood()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood()->unsafe_goal(),
+ superstructure_status_fetcher_->hood()->position(), 0.001);
+ }
- EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
- superstructure_status_fetcher_->intake()->position(), 0.001);
+ if (superstructure_goal_fetcher_->has_intake()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_turret()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ }
}
void CheckIfZeroed() {
@@ -314,10 +395,15 @@
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kIntakeRange().middle());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -349,10 +435,15 @@
*builder.fbb(), 0.2,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -379,10 +470,15 @@
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kIntakeRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -404,10 +500,15 @@
*builder.fbb(), constants::Values::kIntakeRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -417,6 +518,9 @@
superstructure_plant_.set_peak_intake_velocity(23.0);
superstructure_plant_.set_peak_intake_acceleration(0.2);
+ superstructure_plant_.set_peak_turret_velocity(23.0);
+ superstructure_plant_.set_peak_turret_acceleration(0.2);
+
// Intake needs over 8 seconds to reach the goal
RunFor(chrono::seconds(9));
VerifyNearGoal();
@@ -432,6 +536,9 @@
EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.intake_joint().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.turret().state());
}
// Tests that running disabled works
@@ -440,6 +547,47 @@
CheckIfZeroed();
}
+// Tests that the climber passes through per the design.
+TEST_F(SuperstructureTest, Climber) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+
+ superstructure_plant_.InitializeHoodPosition(0.7);
+ superstructure_plant_.InitializeIntakePosition(0.7);
+
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_voltage(-10.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ // Can't go backwards.
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), 0.0);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_voltage(10.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+ RunFor(chrono::seconds(1));
+ // But forwards works.
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), 10.0);
+
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops