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);
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index adfa2a5..b94054d 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -394,6 +394,8 @@
// status before checking it.
} while (superstructure_status_fetcher_.get() == nullptr ||
!superstructure_status_fetcher_.get()->zeroed());
+
+ superstructure_plant_.wrist()->set_voltage_offset(1.0);
}
void SendRobotVelocity(double robot_velocity) {
@@ -439,7 +441,7 @@
std::unique_ptr<aos::logger::Logger> logger_;
const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
-}; // namespace testing
+};
// Tests that the superstructure does nothing when the goal is to remain
// still.
@@ -736,6 +738,7 @@
// Tests that we don't freak out without a goal.
TEST_F(SuperstructureTest, ArmSimpleGoal) {
SetEnabled(true);
+ WaitUntilZeroed();
RunFor(chrono::seconds(20));
{
@@ -753,6 +756,7 @@
// Tests that we can can execute a move.
TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
SetEnabled(true);
+ WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -778,6 +782,7 @@
// Tests that we can can execute a move which moves through multiple nodes.
TEST_F(SuperstructureTest, ArmMultistepMove) {
SetEnabled(true);
+ WaitUntilZeroed();
superstructure_plant_.InitializeArmPosition(arm::NeutralPosPoint());
{