Teach superstructure when the motors are disabled.
Otherwise we are winding up when disabled.
Change-Id: Ie4773793fbc6aa602188c1cdbd2cc9e0759b032d
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index aa5eee8..affc25b 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -349,24 +349,29 @@
const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
climber_status_offset = climber_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->climber() : nullptr,
- position->climber(), &output_struct.climber_voltage, status->fbb());
+ position->climber(),
+ output != nullptr ? &output_struct.climber_voltage : nullptr,
+ status->fbb());
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
intake_status_offset_front = intake_front_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake_front() : nullptr,
- position->intake_front(), &output_struct.intake_voltage_front,
+ position->intake_front(),
+ output != nullptr ? &output_struct.intake_voltage_front : nullptr,
status->fbb());
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
intake_status_offset_back = intake_back_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake_back() : nullptr,
- position->intake_back(), &output_struct.intake_voltage_back,
+ position->intake_back(),
+ output != nullptr ? &output_struct.intake_voltage_back : nullptr,
status->fbb());
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- turret_status_offset =
- turret_.Iterate(turret_goal, position->turret(),
- &output_struct.turret_voltage, status->fbb());
+ turret_status_offset = turret_.Iterate(
+ turret_goal, position->turret(),
+ output != nullptr ? &output_struct.turret_voltage : nullptr,
+ status->fbb());
if (output != nullptr) {
output_struct.roller_voltage_front = roller_speed_compensated_front;