Add constructs for superstructure
Signed-off-by: Thiago Monteiro <oculusdev124@gmail.com>
Change-Id: Ic3c3be9cd6705ba9dd7c0f4186e491e6e9b3be85
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
diff --git a/y2022_bot3/control_loops/superstructure/superstructure.cc b/y2022_bot3/control_loops/superstructure/superstructure.cc
index 5d12880..f701907 100644
--- a/y2022_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2022_bot3/control_loops/superstructure/superstructure.cc
@@ -18,6 +18,9 @@
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
values_(values),
+ climber_left_(values_->climber_left.subsystem_params),
+ climber_right_(values_->climber_right.subsystem_params),
+ intake_(values_->intake.subsystem_params),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
@@ -30,8 +33,6 @@
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
- (void)unsafe_goal;
- (void)position;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
}
@@ -45,12 +46,45 @@
drivetrain_status_fetcher_.Fetch();
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_status_offset = intake_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake(),
+ output != nullptr ? &output_struct.intake_voltage : nullptr,
+ status->fbb());
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ climber_left_status_offset = climber_left_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->climber_left() : nullptr,
+ position->climber_left(),
+ output != nullptr ? &output_struct.climber_voltage_left : nullptr,
+ status->fbb());
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ climber_right_status_offset = climber_right_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->climber_right() : nullptr,
+ position->climber_right(),
+ output != nullptr ? &output_struct.climber_voltage_right : nullptr,
+ status->fbb());
+
+ if (output != nullptr) {
+ output_struct.roller_voltage =
+ (unsafe_goal != nullptr ? unsafe_goal->roller_speed() : 0.0);
+ }
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(true);
- status_builder.add_estopped(false);
+ const bool zeroed =
+ intake_.zeroed() && climber_left_.zeroed() && climber_right_.zeroed();
+ const bool estopped = intake_.estopped() || climber_left_.estopped() ||
+ climber_right_.estopped();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+ status_builder.add_intake(intake_status_offset);
+ status_builder.add_climber_left(climber_left_status_offset);
+ status_builder.add_climber_right(climber_right_status_offset);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2022_bot3/control_loops/superstructure/superstructure.h b/y2022_bot3/control_loops/superstructure/superstructure.h
index 91f80b4..4f33c3c 100644
--- a/y2022_bot3/control_loops/superstructure/superstructure.h
+++ b/y2022_bot3/control_loops/superstructure/superstructure.h
@@ -31,6 +31,16 @@
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
+ inline const PotAndAbsoluteEncoderSubsystem &intake() const {
+ return intake_;
+ }
+ inline const PotAndAbsoluteEncoderSubsystem &climber_left() const {
+ return climber_left_;
+ }
+ inline const PotAndAbsoluteEncoderSubsystem &climber_right() const {
+ return climber_right_;
+ }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -39,6 +49,10 @@
private:
std::shared_ptr<const constants::Values> values_;
+ PotAndAbsoluteEncoderSubsystem climber_left_;
+ PotAndAbsoluteEncoderSubsystem climber_right_;
+ PotAndAbsoluteEncoderSubsystem intake_;
+
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;