Add intake and turret code plus superstructure tests
Signed-off-by: Milo Lin <100027790@mvla.net>
Change-Id: I9885bd1e839ba0356147606415ae915cd295faf6
Change-Id: I33bc83673645869e255136198c0789f722c881a0
Signed-off-by: Siddhartha Chatterjee <ninja.siddhartha@gmail.com>
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
Signed-off-by: Henry Speiser <henry@speiser.net>
diff --git a/y2022/BUILD b/y2022/BUILD
index 640058b..df1de1d 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -191,6 +191,7 @@
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
"//y2022/control_loops/superstructure/intake:intake_plants",
+ "//y2022/control_loops/superstructure/turret:turret_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2022/constants.cc b/y2022/constants.cc
index e71fb64..b89ef5a 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -11,8 +11,9 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
-#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
+#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
+#include "y2022/control_loops/superstructure/turret/integral_turret_plant.h"
namespace y2022 {
namespace constants {
@@ -25,42 +26,64 @@
const uint16_t kPracticeTeamNumber = 9971;
const uint16_t kCodingRobotTeamNumber = 7971;
-const Values *DoGetValuesForTeam(uint16_t team) {
- Values *const r = new Values();
+} // namespace
+
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+
+ Values r;
// TODO(Yash): Set constants
// Intake constants.
- auto *const intake = &r->intake;
+ auto *const intake_front = &r.intake_front;
+ auto *const intake_back = &r.intake_back;
- intake->zeroing_voltage = 3.0;
- intake->operating_voltage = 12.0;
- intake->zeroing_profile_params = {0.5, 3.0};
- intake->default_profile_params = {6.0, 30.0};
- intake->range = Values::kIntakeRange();
- intake->make_integral_loop =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ intake_params;
+
+ intake_params.zeroing_voltage = 3.0;
+ intake_params.operating_voltage = 12.0;
+ intake_params.zeroing_profile_params = {0.5, 3.0};
+ intake_params.default_profile_params = {6.0, 30.0};
+ intake_params.range = Values::kIntakeRange();
+ intake_params.make_integral_loop =
control_loops::superstructure::intake::MakeIntegralIntakeLoop;
-
- // The number of samples in the moving average filter.
- intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
- // The distance that the absolute encoder needs to complete a full rotation.
- intake->zeroing_constants.one_revolution_distance =
+ intake_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ intake_params.zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake_params.zeroing_constants.zeroing_threshold = 0.0005;
+ intake_params.zeroing_constants.moving_buffer_size = 20;
+ intake_params.zeroing_constants.allowable_encoder_error = 0.9;
+ intake_params.zeroing_constants.measured_absolute_position = 0.0;
- // Threshold for deciding if we are moving. moving_buffer_size samples need to
- // be within this distance of each other before we use the middle one to zero.
- intake->zeroing_constants.zeroing_threshold = 0.0005;
- // Buffer size for deciding if we are moving.
- intake->zeroing_constants.moving_buffer_size = 20;
+ intake_front->subsystem_params = intake_params;
+ intake_back->subsystem_params = intake_params;
- // Value between 0 and 1 indicating what fraction of one_revolution_distance
- // it is acceptable for the offset to move.
- intake->zeroing_constants.allowable_encoder_error = 0.9;
+ // TODO(Yash): Set constants
+ // Turret constants.
+ auto *const turret = &r.turret;
+ auto *const turret_params = &turret->subsystem_params;
- // Measured absolute position of the encoder when at zero.
- intake->zeroing_constants.measured_absolute_position = 0.0;
+ turret_params->zeroing_voltage = 4.0;
+ turret_params->operating_voltage = 8.0;
+ 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;
+ turret_params->zeroing_constants.measured_absolute_position = 0.0;
// Climber constants
- auto *const climber = &r->climber;
+ auto *const climber = &r.climber;
climber->subsystem_params.zeroing_voltage = 3.0;
climber->subsystem_params.operating_voltage = 12.0;
climber->subsystem_params.zeroing_profile_params = {0.5, 0.1};
@@ -73,18 +96,54 @@
// A set of constants for tests.
case 1:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kCompTeamNumber:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kPracticeTeamNumber:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kCodingRobotTeamNumber:
climber->potentiometer_offset = 0.0;
+ intake_front->potentiometer_offset = 0.0;
+ intake_front->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ intake_back->potentiometer_offset = 0.0;
+ intake_back->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 0.0;
+ turret->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
default:
@@ -94,26 +153,7 @@
return r;
}
-const Values *values = nullptr;
-
-void DoGetValues() {
- uint16_t team = ::aos::network::GetTeamNumber();
- LOG(INFO) << "creating a Constants for team: " << team;
- values = DoGetValuesForTeam(team);
-}
-
-} // namespace
-
-void InitValues() {
- static absl::once_flag once;
- absl::call_once(once, DoGetValues);
-}
-
-const Values &GetValues() {
- CHECK(values)
- << "Values are uninitialized. Call InitValues before accessing them.";
- return *values;
-}
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
} // namespace constants
} // namespace y2022
diff --git a/y2022/constants.h b/y2022/constants.h
index 6927567..65aad44 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,7 +9,9 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/superstructure/climber/climber_plant.h"
#include "y2022/control_loops/superstructure/intake/intake_plant.h"
+#include "y2022/control_loops/superstructure/turret/turret_plant.h"
namespace y2022 {
namespace constants {
@@ -35,35 +37,13 @@
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
(2.0 * M_PI)) *
- kDrivetrainEncoderRatio() *
- control_loops::drivetrain::kWheelRadius;
- }
-
- static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
- static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
-
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- intake;
-
- // TODO (Yash): Constants need to be tuned
- static constexpr ::frc971::constants::Range kIntakeRange() {
- return ::frc971::constants::Range{
- .lower_hard = -0.5, // Back Hard
- .upper_hard = 2.85 + 0.05, // Front Hard
- .lower = -0.300, // Back Soft
- .upper = 2.725 // Front Soft
- };
+ kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
}
// Climber
static constexpr ::frc971::constants::Range kClimberRange() {
return ::frc971::constants::Range{
- .lower_hard = -0.01,
- .upper_hard = 0.6,
- .lower = 0.0,
- .upper = 0.5
- };
+ .lower_hard = -0.01, .upper_hard = 0.6, .lower = 0.0, .upper = 0.5};
}
static constexpr double kClimberPotMetersPerRevolution() {
return 22 * 0.25 * 0.0254;
@@ -81,23 +61,76 @@
// Intake
// two encoders with same gear ratio for intake
- static constexpr double kIntakeEncoderCountsPerRevolution() { return 512.0; }
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kIntakeEncoderRatio() {
- return ((16.0 / 60.0) * (18.0 / 62.0));
+ return (16.0 / 64.0) * (20.0 / 50.0);
}
- // TODO(Milo): Also need to add specific PPR (Pulse per revolution)
+ static constexpr double kIntakePotRatio() { return 16.0 / 64.0; }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
+ }
+
+ struct PotAndAbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+
+ PotAndAbsEncoderConstants intake_front;
+ PotAndAbsEncoderConstants intake_back;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.5, // Back Hard
+ .upper_hard = 2.85 + 0.05, // Front Hard
+ .lower = -0.300, // Back Soft
+ .upper = 2.725 // Front Soft
+ };
+ }
+
+ // Intake rollers
+ static constexpr double kIntakeRollerSupplyCurrentLimit() { return 40.0; }
+ static constexpr double kIntakeRollerStatorCurrentLimit() { return 60.0; }
+
+ // Turret
+ PotAndAbsEncoderConstants turret;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kTurretRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -3.45, // Back Hard
+ .upper_hard = 3.45, // Front Hard
+ .lower = -3.3, // Back Soft
+ .upper = 3.3 // Front Soft
+ };
+ }
+
+ // Turret
+ static constexpr double kTurretPotRatio() { return 27.0 / 110.0; }
+ static constexpr double kTurretEncoderRatio() { return kTurretPotRatio(); }
+ static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kMaxTurretEncoderPulsesPerSecond() {
+ return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::turret::kOutputRatio /
+ kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
+ }
};
-// Creates (once) a Values instance for ::aos::network::GetTeamNumber(). Should
-// be called before realtime because this allocates memory.
-void InitValues();
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+Values MakeValues(uint16_t team);
-// Returns a reference to the Values instance for
-// ::aos::network::GetTeamNumber(). Values must be initialized through
-// InitValues() before calling this.
-const Values &GetValues();
+// Calls MakeValues with aos::network::GetTeamNumber()
+Values MakeValues();
} // namespace constants
} // namespace y2022
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index ccfb0ee..9bae7c7 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -62,6 +62,7 @@
":superstructure_status_fbs",
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2022:constants",
],
)
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index a7af922..bf04774 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -8,13 +8,21 @@
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
-using frc971::control_loops:: RelativeEncoderProfiledJointStatus;
+using frc971::control_loops::RelativeEncoderProfiledJointStatus;
Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- climber_(constants::GetValues().climber.subsystem_params) {
+
+ climber_(values->climber.subsystem_params),
+ intake_front_(values->intake_front.subsystem_params),
+ intake_back_(values->intake_back.subsystem_params),
+ turret_(values->turret.subsystem_params),
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -24,6 +32,29 @@
aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ intake_front_.Reset();
+ intake_back_.Reset();
+ turret_.Reset();
+ climber_.Reset();
+ }
+
+ drivetrain_status_fetcher_.Fetch();
+ const float velocity = robot_velocity();
+
+ double roller_speed_compensated_front = 0;
+ double roller_speed_compensated_back = 0;
+ double transfer_roller_speed = 0;
+
+ if (unsafe_goal != nullptr) {
+ roller_speed_compensated_front =
+ unsafe_goal->roller_speed_front() +
+ std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
+
+ roller_speed_compensated_back =
+ unsafe_goal->roller_speed_back() -
+ std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
+
+ transfer_roller_speed = unsafe_goal->transfer_roller_speed();
}
OutputT output_struct;
@@ -35,20 +66,59 @@
output != nullptr ? &(output_struct.climber_voltage) : nullptr,
status->fbb());
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_status_offset_front = intake_front_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake_front() : nullptr,
+ position->intake_front(),
+ output != nullptr ? &(output_struct.intake_voltage_front) : nullptr,
+ status->fbb());
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_status_offset_back = intake_back_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake_back() : nullptr,
+ position->intake_back(),
+ output != nullptr ? &(output_struct.intake_voltage_back) : 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());
+
if (output != nullptr) {
+ output_struct.roller_voltage_front = roller_speed_compensated_front;
+ output_struct.roller_voltage_back = roller_speed_compensated_back;
+ output_struct.transfer_roller_voltage = transfer_roller_speed;
+
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- // Climber is always zeroed; only has a pot
- status_builder.add_zeroed(climber_.zeroed());
- status_builder.add_estopped(climber_.estopped());
+ const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
+ turret_.zeroed() && climber_.zeroed();
+ const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
+ turret_.estopped() || climber_.zeroed();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_intake_front(intake_status_offset_front);
+ status_builder.add_intake_back(intake_status_offset_back);
+ status_builder.add_turret(turret_status_offset);
status_builder.add_climber(climber_status_offset);
(void)status->Send(status_builder.Finish());
}
+double Superstructure::robot_velocity() const {
+ return (drivetrain_status_fetcher_.get() != nullptr
+ ? drivetrain_status_fetcher_->robot_speed()
+ : 0.0);
+}
+
} // namespace superstructure
} // namespace control_loops
} // namespace y2022
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 4c03a08..d97befc 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
@@ -21,9 +22,28 @@
::frc971::zeroing::RelativeEncoderZeroingEstimator,
::frc971::control_loops::RelativeEncoderProfiledJointStatus>;
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
+ inline const PotAndAbsoluteEncoderSubsystem &intake_front() const {
+ return intake_front_;
+ }
+ inline const PotAndAbsoluteEncoderSubsystem &intake_back() const {
+ return intake_back_;
+ }
+ inline const PotAndAbsoluteEncoderSubsystem &turret() const {
+ return turret_;
+ }
+ inline const RelativeEncoderSubsystem &climber() const { return climber_; }
+
+ double robot_velocity() const;
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -31,6 +51,12 @@
private:
RelativeEncoderSubsystem climber_;
+ PotAndAbsoluteEncoderSubsystem intake_front_;
+ PotAndAbsoluteEncoderSubsystem intake_back_;
+ PotAndAbsoluteEncoderSubsystem turret_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8107363..37e63b5 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -5,6 +5,22 @@
table Goal {
// Height of the climber above rest point
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
+
+ // Goal angles of intake joints.
+ // Positive is out, 0 is up.
+ intake_front:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 1);
+ intake_back:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
+
+ // Positive is rollers intaking.
+ roller_speed_front:double (id: 3);
+ roller_speed_back:double (id: 4);
+ // One transfer motor for both sides
+ transfer_roller_speed:double (id: 5);
+
+ // Factor to multiply robot velocity by and add to roller voltage.
+ roller_speed_compensation:double (id: 6);
+
+ turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 7);
}
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 82c25b3..493fe95 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -16,6 +16,237 @@
namespace control_loops {
namespace superstructure {
namespace testing {
+namespace chrono = std::chrono;
+namespace {
+constexpr double kNoiseScalar = 0.01;
+}
+
+using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
+using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::RelativeEncoderSubsystem RelativeEncoderSubsystem;
+using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+
+template <typename SubsystemStatus, typename SubsystemState,
+ typename SubsystemConstants>
+class SubsystemSimulator {
+ public:
+ SubsystemSimulator(CappedTestPlant *plant, PositionSensorSimulator encoder,
+ const SubsystemConstants subsystem_constants,
+ const frc971::constants::Range range,
+ double encoder_offset, const chrono::nanoseconds dt)
+ : plant_(plant),
+ encoder_(encoder),
+ subsystem_constants_(subsystem_constants),
+ range_(range),
+ encoder_offset_(encoder_offset),
+ dt_(dt) {}
+
+ void InitializePosition(double start_pos) {
+ plant_->mutable_X(0, 0) = start_pos;
+ plant_->mutable_X(1, 0) = 0.0;
+
+ encoder_.Initialize(start_pos, kNoiseScalar, 0.0, encoder_offset_);
+ }
+
+ // Simulates the superstructure for a single timestep.
+ void Simulate(double voltage, const SubsystemStatus *status) {
+ double last_velocity = plant_->X(1, 0);
+
+ const double voltage_check =
+ (static_cast<SubsystemState>(status->state()) ==
+ SubsystemState::RUNNING)
+ ? subsystem_constants_.subsystem_params.operating_voltage
+ : subsystem_constants_.subsystem_params.zeroing_voltage;
+
+ EXPECT_NEAR(voltage, 0.0, voltage_check);
+
+ ::Eigen::Matrix<double, 1, 1> U;
+ U << voltage + plant_->voltage_offset();
+ plant_->Update(U);
+
+ const double position = plant_->Y(0, 0);
+
+ encoder_.MoveTo(position);
+
+ EXPECT_GE(position, range_.lower_hard);
+ EXPECT_LE(position, range_.upper_hard);
+
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+
+ const double velocity = plant_->X(1, 0);
+ const double acceleration = (velocity - last_velocity) / loop_time;
+
+ EXPECT_GE(peak_acceleration_, acceleration);
+ EXPECT_LE(-peak_acceleration_, acceleration);
+ EXPECT_GE(peak_velocity_, velocity);
+ EXPECT_LE(-peak_velocity_, velocity);
+ }
+
+ void set_peak_acceleration(double value) { peak_acceleration_ = value; }
+ void set_peak_velocity(double value) { peak_velocity_ = value; }
+
+ PositionSensorSimulator *encoder() { return &encoder_; }
+
+ private:
+ std::unique_ptr<CappedTestPlant> plant_;
+ PositionSensorSimulator encoder_;
+ const SubsystemConstants subsystem_constants_;
+ const frc971::constants::Range range_;
+
+ double encoder_offset_ = 0.0;
+
+ double peak_velocity_ = std::numeric_limits<double>::infinity();
+ double peak_acceleration_ = std::numeric_limits<double>::infinity();
+
+ const chrono::nanoseconds dt_;
+};
+
+using PotAndAbsoluteEncoderSimulator = SubsystemSimulator<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ PotAndAbsoluteEncoderSubsystem::State,
+ constants::Values::PotAndAbsEncoderConstants>;
+using RelativeEncoderSimulator = SubsystemSimulator<
+ frc971::control_loops::RelativeEncoderProfiledJointStatus,
+ RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")),
+ intake_front_(new CappedTestPlant(intake::MakeIntakePlant()),
+ PositionSensorSimulator(
+ values->intake_front.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->intake_front, constants::Values::kIntakeRange(),
+ values->intake_front.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
+ intake_back_(new CappedTestPlant(intake::MakeIntakePlant()),
+ PositionSensorSimulator(
+ values->intake_back.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->intake_back, constants::Values::kIntakeRange(),
+ values->intake_back.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
+ turret_(new CappedTestPlant(turret::MakeTurretPlant()),
+ PositionSensorSimulator(
+ values->turret.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->turret, constants::Values::kTurretRange(),
+ values->turret.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
+ climber_(new CappedTestPlant(climber::MakeClimberPlant()),
+ PositionSensorSimulator(
+ constants::Values::kClimberPotMetersPerRevolution()),
+ values->climber, constants::Values::kClimberRange(),
+ values->climber.potentiometer_offset, dt_) {
+ intake_front_.InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ intake_back_.InitializePosition(constants::Values::kIntakeRange().middle());
+ turret_.InitializePosition(constants::Values::kTurretRange().middle());
+ climber_.InitializePosition(constants::Values::kClimberRange().middle());
+
+ phased_loop_handle_ = event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ intake_front_.Simulate(
+ superstructure_output_fetcher_->intake_voltage_front(),
+ superstructure_status_fetcher_->intake_front());
+ intake_back_.Simulate(
+ superstructure_output_fetcher_->intake_voltage_back(),
+ superstructure_status_fetcher_->intake_back());
+ turret_.Simulate(superstructure_output_fetcher_->turret_voltage(),
+ superstructure_status_fetcher_->turret());
+ climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
+ superstructure_status_fetcher_->climber());
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
+ }
+
+ // Sends a queue message with the position of the superstructure.
+ void SendPositionMessage() {
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
+
+ frc971::PotAndAbsolutePosition::Builder turret_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+ turret_.encoder()->GetSensorValues(&turret_builder);
+
+ frc971::PotAndAbsolutePosition::Builder intake_front_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_front_offset =
+ intake_front_.encoder()->GetSensorValues(&intake_front_builder);
+
+ frc971::PotAndAbsolutePosition::Builder intake_back_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_back_offset =
+ intake_back_.encoder()->GetSensorValues(&intake_back_builder);
+
+ frc971::RelativePosition::Builder climber_builder =
+ builder.MakeBuilder<frc971::RelativePosition>();
+ flatbuffers::Offset<frc971::RelativePosition> climber_offset =
+ climber_.encoder()->GetSensorValues(&climber_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake_front(intake_front_offset);
+ position_builder.add_intake_back(intake_back_offset);
+ position_builder.add_turret(turret_offset);
+ position_builder.add_climber(climber_offset);
+
+ CHECK_EQ(builder.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
+
+ PotAndAbsoluteEncoderSimulator *intake_front() { return &intake_front_; }
+ PotAndAbsoluteEncoderSimulator *intake_back() { return &intake_back_; }
+ PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
+ RelativeEncoderSimulator *climber() { return &climber_; }
+
+ private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+
+ bool first_ = true;
+
+ PotAndAbsoluteEncoderSimulator intake_front_;
+ PotAndAbsoluteEncoderSimulator intake_back_;
+ PotAndAbsoluteEncoderSimulator turret_;
+ RelativeEncoderSimulator climber_;
+};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
public:
@@ -23,9 +254,12 @@
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2022/config.json"),
std::chrono::microseconds(5050)),
- superstructure_event_loop(MakeEventLoop("Superstructure")),
- superstructure_(superstructure_event_loop.get()),
- test_event_loop_(MakeEventLoop("test")),
+ values_(std::make_shared<constants::Values>(constants::MakeValues(
+ frc971::control_loops::testing::kTeamNumber))),
+ roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+ superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
+ superstructure_(superstructure_event_loop.get(), values_),
+ test_event_loop_(MakeEventLoop("test", roborio_)),
superstructure_goal_fetcher_(
test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
@@ -37,12 +271,15 @@
superstructure_position_fetcher_(
test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_position_sender_(
- test_event_loop_->MakeSender<Position>("/superstructure")) {
+ test_event_loop_->MakeSender<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+ superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), values_,
+ dt()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
- SetEnabled(true);
- phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
- [this](int) { SendPositionMessage(); }, dt());
+ SetEnabled(true);
if (!FLAGS_output_folder.empty()) {
unlink(FLAGS_output_folder.c_str());
@@ -52,14 +289,119 @@
}
}
- void SendPositionMessage() {
- auto builder = superstructure_position_sender_.MakeBuilder();
- Position::Builder position_builder = builder.MakeBuilder<Position>();
- builder.CheckOk(builder.Send(position_builder.Finish()));
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ // Only check the goal if there is one.
+
+ if (superstructure_goal_fetcher_->has_intake_front()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake_front()->unsafe_goal(),
+ superstructure_status_fetcher_->intake_front()->position(),
+ 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_intake_back()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake_back()->unsafe_goal(),
+ superstructure_status_fetcher_->intake_back()->position(),
+ 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_turret()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ }
+
+ if (superstructure_goal_fetcher_->has_climber()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
+ superstructure_status_fetcher_->climber()->position(), 0.001);
+ }
}
- // Because the third robot is single node, the roborio node is nullptr
- const aos::Node *const roborio_ = nullptr;
+ void CheckIfZeroed() {
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ // 2 Seconds
+ ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
+ } while (superstructure_status_fetcher_.get() == nullptr ||
+ !superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void SendRobotVelocity(double robot_velocity) {
+ // Send a robot velocity to test compensation
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+ auto drivetrain_status_builder = builder.MakeBuilder<DrivetrainStatus>();
+ drivetrain_status_builder.add_robot_speed(robot_velocity);
+ builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+ }
+
+ void TestRollerFront(double roller_speed_front,
+ double roller_speed_compensation) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_roller_speed_front(roller_speed_front);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
+ roller_speed_front + std::max((superstructure_.robot_velocity() *
+ roller_speed_compensation),
+ 0.0));
+ if (superstructure_.robot_velocity() <= 0) {
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
+ roller_speed_front);
+ }
+ }
+
+ void TestRollerBack(double roller_speed_back,
+ double roller_speed_compensation) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_roller_speed_back(roller_speed_back);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
+ roller_speed_back - std::min(superstructure_.robot_velocity() *
+ roller_speed_compensation,
+ 0.0));
+ if (superstructure_.robot_velocity() >= 0) {
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
+ roller_speed_back);
+ }
+ }
+
+ void TestTransferRoller(double transfer_roller_speed,
+ double roller_speed_compensation) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_transfer_roller_speed(transfer_roller_speed);
+ goal_builder.add_roller_speed_compensation(roller_speed_compensation);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ transfer_roller_speed);
+ }
+
+ std::shared_ptr<const constants::Values> values_;
+
+ const aos::Node *const roborio_;
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
::y2022::control_loops::superstructure::Superstructure superstructure_;
@@ -72,11 +414,306 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::Logger> logger_;
};
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
+TEST_F(SuperstructureTest, DoesNothing) {
+ SetEnabled(true);
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.turret()->InitializePosition(
+ constants::Values::kTurretRange().middle());
+ superstructure_plant_.climber()->InitializePosition(
+ constants::Values::kClimberRange().middle());
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().middle());
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.turret()->InitializePosition(
+ constants::Values::kTurretRange().middle());
+ superstructure_plant_.climber()->InitializePosition(
+ constants::Values::kClimberRange().middle());
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(15));
+
+ VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
+TEST_F(SuperstructureTest, SaturationTest) {
+ SetEnabled(true);
+ // Zero it before we move.
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().upper);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kClimberRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ goal_builder.add_turret(turret_offset);
+
+ goal_builder.add_climber(climber_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.intake_front()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_front()->set_peak_acceleration(0.2);
+ superstructure_plant_.intake_back()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_back()->set_peak_acceleration(0.2);
+
+ superstructure_plant_.turret()->set_peak_velocity(23.0);
+ superstructure_plant_.turret()->set_peak_acceleration(6.0);
+
+ superstructure_plant_.climber()->set_peak_velocity(23.0);
+ superstructure_plant_.climber()->set_peak_acceleration(0.2);
+
+ // Intake needs over 9 seconds to reach the goal
+ // TODO(Milo): Make this a sane time
+ RunFor(chrono::seconds(20));
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+ RunFor(chrono::seconds(2));
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake_front().state());
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.intake_back().state());
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.turret().state());
+ EXPECT_EQ(RelativeEncoderSubsystem::State::RUNNING,
+ superstructure_.climber().state());
+}
+
+// Tests that running disabled works
+TEST_F(SuperstructureTest, DisableTest) {
+ RunFor(chrono::seconds(2));
+ CheckIfZeroed();
+}
+
+// Makes sure the roller speeds are getting compensated correctly
+TEST_F(SuperstructureTest, RunRollers) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ SendRobotVelocity(3.0);
+ TestRollerFront(-12.0, 1.5);
+ TestRollerFront(12.0, 1.5);
+ TestRollerFront(0.0, 1.5);
+
+ SendRobotVelocity(-3.0);
+ TestRollerFront(-12.0, 1.5);
+ TestRollerFront(12.0, 1.5);
+ TestRollerFront(0.0, 1.5);
+
+ SendRobotVelocity(3.0);
+ TestRollerBack(-12.0, 1.5);
+ TestRollerBack(12.0, 1.5);
+ TestRollerBack(0.0, 1.5);
+
+ SendRobotVelocity(-3.0);
+ TestRollerBack(-12.0, 1.5);
+ TestRollerBack(12.0, 1.5);
+ TestRollerBack(0.0, 1.5);
+
+ TestTransferRoller(-12.0, 1.5);
+ TestTransferRoller(12.0, 1.5);
+ TestTransferRoller(0.0, 1.5);
+}
+
+// Make sure that the front and back intakes are never switched
+TEST_F(SuperstructureTest, RunIntakes) {
+ SetEnabled(true);
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+
+ WaitUntilZeroed();
+
+ superstructure_plant_.intake_front()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_front()->set_peak_acceleration(0.3);
+ superstructure_plant_.intake_back()->set_peak_velocity(23.0);
+ superstructure_plant_.intake_back()->set_peak_acceleration(0.3);
+
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_front(intake_offset_front);
+ goal_builder.add_intake_back(intake_offset_back);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ // TODO(Milo): Make this a sane time
+ RunFor(chrono::seconds(10));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_FLOAT_EQ(superstructure_status_fetcher_->intake_front()->position(),
+ constants::Values::kIntakeRange().lower);
+ EXPECT_FLOAT_EQ(superstructure_status_fetcher_->intake_back()->position(),
+ constants::Values::kIntakeRange().upper);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure_main.cc b/y2022/control_loops/superstructure/superstructure_main.cc
index f53e042..c6c12b7 100644
--- a/y2022/control_loops/superstructure/superstructure_main.cc
+++ b/y2022/control_loops/superstructure/superstructure_main.cc
@@ -1,7 +1,6 @@
-#include "y2022/control_loops/superstructure/superstructure.h"
-
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "y2022/control_loops/superstructure/superstructure.h"
int main(int argc, char **argv) {
::aos::InitGoogle(&argc, &argv);
@@ -10,8 +9,11 @@
aos::configuration::ReadConfig("config.json");
::aos::ShmEventLoop event_loop(&config.message());
+ std::shared_ptr<const y2022::constants::Values> values =
+ std::make_shared<const y2022::constants::Values>(
+ y2022::constants::MakeValues());
::y2022::control_loops::superstructure::Superstructure superstructure(
- &event_loop);
+ &event_loop, values);
event_loop.Run();
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index 7fa390f..aecc090 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -1,18 +1,28 @@
namespace y2022.control_loops.superstructure;
table Output {
- // Voltage of the climber falcon
- // - is down + is up
- climber_voltage:double (id: 0);
+ // Voltage of the climber falcon
+ // - is down + is up
+ climber_voltage:double (id: 0);
- // Voltage of the catapult falcon
- // Positive lifts the catapult to fire.
- catapult_voltage:double (id: 1);
+ // Voltage of the catapult falcon
+ // Positive lifts the catapult to fire.
+ catapult_voltage:double (id: 1);
- // Voltage of the turret falcon
- // Positive rotates the turret around the Z axis (up) according to the
- // right hand rule.
- turret_voltage:double (id: 2);
+ // Voltage of the turret falcon
+ // Positive rotates the turret around the Z axis (up) according to the
+ // right hand rule.
+ turret_voltage:double (id: 2);
+
+ // Intake joint voltages.
+ intake_voltage_front:double (id: 3);
+ intake_voltage_back:double (id: 4);
+
+ // Intake roller voltages
+ roller_voltage_front:double (id: 5);
+ roller_voltage_back:double (id: 6);
+ // One transfer motor for both sides
+ transfer_roller_voltage:double (id: 7);
}
root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index 1f1a4c5..6581758 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -1,7 +1,7 @@
// Provides a plot for debugging robot state-related issues.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
-import * as proxy from 'org_frc971/aos/network/www/proxy';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
import Connection = proxy.Connection;
@@ -9,11 +9,15 @@
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
-export function plotSuperstructure(conn: Connection, element: Element) : void {
+export function plotSuperstructure(conn: Connection, element: Element): void {
const aosPlotter = new AosPlotter(conn);
- const goal = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Goal');
- const output = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Output');
- const status = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Status');
- const position = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Position');
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2022.control_loops.superstructure.Position');
const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
}
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index a36f4f6..38babaf 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -4,6 +4,11 @@
table Position {
climber:frc971.RelativePosition (id: 0);
+ // Zero for the intake position value is up, and positive is
+ // down.
+ intake_front:frc971.PotAndAbsolutePosition (id: 1);
+ intake_back:frc971.PotAndAbsolutePosition (id: 2);
+ turret:frc971.PotAndAbsolutePosition (id: 3);
}
root_type Position;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 0ee0994..b4bad2a 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -12,6 +12,12 @@
// Subsystem statuses
climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);
+
+ // Estimated angles and angular velocities of the intakes.
+ intake_front:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 3);
+ intake_back:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
+
+ turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
}
root_type Status;
\ No newline at end of file
diff --git a/y2022/control_loops/superstructure/turret/BUILD b/y2022/control_loops/superstructure/turret/BUILD
new file mode 100644
index 0000000..c2948d7
--- /dev/null
+++ b/y2022/control_loops/superstructure/turret/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+ name = "genrule_turret",
+ outs = [
+ "turret_plant.h",
+ "turret_plant.cc",
+ "integral_turret_plant.h",
+ "integral_turret_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:turret) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:turret",
+ ],
+)
+
+cc_library(
+ name = "turret_plants",
+ srcs = [
+ "integral_turret_plant.cc",
+ "turret_plant.cc",
+ ],
+ hdrs = [
+ "integral_turret_plant.h",
+ "turret_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 6818ae5..31f2013 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -81,12 +81,23 @@
Values::kClimberPotMetersPerRevolution();
}
+double intake_pot_translate(double voltage) {
+ return voltage * Values::kIntakePotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+}
+
+double turret_pot_translate(double voltage) {
+ return voltage * Values::kTurretPotRatio() *
+ (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
constexpr double kMaxFastEncoderPulsesPerSecond =
- Values::kMaxDrivetrainEncoderPulsesPerSecond();
+ std::max({Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxIntakeEncoderPulsesPerSecond()});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
constexpr double kMaxMediumEncoderPulsesPerSecond =
- kMaxFastEncoderPulsesPerSecond;
+ Values::kMaxTurretEncoderPulsesPerSecond();
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");
@@ -96,8 +107,10 @@
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader(::aos::ShmEventLoop *event_loop)
+ SensorReader(::aos::ShmEventLoop *event_loop,
+ std::shared_ptr<const Values> values)
: ::frc971::wpilib::SensorReader(event_loop),
+ values_(std::move(values)),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
"/autonomous")),
@@ -120,7 +133,47 @@
}
void RunIteration() override {
- const constants::Values &values = constants::GetValues();
+ {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+
+ frc971::RelativePositionT climber;
+ CopyPosition(*climber_potentiometer_, &climber, climber_pot_translate,
+ false, values_->climber.potentiometer_offset);
+ flatbuffers::Offset<frc971::RelativePosition> climber_offset =
+ frc971::RelativePosition::Pack(*builder.fbb(), &climber);
+
+ // Intake
+ frc971::PotAndAbsolutePositionT intake_front;
+ frc971::PotAndAbsolutePositionT intake_back;
+ frc971::PotAndAbsolutePositionT turret;
+ CopyPosition(intake_encoder_front_, &intake_front,
+ Values::kIntakeEncoderCountsPerRevolution(),
+ Values::kIntakeEncoderRatio(), intake_pot_translate, false,
+ values_->intake_front.potentiometer_offset);
+ CopyPosition(intake_encoder_back_, &intake_back,
+ Values::kIntakeEncoderCountsPerRevolution(),
+ Values::kIntakeEncoderRatio(), intake_pot_translate, false,
+ values_->intake_back.potentiometer_offset);
+ CopyPosition(turret_encoder_, &turret,
+ Values::kTurretEncoderCountsPerRevolution(),
+ Values::kTurretEncoderRatio(), turret_pot_translate, false,
+ values_->turret.potentiometer_offset);
+
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset_front =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_front);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset_back =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake_back);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+ position_builder.add_climber(climber_offset);
+ position_builder.add_intake_front(intake_offset_front);
+ position_builder.add_intake_back(intake_offset_back);
+ position_builder.add_turret(turret_offset);
+ builder.CheckOk(builder.Send(position_builder.Finish()));
+ }
{
auto builder = drivetrain_position_sender_.MakeBuilder();
@@ -142,21 +195,6 @@
}
{
- auto builder = superstructure_position_sender_.MakeBuilder();
-
- frc971::RelativePositionT climber;
- CopyPosition(*climber_potentiometer_, &climber, climber_pot_translate,
- false, values.climber.potentiometer_offset);
- flatbuffers::Offset<frc971::RelativePosition> climber_offset =
- frc971::RelativePosition::Pack(*builder.fbb(), &climber);
-
- superstructure::Position::Builder position_builder =
- builder.MakeBuilder<superstructure::Position>();
- position_builder.add_climber(climber_offset);
- builder.CheckOk(builder.Send(position_builder.Finish()));
- }
-
- {
auto builder = auto_mode_sender_.MakeBuilder();
uint32_t mode = 0;
@@ -180,15 +218,64 @@
climber_potentiometer_ = ::std::move(potentiometer);
}
+ void set_intake_encoder_front(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ intake_encoder_front_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_encoder_back(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ intake_encoder_back_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_front_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ intake_encoder_front_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_intake_front_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ intake_encoder_front_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_intake_back_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ intake_encoder_back_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_intake_back_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ intake_encoder_back_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ void set_turret_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ turret_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_turret_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ turret_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_turret_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ turret_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
private:
+ std::shared_ptr<const Values> values_;
+
aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
aos::Sender<superstructure::Position> superstructure_position_sender_;
aos::Sender<frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
- std::unique_ptr<frc::AnalogInput> climber_potentiometer_;
-
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
+ std::unique_ptr<frc::AnalogInput> climber_potentiometer_;
+ frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
+ intake_encoder_back_, turret_encoder_;
};
class SuperstructureWriter
@@ -198,8 +285,7 @@
: frc971::wpilib::LoopOutputHandler<superstructure::Output>(
event_loop, "/superstructure") {}
- void set_climber_falcon(
- std::unique_ptr<frc::TalonFX> t) {
+ void set_climber_falcon(std::unique_ptr<frc::TalonFX> t) {
climber_falcon_ = std::move(t);
}
@@ -215,34 +301,89 @@
catapult_falcon_2_ = ::std::move(t);
}
- private:
- void Write(const superstructure::Output &output) override {
- climber_falcon_->SetSpeed(std::clamp(output.climber_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- catapult_falcon_1_->SetSpeed(std::clamp(output.catapult_voltage(),
- -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
- catapult_falcon_2_->SetSpeed(std::clamp(output.catapult_voltage(),
- -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
- turret_falcon_->SetSpeed(std::clamp(output.turret_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ void set_intake_falcon_front(::std::unique_ptr<frc::TalonFX> t) {
+ intake_falcon_front_ = ::std::move(t);
}
+ void set_intake_falcon_back(::std::unique_ptr<frc::TalonFX> t) {
+ intake_falcon_back_ = ::std::move(t);
+ }
+
+ void set_roller_falcon_front(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ roller_falcon_front_ = ::std::move(t);
+ roller_falcon_front_->ConfigSupplyCurrentLimit(
+ {true, Values::kIntakeRollerSupplyCurrentLimit(),
+ Values::kIntakeRollerSupplyCurrentLimit(), 0});
+ roller_falcon_front_->ConfigStatorCurrentLimit(
+ {true, Values::kIntakeRollerStatorCurrentLimit(),
+ Values::kIntakeRollerStatorCurrentLimit(), 0});
+ }
+
+ void set_roller_falcon_back(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ roller_falcon_back_ = ::std::move(t);
+ roller_falcon_back_->ConfigSupplyCurrentLimit(
+ {true, Values::kIntakeRollerSupplyCurrentLimit(),
+ Values::kIntakeRollerSupplyCurrentLimit(), 0});
+ roller_falcon_back_->ConfigStatorCurrentLimit(
+ {true, Values::kIntakeRollerStatorCurrentLimit(),
+ Values::kIntakeRollerStatorCurrentLimit(), 0});
+ }
+
+ void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ transfer_roller_victor_ = ::std::move(t);
+ }
+
+ private:
void Stop() override {
AOS_LOG(WARNING, "Superstructure output too old.\n");
climber_falcon_->SetDisabled();
+ roller_falcon_front_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ roller_falcon_back_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled,
+ 0);
+ intake_falcon_front_->SetDisabled();
+ intake_falcon_back_->SetDisabled();
+ transfer_roller_victor_->SetDisabled();
catapult_falcon_1_->SetDisabled();
catapult_falcon_2_->SetDisabled();
turret_falcon_->SetDisabled();
}
+ void Write(const superstructure::Output &output) override {
+ WritePwm(output.climber_voltage(), climber_falcon_.get());
+ WritePwm(output.intake_voltage_front(), intake_falcon_front_.get());
+ WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
+ WriteCan(output.roller_voltage_front(), roller_falcon_front_.get());
+ WriteCan(output.roller_voltage_back(), roller_falcon_back_.get());
+ WritePwm(output.transfer_roller_voltage(), transfer_roller_victor_.get());
+ WritePwm(output.catapult_voltage(), catapult_falcon_1_.get());
+ WritePwm(output.catapult_voltage(), catapult_falcon_2_.get());
+ WritePwm(output.turret_voltage(), turret_falcon_.get());
+ }
+
+ static void WriteCan(const double voltage,
+ ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+ falcon->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ }
+
+ template <typename T>
+ static void WritePwm(const double voltage, T *motor) {
+ motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
+
+ ::std::unique_ptr<frc::TalonFX> intake_falcon_front_, intake_falcon_back_;
+
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ roller_falcon_front_, roller_falcon_back_;
+
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
catapult_falcon_2_, climber_falcon_;
+ ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -253,6 +394,9 @@
}
void Run() override {
+ std::shared_ptr<const Values> values =
+ std::make_shared<const Values>(constants::MakeValues());
+
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("config.json");
@@ -269,10 +413,27 @@
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
- SensorReader sensor_reader(&sensor_reader_event_loop);
+ SensorReader sensor_reader(&sensor_reader_event_loop, values);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
- sensor_reader.set_climber_potentiometer(make_unique<frc::AnalogInput>(0));
+
+ sensor_reader.set_intake_encoder_front(make_encoder(2));
+ sensor_reader.set_intake_front_absolute_pwm(
+ make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_intake_front_potentiometer(
+ make_unique<frc::AnalogInput>(2));
+
+ sensor_reader.set_intake_encoder_back(make_encoder(3));
+ sensor_reader.set_intake_back_absolute_pwm(
+ make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_intake_back_potentiometer(
+ make_unique<frc::AnalogInput>(3));
+
+ sensor_reader.set_turret_encoder(make_encoder(4));
+ sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(4));
+ sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(4));
+
+ sensor_reader.set_climber_potentiometer(make_unique<frc::AnalogInput>(5));
AddLoop(&sensor_reader_event_loop);
@@ -286,10 +447,18 @@
SuperstructureWriter superstructure_writer(&output_event_loop);
- superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(5));
superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(2));
superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(3));
superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(4));
+ superstructure_writer.set_roller_falcon_front(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
+ superstructure_writer.set_roller_falcon_back(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
+ superstructure_writer.set_transfer_roller_victor(
+ make_unique<::frc::VictorSP>(4));
+ superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(5));
+ superstructure_writer.set_intake_falcon_back(make_unique<frc::TalonFX>(6));
+ superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(7));
AddLoop(&output_event_loop);