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/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