Started work on creating a shooter action with Austin.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index e82710b..815210f 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -738,7 +738,19 @@
output->bottom_claw_voltage = -kMaxVoltage;
}
}
- status->done = false;
+
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.005;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) <
+ 0.005;
+ status->done = is_ready() && separation_done && bottom_done;
+
+ status->bottom = bottom_absolute_position();
+ status->separation = top_absolute_position() - bottom_absolute_position();
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
was_enabled_ = ::aos::robot_state->enabled;
}
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index f1d2f93..c67ed20 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -51,6 +51,16 @@
double tusk_voltage;
};
+ message Status {
+ // True if zeroed and within tolerance for separation and bottom angle.
+ bool done;
+ // Dump the values of the state matrix.
+ double bottom;
+ double bottom_velocity;
+ double separation;
+ double separation_velocity;
+ };
+
queue Goal goal;
queue Position position;
queue Output output;