Add voltage offset to the test to confirm it works
This was one of my theories for why the wrist wasn't working.
Change-Id: If79465b562a799587b7b0f953143381cb290c6a5
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 3b4d2b0..03c6269 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -44,6 +44,7 @@
AOS_LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
end_effector_.Reset();
+ wrist_.Reset();
}
OutputT output_struct;
@@ -67,10 +68,11 @@
status->fbb());
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> wrist_offset =
- wrist_.Iterate(unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
- position->wrist(),
- output != nullptr ? &output_struct.wrist_voltage : nullptr,
- status->fbb());
+ wrist_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
+ position->wrist(),
+ output != nullptr ? &(output_struct.wrist_voltage) : nullptr,
+ status->fbb());
EndEffectorState end_effector_state = end_effector_.RunIteration(
timestamp,
@@ -83,8 +85,8 @@
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(true);
- status_builder.add_estopped(false);
+ status_builder.add_zeroed(wrist_.zeroed() && arm_.zeroed());
+ status_builder.add_estopped(wrist_.estopped() || arm_.estopped());
status_builder.add_arm(arm_status_offset);
status_builder.add_wrist(wrist_offset);
status_builder.add_end_effector_state(end_effector_state);