Add wrist logic and tests in superstructure
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ib71a65aacfc37858e8111a310dd3a97d308fb61c
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index d8f3d59..827cc40 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -29,12 +29,21 @@
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
double robot_velocity() const;
+ inline const arm::Arm &arm() const { return arm_; }
+ inline const EndEffector &end_effector() const { return end_effector_; }
+ inline const AbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -49,6 +58,7 @@
arm::Arm arm_;
EndEffector end_effector_;
+ AbsoluteEncoderSubsystem wrist_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;