blob: a5b4df949310f7ad28b99ea6c74e1a5abc4fd5cc [file] [log] [blame]
milind-u086d7262022-01-19 20:44:18 -08001#include "y2022/control_loops/superstructure/superstructure.h"
2
3#include "aos/events/event_loop.h"
Milind Upadhyay225156b2022-02-25 22:42:12 -08004#include "y2022/control_loops/superstructure/collision_avoidance.h"
milind-u086d7262022-01-19 20:44:18 -08005
6namespace y2022 {
7namespace control_loops {
8namespace superstructure {
9
10using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
11using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
Henry Speiser55aa3ba2022-02-21 23:21:12 -080012using frc971::control_loops::RelativeEncoderProfiledJointStatus;
milind-u086d7262022-01-19 20:44:18 -080013
14Superstructure::Superstructure(::aos::EventLoop *event_loop,
Henry Speiser55aa3ba2022-02-21 23:21:12 -080015 std::shared_ptr<const constants::Values> values,
milind-u086d7262022-01-19 20:44:18 -080016 const ::std::string &name)
17 : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
Siddhant Kanwar0e37f592022-02-21 19:26:50 -080018 name),
Austin Schuh39f26f62022-02-24 21:34:46 -080019 values_(values),
20 climber_(values_->climber.subsystem_params),
21 intake_front_(values_->intake_front.subsystem_params),
22 intake_back_(values_->intake_back.subsystem_params),
23 turret_(values_->turret.subsystem_params),
Ravago Jones5da06352022-03-04 20:26:24 -080024 catapult_(*values_),
Henry Speiser55aa3ba2022-02-21 23:21:12 -080025 drivetrain_status_fetcher_(
26 event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
Austin Schuh39f26f62022-02-24 21:34:46 -080027 "/drivetrain")),
Ravago Jones5da06352022-03-04 20:26:24 -080028 can_position_fetcher_(
29 event_loop->MakeFetcher<CANPosition>("/superstructure")) {
milind-u086d7262022-01-19 20:44:18 -080030 event_loop->SetRuntimeRealtimePriority(30);
31}
32
33void Superstructure::RunIteration(const Goal *unsafe_goal,
Siddhant Kanwar0e37f592022-02-21 19:26:50 -080034 const Position *position,
milind-u086d7262022-01-19 20:44:18 -080035 aos::Sender<Output>::Builder *output,
36 aos::Sender<Status>::Builder *status) {
37 if (WasReset()) {
38 AOS_LOG(ERROR, "WPILib reset, restarting\n");
Henry Speiser55aa3ba2022-02-21 23:21:12 -080039 intake_front_.Reset();
40 intake_back_.Reset();
41 turret_.Reset();
42 climber_.Reset();
Austin Schuh39f26f62022-02-24 21:34:46 -080043 catapult_.Reset();
Henry Speiser55aa3ba2022-02-21 23:21:12 -080044 }
45
Ravago Jones5da06352022-03-04 20:26:24 -080046 OutputT output_struct;
Milind Upadhyay225156b2022-02-25 22:42:12 -080047
Ravago Jones5da06352022-03-04 20:26:24 -080048 aos::FlatbufferFixedAllocatorArray<
49 frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
50 turret_goal_buffer;
51
52 const aos::monotonic_clock::time_point timestamp =
53 event_loop()->context().monotonic_event_time;
Milind Upadhyay225156b2022-02-25 22:42:12 -080054
Henry Speiser55aa3ba2022-02-21 23:21:12 -080055 drivetrain_status_fetcher_.Fetch();
56 const float velocity = robot_velocity();
57
James Kuszmaul84083f42022-02-27 17:24:38 -080058 const turret::Aimer::Goal *auto_aim_goal = nullptr;
59 if (drivetrain_status_fetcher_.get() != nullptr) {
60 aimer_.Update(drivetrain_status_fetcher_.get(),
61 turret::Aimer::ShotMode::kShootOnTheFly);
62 auto_aim_goal = aimer_.TurretGoal();
63 }
64
Ravago Jones5da06352022-03-04 20:26:24 -080065 const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
66 *turret_goal = nullptr;
67 double roller_speed_compensated_front = 0.0;
68 double roller_speed_compensated_back = 0.0;
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -080069 double transfer_roller_speed_front = 0.0;
70 double transfer_roller_speed_back = 0.0;
Ravago Jones5da06352022-03-04 20:26:24 -080071 double flipper_arms_voltage = 0.0;
Henry Speiser55aa3ba2022-02-21 23:21:12 -080072
73 if (unsafe_goal != nullptr) {
74 roller_speed_compensated_front =
75 unsafe_goal->roller_speed_front() +
76 std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
77
78 roller_speed_compensated_back =
79 unsafe_goal->roller_speed_back() -
80 std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
81
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -080082 transfer_roller_speed_front = unsafe_goal->transfer_roller_speed_front();
83 transfer_roller_speed_back = unsafe_goal->transfer_roller_speed_back();
milind-u086d7262022-01-19 20:44:18 -080084
James Kuszmaul84083f42022-02-27 17:24:38 -080085 turret_goal =
86 unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
87 }
Austin Schuh39f26f62022-02-24 21:34:46 -080088
Ravago Jones5da06352022-03-04 20:26:24 -080089 // Supersturcture state machine:
90 // 1. IDLE: Wait until an intake beambreak is triggerred, meaning that a ball
91 // is being intaked. This means that the transfer rollers have a ball. If
92 // we've been waiting here for too long without any beambreak triggered, the
93 // ball got lost, so reset.
94 // 2. TRANSFERRING: Until the turret reaches the loading position where the
95 // ball can be transferred into the catapult, wiggle the ball in place.
96 // Once the turret reaches the loading position, send the ball forward with
97 // the transfer rollers until the turret beambreak is triggered.
98 // If we have been in this state for too long, the ball probably got lost so
99 // reset back to IDLE.
100 // 3. LOADING: To load the ball into the catapult, put the flippers at the
101 // feeding speed. Wait for a timeout, and then wait until the ball has gone
102 // past the turret beambreak and the flippers have stopped moving, meaning
103 // that the ball is fully loaded in the catapult.
104 // 4. LOADED: Wait until the user asks us to fire to transition to the
105 // shooting stage. If asked to cancel the shot, reset back to the IDLE state.
106 // 5. SHOOTING: Open the flippers to get ready for the shot. If they don't
107 // open quickly enough, try reseating the ball and going back to the LOADING
108 // stage, which moves the flippers in the opposite direction first.
109 // Now, hold the flippers open and wait until the turret has reached its
110 // aiming goal. Once the turret is ready, tell the catapult to fire.
111 // If the flippers move back for some reason now, it could damage the
112 // catapult, so estop it. Otherwise, wait until the catapult shoots a ball and
113 // goes back to its return position. We have now finished the shot, so return
114 // to IDLE.
115
116 const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800117 transfer_roller_speed_front < 0) ||
Ravago Jones5da06352022-03-04 20:26:24 -0800118 (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800119 transfer_roller_speed_back < 0));
Ravago Jones5da06352022-03-04 20:26:24 -0800120
121 // Intake handling should happen regardless of the turret state
122 if (position->intake_beambreak_front() || position->intake_beambreak_back()) {
123 if (intake_state_ == IntakeState::NO_BALL) {
124 if (position->intake_beambreak_front()) {
125 intake_state_ = IntakeState::INTAKE_FRONT_BALL;
126 } else if (position->intake_beambreak_back()) {
127 intake_state_ = IntakeState::INTAKE_BACK_BALL;
128 }
129 }
130
131 intake_beambreak_timer_ = timestamp;
132 }
133
Milind Upadhyay5dfabef2022-03-06 14:08:15 -0800134 if (timestamp >
135 intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
136 intake_state_ = IntakeState::NO_BALL;
137 }
138
Ravago Jones5da06352022-03-04 20:26:24 -0800139 if (intake_state_ != IntakeState::NO_BALL) {
Ravago Jones5da06352022-03-04 20:26:24 -0800140 roller_speed_compensated_front = 0.0;
141 roller_speed_compensated_back = 0.0;
142
143 const double wiggle_voltage =
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800144 constants::Values::kTransferRollerWiggleVoltage();
Ravago Jones5da06352022-03-04 20:26:24 -0800145 // Wiggle transfer rollers: send the ball back and forth while waiting
146 // for the turret or waiting for another shot to be completed
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800147 if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
148 if (position->intake_beambreak_front()) {
149 transfer_roller_speed_front = -wiggle_voltage;
150 transfer_roller_speed_back = wiggle_voltage;
151 } else {
152 transfer_roller_speed_front = wiggle_voltage;
153 transfer_roller_speed_back = -wiggle_voltage;
154 }
Ravago Jones5da06352022-03-04 20:26:24 -0800155 } else {
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800156 if (position->intake_beambreak_back()) {
157 transfer_roller_speed_back = -wiggle_voltage;
158 transfer_roller_speed_front = wiggle_voltage;
159 } else {
160 transfer_roller_speed_back = wiggle_voltage;
161 transfer_roller_speed_front = -wiggle_voltage;
162 }
Ravago Jones5da06352022-03-04 20:26:24 -0800163 }
164 }
165
166 switch (state_) {
167 case SuperstructureState::IDLE: {
Ravago Jones5da06352022-03-04 20:26:24 -0800168 if (is_spitting) {
169 intake_state_ = IntakeState::NO_BALL;
170 }
171
172 if (intake_state_ == IntakeState::NO_BALL ||
173 !(position->intake_beambreak_front() ||
174 position->intake_beambreak_back())) {
175 break;
176 }
177
178 state_ = SuperstructureState::TRANSFERRING;
Ravago Jones5da06352022-03-04 20:26:24 -0800179 // Save the side the ball is on for later
180
181 break;
182 }
183 case SuperstructureState::TRANSFERRING: {
184 // If we've been transferring for too long, the ball probably got lost
Ravago Jones5da06352022-03-04 20:26:24 -0800185 if (intake_state_ == IntakeState::NO_BALL) {
186 state_ = SuperstructureState::IDLE;
187 break;
188 }
189
190 double turret_loading_position =
191 (intake_state_ == IntakeState::INTAKE_FRONT_BALL
192 ? constants::Values::kTurretFrontIntakePos()
193 : constants::Values::kTurretBackIntakePos());
194
195 turret_goal_buffer.Finish(
196 frc971::control_loops::
197 CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
198 *turret_goal_buffer.fbb(), turret_loading_position));
199 turret_goal = &turret_goal_buffer.message();
200
201 const bool turret_near_goal =
202 std::abs(turret_.estimated_position() - turret_loading_position) <
203 kTurretGoalThreshold;
204 if (!turret_near_goal) {
205 break; // Wait for turret to reach the chosen intake
206 }
207
208 // Transfer rollers and flipper arm belt on
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800209 if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
210 transfer_roller_speed_front =
211 constants::Values::kTransferRollerVoltage();
212 transfer_roller_speed_back =
213 -constants::Values::kTransferRollerVoltage();
214 } else {
215 transfer_roller_speed_back =
216 constants::Values::kTransferRollerVoltage();
217 transfer_roller_speed_front =
218 -constants::Values::kTransferRollerVoltage();
219 }
Ravago Jones5da06352022-03-04 20:26:24 -0800220 flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
221
222 // Ball is in catapult
223 if (position->turret_beambreak()) {
224 intake_state_ = IntakeState::NO_BALL;
225 state_ = SuperstructureState::LOADING;
226 loading_timer_ = timestamp;
227 }
228 break;
229 }
230 case SuperstructureState::LOADING: {
231 flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
232
233 // Keep feeding for kExtraLoadingTime
234
Ravago Jones5da06352022-03-04 20:26:24 -0800235 // The ball should go past the turret beambreak to be loaded.
Milind Upadhyay5dfabef2022-03-06 14:08:15 -0800236 // If we got a CAN reading not too long ago, the flippers should have
237 // also stopped.
Milind Upadhyay47e0d032022-03-05 23:06:25 -0800238 if (position->turret_beambreak()) {
239 loading_timer_ = timestamp;
240 } else if (timestamp >
241 loading_timer_ + constants::Values::kExtraLoadingTime()) {
Ravago Jones5da06352022-03-04 20:26:24 -0800242 state_ = SuperstructureState::LOADED;
243 reseating_in_catapult_ = false;
244 }
245 break;
246 }
247 case SuperstructureState::LOADED: {
248 if (unsafe_goal != nullptr) {
249 if (unsafe_goal->cancel_shot()) {
250 // Cancel the shot process
251 state_ = SuperstructureState::IDLE;
252 } else if (unsafe_goal->fire()) {
253 // Start if we were asked to and the turret is at goal
254 state_ = SuperstructureState::SHOOTING;
255 prev_shot_count_ = catapult_.shot_count();
256
257 // Reset opening timeout
258 flipper_opening_start_time_ = timestamp;
259 }
260 }
261 break;
262 }
263 case SuperstructureState::SHOOTING: {
264 // Opening flipper arms could fail, wait until they are open using their
265 // potentiometers (the member below is just named encoder).
266 // Be a little more lenient if the flippers were already open in case of
267 // noise or collisions.
268 const double flipper_open_position =
269 (flippers_open_ ? constants::Values::kReseatFlipperPosition()
270 : constants::Values::kFlipperOpenPosition());
milind-u37dc40b2022-03-08 19:51:27 -0800271
272 // TODO(milind): add left arm back once it's fixed
Ravago Jones5da06352022-03-04 20:26:24 -0800273 flippers_open_ =
Ravago Jones5da06352022-03-04 20:26:24 -0800274 position->flipper_arm_right()->encoder() >= flipper_open_position;
275
276 if (flippers_open_) {
277 // Hold at kFlipperHoldVoltage
278 flipper_arms_voltage = constants::Values::kFlipperHoldVoltage();
279 } else {
280 // Open at kFlipperOpenVoltage
281 flipper_arms_voltage = constants::Values::kFlipperOpenVoltage();
282 }
283
284 if (!flippers_open_ &&
285 timestamp >
286 loading_timer_ + constants::Values::kFlipperOpeningTimeout()) {
287 // Reseat the ball and try again
288 state_ = SuperstructureState::LOADING;
289 loading_timer_ = timestamp;
290 reseating_in_catapult_ = true;
291 break;
292 }
293
294 const bool turret_near_goal =
295 turret_goal != nullptr &&
296 std::abs(turret_goal->unsafe_goal() - turret_.position()) <
297 kTurretGoalThreshold;
Henry Speiser28288cc2022-03-09 22:59:24 -0800298 const bool collided = collision_avoidance_.IsCollided(
299 {.intake_front_position = intake_front_.estimated_position(),
300 .intake_back_position = intake_back_.estimated_position(),
301 .turret_position = turret_.estimated_position(),
302 .shooting = true});
Ravago Jones5da06352022-03-04 20:26:24 -0800303
Henry Speiser28288cc2022-03-09 22:59:24 -0800304 // If the turret reached the aiming goal and the catapult is safe to move
305 // up, fire!
306 if (flippers_open_ && turret_near_goal && !collided) {
Ravago Jones5da06352022-03-04 20:26:24 -0800307 fire_ = true;
308 }
309
310 // If we started firing and the flippers closed a bit, estop to prevent
311 // damage
312 if (fire_ && !flippers_open_) {
313 catapult_.Estop();
314 }
315
316 const bool near_return_position =
317 (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
318 unsafe_goal->catapult()->has_return_position() &&
319 std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
320 catapult_.estimated_position()) < kCatapultGoalThreshold);
321
322 // Once the shot is complete and the catapult is back to its return
323 // position, go back to IDLE
324 if (catapult_.shot_count() > prev_shot_count_ && near_return_position) {
325 prev_shot_count_ = catapult_.shot_count();
326 fire_ = false;
327 state_ = SuperstructureState::IDLE;
328 }
329
330 break;
331 }
332 }
333
334 collision_avoidance_.UpdateGoal(
335 {.intake_front_position = intake_front_.estimated_position(),
336 .intake_back_position = intake_back_.estimated_position(),
Henry Speiser28288cc2022-03-09 22:59:24 -0800337 .turret_position = turret_.estimated_position(),
338 .shooting = state_ == SuperstructureState::SHOOTING},
Ravago Jones5da06352022-03-04 20:26:24 -0800339 turret_goal);
340
341 turret_.set_min_position(collision_avoidance_.min_turret_goal());
342 turret_.set_max_position(collision_avoidance_.max_turret_goal());
343 intake_front_.set_min_position(collision_avoidance_.min_intake_front_goal());
344 intake_front_.set_max_position(collision_avoidance_.max_intake_front_goal());
345 intake_back_.set_min_position(collision_avoidance_.min_intake_back_goal());
346 intake_back_.set_max_position(collision_avoidance_.max_intake_back_goal());
347
James Kuszmaul84083f42022-02-27 17:24:38 -0800348 const flatbuffers::Offset<AimerStatus> aimer_offset =
349 aimer_.PopulateStatus(status->fbb());
350
Milind Upadhyay5dfabef2022-03-06 14:08:15 -0800351 // Disable the catapult if we want to restart to prevent damage with
352 // flippers
Austin Schuh39f26f62022-02-24 21:34:46 -0800353 const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
Ravago Jones5da06352022-03-04 20:26:24 -0800354 catapult_status_offset =
355 catapult_.Iterate(unsafe_goal, position,
356 output != nullptr && !catapult_.estopped()
357 ? &(output_struct.catapult_voltage)
358 : nullptr,
359 fire_, status->fbb());
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800360
Ravago Jones5da06352022-03-04 20:26:24 -0800361 const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800362 climber_status_offset = climber_.Iterate(
363 unsafe_goal != nullptr ? unsafe_goal->climber() : nullptr,
Austin Schuh7f120362022-03-05 17:10:11 -0800364 position->climber(),
365 output != nullptr ? &output_struct.climber_voltage : nullptr,
366 status->fbb());
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800367
Ravago Jones5da06352022-03-04 20:26:24 -0800368 const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800369 intake_status_offset_front = intake_front_.Iterate(
370 unsafe_goal != nullptr ? unsafe_goal->intake_front() : nullptr,
Austin Schuh7f120362022-03-05 17:10:11 -0800371 position->intake_front(),
372 output != nullptr ? &output_struct.intake_voltage_front : nullptr,
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800373 status->fbb());
374
Ravago Jones5da06352022-03-04 20:26:24 -0800375 const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800376 intake_status_offset_back = intake_back_.Iterate(
377 unsafe_goal != nullptr ? unsafe_goal->intake_back() : nullptr,
Austin Schuh7f120362022-03-05 17:10:11 -0800378 position->intake_back(),
379 output != nullptr ? &output_struct.intake_voltage_back : nullptr,
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800380 status->fbb());
381
Ravago Jones5da06352022-03-04 20:26:24 -0800382 const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
Austin Schuh7f120362022-03-05 17:10:11 -0800383 turret_status_offset = turret_.Iterate(
384 turret_goal, position->turret(),
385 output != nullptr ? &output_struct.turret_voltage : nullptr,
386 status->fbb());
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800387
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800388 if (output != nullptr) {
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800389 output_struct.roller_voltage_front = roller_speed_compensated_front;
390 output_struct.roller_voltage_back = roller_speed_compensated_back;
Milind Upadhyayb1a74ea2022-03-09 20:34:35 -0800391 output_struct.transfer_roller_voltage_front = transfer_roller_speed_front;
392 output_struct.transfer_roller_voltage_back = transfer_roller_speed_back;
Ravago Jones5da06352022-03-04 20:26:24 -0800393 output_struct.flipper_arms_voltage = flipper_arms_voltage;
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800394
milind-u086d7262022-01-19 20:44:18 -0800395 output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
396 }
397
398 Status::Builder status_builder = status->MakeBuilder<Status>();
399
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800400 const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
Ravago Jones5da06352022-03-04 20:26:24 -0800401 turret_.zeroed() && climber_.zeroed() &&
402 catapult_.zeroed();
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800403 const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
Ravago Jones5da06352022-03-04 20:26:24 -0800404 turret_.estopped() || climber_.estopped() ||
405 catapult_.estopped();
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800406
407 status_builder.add_zeroed(zeroed);
408 status_builder.add_estopped(estopped);
409
410 status_builder.add_intake_front(intake_status_offset_front);
411 status_builder.add_intake_back(intake_status_offset_back);
412 status_builder.add_turret(turret_status_offset);
Siddhant Kanwar0e37f592022-02-21 19:26:50 -0800413 status_builder.add_climber(climber_status_offset);
Ravago Jones5da06352022-03-04 20:26:24 -0800414
Austin Schuh39f26f62022-02-24 21:34:46 -0800415 status_builder.add_catapult(catapult_status_offset);
416 status_builder.add_solve_time(catapult_.solve_time());
Austin Schuh80fc2752022-02-25 13:33:56 -0800417 status_builder.add_shot_count(catapult_.shot_count());
Ravago Jones5da06352022-03-04 20:26:24 -0800418 status_builder.add_mpc_active(catapult_.mpc_active());
419
420 status_builder.add_flippers_open(flippers_open_);
421 status_builder.add_reseating_in_catapult(reseating_in_catapult_);
422 status_builder.add_fire(fire_);
423 status_builder.add_state(state_);
424 status_builder.add_intake_state(intake_state_);
milind-u086d7262022-01-19 20:44:18 -0800425
James Kuszmaul84083f42022-02-27 17:24:38 -0800426 status_builder.add_aimer(aimer_offset);
427
milind-u086d7262022-01-19 20:44:18 -0800428 (void)status->Send(status_builder.Finish());
429}
430
Henry Speiser55aa3ba2022-02-21 23:21:12 -0800431double Superstructure::robot_velocity() const {
432 return (drivetrain_status_fetcher_.get() != nullptr
433 ? drivetrain_status_fetcher_->robot_speed()
434 : 0.0);
435}
436
milind-u086d7262022-01-19 20:44:18 -0800437} // namespace superstructure
438} // namespace control_loops
439} // namespace y2022