Use setpoint in joystick reader
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I48f5b367db3c5f161e74edb20f1c4a78bfe53d7e
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index ce5c755..b5b26d1 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -182,11 +182,20 @@
}
if (data.IsPressed(kTurret)) {
- turret_pos = -1.5;
+ if (setpoint_fetcher_.get()) {
+ turret_pos = setpoint_fetcher_->turret();
+ } else {
+ turret_pos = -1.5;
+ }
} else {
turret_pos = 0.0;
}
+ if (setpoint_fetcher_.get()) {
+ catapult_pos = setpoint_fetcher_->catapult_position();
+ catapult_speed = setpoint_fetcher_->catapult_velocity();
+ }
+
// Keep the catapult return position at the shot one if kCatapultPos is
// pressed
if (data.IsPressed(kCatapultPos)) {